root / scout / scoutsim / src / scout_teleop.cpp @ 04114d13
History | View | Annotate | Download (3.45 KB)
1 |
#include "scout_teleop.h" |
---|---|
2 |
|
3 |
using namespace std; |
4 |
|
5 |
TeleopScout::TeleopScout(string scoutname)
|
6 |
{ |
7 |
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 |
} |
16 |
|
17 |
void TeleopScout::teleop_input_callback(
|
18 |
const scoutsim::teleop_input::ConstPtr& msg)
|
19 |
{ |
20 |
last_key = msg->last_key; |
21 |
last_time = msg->header.stamp; |
22 |
//ROS_INFO("received key: 0x%02X ", last_key);
|
23 |
//ROS_INFO("at last time: %f\n", last_time.toSec());
|
24 |
} |
25 |
|
26 |
void TeleopScout::keyListenerLoop()
|
27 |
{ |
28 |
ROS_INFO("waiting for userinput commands...\n");
|
29 |
ROS_INFO("current max wait time is: %f\n", MAX_WAIT_TIME.toSec());
|
30 |
while(ros::ok())
|
31 |
{ |
32 |
// if we haven't passed the waiting threshold
|
33 |
if(last_time + MAX_WAIT_TIME > ros::Time::now())
|
34 |
{ |
35 |
switch(last_key)
|
36 |
{ |
37 |
// TODO: for some reason negative speeds go forward, not backward
|
38 |
|
39 |
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 |
fl_speed = bl_speed = -200;
|
52 |
fr_speed = br_speed = -200;
|
53 |
break;
|
54 |
case KEYCODE_D:
|
55 |
//ROS_INFO("DOWN");
|
56 |
fl_speed = bl_speed = 200;
|
57 |
fr_speed = br_speed = 200;
|
58 |
break;
|
59 |
default:
|
60 |
//don't publish a message yet; no change to key,
|
61 |
// and no timeout yet
|
62 |
//ROS_INFO("no time out yet\n");
|
63 |
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 |
ROS_INFO("no key press; stopping movement");
|
72 |
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 |
} |
89 |
return;
|
90 |
} |
91 |
|
92 |
int main(int argc, char** argv) |
93 |
{ |
94 |
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 |
|
107 |
teleop_scout.keyListenerLoop(); |
108 |
|
109 |
return(0); |
110 |
} |