scoutos / scout / scoutsim / src / scout_teleop_userinput.cpp @ 4c495190
History | View | Annotate | Download (1.52 KB)
1 |
#include "scout_teleop_userinput.h" |
---|---|
2 |
|
3 |
TeleopInput::TeleopInput(std::string scoutname)
|
4 |
{ |
5 |
input_pub = node.advertise<scoutsim::teleop_input>(scoutname+"/teleop_input", QUEUE_SIZE);
|
6 |
kfd = 0;
|
7 |
|
8 |
// get the console in raw mode
|
9 |
tcgetattr(kfd, &cooked); |
10 |
memcpy(&raw, &cooked, sizeof(struct termios)); |
11 |
raw.c_lflag &=~ (ICANON | ECHO); |
12 |
// Setting a new line, then end of file
|
13 |
raw.c_cc[VEOL] = 1;
|
14 |
raw.c_cc[VEOF] = 2;
|
15 |
tcsetattr(kfd, TCSANOW, &raw); |
16 |
|
17 |
//TeleopInput::keyLoop();
|
18 |
return;
|
19 |
} |
20 |
|
21 |
TeleopInput::~TeleopInput() |
22 |
{ |
23 |
// reset console mode to no longer be raw
|
24 |
tcsetattr(kfd, TCSANOW, &cooked); |
25 |
ros::shutdown(); |
26 |
exit(0);
|
27 |
} |
28 |
|
29 |
void TeleopInput::keyLoop()
|
30 |
{ |
31 |
ROS_INFO("press keys to start moving!");
|
32 |
char pressed_key;
|
33 |
|
34 |
while (ros::ok())
|
35 |
{ |
36 |
// get the next event from the keyboard
|
37 |
if(read(kfd, &pressed_key, 1) < 0) |
38 |
{ |
39 |
perror("read():");
|
40 |
exit(-1);
|
41 |
} |
42 |
|
43 |
// note: printing this normally slows things the hell down
|
44 |
ROS_DEBUG("pressed key 0x%02X at time: %f\n", pressed_key, ros::Time::now().toSec());
|
45 |
|
46 |
scoutsim::teleop_input msg; |
47 |
msg.last_key = pressed_key; |
48 |
msg.header.stamp = ros::Time::now(); |
49 |
input_pub.publish(msg); |
50 |
|
51 |
ros::spinOnce(); |
52 |
} |
53 |
return;
|
54 |
} |
55 |
|
56 |
int main(int argc, char** argv) |
57 |
{ |
58 |
ros::init(argc, argv, "scout_teleop_userinput");
|
59 |
TeleopInput teleop_input = TeleopInput("scout1");
|
60 |
//signal(SIGINT,quit);
|
61 |
teleop_input.keyLoop(); |
62 |
|
63 |
return(0); |
64 |
} |
65 |
|