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