add serial number selection support

ICube-Robotics · Sep 2, 2024 · 296a0b0 · 296a0b0
1 parent 97a8009
commit 296a0b0
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 8 deletions.
diff --git a/fd_description/config/fd.config.xacro b/fd_description/config/fd.config.xacro
@@ -18,6 +18,7 @@
     <xacro:include filename="$(find fd_description)/ros2_control/fd.r2c_hardware.xacro" />
     <xacro:fd_ros2_control
         interface_id="-1"
+        interface_sn="-1"
         robot_id= "$(arg robot_id)"
         use_fake_hardware="$(arg use_fake_hardware)"
         use_orientation="$(arg use_orientation)"

diff --git a/fd_description/ros2_control/fd.r2c_hardware.xacro b/fd_description/ros2_control/fd.r2c_hardware.xacro
@@ -2,7 +2,7 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:macro
         name="fd_ros2_control"
-        params="robot_id plugin_name:='FDHapticInterface' interface_id:='-1' effector_mass:='-1.0' use_fake_hardware:='false' use_orientation:='false' orientation_is_actuated:='false' use_clutch:='false' emulate_button:='false' ignore_orientation_readings:='false'">
+        params="robot_id plugin_name:='FDHapticInterface' interface_id:='-1' interface_sn:='-1' effector_mass:='-1.0' use_fake_hardware:='false' use_orientation:='false' orientation_is_actuated:='false' use_clutch:='false' emulate_button:='false' ignore_orientation_readings:='false'">
         <ros2_control name="${plugin_name}" type="system">
             <hardware>
                 <xacro:if value="${use_fake_hardware}">
@@ -11,6 +11,7 @@
                 <xacro:unless value="${use_fake_hardware}">
                     <plugin>fd_hardware/FDEffortHardwareInterface</plugin>
                     <param name="interface_id">${interface_id}</param>
+                    <param name="interface_serial_number">${interface_sn}</param>
                     <param name="emulate_button">${emulate_button}</param>
                     <param name="inertia_interface_name">${robot_id}_inertia</param>
                     <param name="effector_mass">${effector_mass}</param>

diff --git a/fd_hardware/include/fd_hardware/fd_effort_hi.hpp b/fd_hardware/include/fd_hardware/fd_effort_hi.hpp
@@ -62,6 +62,9 @@ class FDEffortHardwareInterface : public hardware_interface::SystemInterface
   /// ID of the interface (Rq: "-1" = invalid/any that is connected)
   char interface_ID_ = -1;
 
+  /// Serial number of the interface (Rq: "-1" = invalid / unspecified)
+  int interface_SN_ = -1;
+
   /// Turned to true after the connection
   bool isConnected_ = false;
 

diff --git a/fd_hardware/src/fd_effort_hi.cpp b/fd_hardware/src/fd_effort_hi.cpp
@@ -161,6 +161,15 @@ CallbackReturn FDEffortHardwareInterface::on_init(
     interface_ID_ = -1;
   }
 
+
+  auto it_interface_serial_number = info_.hardware_parameters.find("interface_serial_number");
+  if (it_interface_serial_number != info_.hardware_parameters.end()) {
+    interface_SN_ = stoi(it_interface_serial_number->second);
+    RCLCPP_INFO(LOGGER, "Using interface serial number: %d", interface_SN_);
+  } else {
+    interface_SN_ = -1;
+  }
+
   auto it_emulate_button = info_.hardware_parameters.find("emulate_button");
   if (it_emulate_button != info_.hardware_parameters.end()) {
     emulate_button_ = hardware_interface::parse_bool(it_emulate_button->second);
@@ -462,16 +471,23 @@ bool FDEffortHardwareInterface::connectToDevice()
 
   // Open connection
   bool dhd_open_success = false;
-  if (interface_ID_ < 0) {
-    // Open default device
-    RCLCPP_INFO(LOGGER, "dhd : Connecting to default device...");
-    interface_ID_ = dhdOpen();
+  if (interface_SN_ >= 0) {
+    // Open specified device from serial number
+    RCLCPP_INFO(
+      LOGGER, "dhd : Connecting to device with serial number %d, please wait...", interface_SN_);
+    uint16_t serialNumber = static_cast<uint16_t>(interface_SN_);
+    interface_ID_ = dhdOpenSerial(serialNumber);
     dhd_open_success = (interface_ID_ >= 0);
-  } else {
-    // Open specified device
-    RCLCPP_INFO(LOGGER, "dhd : Connecting to device with ID= %d...", interface_ID_);
+  } else if (interface_ID_ >= 0) {
+    // Open specified device from device ID
+    RCLCPP_INFO(LOGGER, "dhd : Connecting to device with ID= %d, please wait...", interface_ID_);
     interface_ID_ = dhdOpenID(interface_ID_);
     dhd_open_success = (interface_ID_ >= 0);
+  } else {
+    // Open default device
+    RCLCPP_INFO(LOGGER, "dhd : Connecting to default device., please wait..");
+    interface_ID_ = dhdOpen();
+    dhd_open_success = (interface_ID_ >= 0);
   }
 
   // Check connection and setup dhd device