enable serial number device selection

(cherry picked from commit 731d7a9)

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)"

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>

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;
 

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);
@@ -381,7 +390,7 @@ hardware_interface::return_type FDEffortHardwareInterface::read(
   flag += dhdGetJointAngles(joint_position, interface_ID_);
   // use "dhdJointAnglesToInertiaMatrix (double j[DHD_MAX_DOF], double inertia[6][6], char ID=-1)"
   // to get the current inertia matrix
-  flag += dhdJointAnglesToInertiaMatrix(joint_position, inertia_array);
+  flag += dhdJointAnglesToInertiaMatrix(joint_position, inertia_array, interface_ID_);
   flag += dhdDisableExpertMode();
 
   // Map upper triangular part of inertia to inertia state interface
@@ -462,28 +471,39 @@ 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_);
+    // dhd_open_success = (dhdOpenSerial(serialNumber) >= 0);
+  } 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);
+    // dhd_open_success = (dhdOpenID(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);
+    // dhd_open_success = (dhdOpen() >= 0);
   }
 
   // Check connection and setup dhd device
   if (dhd_open_success) {
-    RCLCPP_INFO(LOGGER, "dhd : %s device detected", dhdGetSystemName());
+    RCLCPP_INFO(LOGGER, "dhd : %s device detected", dhdGetSystemName(interface_ID_));
     uint16_t serialNumber = 0;
     if (dhdGetSerialNumber(&serialNumber, interface_ID_) < 0) {
       RCLCPP_WARN(LOGGER, "dhd : Impossible to retrieve device serial number: %s!",
         dhdErrorGetLastStr());
     } else {
       RCLCPP_INFO(LOGGER, "dhd : device serial number = %d", serialNumber);
     }
+    RCLCPP_INFO(LOGGER, "dhd : device interface ID = %d", interface_ID_);
 
     // Check if the device has 3 dof or more
     if (dhdHasWrist(interface_ID_)) {
@@ -591,7 +611,7 @@ bool FDEffortHardwareInterface::disconnectFromDevice()
   int connectionIsClosed = dhdClose(interface_ID_);
   if (connectionIsClosed >= 0) {
     RCLCPP_INFO(LOGGER, "dhd :  Disconnected ! ");
-    interface_ID_ = false;
+    interface_ID_ = -1;
     return true;
   } else {
     RCLCPP_ERROR(LOGGER, "dhd : Failed to disconnect !");