From 152e381f91f8eab6df7ad348c90904d96ebf12c7 Mon Sep 17 00:00:00 2001 From: rayferric <63957587+rayferric@users.noreply.github.com> Date: Fri, 24 Jan 2025 20:04:23 +0100 Subject: [PATCH 1/3] continuously look for devices treat dev_name as a substring --- joy_linux/src/joy_linux_node.cpp | 68 ++++++++++++++++++-------------- 1 file changed, 38 insertions(+), 30 deletions(-) diff --git a/joy_linux/src/joy_linux_node.cpp b/joy_linux/src/joy_linux_node.cpp index 77ab144d..726a4e8f 100644 --- a/joy_linux/src/joy_linux_node.cpp +++ b/joy_linux/src/joy_linux_node.cpp @@ -142,9 +142,9 @@ class Joystick close(joy_fd); - RCLCPP_INFO(logger, "Found joystick: %s (%s).", current_joy_name, current_path.c_str()); - - if (strcmp(current_joy_name, joy_name.c_str()) == 0) { + // Check if the joystick device name contains joy_name, if so return the path. + std::string current_joy_name_str(current_joy_name); + if (current_joy_name_str.find(joy_name) != std::string::npos) { closedir(dev_dir); return current_path; } @@ -217,20 +217,6 @@ class Joystick default_trig_val_ = node_->declare_parameter("default_trig_val", false); sticky_buttons_ = node_->declare_parameter("sticky_buttons", false); - // Checks on parameters - if (!joy_dev_name_.empty()) { - std::string joy_dev_path = get_dev_by_joy_name(joy_dev_name_, node_->get_logger()); - if (joy_dev_path.empty()) { - RCLCPP_ERROR( - node_->get_logger(), "Couldn't find a joystick with name %s. " - "Falling back to default device.", - joy_dev_name_.c_str()); - } else { - RCLCPP_INFO(node_->get_logger(), "Using %s as joystick device.", joy_dev_path.c_str()); - joy_dev_ = joy_dev_path; - } - } - if (autorepeat_rate_ > 1 / coalesce_interval_) { RCLCPP_WARN( node_->get_logger(), "joy_linux_node: autorepeat_rate (%f Hz) > " @@ -282,7 +268,7 @@ class Joystick js_event event; struct timeval tv; fd_set set; - int joy_fd; + int joy_fd = -1; event_count_ = 0; pub_count_ = 0; @@ -312,24 +298,46 @@ class Joystick if (!rclcpp::ok()) { goto cleanup; } - joy_fd = open(joy_dev_.c_str(), O_RDONLY); - if (joy_fd != -1) { - // There seems to be a bug in the driver or something where the - // initial events that are to define the initial state of the - // joystick are not the values of the joystick when it was opened - // but rather the values of the joystick when it was last closed. - // Opening then closing and opening again is a hack to get more - // accurate initial state data. - close(joy_fd); + + if (!joy_dev_name_.empty()) + { + joy_dev_ = get_dev_by_joy_name(joy_dev_name_, node_->get_logger()); + } + + if (!joy_dev_.empty()) + { joy_fd = open(joy_dev_.c_str(), O_RDONLY); + if (joy_fd != -1) { + // There seems to be a bug in the driver or something where the + // initial events that are to define the initial state of the + // joystick are not the values of the joystick when it was opened + // but rather the values of the joystick when it was last closed. + // Opening then closing and opening again is a hack to get more + // accurate initial state data. + close(joy_fd); + joy_fd = open(joy_dev_.c_str(), O_RDONLY); + } + } else { + joy_fd = -1; } + if (joy_fd != -1) { + // Reset first_fault so that in case connection is lost, + // the first retry will run immediately. + first_fault = true; break; } if (first_fault) { - RCLCPP_ERROR( - node_->get_logger(), "Couldn't open joystick %s. Will retry every second.", - joy_dev_.c_str()); + // Send a differently worded log message if dev_name was specified. + if (!joy_dev_name_.empty()) { + RCLCPP_ERROR( + node_->get_logger(), "Couldn't find a joystick with name containing %s. Will retry every second.", + joy_dev_name_.c_str()); + } else { + RCLCPP_ERROR( + node_->get_logger(), "Couldn't open joystick %s. Will retry every second.", + joy_dev_.c_str()); + } first_fault = false; } } From 6ed56b5804ee929df4c72cd9880c6d01adac9dbe Mon Sep 17 00:00:00 2001 From: rayferric <63957587+rayferric@users.noreply.github.com> Date: Sat, 25 Jan 2025 00:21:06 +0100 Subject: [PATCH 2/3] publish zeros when joy is disconnected --- joy_linux/src/joy_linux_node.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/joy_linux/src/joy_linux_node.cpp b/joy_linux/src/joy_linux_node.cpp index 726a4e8f..e62c15a3 100644 --- a/joy_linux/src/joy_linux_node.cpp +++ b/joy_linux/src/joy_linux_node.cpp @@ -421,7 +421,22 @@ class Joystick if (FD_ISSET(joy_fd, &set)) { if (read(joy_fd, &event, sizeof(js_event)) == -1 && errno != EAGAIN) { - break; // Joystick is probably closed. Definitely occurs. + // Joystick is probably closed. Definitely occurs. + // Publish zeros and break out of the read loop to find the device again. + + RCLCPP_ERROR( + node_->get_logger(), "Joystick device read error. Will reopen. %s", strerror(errno)); + + joy_msg->header.stamp = node_->now(); + for (size_t i = 0; i < joy_msg->buttons.size(); i++) { + joy_msg->buttons[i] = 0.0; + } + for (size_t i = 0; i < joy_msg->axes.size(); i++) { + joy_msg->axes[i] = 0.0; + } + pub_->publish(*joy_msg); + + break; } joy_msg->header.stamp = node_->now(); From 146a79cc1b24a4eddf3c4098953253ae09a7ec6f Mon Sep 17 00:00:00 2001 From: rayferric <63957587+rayferric@users.noreply.github.com> Date: Sat, 22 Feb 2025 17:49:36 +0100 Subject: [PATCH 3/3] uncrustify --- joy_linux/src/joy_linux_node.cpp | 27 +++++++++++++-------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/joy_linux/src/joy_linux_node.cpp b/joy_linux/src/joy_linux_node.cpp index e62c15a3..98db1705 100644 --- a/joy_linux/src/joy_linux_node.cpp +++ b/joy_linux/src/joy_linux_node.cpp @@ -299,13 +299,11 @@ class Joystick goto cleanup; } - if (!joy_dev_name_.empty()) - { + if (!joy_dev_name_.empty()) { joy_dev_ = get_dev_by_joy_name(joy_dev_name_, node_->get_logger()); } - - if (!joy_dev_.empty()) - { + + if (!joy_dev_.empty()) { joy_fd = open(joy_dev_.c_str(), O_RDONLY); if (joy_fd != -1) { // There seems to be a bug in the driver or something where the @@ -330,13 +328,14 @@ class Joystick if (first_fault) { // Send a differently worded log message if dev_name was specified. if (!joy_dev_name_.empty()) { - RCLCPP_ERROR( - node_->get_logger(), "Couldn't find a joystick with name containing %s. Will retry every second.", - joy_dev_name_.c_str()); + RCLCPP_ERROR( + node_->get_logger(), "Couldn't find a joystick with name containing %s. " + "Will retry every second.", + joy_dev_name_.c_str()); } else { - RCLCPP_ERROR( - node_->get_logger(), "Couldn't open joystick %s. Will retry every second.", - joy_dev_.c_str()); + RCLCPP_ERROR( + node_->get_logger(), "Couldn't open joystick %s. Will retry every second.", + joy_dev_.c_str()); } first_fault = false; } @@ -423,10 +422,10 @@ class Joystick if (read(joy_fd, &event, sizeof(js_event)) == -1 && errno != EAGAIN) { // Joystick is probably closed. Definitely occurs. // Publish zeros and break out of the read loop to find the device again. - + RCLCPP_ERROR( node_->get_logger(), "Joystick device read error. Will reopen. %s", strerror(errno)); - + joy_msg->header.stamp = node_->now(); for (size_t i = 0; i < joy_msg->buttons.size(); i++) { joy_msg->buttons[i] = 0.0; @@ -435,7 +434,7 @@ class Joystick joy_msg->axes[i] = 0.0; } pub_->publish(*joy_msg); - + break; }