37 #include <linux/input.h> 38 #include <linux/joystick.h> 45 #include <sensor_msgs/Joy.h> 46 #include <sensor_msgs/JoyFeedbackArray.h> 85 stat.
summary(2,
"Joystick not open.");
89 stat.
add(
"device", joy_dev_);
90 stat.
add(
"device name", joy_dev_name_);
91 stat.
add(
"dead zone", deadzone_);
92 stat.
add(
"autorepeat rate (Hz)", autorepeat_rate_);
93 stat.
add(
"coalesce interval (s)", coalesce_interval_);
94 stat.
add(
"recent joystick event rate (Hz)", event_count_ / interval);
95 stat.
add(
"recent publication rate (Hz)", pub_count_ / interval);
97 stat.
add(
"default trig val", default_trig_val_);
98 stat.
add(
"sticky buttons", sticky_buttons_);
109 const char path[] =
"/dev/input";
110 struct dirent *entry;
111 struct stat stat_buf;
113 DIR *dev_dir = opendir(path);
114 if (dev_dir ==
nullptr)
116 ROS_ERROR(
"Couldn't open %s. Error %i: %s.", path, errno, strerror(errno));
120 while ((entry = readdir(dev_dir)) !=
nullptr)
123 if (strncmp(entry->d_name,
"js", 2) != 0)
127 std::string current_path = std::string(path) +
"/" + entry->d_name;
128 if (stat(current_path.c_str(), &stat_buf) == -1)
132 if (!S_ISCHR(stat_buf.st_mode))
138 int joy_fd = open(current_path.c_str(), O_RDONLY);
144 char current_joy_name[128];
145 if (ioctl(joy_fd, JSIOCGNAME(
sizeof(current_joy_name)), current_joy_name) < 0)
147 strncpy(current_joy_name,
"Unknown",
sizeof(current_joy_name));
152 ROS_INFO(
"Found joystick: %s (%s).", current_joy_name, current_path.c_str());
154 if (strcmp(current_joy_name, joy_name.c_str()) == 0)
176 size_t size = msg->array.size();
177 for (
size_t i = 0; i < size; i++)
180 if (msg->array[i].type == 1 && ff_fd_ != -1)
185 if (msg->array[i].id == 0)
187 joy_effect_.u.rumble.strong_magnitude = (
static_cast<float>(0xFFFFU))*msg->array[i].intensity;
191 joy_effect_.u.rumble.weak_magnitude = (
static_cast<float>(0xFFFFU))*msg->array[i].intensity;
197 update_feedback_ =
true;
203 int main(
int argc,
char **argv)
210 pub_ = nh_.
advertise<sensor_msgs::Joy>(
"joy", 1);
212 nh_param.
param<std::string>(
"dev",
joy_dev_,
"/dev/input/js0");
222 if (!joy_dev_name_.empty())
225 if (joy_dev_path.empty())
227 ROS_ERROR(
"Couldn't find a joystick with name %s. Falling back to default device.", joy_dev_name_.c_str());
231 ROS_INFO(
"Using %s as joystick device.", joy_dev_path.c_str());
232 joy_dev_ = joy_dev_path;
236 if (autorepeat_rate_ > 1 / coalesce_interval_)
238 ROS_WARN(
"joy_node: autorepeat_rate (%f Hz) > 1/coalesce_interval (%f Hz) " 239 "does not make sense. Timing behavior is not well defined.", autorepeat_rate_, 1/coalesce_interval_);
244 ROS_WARN(
"joy_node: deadzone greater than 1 was requested. The semantics of deadzone have changed. " 245 "It is now related to the range [-1:1] instead of [-32767:32767]. For now I am dividing your deadzone " 246 "by 32767, but this behavior is deprecated so you need to update your launch file.");
252 ROS_WARN(
"joy_node: deadzone (%f) greater than 0.9, setting it to 0.9", deadzone_);
258 ROS_WARN(
"joy_node: deadzone_ (%f) less than 0, setting to 0.", deadzone_);
262 if (autorepeat_rate_ < 0)
264 ROS_WARN(
"joy_node: autorepeat_rate (%f) less than 0, setting to 0.", autorepeat_rate_);
265 autorepeat_rate_ = 0;
268 if (coalesce_interval_ < 0)
270 ROS_WARN(
"joy_node: coalesce_interval (%f) less than 0, setting to 0.", coalesce_interval_);
271 coalesce_interval_ = 0;
276 double scale = -1. / (1. -
deadzone_) / 32767.;
277 double unscaled_deadzone = 32767. *
deadzone_;
292 bool first_fault =
true;
300 joy_fd = open(joy_dev_.c_str(), O_RDONLY);
310 joy_fd = open(joy_dev_.c_str(), O_RDONLY);
318 ROS_ERROR(
"Couldn't open joystick %s. Will retry every second.", joy_dev_.c_str());
325 if (!joy_dev_ff_.empty())
327 ff_fd_ = open(joy_dev_ff_.c_str(), O_RDWR);
331 struct input_event ie;
335 ie.value = 0xFFFFUL * gain / 100;
337 if (write(ff_fd_, &ie,
sizeof(ie)) == -1)
339 ROS_WARN(
"Couldn't set gain on joystick force feedback: %s", strerror(errno));
355 char current_joy_name[128];
356 if (ioctl(joy_fd, JSIOCGNAME(
sizeof(current_joy_name)), current_joy_name) < 0)
358 strncpy(current_joy_name,
"Unknown",
sizeof(current_joy_name));
361 ROS_INFO(
"Opened joystick: %s (%s). deadzone_: %f.", joy_dev_.c_str(), current_joy_name,
deadzone_);
366 bool publication_pending =
false;
369 sensor_msgs::Joy joy_msg;
375 bool publish_now =
false;
376 bool publish_soon =
false;
378 FD_SET(joy_fd, &
set);
380 int select_out = select(joy_fd+1, &
set,
nullptr,
nullptr, &tv);
381 if (select_out == -1)
391 struct input_event start;
395 if (write(ff_fd_, (
const void*) &start,
sizeof(start)) == -1)
401 if (update_feedback_ ==
true)
404 update_feedback_ =
false;
408 if (FD_ISSET(joy_fd, &
set))
410 if (read(joy_fd, &event,
sizeof(js_event)) == -1 && errno != EAGAIN)
419 case JS_EVENT_BUTTON:
420 case JS_EVENT_BUTTON | JS_EVENT_INIT:
421 if (event.number >= joy_msg.buttons.size())
423 size_t old_size = joy_msg.buttons.size();
424 joy_msg.buttons.resize(event.number+1);
425 for (
size_t i = old_size; i < joy_msg.buttons.size(); i++)
427 joy_msg.buttons[i] = 0.0;
432 if (event.value == 1)
434 joy_msg.buttons[
event.number] = 1 - joy_msg.buttons[
event.number];
439 joy_msg.buttons[
event.number] = (
event.value ? 1 : 0);
443 if (!(event.type & JS_EVENT_INIT))
453 case JS_EVENT_AXIS | JS_EVENT_INIT:
455 if (event.number >= joy_msg.axes.size())
457 size_t old_size = joy_msg.axes.size();
458 joy_msg.axes.resize(event.number+1);
459 for (
size_t i = old_size; i < joy_msg.axes.size(); i++)
461 joy_msg.axes[i] = 0.0;
464 if (default_trig_val_)
467 if (val > unscaled_deadzone)
469 val -= unscaled_deadzone;
471 else if (val < -unscaled_deadzone)
473 val += unscaled_deadzone;
479 joy_msg.axes[
event.number] = val * scale;
486 if (!(event.type & JS_EVENT_INIT))
489 if (val > unscaled_deadzone)
491 val -= unscaled_deadzone;
493 else if (val < -unscaled_deadzone)
495 val += unscaled_deadzone;
501 joy_msg.axes[
event.number] = val * scale;
508 ROS_WARN(
"joy_node: Unknown event type. Please file a ticket. " 509 "time=%u, value=%d, type=%Xh, number=%d", event.time, event.value, event.type, event.number);
525 joy_msg.header.frame_id = joy_dev_.c_str();
530 publication_pending =
false;
531 publish_soon =
false;
537 if (!publication_pending && publish_soon)
539 tv.tv_sec = trunc(coalesce_interval_);
540 tv.tv_usec = (coalesce_interval_ - tv.tv_sec) * 1e6;
541 publication_pending =
true;
546 if (!tv_set && autorepeat_rate_ > 0)
548 tv.tv_sec = trunc(autorepeat_interval);
549 tv.tv_usec = (autorepeat_interval - tv.tv_sec) * 1e6;
567 ROS_ERROR(
"Connection to joystick device lost unexpectedly. Will reopen.");
578 int main(
int argc,
char **argv)
582 return j.
main(argc, argv);
void summary(unsigned char lvl, const std::string s)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void setHardwareID(const std::string &hwid)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void add(const std::string &name, TaskFunction f)
void set_feedback(const sensor_msgs::JoyFeedbackArray::ConstPtr &msg)
std::string get_dev_by_joy_name(const std::string &joy_name)
Returns the device path of the first joystick that matches joy_name. If no match is found...
int main(int argc, char **argv)
Opens joystick port, reads from port and publishes while node is active.
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void publish(const boost::shared_ptr< M > &message) const
double coalesce_interval_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Opens, reads from and publishes joystick events.
struct ff_effect joy_effect_
std::string getTopic() const
void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
Publishes diagnostics and status.
void add(const std::string &key, const T &val)
std::string joy_dev_name_
uint32_t getNumSubscribers() const
ROSCPP_DECL void spinOnce()
diagnostic_updater::Updater diagnostic_