diff --git a/fd_hardware/src/fd_effort_hi.cpp b/fd_hardware/src/fd_effort_hi.cpp index e652670..a444b1e 100644 --- a/fd_hardware/src/fd_effort_hi.cpp +++ b/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