scoutos / scout / scoutsim / src / old_scout_teleop.cpp @ 5755691e
History | View | Annotate | Download (3.07 KB)
1 |
#include <ros/ros.h> |
---|---|
2 |
#include <motors/set_motors.h> |
3 |
#include <signal.h> |
4 |
#include <termios.h> |
5 |
#include <stdio.h> |
6 |
|
7 |
// TODO: add this to a constants.h
|
8 |
#define QUEUE_SIZE 10 |
9 |
|
10 |
#define KEYCODE_R 0x43 |
11 |
#define KEYCODE_L 0x44 |
12 |
#define KEYCODE_U 0x41 |
13 |
#define KEYCODE_D 0x42 |
14 |
#define KEYCODE_Q 0x71 |
15 |
#define KEYCODE_S 0x73 |
16 |
|
17 |
class TeleopScout |
18 |
{ |
19 |
public:
|
20 |
TeleopScout(); |
21 |
void keyLoop();
|
22 |
|
23 |
private:
|
24 |
ros::NodeHandle nh_; |
25 |
bool fl_set_, fr_set_, bl_set_, br_set_;
|
26 |
int fl_speed_, fr_speed_, bl_speed_, br_speed_;
|
27 |
char last_key;
|
28 |
ros::Publisher motors_pub_; |
29 |
}; |
30 |
|
31 |
TeleopScout::TeleopScout() |
32 |
{ |
33 |
fl_set_ = fr_set_ = bl_set_ = br_set_ = true;
|
34 |
fl_speed_ = fr_speed_ = bl_speed_ = br_speed_ = 0;
|
35 |
motors_pub_ = nh_.advertise<motors::set_motors>( |
36 |
"scout1/set_motors", QUEUE_SIZE);
|
37 |
} |
38 |
|
39 |
int kfd = 0; |
40 |
struct termios cooked, raw;
|
41 |
|
42 |
void quit(int sig) |
43 |
{ |
44 |
tcsetattr(kfd, TCSANOW, &cooked); |
45 |
ros::shutdown(); |
46 |
exit(0);
|
47 |
} |
48 |
|
49 |
|
50 |
int main(int argc, char** argv) |
51 |
{ |
52 |
ros::init(argc, argv, "teleop_scout");
|
53 |
TeleopScout teleop_scout; |
54 |
|
55 |
signal(SIGINT,quit); |
56 |
|
57 |
teleop_scout.keyLoop(); |
58 |
|
59 |
return(0); |
60 |
} |
61 |
|
62 |
|
63 |
void TeleopScout::keyLoop()
|
64 |
{ |
65 |
char c;
|
66 |
bool dirty=false; |
67 |
|
68 |
// get the console in raw mode
|
69 |
tcgetattr(kfd, &cooked); |
70 |
memcpy(&raw, &cooked, sizeof(struct termios)); |
71 |
raw.c_lflag &=~ (ICANON | ECHO); |
72 |
// Setting a new line, then end of file
|
73 |
raw.c_cc[VEOL] = 1;
|
74 |
raw.c_cc[VEOF] = 2;
|
75 |
tcsetattr(kfd, TCSANOW, &raw); |
76 |
|
77 |
puts("Reading from keyboard");
|
78 |
puts("---------------------------");
|
79 |
puts("Use arrow keys to move the scout.");
|
80 |
|
81 |
|
82 |
for(;;)
|
83 |
{ |
84 |
ROS_INFO("loop pass engaged");
|
85 |
// get the next event from the keyboard
|
86 |
if(read(kfd, &c, 1) < 0) |
87 |
{ |
88 |
perror("read():");
|
89 |
exit(-1);
|
90 |
} |
91 |
|
92 |
//ROS_DEBUG("value: 0x%02X\n", c);
|
93 |
//ROS_INFO("value: 0x%02X\n", c);
|
94 |
switch(c)
|
95 |
{ |
96 |
case KEYCODE_L:
|
97 |
ROS_INFO("LEFT");
|
98 |
fl_speed_ = bl_speed_ = 200;
|
99 |
fr_speed_ = br_speed_ = -200;
|
100 |
dirty = true;
|
101 |
break;
|
102 |
case KEYCODE_R:
|
103 |
ROS_INFO("RIGHT");
|
104 |
fl_speed_ = bl_speed_ = -200;
|
105 |
fr_speed_ = br_speed_ = 200;
|
106 |
dirty = true;
|
107 |
break;
|
108 |
case KEYCODE_U:
|
109 |
ROS_INFO("UP");
|
110 |
fl_speed_ = bl_speed_ = 200;
|
111 |
fr_speed_ = br_speed_ = 200;
|
112 |
dirty = true;
|
113 |
break;
|
114 |
case KEYCODE_D:
|
115 |
ROS_INFO("DOWN");
|
116 |
fl_speed_ = bl_speed_ = -200;
|
117 |
fr_speed_ = br_speed_ = -200;
|
118 |
dirty = true;
|
119 |
break;
|
120 |
default:
|
121 |
ROS_INFO("test");
|
122 |
fl_speed_ = bl_speed_ = 0;
|
123 |
fr_speed_ = br_speed_ = 0;
|
124 |
dirty = true;
|
125 |
break;
|
126 |
} |
127 |
|
128 |
motors::set_motors msg; |
129 |
msg.fl_set = fl_set_; |
130 |
msg.fr_set = fr_set_; |
131 |
msg.bl_set = bl_set_; |
132 |
msg.br_set = br_set_; |
133 |
msg.fl_speed = fl_speed_; |
134 |
msg.fr_speed = fr_speed_; |
135 |
msg.bl_speed = bl_speed_; |
136 |
msg.br_speed = br_speed_; |
137 |
|
138 |
if(dirty == true) |
139 |
{ |
140 |
if (last_key != c)
|
141 |
{ |
142 |
motors_pub_.publish(msg); |
143 |
} |
144 |
dirty=false;
|
145 |
} |
146 |
last_key = c; |
147 |
} |
148 |
|
149 |
return;
|
150 |
} |
151 |
|
152 |
|
153 |
|