Advertisement
Guest User

teleop modification

a guest
Feb 27th, 2012
68
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 2.83 KB | None | 0 0
  1. #include <sys/poll.h>
  2. #include "ros/ros.h"
  3. #include <geometry_msgs/Twist.h>
  4. #define KEYCODE_W 0x77
  5. #define KEYCODE_X 0x78
  6. #define KEYCODE_E 0x65
  7. #define KEYCODE_C 0x63
  8.  
  9. double max_speed = 0.500; // m/second
  10. double max_turn = 60.0*M_PI/180.0; // rad/second
  11. int kfd = 0;
  12.  
  13. int main(int argc, char **argv)
  14. {
  15. ros::init(argc, argv, "talker");
  16. ros::NodeHandle n;
  17. ros::Publisher chatter_pub = n.advertise<geometry_msgs::Twist>("chatter", 1000);
  18. ros::Rate loop_rate(10);
  19.  
  20. int count = 1;
  21. while (ros::ok())
  22. {
  23. geometry_msgs::Twist cmdvel;
  24. char c;
  25. int speed=0;
  26. int turn=0;
  27. bool dirty=false;
  28. puts("Reading from keyboard");
  29. puts("---------------------------");
  30. puts("w/x : increase/decrease max linear speed by 10%");
  31. puts("e/c : increase/decrease max angular speed by 10%");
  32. puts("---------------------------");
  33. puts("anything else : stop");
  34. puts("---------------------------");
  35.  
  36. //Keyboard polling
  37. struct pollfd ufd;
  38. ufd.fd = kfd;
  39. ufd.events = POLLIN;
  40. for(;;)
  41. {
  42. // get the next event from the keyboard
  43. int num;
  44. if((num = poll(&ufd, 1, 250)) < 0)
  45. {
  46. perror("poll():");
  47. break;
  48. //return;
  49. }
  50. else if(num > 0)
  51. {
  52. if(read(kfd, &c, 1) < 0)
  53. {
  54. perror("read():");
  55. break;
  56. // return;
  57. }
  58. }
  59. else
  60. continue;
  61.  
  62. switch(c)
  63. {
  64. case KEYCODE_W:
  65. max_speed += max_speed / 10.0;
  66. // if(always_command)
  67. // dirty = true;
  68. break;
  69. case KEYCODE_X:
  70. max_speed -= max_speed / 10.0;
  71. // if(always_command)
  72. // dirty = true;
  73. break;
  74. case KEYCODE_E:
  75. max_turn += max_turn / 10.0;
  76. // if(always_command)
  77. // dirty = true;
  78. break;
  79. case KEYCODE_C:
  80. max_turn -= max_turn / 10.0;
  81. // if(always_command)
  82. // dirty = true;
  83. break;
  84. default:
  85. speed = 0;
  86. turn = 0;
  87. dirty = true;
  88. }
  89. if (dirty == true)
  90. {
  91. cmdvel.linear.x = speed * max_speed;
  92. cmdvel.angular.z = turn * max_turn;
  93.  
  94. chatter_pub.publish(cmdvel);
  95. //Actual information:
  96. printf("The speed information sent is: \n Linear speed x %f \n Angular speed z %f \n ", cmdvel.linear.x,cmdvel.angular.z);
  97.  
  98. }
  99. }
  100. ros::spinOnce();
  101. loop_rate.sleep();
  102. }
  103.  
  104.  
  105. return 0;
  106. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement