root / scout / scoutsim / src / scout_teleop_userinput.cpp @ 28999371
History | View | Annotate | Download (1.93 KB)
1 | 86222358 | roboclub | #include "scout_teleop_userinput.h" |
---|---|---|---|
2 | |||
3 | 2bd58ddb | roboclub | using namespace std; |
4 | |||
5 | TeleopInput::TeleopInput(string scoutname)
|
||
6 | 86222358 | roboclub | { |
7 | 2bd58ddb | roboclub | input_pub = node.advertise<scoutsim::teleop_input>( |
8 | scoutname+"/teleop_input", QUEUE_SIZE);
|
||
9 | 86222358 | roboclub | 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 | 4c495190 | roboclub | ros::shutdown(); |
29 | exit(0);
|
||
30 | 86222358 | roboclub | } |
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 | 2bd58ddb | roboclub | 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 | 4c495190 | roboclub | //signal(SIGINT,quit);
|
74 | 86222358 | roboclub | teleop_input.keyLoop(); |
75 | 2bd58ddb | roboclub | |
76 | 86222358 | roboclub | return(0); |
77 | } |