00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00039 #include <ros/ros.h>
00040 #include <std_msgs/Header.h>
00041 #include <tf/transform_listener.h>
00042
00043 #include <signal.h>
00044 #include <termios.h>
00045 #include <stdio.h>
00046 #include <cstdlib>
00047
00048 #define KEYCODE_R 0x43
00049 #define KEYCODE_L 0x44
00050 #define KEYCODE_U 0x41
00051 #define KEYCODE_D 0x42
00052 #define KEYCODE_Q 0x71
00053 #define KEYCODE_E 0x65
00054
00055
00056 class Teleop
00057 {
00058 public:
00059 Teleop();
00060 void keyLoop();
00061
00062 private:
00063
00064 ros::NodeHandle nodehandle;
00065 ros::Publisher tag_pub_;
00066 ros::Subscriber bag_sub_;
00067 std_msgs::Header header;
00068 void bagCallback(const tf::tfMessage& msg);
00069 };
00070
00071
00072 Teleop::Teleop()
00073 {
00074 tag_pub_ = nodehandle.advertise<std_msgs::Header>("/timetag", 1);
00075 bag_sub_ = nodehandle.subscribe("/tf", 1000, &Teleop::bagCallback, this);
00076 }
00077
00078 int kfd = 0;
00079 struct termios cooked, raw;
00080
00081 void quit(int sig)
00082 {
00083 tcsetattr(kfd, TCSANOW, &cooked);
00084 ros::shutdown();
00085 exit(0);
00086 }
00087
00088
00089 int main(int argc, char** argv)
00090 {
00091 ros::init(argc, argv, "keycapture");
00092 Teleop teleop_turtle;
00093
00094 signal(SIGINT,quit);
00095
00096 ros::Rate loop_rate(10);
00097
00098 while (ros::ok())
00099 {
00100 teleop_turtle.keyLoop();
00101 ros::spinOnce();
00102 loop_rate.sleep();
00103 }
00104
00105 return(0);
00106 }
00107
00108
00109 void Teleop::keyLoop()
00110 {
00111 char c;
00112 bool dirty=false;
00113
00114
00115 tcgetattr(kfd, &cooked);
00116 memcpy(&raw, &cooked, sizeof(struct termios));
00117 raw.c_lflag &=~ (ICANON | ECHO);
00118
00119
00120 raw.c_cc[VEOL] = 1;
00121 raw.c_cc[VEOF] = 2;
00122 tcsetattr(kfd, TCSANOW, &raw);
00123
00124 puts("---------------------------");
00125 puts("Use arrow keys to generate tag!");
00126
00127
00128 if(read(kfd, &c, 1) < 0)
00129 {
00130 perror("read():");
00131 exit(-1);
00132 }
00133
00134 ROS_DEBUG("value: 0x%02X\n", c);
00135
00136 switch(c)
00137 {
00138 case KEYCODE_L:
00139 ROS_DEBUG("LEFT");
00140 ROS_INFO("left");
00141 dirty = true;
00142 break;
00143 case KEYCODE_R:
00144 ROS_DEBUG("RIGHT");
00145 ROS_INFO("right");
00146 dirty = true;
00147 break;
00148 case KEYCODE_U:
00149 ROS_DEBUG("UP");
00150 ROS_INFO("up");
00151 dirty = true;
00152 break;
00153 case KEYCODE_D:
00154 ROS_DEBUG("DOWN");
00155 ROS_INFO("down");
00156 dirty = true;
00157 break;
00158 case KEYCODE_Q:
00159 ROS_DEBUG("NEG PHI");
00160 ROS_INFO("neg phi");
00161 dirty = true;
00162 break;
00163 case KEYCODE_E:
00164 ROS_DEBUG("POS PHI");
00165 ROS_INFO("pos phi");
00166 dirty = true;
00167 break;
00168 }
00169
00170 if(dirty ==true)
00171 {
00172 ROS_INFO("time:%f",header.stamp.toSec());
00173 tag_pub_.publish(header);
00174 dirty=false;
00175 }
00176
00177 return;
00178 }
00179
00180 void Teleop::bagCallback(const tf::tfMessage& msg)
00181 {
00182 header.stamp = msg.transforms[0].header.stamp;
00183 return;
00184 }
00185