fix interface ID retrieval + add verbose (serial number and such)

(cherry picked from commit 97a8009)

fd_hardware/src/fd_effort_hi.cpp

@@ -464,6 +464,10 @@ bool FDEffortHardwareInterface::connectToDevice()
 {
   int major, minor, release, revision;
   dhdGetSDKVersion(&major, &minor, &release, &revision);
+  RCLCPP_INFO(
+    LOGGER,
+    "dhd : Using SDK version %d.%d (release %d / revision %d)",
+    major, minor, release, revision);
 
   // Open connection
   bool dhd_open_success = false;
@@ -489,6 +493,13 @@ bool FDEffortHardwareInterface::connectToDevice()
   // Check connection and setup dhd device
   if (dhd_open_success) {
     RCLCPP_INFO(LOGGER, "dhd : %s device detected", dhdGetSystemName());
+    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);
+    }
 
     // Check if the device has 3 dof or more
     if (dhdHasWrist(interface_ID_)) {
@@ -528,8 +539,8 @@ bool FDEffortHardwareInterface::connectToDevice()
         effector_mass_ * 1000.0);
       if (dhdSetEffectorMass(effector_mass_, interface_ID_) < DHD_NO_ERROR) {
         RCLCPP_ERROR(LOGGER, "dhd : Failed to set effector mass!");
-      disconnectFromDevice();
-      return false;
+        disconnectFromDevice();
+        return false;
       }
     }
     // Gravity compensation