root / joy / joy_node.cpp @ c1426757
History | View | Annotate | Download (11.3 KB)
1 |
/*
|
---|---|
2 |
* teleop_pr2
|
3 |
* Copyright (c) 2009, Willow Garage, Inc.
|
4 |
* All rights reserved.
|
5 |
*
|
6 |
* Redistribution and use in source and binary forms, with or without
|
7 |
* modification, are permitted provided that the following conditions are met:
|
8 |
*
|
9 |
* * Redistributions of source code must retain the above copyright
|
10 |
* notice, this list of conditions and the following disclaimer.
|
11 |
* * Redistributions in binary form must reproduce the above copyright
|
12 |
* notice, this list of conditions and the following disclaimer in the
|
13 |
* documentation and/or other materials provided with the distribution.
|
14 |
* * Neither the name of the <ORGANIZATION> nor the names of its
|
15 |
* contributors may be used to endorse or promote products derived from
|
16 |
* this software without specific prior written permission.
|
17 |
*
|
18 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
19 |
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
20 |
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
21 |
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
22 |
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
23 |
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
24 |
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
25 |
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
26 |
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
27 |
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
28 |
* POSSIBILITY OF SUCH DAMAGE.
|
29 |
*/
|
30 |
|
31 |
//\author: Blaise Gassend
|
32 |
|
33 |
#include <unistd.h> |
34 |
#include <math.h> |
35 |
#include <linux/joystick.h> |
36 |
#include <fcntl.h> |
37 |
#include <diagnostic_updater/diagnostic_updater.h> |
38 |
#include "ros/ros.h" |
39 |
#include <sensor_msgs/Joy.h> |
40 |
|
41 |
|
42 |
///\brief Opens, reads from and publishes joystick events
|
43 |
class Joystick |
44 |
{ |
45 |
private:
|
46 |
ros::NodeHandle nh_; |
47 |
bool open_;
|
48 |
std::string joy_dev_;
|
49 |
double deadzone_;
|
50 |
double autorepeat_rate_; // in Hz. 0 for no repeat. |
51 |
double coalesce_interval_; // Defaults to 100 Hz rate limit. |
52 |
int event_count_;
|
53 |
int pub_count_;
|
54 |
ros::Publisher pub_; |
55 |
double lastDiagTime_;
|
56 |
|
57 |
diagnostic_updater::Updater diagnostic_; |
58 |
|
59 |
///\brief Publishes diagnostics and status
|
60 |
void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
|
61 |
{ |
62 |
double now = ros::Time::now().toSec();
|
63 |
double interval = now - lastDiagTime_;
|
64 |
if (open_)
|
65 |
stat.summary(0, "OK"); |
66 |
else
|
67 |
stat.summary(2, "Joystick not open."); |
68 |
|
69 |
stat.add("topic", pub_.getTopic());
|
70 |
stat.add("device", joy_dev_);
|
71 |
stat.add("dead zone", deadzone_);
|
72 |
stat.add("autorepeat rate (Hz)", autorepeat_rate_);
|
73 |
stat.add("coalesce interval (s)", coalesce_interval_);
|
74 |
stat.add("recent joystick event rate (Hz)", event_count_ / interval);
|
75 |
stat.add("recent publication rate (Hz)", pub_count_ / interval);
|
76 |
stat.add("subscribers", pub_.getNumSubscribers());
|
77 |
event_count_ = 0;
|
78 |
pub_count_ = 0;
|
79 |
lastDiagTime_ = now; |
80 |
} |
81 |
|
82 |
public:
|
83 |
Joystick() : nh_(), diagnostic_() |
84 |
{} |
85 |
|
86 |
///\brief Opens joystick port, reads from port and publishes while node is active
|
87 |
int main(int argc, char **argv) |
88 |
{ |
89 |
diagnostic_.add("Joystick Driver Status", this, &Joystick::diagnostics); |
90 |
diagnostic_.setHardwareID("none");
|
91 |
|
92 |
// Parameters
|
93 |
ros::NodeHandle nh_param("~");
|
94 |
pub_ = nh_.advertise<sensor_msgs::Joy>("joy", 1); |
95 |
nh_param.param<std::string>("dev", joy_dev_, "/dev/input/js0"); |
96 |
nh_param.param<double>("deadzone", deadzone_, 0.05); |
97 |
nh_param.param<double>("autorepeat_rate", autorepeat_rate_, 0); |
98 |
nh_param.param<double>("coalesce_interval", coalesce_interval_, 0.001); |
99 |
|
100 |
// Checks on parameters
|
101 |
if (autorepeat_rate_ > 1 / coalesce_interval_) |
102 |
ROS_WARN("joy_node: autorepeat_rate (%f Hz) > 1/coalesce_interval (%f Hz) does not make sense. Timing behavior is not well defined.", autorepeat_rate_, 1/coalesce_interval_); |
103 |
|
104 |
if (deadzone_ >= 1) |
105 |
{ |
106 |
ROS_WARN("joy_node: deadzone greater than 1 was requested. The semantics of deadzone have changed. It is now related to the range [-1:1] instead of [-32767:32767]. For now I am dividing your deadzone by 32767, but this behavior is deprecated so you need to update your launch file.");
|
107 |
deadzone_ /= 32767;
|
108 |
} |
109 |
|
110 |
if (deadzone_ > 0.9) |
111 |
{ |
112 |
ROS_WARN("joy_node: deadzone (%f) greater than 0.9, setting it to 0.9", deadzone_);
|
113 |
deadzone_ = 0.9; |
114 |
} |
115 |
|
116 |
if (deadzone_ < 0) |
117 |
{ |
118 |
ROS_WARN("joy_node: deadzone_ (%f) less than 0, setting to 0.", deadzone_);
|
119 |
deadzone_ = 0;
|
120 |
} |
121 |
|
122 |
if (autorepeat_rate_ < 0) |
123 |
{ |
124 |
ROS_WARN("joy_node: autorepeat_rate (%f) less than 0, setting to 0.", autorepeat_rate_);
|
125 |
autorepeat_rate_ = 0;
|
126 |
} |
127 |
|
128 |
if (coalesce_interval_ < 0) |
129 |
{ |
130 |
ROS_WARN("joy_node: coalesce_interval (%f) less than 0, setting to 0.", coalesce_interval_);
|
131 |
coalesce_interval_ = 0;
|
132 |
} |
133 |
|
134 |
// Parameter conversions
|
135 |
double autorepeat_interval = 1 / autorepeat_rate_; |
136 |
double scale = -1. / (1. - deadzone_) / 32767.; |
137 |
double unscaled_deadzone = 32767. * deadzone_; |
138 |
|
139 |
js_event event; |
140 |
struct timeval tv;
|
141 |
fd_set set; |
142 |
int joy_fd;
|
143 |
event_count_ = 0;
|
144 |
pub_count_ = 0;
|
145 |
lastDiagTime_ = ros::Time::now().toSec(); |
146 |
|
147 |
// Big while loop opens, publishes
|
148 |
while (nh_.ok())
|
149 |
{ |
150 |
open_ = false;
|
151 |
diagnostic_.force_update(); |
152 |
bool first_fault = true; |
153 |
while (true) |
154 |
{ |
155 |
ros::spinOnce(); |
156 |
if (!nh_.ok())
|
157 |
goto cleanup;
|
158 |
joy_fd = open(joy_dev_.c_str(), O_RDONLY); |
159 |
if (joy_fd != -1) |
160 |
{ |
161 |
// There seems to be a bug in the driver or something where the
|
162 |
// initial events that are to define the initial state of the
|
163 |
// joystick are not the values of the joystick when it was opened
|
164 |
// but rather the values of the joystick when it was last closed.
|
165 |
// Opening then closing and opening again is a hack to get more
|
166 |
// accurate initial state data.
|
167 |
close(joy_fd); |
168 |
joy_fd = open(joy_dev_.c_str(), O_RDONLY); |
169 |
} |
170 |
if (joy_fd != -1) |
171 |
break;
|
172 |
if (first_fault)
|
173 |
{ |
174 |
ROS_ERROR("Couldn't open joystick %s. Will retry every second.", joy_dev_.c_str());
|
175 |
first_fault = false;
|
176 |
} |
177 |
sleep(1.0); |
178 |
diagnostic_.update(); |
179 |
} |
180 |
|
181 |
ROS_INFO("Opened joystick: %s. deadzone_: %f.", joy_dev_.c_str(), deadzone_);
|
182 |
open_ = true;
|
183 |
diagnostic_.force_update(); |
184 |
|
185 |
bool tv_set = false; |
186 |
bool publication_pending = false; |
187 |
tv.tv_sec = 1;
|
188 |
tv.tv_usec = 0;
|
189 |
sensor_msgs::Joy joy_msg; // Here because we want to reset it on device close.
|
190 |
while (nh_.ok())
|
191 |
{ |
192 |
ros::spinOnce(); |
193 |
|
194 |
bool publish_now = false; |
195 |
bool publish_soon = false; |
196 |
FD_ZERO(&set); |
197 |
FD_SET(joy_fd, &set); |
198 |
|
199 |
//ROS_INFO("Select...");
|
200 |
int select_out = select(joy_fd+1, &set, NULL, NULL, &tv); |
201 |
//ROS_INFO("Tick...");
|
202 |
if (select_out == -1) |
203 |
{ |
204 |
tv.tv_sec = 0;
|
205 |
tv.tv_usec = 0;
|
206 |
//ROS_INFO("Select returned negative. %i", ros::isShuttingDown());
|
207 |
continue;
|
208 |
// break; // Joystick is probably closed. Not sure if this case is useful.
|
209 |
} |
210 |
|
211 |
if (FD_ISSET(joy_fd, &set))
|
212 |
{ |
213 |
if (read(joy_fd, &event, sizeof(js_event)) == -1 && errno != EAGAIN) |
214 |
break; // Joystick is probably closed. Definitely occurs. |
215 |
|
216 |
//ROS_INFO("Read data...");
|
217 |
joy_msg.header.stamp = ros::Time().now(); |
218 |
event_count_++; |
219 |
switch(event.type)
|
220 |
{ |
221 |
case JS_EVENT_BUTTON:
|
222 |
case JS_EVENT_BUTTON | JS_EVENT_INIT:
|
223 |
if(event.number >= joy_msg.buttons.size())
|
224 |
{ |
225 |
int old_size = joy_msg.buttons.size();
|
226 |
joy_msg.buttons.resize(event.number+1);
|
227 |
for(unsigned int i=old_size;i<joy_msg.buttons.size();i++) |
228 |
joy_msg.buttons[i] = 0.0; |
229 |
} |
230 |
joy_msg.buttons[event.number] = (event.value ? 1 : 0); |
231 |
// For initial events, wait a bit before sending to try to catch
|
232 |
// all the initial events.
|
233 |
if (!(event.type & JS_EVENT_INIT))
|
234 |
publish_now = true;
|
235 |
else
|
236 |
publish_soon = true;
|
237 |
break;
|
238 |
case JS_EVENT_AXIS:
|
239 |
case JS_EVENT_AXIS | JS_EVENT_INIT:
|
240 |
if(event.number >= joy_msg.axes.size())
|
241 |
{ |
242 |
int old_size = joy_msg.axes.size();
|
243 |
joy_msg.axes.resize(event.number+1);
|
244 |
for(unsigned int i=old_size;i<joy_msg.axes.size();i++) |
245 |
joy_msg.axes[i] = 0.0; |
246 |
} |
247 |
if (!(event.type & JS_EVENT_INIT)) // Init event.value is wrong. |
248 |
{ |
249 |
double val = event.value;
|
250 |
// Allows deadzone to be "smooth"
|
251 |
if (val > unscaled_deadzone)
|
252 |
val -= unscaled_deadzone; |
253 |
else if (val < -unscaled_deadzone) |
254 |
val += unscaled_deadzone; |
255 |
else
|
256 |
val = 0;
|
257 |
joy_msg.axes[event.number] = val * scale; |
258 |
} |
259 |
// Will wait a bit before sending to try to combine events.
|
260 |
publish_soon = true;
|
261 |
break;
|
262 |
default:
|
263 |
ROS_WARN("joy_node: Unknown event type. Please file a ticket. time=%u, value=%d, type=%Xh, number=%d", event.time, event.value, event.type, event.number);
|
264 |
break;
|
265 |
} |
266 |
} |
267 |
else if (tv_set) // Assume that the timer has expired. |
268 |
publish_now = true;
|
269 |
|
270 |
if (publish_now)
|
271 |
{ |
272 |
// Assume that all the JS_EVENT_INIT messages have arrived already.
|
273 |
// This should be the case as the kernel sends them along as soon as
|
274 |
// the device opens.
|
275 |
//ROS_INFO("Publish...");
|
276 |
pub_.publish(joy_msg); |
277 |
publish_now = false;
|
278 |
tv_set = false;
|
279 |
publication_pending = false;
|
280 |
publish_soon = false;
|
281 |
pub_count_++; |
282 |
} |
283 |
|
284 |
// If an axis event occurred, start a timer to combine with other
|
285 |
// events.
|
286 |
if (!publication_pending && publish_soon)
|
287 |
{ |
288 |
tv.tv_sec = trunc(coalesce_interval_); |
289 |
tv.tv_usec = (coalesce_interval_ - tv.tv_sec) * 1e6;
|
290 |
publication_pending = true;
|
291 |
tv_set = true;
|
292 |
//ROS_INFO("Pub pending...");
|
293 |
} |
294 |
|
295 |
// If nothing is going on, start a timer to do autorepeat.
|
296 |
if (!tv_set && autorepeat_rate_ > 0) |
297 |
{ |
298 |
tv.tv_sec = trunc(autorepeat_interval); |
299 |
tv.tv_usec = (autorepeat_interval - tv.tv_sec) * 1e6;
|
300 |
tv_set = true;
|
301 |
//ROS_INFO("Autorepeat pending... %i %i", tv.tv_sec, tv.tv_usec);
|
302 |
} |
303 |
|
304 |
if (!tv_set)
|
305 |
{ |
306 |
tv.tv_sec = 1;
|
307 |
tv.tv_usec = 0;
|
308 |
} |
309 |
|
310 |
diagnostic_.update(); |
311 |
} // End of joystick open loop.
|
312 |
|
313 |
close(joy_fd); |
314 |
ros::spinOnce(); |
315 |
if (nh_.ok())
|
316 |
ROS_ERROR("Connection to joystick device lost unexpectedly. Will reopen.");
|
317 |
} |
318 |
|
319 |
cleanup:
|
320 |
ROS_INFO("joy_node shut down.");
|
321 |
|
322 |
return 0; |
323 |
} |
324 |
}; |
325 |
|
326 |
int main(int argc, char **argv) |
327 |
{ |
328 |
ros::init(argc, argv, "joy_node");
|
329 |
Joystick j; |
330 |
return j.main(argc, argv);
|
331 |
} |