Add footswitch driver (#40)

* Add interbotix_ros_common_drivers, footswitch

* Add hidapi dep, basic demo

* Add interfaces

* Make node, library, add rate param

* Lint

* Update udev rules

* Add brief readme

* Remove direct include dependency

* Include hidapi as submodule

.gitmodules

@@ -6,3 +6,6 @@
 	path = interbotix_ros_xseries/interbotix_xs_driver
 	url = https://github.com/Interbotix/interbotix_xs_driver.git
 	branch = v0.3.3
+[submodule "interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/dependencies/hidapi"]
+	path = interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/dependencies/hidapi
+	url = https://github.com/libusb/hidapi.git

interbotix_ros_common_drivers/interbotix_ros_footswitch/README.md

@@ -0,0 +1,4 @@
+# Interbotix ROS Footswitch
+
+This subrepository contains a driver and interfaces for the three-pedal footswitch from PCsensor.
+It requires the use of a device with Vendor ID `3553` and Product ID `b001`.

interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/CMakeLists.txt

@@ -0,0 +1,53 @@
+cmake_minimum_required(VERSION 3.10.0)
+project(interbotix_footswitch_driver)
+
+find_package(ament_cmake REQUIRED)
+find_package(interbotix_footswitch_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+
+add_subdirectory(dependencies/hidapi)
+
+include_directories(include)
+
+set(ROS_DEPENDENCIES
+  interbotix_footswitch_msgs
+  rclcpp
+  rclcpp_components
+)
+
+add_library(footswitch_driver
+  src/footswitch_driver.cpp
+)
+
+ament_target_dependencies(footswitch_driver ${ROS_DEPENDENCIES})
+target_link_libraries(footswitch_driver hidapi::hidraw)
+
+add_executable(footswitch_driver_node
+  src/footswitch_driver_node.cpp
+)
+
+target_link_libraries(footswitch_driver_node
+  footswitch_driver
+)
+
+install(
+  TARGETS footswitch_driver
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  TARGETS
+    footswitch_driver_node
+  RUNTIME DESTINATION
+    lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/include/interbotix_footswitch_driver/footswitch_driver.hpp

@@ -0,0 +1,65 @@
+// Copyright 2024 Trossen Robotics
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the copyright holder nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef INTERBOTIX_FOOTSWITCH_DRIVER__FOOTSWITCH_DRIVER_HPP_
+#define INTERBOTIX_FOOTSWITCH_DRIVER__FOOTSWITCH_DRIVER_HPP_
+
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "hidapi/hidapi.h"
+#include "interbotix_footswitch_msgs/msg/footswitch_state.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace footswitch_driver
+{
+
+const uint16_t VEND_ID = 0x3553;
+const uint16_t PROD_ID = 0xb001;
+
+using interbotix_footswitch_msgs::msg::FootswitchState;
+class FootSwitch : public rclcpp::Node
+{
+public:
+  explicit FootSwitch(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  ~FootSwitch();
+
+private:
+  rclcpp::TimerBase::SharedPtr tmr_update_state_;
+  hid_device * handle_;
+  rclcpp::Publisher<FootswitchState>::SharedPtr pub_footswitch_state_;
+  int update_period_ms_{100};  // ms
+  void update_state();
+};
+
+}  // namespace footswitch_driver
+
+#endif  // INTERBOTIX_FOOTSWITCH_DRIVER__FOOTSWITCH_DRIVER_HPP_

interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>interbotix_footswitch_driver</name>
+  <version>0.0.0</version>
+  <description>
+    The interbotix_footswitch_driver package provides a ROS interface for three-pedal footswitches from PCsensor.
+  </description>
+  <maintainer email="trsupport@trossenrobotics.com">Trossen Robotics</maintainer>
+  <license>BSD-3-Clause</license>
+  <author email="trsupport@trossenrobotics.com">Trossen Robotics</author>
+
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>interbotix_footswitch_msgs</depend>
+  <depend>rclcpp_components</depend>
+  <depend>rclcpp</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/src/footswitch_driver.cpp

@@ -0,0 +1,141 @@
+// Copyright 2024 Trossen Robotics
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the copyright holder nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include <memory>
+
+#include "interbotix_footswitch_driver/footswitch_driver.hpp"
+
+namespace footswitch_driver
+{
+
+FootSwitch::FootSwitch(const rclcpp::NodeOptions & options)
+: rclcpp::Node("footswitch_node")
+{
+  // Initialize the hidapi library
+  hid_init();
+
+  // Open the device using the VID, PID
+  handle_ = hid_open(VEND_ID, PROD_ID, NULL);
+  if (!handle_) {
+    RCLCPP_FATAL(
+      get_logger(),
+      "Unable to open device with VID '%x' and PID '%x'. Exiting...",
+      VEND_ID, PROD_ID);
+    hid_exit();
+    ::exit(1);
+  }
+
+  // Set the hid_read() function to be non-blocking.
+  hid_set_nonblocking(handle_, 1);
+
+  int update_rate;
+  declare_parameter("update_rate", 10);
+  get_parameter("update_rate", update_rate);
+
+  update_period_ms_ = static_cast<int64_t>((1.0 / static_cast<float>(update_rate)) * 1'000);
+
+  pub_footswitch_state_ = create_publisher<FootswitchState>("state", 10);
+
+  RCLCPP_INFO(
+    get_logger(),
+    "Publishing footswitch state on topic '%s'.",
+    pub_footswitch_state_->get_topic_name());
+
+  tmr_update_state_ = create_wall_timer(
+    std::chrono::milliseconds(update_period_ms_),
+    std::bind(&FootSwitch::update_state, this));
+
+  RCLCPP_INFO(get_logger(), "Initialized Footswitch driver.");
+}
+
+FootSwitch::~FootSwitch()
+{
+  RCLCPP_INFO(get_logger(), "Exiting Footswitch driver.");
+  // Close the device
+  if (handle_) {
+    hid_close(handle_);
+  }
+
+  // Finalize the hidapi library
+  hid_exit();
+}
+
+void FootSwitch::update_state()
+{
+  int res = 0;
+  unsigned char buf[65];
+  int i = 0;
+
+  // Continue to poll if no readings have been received
+  while (res == 0 && rclcpp::ok()) {
+    res = hid_read(handle_, buf, sizeof(buf));
+    if (res < 0) {
+      RCLCPP_ERROR(get_logger(), "Unable to read from footswitch: %ls", hid_error(handle_));
+      break;
+    }
+
+    i++;
+    // Break after update period
+    if (i >= update_period_ms_) {
+      break;
+    }
+
+    get_clock()->sleep_for(std::chrono::milliseconds(1));
+  }
+
+  FootswitchState state;
+
+  if (res > 0) {
+    for (int i = 3; i < 6; i++) {
+      switch (buf[i]) {
+        case 0:
+          break;
+        case 4:
+          state.state[0] = true;
+          break;
+        case 5:
+          state.state[1] = true;
+          break;
+        case 6:
+          state.state[2] = true;
+          break;
+
+        default:
+          break;
+      }
+    }
+
+    state.header.stamp = now();
+    pub_footswitch_state_->publish(state);
+  }
+}
+
+}  // namespace footswitch_driver
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(footswitch_driver::FootSwitch)

interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_driver/src/footswitch_driver_node.cpp

@@ -0,0 +1,40 @@
+// Copyright 2024 Trossen Robotics
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the copyright holder nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include "interbotix_footswitch_driver/footswitch_driver.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto footswitch_node = std::make_shared<footswitch_driver::FootSwitch>();
+  rclcpp::spin(footswitch_node);
+
+  rclcpp::shutdown();
+
+  return 0;
+}

interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_msgs/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.5)
+project(interbotix_footswitch_msgs)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+
+set(MSG_FILES
+  msg/FootswitchState.msg
+)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  ${MSG_FILES}
+  DEPENDENCIES std_msgs
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

interbotix_ros_common_drivers/interbotix_ros_footswitch/interbotix_footswitch_msgs/msg/FootswitchState.msg

@@ -0,0 +1,3 @@
+std_msgs/Header header
+
+bool[3] state