refactor: Replace std::cout with RCLCPP logging for torque state changes in DynamixelHardware

Author: Woojin-Crive

src/dynamixel/dynamixel.cpp

@@ -555,15 +555,11 @@ DxlError Dynamixel::WriteItem(uint8_t id, uint16_t addr, uint8_t size, uint32_t
     } else if (dxl_error != 0) {
       fprintf(
         stderr,
-        "[WriteItem][ID:%03d][comm_id:%03d] RX_PACKET_ERROR : %s (retry %d/%d)\n",
+        "[WriteItem][ID:%03d][comm_id:%03d] RX_PACKET_ERROR : %s\n",
         id,
         comm_id,
-        packet_handler_->getRxPacketError(dxl_error),
-        i + 1,
-        MAX_COMM_RETRIES);
-      if (i == MAX_COMM_RETRIES - 1) {
-        return DxlError::ITEM_WRITE_FAIL;
-      }
+        packet_handler_->getRxPacketError(dxl_error));
+      return DxlError::ITEM_WRITE_FAIL;
     } else {
       return DxlError::OK;
     }

src/dynamixel_hardware_interface.cpp

@@ -1300,15 +1300,16 @@ void DynamixelHardware::ChangeDxlTorqueState()
   }
 
   if (dxl_torque_status_ == REQUESTED_TO_ENABLE) {
-    std::cout << "Requested to enable torque, Enabling torque for all Dynamixels" << std::endl;
+    RCLCPP_WARN_STREAM(logger_, "Requested to enable torque, Enabling torque for all Dynamixels");
     dxl_comm_->DynamixelEnable(torque_enabled_ids_);
     SyncJointCommandWithStates();
   } else if (dxl_torque_status_ == REQUESTED_TO_DISABLE) {
-    std::cout << "Requested to disable torque, Disabling torque for all Dynamixels" << std::endl;
+    RCLCPP_WARN_STREAM(logger_, "Requested to disable torque, Disabling torque for all Dynamixels");
     dxl_comm_->DynamixelDisable(torque_enabled_ids_);
     SyncJointCommandWithStates();
   }
 
+  dxl_torque_state_ = dxl_comm_->GetDxlTorqueState();
   for (auto single_torque_state : dxl_torque_state_) {
     if (single_torque_state.second == TORQUE_OFF) {
       dxl_torque_status_ = TORQUE_DISABLED;
@@ -1384,6 +1385,7 @@ void DynamixelHardware::set_dxl_torque_srv_callback(
     if (dxl_torque_status_ == TORQUE_ENABLED) {
       response->success = true;
       response->message = "Already enabled.";
+      RCLCPP_INFO_STREAM(logger_, "Requested to enable torque, but already enabled.");
       return;
     } else {
       dxl_torque_status_ = REQUESTED_TO_ENABLE;
@@ -1392,6 +1394,7 @@ void DynamixelHardware::set_dxl_torque_srv_callback(
     if (dxl_torque_status_ == TORQUE_DISABLED) {
       response->success = true;
       response->message = "Already disabled.";
+      RCLCPP_INFO_STREAM(logger_, "Requested to disable torque, but already disabled.");
       return;
     } else {
       dxl_torque_status_ = REQUESTED_TO_DISABLE;