root / scout / scoutsim / src / scout_teleop.cpp @ c63c9752
History | View | Annotate | Download (3.45 KB)
1 | e1d66e9a | roboclub | #include "scout_teleop.h" |
---|---|---|---|
2 | aa5e4ddc | Leon | |
3 | 2bd58ddb | roboclub | using namespace std; |
4 | |||
5 | TeleopScout::TeleopScout(string scoutname)
|
||
6 | aa5e4ddc | Leon | { |
7 | e1d66e9a | roboclub | fl_set = fr_set = bl_set = br_set = true;
|
8 | fl_speed = fr_speed = bl_speed = br_speed = 0;
|
||
9 | motors_pub = node.advertise<motors::set_motors>( |
||
10 | scoutname+"/set_motors", QUEUE_SIZE);
|
||
11 | key_input_sub = node.subscribe(scoutname+"/teleop_input",
|
||
12 | QUEUE_SIZE, |
||
13 | &TeleopScout::teleop_input_callback, |
||
14 | this);
|
||
15 | aa5e4ddc | Leon | } |
16 | |||
17 | e1d66e9a | roboclub | void TeleopScout::teleop_input_callback(
|
18 | const scoutsim::teleop_input::ConstPtr& msg)
|
||
19 | aa5e4ddc | Leon | { |
20 | e1d66e9a | roboclub | last_key = msg->last_key; |
21 | last_time = msg->header.stamp; |
||
22 | 9bde6dd9 | roboclub | //ROS_INFO("received key: 0x%02X ", last_key);
|
23 | //ROS_INFO("at last time: %f\n", last_time.toSec());
|
||
24 | aa5e4ddc | Leon | } |
25 | |||
26 | e1d66e9a | roboclub | void TeleopScout::keyListenerLoop()
|
27 | 9bde6dd9 | roboclub | { |
28 | ROS_INFO("waiting for userinput commands...\n");
|
||
29 | ROS_INFO("current max wait time is: %f\n", MAX_WAIT_TIME.toSec());
|
||
30 | e1d66e9a | roboclub | while(ros::ok())
|
31 | aa5e4ddc | Leon | { |
32 | 9bde6dd9 | roboclub | // if we haven't passed the waiting threshold
|
33 | if(last_time + MAX_WAIT_TIME > ros::Time::now())
|
||
34 | e1d66e9a | roboclub | { |
35 | switch(last_key)
|
||
36 | { |
||
37 | 4c495190 | roboclub | // TODO: for some reason negative speeds go forward, not backward
|
38 | |||
39 | e1d66e9a | roboclub | case KEYCODE_L:
|
40 | //ROS_INFO("LEFT");
|
||
41 | fl_speed = bl_speed = 200;
|
||
42 | fr_speed = br_speed = -200;
|
||
43 | break;
|
||
44 | case KEYCODE_R:
|
||
45 | //ROS_INFO("RIGHT");
|
||
46 | fl_speed = bl_speed = -200;
|
||
47 | fr_speed = br_speed = 200;
|
||
48 | break;
|
||
49 | case KEYCODE_U:
|
||
50 | //ROS_INFO("UP");
|
||
51 | 4c495190 | roboclub | fl_speed = bl_speed = -200;
|
52 | fr_speed = br_speed = -200;
|
||
53 | e1d66e9a | roboclub | break;
|
54 | case KEYCODE_D:
|
||
55 | //ROS_INFO("DOWN");
|
||
56 | 4c495190 | roboclub | fl_speed = bl_speed = 200;
|
57 | fr_speed = br_speed = 200;
|
||
58 | e1d66e9a | roboclub | break;
|
59 | default:
|
||
60 | //don't publish a message yet; no change to key,
|
||
61 | // and no timeout yet
|
||
62 | 9bde6dd9 | roboclub | //ROS_INFO("no time out yet\n");
|
63 | e1d66e9a | roboclub | ros::spinOnce(); |
64 | continue;
|
||
65 | break;
|
||
66 | } |
||
67 | } |
||
68 | else if (fl_speed != 0 || fr_speed != 0 || |
||
69 | bl_speed != 0 || br_speed != 0) |
||
70 | { |
||
71 | 9bde6dd9 | roboclub | ROS_INFO("no key press; stopping movement");
|
72 | e1d66e9a | roboclub | fl_speed = fr_speed = bl_speed = br_speed = 0;
|
73 | } |
||
74 | |||
75 | //send command to the motors
|
||
76 | motors::set_motors msg; |
||
77 | msg.fl_set = fl_set; |
||
78 | msg.fr_set = fr_set; |
||
79 | msg.bl_set = bl_set; |
||
80 | msg.br_set = br_set; |
||
81 | msg.fl_speed = fl_speed; |
||
82 | msg.fr_speed = fr_speed; |
||
83 | msg.bl_speed = bl_speed; |
||
84 | msg.br_speed = br_speed; |
||
85 | motors_pub.publish(msg); |
||
86 | |||
87 | ros::spinOnce(); |
||
88 | aa5e4ddc | Leon | } |
89 | e1d66e9a | roboclub | return;
|
90 | } |
||
91 | aa5e4ddc | Leon | |
92 | e1d66e9a | roboclub | int main(int argc, char** argv) |
93 | { |
||
94 | 2bd58ddb | roboclub | string scoutname = ""; |
95 | if(argc != 2) |
||
96 | { |
||
97 | cout << "Opens up a teleop process to send input to the motors" << endl;
|
||
98 | cout << "(Remember to create a scout_teleop_userinput process as well to actually capture keyboard input!)" << endl;
|
||
99 | cout << "Usage: " << argv[0] <<" <scoutname>" << endl; |
||
100 | exit(0);
|
||
101 | } |
||
102 | |||
103 | scoutname = argv[1];
|
||
104 | ros::init(argc, argv, scoutname+"_scout_teleop");
|
||
105 | TeleopScout teleop_scout = TeleopScout(scoutname); |
||
106 | aa5e4ddc | Leon | |
107 | e1d66e9a | roboclub | teleop_scout.keyListenerLoop(); |
108 | aa5e4ddc | Leon | |
109 | e1d66e9a | roboclub | return(0); |
110 | aa5e4ddc | Leon | } |