Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <sys/poll.h>
- #include "ros/ros.h"
- #include <geometry_msgs/Twist.h>
- #define KEYCODE_W 0x77
- #define KEYCODE_X 0x78
- #define KEYCODE_E 0x65
- #define KEYCODE_C 0x63
- double max_speed = 0.500; // m/second
- double max_turn = 60.0*M_PI/180.0; // rad/second
- int kfd = 0;
- int main(int argc, char **argv)
- {
- ros::init(argc, argv, "talker");
- ros::NodeHandle n;
- ros::Publisher chatter_pub = n.advertise<geometry_msgs::Twist>("chatter", 1000);
- ros::Rate loop_rate(10);
- int count = 1;
- while (ros::ok())
- {
- geometry_msgs::Twist cmdvel;
- char c;
- int speed=0;
- int turn=0;
- bool dirty=false;
- puts("Reading from keyboard");
- puts("---------------------------");
- puts("w/x : increase/decrease max linear speed by 10%");
- puts("e/c : increase/decrease max angular speed by 10%");
- puts("---------------------------");
- puts("anything else : stop");
- puts("---------------------------");
- //Keyboard polling
- struct pollfd ufd;
- ufd.fd = kfd;
- ufd.events = POLLIN;
- for(;;)
- {
- // get the next event from the keyboard
- int num;
- if((num = poll(&ufd, 1, 250)) < 0)
- {
- perror("poll():");
- break;
- //return;
- }
- else if(num > 0)
- {
- if(read(kfd, &c, 1) < 0)
- {
- perror("read():");
- break;
- // return;
- }
- }
- else
- continue;
- switch(c)
- {
- case KEYCODE_W:
- max_speed += max_speed / 10.0;
- // if(always_command)
- // dirty = true;
- break;
- case KEYCODE_X:
- max_speed -= max_speed / 10.0;
- // if(always_command)
- // dirty = true;
- break;
- case KEYCODE_E:
- max_turn += max_turn / 10.0;
- // if(always_command)
- // dirty = true;
- break;
- case KEYCODE_C:
- max_turn -= max_turn / 10.0;
- // if(always_command)
- // dirty = true;
- break;
- default:
- speed = 0;
- turn = 0;
- dirty = true;
- }
- if (dirty == true)
- {
- cmdvel.linear.x = speed * max_speed;
- cmdvel.angular.z = turn * max_turn;
- chatter_pub.publish(cmdvel);
- //Actual information:
- printf("The speed information sent is: \n Linear speed x %f \n Angular speed z %f \n ", cmdvel.linear.x,cmdvel.angular.z);
- }
- }
- ros::spinOnce();
- loop_rate.sleep();
- }
- return 0;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement