diff --git a/CHANGELOG.rst b/CHANGELOG.rst index fdbacb8..4427334 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package dynamixel_hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.4.3 (2025-04-10) +------------------ +* Fixed build errors +* Contributors: Wonho Yun + 1.4.2 (2025-04-05) ------------------ * Added OM-Y dynamixel model files diff --git a/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp b/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp index a7dc825..106304d 100644 --- a/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp +++ b/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp @@ -86,9 +86,9 @@ class DynamixelInfo {return static_cast(effort / dxl_info_[id].torque_constant);} inline double ConvertCurrentToEffort(uint8_t id, int16_t current) {return static_cast(current * dxl_info_[id].torque_constant);} - inline double ConvertValueRPMToVelocityRPS(uint8_t id, int32_t value_rpm) + inline double ConvertValueRPMToVelocityRPS(int32_t value_rpm) {return static_cast(value_rpm * 0.01 / 60.0 * 2.0 * M_PI);} - inline int32_t ConvertVelocityRPSToValueRPM(uint8_t id, double vel_rps) + inline int32_t ConvertVelocityRPSToValueRPM(double vel_rps) {return static_cast(vel_rps * 100.0 * 60.0 / 2.0 / M_PI);} }; diff --git a/package.xml b/package.xml index d1ce6aa..08d8def 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ dynamixel_hardware_interface - 1.4.2 + 1.4.3 ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework. diff --git a/src/dynamixel/dynamixel.cpp b/src/dynamixel/dynamixel.cpp index 6f89c57..f80fd69 100644 --- a/src/dynamixel/dynamixel.cpp +++ b/src/dynamixel/dynamixel.cpp @@ -635,6 +635,8 @@ std::string Dynamixel::DxlErrorToString(DxlError error_num) return "DLX_HARDWARE_ERROR"; case DXL_REBOOT_FAIL: return "DXL_REBOOT_FAIL"; + default: + return "UNKNOWN_ERROR"; } } @@ -965,7 +967,7 @@ DxlError Dynamixel::ProcessReadCommunication( dxl_comm_result = group_sync_read->txPacket(); if (dxl_comm_result != COMM_SUCCESS) { fprintf( - stderr, "SyncRead Tx Fail [Dxl Size : %d] [Error code : %d]\n", + stderr, "SyncRead Tx Fail [Dxl Size : %ld] [Error code : %d]\n", read_data_list_.size(), dxl_comm_result); return DxlError::SYNC_READ_FAIL; } @@ -973,7 +975,7 @@ DxlError Dynamixel::ProcessReadCommunication( dxl_comm_result = group_bulk_read->txPacket(); if (dxl_comm_result != COMM_SUCCESS) { fprintf( - stderr, "BulkRead Tx Fail [Dxl Size : %d] [Error code : %d]\n", + stderr, "BulkRead Tx Fail [Dxl Size : %ld] [Error code : %d]\n", read_data_list_.size(), dxl_comm_result); return DxlError::BULK_READ_FAIL; } @@ -989,7 +991,7 @@ DxlError Dynamixel::ProcessReadCommunication( dxl_comm_result = group_sync_read->rxPacket(); if (dxl_comm_result != COMM_SUCCESS) { fprintf( - stderr, "SyncRead Rx Fail [Dxl Size : %d] [Error code : %d]\n", + stderr, "SyncRead Rx Fail [Dxl Size : %ld] [Error code : %d]\n", read_data_list_.size(), dxl_comm_result); return DxlError::SYNC_READ_FAIL; } @@ -997,7 +999,7 @@ DxlError Dynamixel::ProcessReadCommunication( dxl_comm_result = group_bulk_read->rxPacket(); if (dxl_comm_result != COMM_SUCCESS) { fprintf( - stderr, "BulkRead Rx Fail [Dxl Size : %d] [Error code : %d]\n", + stderr, "BulkRead Rx Fail [Dxl Size : %ld] [Error code : %d]\n", read_data_list_.size(), dxl_comm_result); return DxlError::BULK_READ_FAIL; } @@ -1027,9 +1029,8 @@ DxlError Dynamixel::ProcessReadData( id, static_cast(dxl_getdata)); } else if (item_names[item_index] == "Present Velocity") { - *data_ptrs[item_index] = dxl_info_.ConvertValueRPMToVelocityRPS( - id, - static_cast(dxl_getdata)); + *data_ptrs[item_index] = + dxl_info_.ConvertValueRPMToVelocityRPS(static_cast(dxl_getdata)); } else if (item_names[item_index] == "Present Current") { *data_ptrs[item_index] = dxl_info_.ConvertCurrentToEffort( id, @@ -1173,7 +1174,7 @@ DxlError Dynamixel::SetDxlValueToSyncWrite() param_write_value[added_byte + 0] = DXL_LOBYTE(goal_current); param_write_value[added_byte + 1] = DXL_HIBYTE(goal_current); } else if (indirect_info_write_[ID].item_name.at(item_index) == "Goal Velocity") { - int16_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM(ID, data); + int16_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM(data); param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(goal_velocity)); param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(goal_velocity)); param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_velocity)); @@ -1288,7 +1289,7 @@ DxlError Dynamixel::SetDxlValueToBulkWrite() param_write_value[added_byte + 0] = DXL_LOBYTE(goal_current); param_write_value[added_byte + 1] = DXL_HIBYTE(goal_current); } else if (indirect_info_write_[ID].item_name.at(item_index) == "Goal Velocity") { - int32_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM(ID, data); + int32_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM(data); param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(goal_velocity)); param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(goal_velocity)); param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_velocity)); diff --git a/src/dynamixel_hardware_interface.cpp b/src/dynamixel_hardware_interface.cpp index 21e75fc..091e7fd 100644 --- a/src/dynamixel_hardware_interface.cpp +++ b/src/dynamixel_hardware_interface.cpp @@ -376,13 +376,13 @@ DynamixelHardware::export_command_interfaces() } hardware_interface::CallbackReturn DynamixelHardware::on_activate( - const rclcpp_lifecycle::State & previous_state) + [[maybe_unused]] const rclcpp_lifecycle::State & previous_state) { return start(); } hardware_interface::CallbackReturn DynamixelHardware::on_deactivate( - const rclcpp_lifecycle::State & previous_state) + [[maybe_unused]] const rclcpp_lifecycle::State & previous_state) { return stop(); } @@ -440,7 +440,7 @@ hardware_interface::CallbackReturn DynamixelHardware::stop() } hardware_interface::return_type DynamixelHardware::read( - const rclcpp::Time & time, const rclcpp::Duration & period) + [[maybe_unused]] const rclcpp::Time & time, const rclcpp::Duration & period) { double period_ms = period.seconds() * 1000; if (dxl_status_ == REBOOTING) { @@ -503,7 +503,7 @@ hardware_interface::return_type DynamixelHardware::read( return hardware_interface::return_type::OK; } hardware_interface::return_type DynamixelHardware::write( - const rclcpp::Time & time, const rclcpp::Duration & period) + [[maybe_unused]] const rclcpp::Time & time, const rclcpp::Duration & period) { if (dxl_status_ == DXL_OK || dxl_status_ == HW_ERROR) { dxl_comm_->WriteItemBuf(); @@ -1051,7 +1051,7 @@ void DynamixelHardware::set_dxl_data_srv_callback( } void DynamixelHardware::reboot_dxl_srv_callback( - const std::shared_ptr request, + [[maybe_unused]] const std::shared_ptr request, std::shared_ptr response) { if (CommReset()) {