Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -86,9 +86,9 @@ class DynamixelInfo
{return static_cast<int16_t>(effort / dxl_info_[id].torque_constant);}
inline double ConvertCurrentToEffort(uint8_t id, int16_t current)
{return static_cast<double>(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<double>(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<int32_t>(vel_rps * 100.0 * 60.0 / 2.0 / M_PI);}
};

Expand Down
2 changes: 1 addition & 1 deletion package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>dynamixel_hardware_interface</name>
<version>1.4.2</version>
<version>1.4.3</version>
<description>
ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework.
</description>
Expand Down
19 changes: 10 additions & 9 deletions src/dynamixel/dynamixel.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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";
}
}

Expand Down Expand Up @@ -965,15 +967,15 @@ DxlError Dynamixel::ProcessReadCommunication(
dxl_comm_result = group_sync_read->txPacket();
if (dxl_comm_result != COMM_SUCCESS) {
fprintf(
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

Consider using static_cast<long> to explicitly cast read_data_list_.size() to long to avoid potential compiler warnings or errors. It is generally safer to use %ld for size_t as size_t is an unsigned type, and %d is for signed integers.

Suggested change
fprintf(
stderr, "SyncRead Tx Fail [Dxl Size : %ld] [Error code : %d]\n",
static_cast<long>(read_data_list_.size()), dxl_comm_result);

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;
}
} else {
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",
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

Consider using static_cast<long> to explicitly cast read_data_list_.size() to long to avoid potential compiler warnings or errors. It is generally safer to use %ld for size_t as size_t is an unsigned type, and %d is for signed integers.

Suggested change
stderr, "BulkRead Tx Fail [Dxl Size : %ld] [Error code : %d]\n",
stderr, "BulkRead Tx Fail [Dxl Size : %ld] [Error code : %d]\n",
static_cast<long>(read_data_list_.size()), dxl_comm_result);

read_data_list_.size(), dxl_comm_result);
return DxlError::BULK_READ_FAIL;
}
Expand All @@ -989,15 +991,15 @@ DxlError Dynamixel::ProcessReadCommunication(
dxl_comm_result = group_sync_read->rxPacket();
if (dxl_comm_result != COMM_SUCCESS) {
fprintf(
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

Consider using static_cast<long> to explicitly cast read_data_list_.size() to long to avoid potential compiler warnings or errors. It is generally safer to use %ld for size_t as size_t is an unsigned type, and %d is for signed integers.

Suggested change
fprintf(
stderr, "SyncRead Rx Fail [Dxl Size : %ld] [Error code : %d]\n",
static_cast<long>(read_data_list_.size()), dxl_comm_result);

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;
}
} else {
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",
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

Consider using static_cast<long> to explicitly cast read_data_list_.size() to long to avoid potential compiler warnings or errors. It is generally safer to use %ld for size_t as size_t is an unsigned type, and %d is for signed integers.

Suggested change
stderr, "BulkRead Rx Fail [Dxl Size : %ld] [Error code : %d]\n",
stderr, "BulkRead Rx Fail [Dxl Size : %ld] [Error code : %d]\n",
static_cast<long>(read_data_list_.size()), dxl_comm_result);

read_data_list_.size(), dxl_comm_result);
return DxlError::BULK_READ_FAIL;
}
Expand Down Expand Up @@ -1027,9 +1029,8 @@ DxlError Dynamixel::ProcessReadData(
id,
static_cast<int32_t>(dxl_getdata));
} else if (item_names[item_index] == "Present Velocity") {
*data_ptrs[item_index] = dxl_info_.ConvertValueRPMToVelocityRPS(
id,
static_cast<int32_t>(dxl_getdata));
*data_ptrs[item_index] =
dxl_info_.ConvertValueRPMToVelocityRPS(static_cast<int32_t>(dxl_getdata));
} else if (item_names[item_index] == "Present Current") {
*data_ptrs[item_index] = dxl_info_.ConvertCurrentToEffort(
id,
Expand Down Expand Up @@ -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));
Expand Down Expand Up @@ -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));
Expand Down
10 changes: 5 additions & 5 deletions src/dynamixel_hardware_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -1051,7 +1051,7 @@ void DynamixelHardware::set_dxl_data_srv_callback(
}

void DynamixelHardware::reboot_dxl_srv_callback(
const std::shared_ptr<dynamixel_interfaces::srv::RebootDxl::Request> request,
[[maybe_unused]] const std::shared_ptr<dynamixel_interfaces::srv::RebootDxl::Request> request,
std::shared_ptr<dynamixel_interfaces::srv::RebootDxl::Response> response)
{
if (CommReset()) {
Expand Down
Loading