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