diff --git a/README.md b/README.md
index 844b299..d4b7969 100644
--- a/README.md
+++ b/README.md
@@ -25,7 +25,8 @@ $ . install/setup.bash
### Configure Dynamixel motor parameters
-Update the `usb_port`, `baud_rate`, and `joint_ids` parameters on [`open_manipulator_x_description/urdf/open_manipulator_x.ros2_control.xacro`](https://github.com/youtalk/dynamixel_control/blob/main/open_manipulator_x_description/urdf/open_manipulator_x.ros2_control.xacro#L9-L12) to correctly communicate with Dynamixel motors.
+Update the `usb_port`, `baud_rate`, `return_delay_time` and `joint_ids` parameters on [`open_manipulator_x_description/urdf/open_manipulator_x.ros2_control.xacro`](https://github.com/youtalk/dynamixel_control/blob/main/open_manipulator_x_description/urdf/open_manipulator_x.ros2_control.xacro#L9-L12) to correctly communicate with Dynamixel motors.
+The `return_delay_time` sets the amount of delay in microseconds before they respond to a packet.
The `use_dummy` parameter is required if you don't have a real OpenManipulator-X.
Note that `joint_ids` parameters must be splited by `,`.
@@ -35,6 +36,7 @@ Note that `joint_ids` parameters must be splited by `,`.
dynamixel_hardware/DynamixelHardware
/dev/ttyUSB0
1000000
+ 10
```
@@ -86,6 +88,7 @@ index c6cdb74..111846d 100644
@@ -9,7 +9,7 @@
/dev/ttyUSB0
1000000
+ 250
-
+ true
diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp
index 720db9e..e83f41e 100644
--- a/dynamixel_hardware/src/dynamixel_hardware.cpp
+++ b/dynamixel_hardware/src/dynamixel_hardware.cpp
@@ -38,6 +38,7 @@ constexpr const char * kPresentVelocityItem = "Present_Velocity";
constexpr const char * kPresentSpeedItem = "Present_Speed";
constexpr const char * kPresentCurrentItem = "Present_Current";
constexpr const char * kPresentLoadItem = "Present_Load";
+constexpr const char * kReturnDelayTimeItem = "Return_Delay_Time";
constexpr const char * const kExtraJointParameters[] = {
"Profile_Velocity",
"Profile_Acceleration",
@@ -83,10 +84,12 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo
auto usb_port = info_.hardware_parameters.at("usb_port");
auto baud_rate = std::stoi(info_.hardware_parameters.at("baud_rate"));
+ auto return_delay_time = std::stoi(info_.hardware_parameters.at("return_delay_time"));
const char * log = nullptr;
RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "usb_port: %s", usb_port.c_str());
RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "baud_rate: %d", baud_rate);
+ RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "return_delay_time: %d", return_delay_time);
if (!dynamixel_workbench_.init(usb_port.c_str(), baud_rate, &log)) {
RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log);
@@ -99,6 +102,16 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo
RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log);
return CallbackReturn::ERROR;
}
+ RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Ping successful for ID %d, Model number: %d", joint_ids_[i], model_number);
+ int32_t current_return_delay_time = 0;
+ if (dynamixel_workbench_.itemRead(joint_ids_[i], kReturnDelayTimeItem, ¤t_return_delay_time, &log) && current_return_delay_time != return_delay_time)
+ {
+ if (!dynamixel_workbench_.itemWrite(joint_ids_[i], kReturnDelayTimeItem, return_delay_time, &log)) {
+ RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log);
+ return CallbackReturn::ERROR;
+ }
+ RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Successfully set return delay time for ID %d from %d to %d.", joint_ids_[i], current_return_delay_time, return_delay_time);
+ }
}
enable_torque(false);
diff --git a/open_manipulator_x_description/urdf/open_manipulator_x.ros2_control.xacro b/open_manipulator_x_description/urdf/open_manipulator_x.ros2_control.xacro
index ee84c94..76fb518 100644
--- a/open_manipulator_x_description/urdf/open_manipulator_x.ros2_control.xacro
+++ b/open_manipulator_x_description/urdf/open_manipulator_x.ros2_control.xacro
@@ -8,6 +8,7 @@
dynamixel_hardware/DynamixelHardware
/dev/ttyUSB0
1000000
+ 250
diff --git a/pantilt_bot_description/urdf/pantilt_bot.ros2_control.xacro b/pantilt_bot_description/urdf/pantilt_bot.ros2_control.xacro
index ad25ae7..b97be17 100644
--- a/pantilt_bot_description/urdf/pantilt_bot.ros2_control.xacro
+++ b/pantilt_bot_description/urdf/pantilt_bot.ros2_control.xacro
@@ -8,6 +8,7 @@
dynamixel_hardware/DynamixelHardware
/dev/ttyUSB0
1000000
+ 250