root / scout / scoutsim / src / turtle_teleop.cpp @ 2237e1f0
History | View | Annotate | Download (2.36 KB)
1 | aa5e4ddc | Leon | #include <ros/ros.h> |
---|---|---|---|
2 | #include <signal.h> |
||
3 | #include <termios.h> |
||
4 | #include <stdio.h> |
||
5 | #include "constants.h" |
||
6 | |||
7 | #define KEYCODE_R 0x43 |
||
8 | #define KEYCODE_L 0x44 |
||
9 | #define KEYCODE_U 0x41 |
||
10 | #define KEYCODE_D 0x42 |
||
11 | #define KEYCODE_Q 0x71 |
||
12 | |||
13 | class TeleopTurtle |
||
14 | { |
||
15 | public:
|
||
16 | TeleopTurtle(); |
||
17 | void keyLoop();
|
||
18 | |||
19 | private:
|
||
20 | |||
21 | |||
22 | ros::NodeHandle nh_; |
||
23 | double linear_, angular_, l_scale_, a_scale_;
|
||
24 | ros::Publisher vel_pub_; |
||
25 | |||
26 | }; |
||
27 | |||
28 | TeleopTurtle::TeleopTurtle(): |
||
29 | linear_(0),
|
||
30 | angular_(0),
|
||
31 | l_scale_(2.0), |
||
32 | a_scale_(2.0) |
||
33 | { |
||
34 | nh_.param("scale_angular", a_scale_, a_scale_);
|
||
35 | nh_.param("scale_linear", l_scale_, l_scale_);
|
||
36 | |||
37 | vel_pub_ = nh_.advertise<turtlesim::Velocity>("turtle1/command_velocity", 1); |
||
38 | } |
||
39 | |||
40 | int kfd = 0; |
||
41 | struct termios cooked, raw;
|
||
42 | |||
43 | void quit(int sig) |
||
44 | { |
||
45 | tcsetattr(kfd, TCSANOW, &cooked); |
||
46 | ros::shutdown(); |
||
47 | exit(0);
|
||
48 | } |
||
49 | |||
50 | |||
51 | int main(int argc, char** argv) |
||
52 | { |
||
53 | ros::init(argc, argv, "teleop_turtle");
|
||
54 | TeleopTurtle teleop_turtle; |
||
55 | |||
56 | signal(SIGINT,quit); |
||
57 | |||
58 | teleop_turtle.keyLoop(); |
||
59 | |||
60 | return(0); |
||
61 | } |
||
62 | |||
63 | |||
64 | void TeleopTurtle::keyLoop()
|
||
65 | { |
||
66 | char c;
|
||
67 | bool dirty=false; |
||
68 | |||
69 | |||
70 | // get the console in raw mode
|
||
71 | tcgetattr(kfd, &cooked); |
||
72 | memcpy(&raw, &cooked, sizeof(struct termios)); |
||
73 | raw.c_lflag &=~ (ICANON | ECHO); |
||
74 | // Setting a new line, then end of file
|
||
75 | raw.c_cc[VEOL] = 1;
|
||
76 | raw.c_cc[VEOF] = 2;
|
||
77 | tcsetattr(kfd, TCSANOW, &raw); |
||
78 | |||
79 | puts("Reading from keyboard");
|
||
80 | puts("---------------------------");
|
||
81 | puts("Use arrow keys to move the turtle.");
|
||
82 | |||
83 | |||
84 | for(;;)
|
||
85 | { |
||
86 | // get the next event from the keyboard
|
||
87 | if(read(kfd, &c, 1) < 0) |
||
88 | { |
||
89 | perror("read():");
|
||
90 | exit(-1);
|
||
91 | } |
||
92 | |||
93 | linear_=angular_=0;
|
||
94 | ROS_DEBUG("value: 0x%02X\n", c);
|
||
95 | |||
96 | switch(c)
|
||
97 | { |
||
98 | case KEYCODE_L:
|
||
99 | ROS_DEBUG("LEFT");
|
||
100 | angular_ = 1.0; |
||
101 | dirty = true;
|
||
102 | break;
|
||
103 | case KEYCODE_R:
|
||
104 | ROS_DEBUG("RIGHT");
|
||
105 | angular_ = -1.0; |
||
106 | dirty = true;
|
||
107 | break;
|
||
108 | case KEYCODE_U:
|
||
109 | ROS_DEBUG("UP");
|
||
110 | linear_ = 1.0; |
||
111 | dirty = true;
|
||
112 | break;
|
||
113 | case KEYCODE_D:
|
||
114 | ROS_DEBUG("DOWN");
|
||
115 | linear_ = -1.0; |
||
116 | dirty = true;
|
||
117 | break;
|
||
118 | } |
||
119 | |||
120 | |||
121 | turtlesim::Velocity vel; |
||
122 | vel.angular = a_scale_*angular_; |
||
123 | vel.linear = l_scale_*linear_; |
||
124 | if(dirty ==true) |
||
125 | { |
||
126 | vel_pub_.publish(vel); |
||
127 | dirty=false;
|
||
128 | } |
||
129 | } |
||
130 | |||
131 | |||
132 | return;
|
||
133 | } |
||
134 | |||
135 |