diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml index e347923..48662bb 100644 --- a/.github/workflows/ros-ci.yml +++ b/.github/workflows/ros-ci.yml @@ -4,9 +4,9 @@ name: CI # Specifies the events that trigger the workflow on: push: - branches: [ main, humble, jazzy ] + branches: [ humble, jazzy, main] pull_request: - branches: [ main, humble, jazzy ] + branches: [ humble, jazzy, main] # Defines a set of jobs to be run as part of the workflow jobs: diff --git a/.gitignore b/.gitignore index 95e3931..9e5f48a 100644 --- a/.gitignore +++ b/.gitignore @@ -505,6 +505,9 @@ CATKIN_IGNORE AMENT_IGNORE COLCON_IGNORE +## Dynamixel model files +!*.model + ############################## # ETC. diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 4b50831..fdbacb8 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package dynamixel_hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.4.2 (2025-04-05) +------------------ +* Added OM-Y dynamixel model files +* Added a function to enable torque +* Fixed the configuration for OM-Y robots +* Contributors: Woojin Wie, Wonho Yun + 1.4.1 (2025-03-31) ------------------ * Modified the Model File diff --git a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp index ccace4a..4459205 100644 --- a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp +++ b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp @@ -27,6 +27,7 @@ #include #include #include +#include namespace dynamixel_hardware_interface { @@ -166,7 +167,7 @@ class Dynamixel DxlError SetMultiDxlWrite(); // Read Item (sync or bulk) - DxlError ReadMultiDxlData(); + DxlError ReadMultiDxlData(double period_ms); // Write Item (sync or bulk) DxlError WriteMultiDxlData(); @@ -200,12 +201,12 @@ class Dynamixel // SyncRead DxlError SetSyncReadItemAndHandler(); DxlError SetSyncReadHandler(std::vector id_arr); - DxlError GetDxlValueFromSyncRead(); + DxlError GetDxlValueFromSyncRead(double period_ms); // BulkRead DxlError SetBulkReadItemAndHandler(); DxlError SetBulkReadHandler(std::vector id_arr); - DxlError GetDxlValueFromBulkRead(); + DxlError GetDxlValueFromBulkRead(double period_ms); // Read - Indirect Address void ResetIndirectRead(std::vector id_arr); @@ -215,6 +216,23 @@ class Dynamixel uint16_t item_addr, uint8_t item_size); + // Read - Data Processing + DxlError ProcessReadData( + uint8_t id, + uint16_t indirect_addr, + const std::vector & item_names, + const std::vector & item_sizes, + const std::vector> & data_ptrs, + std::function get_data_func); + + // Read - Communication + DxlError ProcessReadCommunication( + dynamixel::GroupFastSyncRead * group_sync_read, + dynamixel::GroupFastBulkRead * group_bulk_read, + dynamixel::PortHandler * port_handler, + double period_ms, + bool is_sync); + // SyncWrite DxlError SetSyncWriteItemAndHandler(); DxlError SetSyncWriteHandler(std::vector id_arr); diff --git a/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp b/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp index 5fc691a..365b437 100644 --- a/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp +++ b/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp @@ -191,6 +191,8 @@ class DynamixelHardware : public bool is_read_in_error_{false}; bool is_write_in_error_{false}; + bool global_torque_enable_{true}; + bool use_revolute_to_prismatic_{false}; std::string conversion_dxl_name_{""}; std::string conversion_joint_name_{""}; @@ -235,6 +237,9 @@ class DynamixelHardware : public std::vector sensor_id_; std::map sensor_item_; + std::vector controller_id_; + std::map controller_item_; + ///// handler variable std::vector hdl_trans_states_; std::vector hdl_trans_commands_; @@ -245,6 +250,8 @@ class DynamixelHardware : public std::vector hdl_gpio_sensor_states_; std::vector hdl_sensor_states_; + bool is_set_hdl_{false}; + // joint <-> transmission matrix size_t num_of_joints_; size_t num_of_transmissions_; @@ -252,11 +259,33 @@ class DynamixelHardware : public double ** joint_to_transmission_matrix_; /** - * @brief Initializes the Dynamixel items. + * @brief Helper function to initialize items for a specific type. + * @param type_filter The type of items to initialize ("controller" or "dxl" or "sensor"). + * @return True if initialization was successful, false otherwise. + */ + bool initItems(const std::string & type_filter); + + /** + * @brief Helper function to retry writing an item to a Dynamixel device. + * @param id The ID of the Dynamixel device. + * @param item_name The name of the item to write. + * @param value The value to write. + * @return True if write was successful, false if timeout occurred. + */ + bool retryWriteItem(uint8_t id, const std::string & item_name, uint32_t value); + + /** + * @brief Initializes Dynamixel items. * @return True if initialization was successful, false otherwise. */ bool InitDxlItems(); + /** + * @brief Initializes the controller items. + * @return True if initialization was successful, false otherwise. + */ + bool InitControllerItems(); + /** * @brief Initializes the read items for Dynamixel. * @return True if initialization was successful, false otherwise. diff --git a/package.xml b/package.xml index 0d7f149..d1ce6aa 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,8 @@ - + + dynamixel_hardware_interface - 1.4.1 + 1.4.2 ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework. @@ -10,6 +11,7 @@ Hye-Jong KIM Sungho Woo Woojin Wie + Wonho Yun ament_cmake rclcpp hardware_interface diff --git a/param/dxl_model/dynamixel.model b/param/dxl_model/dynamixel.model index 985850d..725444d 100644 --- a/param/dxl_model/dynamixel.model +++ b/param/dxl_model/dynamixel.model @@ -44,3 +44,5 @@ Number Name 4150 ym080_230_r099.model 4170 ym080_230_a099.model 35074 rh_p12_rn.model +220 omy_hat.model +230 omy_end.model diff --git a/param/dxl_model/omy_end.model b/param/dxl_model/omy_end.model new file mode 100644 index 0000000..31487a4 --- /dev/null +++ b/param/dxl_model/omy_end.model @@ -0,0 +1,52 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 740 +value_of_min_radian_position -740 +min_radian -1.099 +max_radian 1.099 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate (Bus) +10 1 Tool Analog Enable +11 1 Tool Baud Rate +12 1 Tool Protocol +13 1 Tool ID +14 1 Tool Status +30 1 Button Status +32 1 R LED +33 1 G LED +34 1 B LED +36 2 Realtime Tick +40 1 Digital Input +41 1 Digital Output +42 2 Analog Input1 +44 2 Analog Input2 +46 1 SyncTable Enable +48 2 SyncTable Read Address +52 2 SyncTable Read Size +56 2 SyncTable Write Address +60 2 SyncTable Write Size +378 2 Indirect Address +634 1 Indirect Data +72 1 Hardware Error Status +73 1 Moving +74 2 Present Current +76 4 Present Velocity +80 4 Present Position +84 4 Position Trajectory +200 1 Torque Enable +202 2 Goal Current +204 4 Goal Velocity +208 4 Goal Position +378 2 Indirect Address 1 +634 1 Indirect Data 1 +378 2 Indirect Address Write +634 1 Indirect Data Write +402 2 Indirect Address Read +646 1 Indirect Data Read diff --git a/param/dxl_model/omy_hat.model b/param/dxl_model/omy_hat.model new file mode 100644 index 0000000..e51bd9f --- /dev/null +++ b/param/dxl_model/omy_hat.model @@ -0,0 +1,35 @@ +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate (Bus) +9 1 DXL Baud Rate +10 2 DB Voltage +12 1 DB Voltage Thresh +14 2 DB Voltage D Gain +16 2 DB Voltage I Gain +18 2 DB Voltage P Gain +20 1 SyncTable ID +30 2 SyncTable Read Address +50 2 SyncTable Read Size +70 2 SyncTable Write Address +90 2 SyncTable Write Size +110 2 SyncTable Period +112 2 Indirect Address +512 1 Power Enable +513 1 Voltage Control Enable +514 1 Table Sync Enable +515 1 R LED +516 1 G LED +517 1 B LED +518 1 Status +519 1 EMG Power Ctrl +520 2 Realtime Tick +522 2 Present Input Voltage +524 2 Present Input Current +550 1 Status Return Level +560 1 SyncTable Read Data +688 1 SyncTable Write Data +816 1 Indirect Data diff --git a/param/dxl_model/rh_p12_rn.model b/param/dxl_model/rh_p12_rn.model index 428fbee..afb407d 100644 --- a/param/dxl_model/rh_p12_rn.model +++ b/param/dxl_model/rh_p12_rn.model @@ -2,8 +2,8 @@ name value value_of_zero_radian_position 0 value_of_max_radian_position 740 -value_of_min_radian_position 0 -min_radian 0.0 +value_of_min_radian_position -740 +min_radian -1.099 max_radian 1.099 [control table] diff --git a/src/dynamixel/dynamixel.cpp b/src/dynamixel/dynamixel.cpp index 0ac8398..6f89c57 100644 --- a/src/dynamixel/dynamixel.cpp +++ b/src/dynamixel/dynamixel.cpp @@ -20,6 +20,7 @@ #include #include #include +#include namespace dynamixel_hardware_interface { @@ -300,18 +301,19 @@ DxlError Dynamixel::DynamixelEnable(std::vector id_arr) DxlError Dynamixel::DynamixelDisable(std::vector id_arr) { + DxlError result = DxlError::OK; for (auto it_id : id_arr) { if (torque_state_[it_id] == TORQUE_ON) { if (WriteItem(it_id, "Torque Enable", TORQUE_OFF) < 0) { fprintf(stderr, "[ID:%03d] Cannot write \"Torque Off\" command!\n", it_id); - return DxlError::ITEM_WRITE_FAIL; + result = DxlError::ITEM_WRITE_FAIL; } else { torque_state_[it_id] = TORQUE_OFF; fprintf(stderr, "[ID:%03d] Torque OFF\n", it_id); } } } - return DxlError::OK; + return result; } DxlError Dynamixel::SetOperatingMode(uint8_t dxl_id, uint8_t dynamixel_mode) @@ -636,12 +638,12 @@ std::string Dynamixel::DxlErrorToString(DxlError error_num) } } -DxlError Dynamixel::ReadMultiDxlData() +DxlError Dynamixel::ReadMultiDxlData(double period_ms) { if (read_type_ == SYNC) { - return GetDxlValueFromSyncRead(); + return GetDxlValueFromSyncRead(period_ms); } else { - return GetDxlValueFromBulkRead(); + return GetDxlValueFromBulkRead(period_ms); } } @@ -813,39 +815,27 @@ DxlError Dynamixel::SetSyncReadHandler(std::vector id_arr) return DxlError::OK; } -DxlError Dynamixel::GetDxlValueFromSyncRead() +DxlError Dynamixel::GetDxlValueFromSyncRead(double period_ms) { - // SyncRead tx - int dxl_comm_result = group_sync_read_->txRxPacket(); - if (dxl_comm_result != COMM_SUCCESS) { - fprintf(stderr, "SyncRead TxRx Fail [Error code : %d]\n", dxl_comm_result); - return DxlError::SYNC_READ_FAIL; + DxlError comm_result = ProcessReadCommunication( + group_sync_read_, group_bulk_read_, port_handler_, period_ms, true); + if (comm_result != DxlError::OK) { + return comm_result; } for (auto it_read_data : read_data_list_) { - uint8_t ID = it_read_data.id; - uint16_t IN_ADDR = indirect_info_read_[ID].indirect_data_addr; - - for (size_t item_index = 0; item_index < indirect_info_read_[ID].cnt; item_index++) { - uint8_t SIZE = indirect_info_read_[ID].item_size.at(item_index); - if (item_index > 0) {IN_ADDR += indirect_info_read_[ID].item_size.at(item_index - 1);} - - uint32_t dxl_getdata = group_sync_read_->getData(ID, IN_ADDR, SIZE); - - if (indirect_info_read_[ID].item_name.at(item_index) == "Present Position") { - *it_read_data.item_data_ptr_vec.at(item_index) = - dxl_info_.ConvertValueToRadian(ID, static_cast(dxl_getdata)); - } else if (indirect_info_read_[ID].item_name.at(item_index) == "Present Velocity") { - *it_read_data.item_data_ptr_vec.at(item_index) = - dxl_info_.ConvertValueRPMToVelocityRPS(ID, static_cast(dxl_getdata)); - } else if (indirect_info_read_[ID].item_name.at(item_index) == "Present Current") { - *it_read_data.item_data_ptr_vec.at(item_index) = - dxl_info_.ConvertCurrentToEffort(ID, static_cast(dxl_getdata)); - } else { - *it_read_data.item_data_ptr_vec.at(item_index) = - static_cast(dxl_getdata); - } - } + uint8_t id = it_read_data.id; + uint16_t indirect_addr = indirect_info_read_[id].indirect_data_addr; + + ProcessReadData( + id, + indirect_addr, + indirect_info_read_[id].item_name, + indirect_info_read_[id].item_size, + it_read_data.item_data_ptr_vec, + [this](uint8_t id, uint16_t addr, uint8_t size) { + return group_sync_read_->getData(id, addr, size); + }); } return DxlError::OK; } @@ -887,7 +877,7 @@ DxlError Dynamixel::SetBulkReadItemAndHandler() if (SetBulkReadHandler(id_arr) != DxlError::OK) { fprintf(stderr, "Cannot set the BulkRead handler.\n"); - return DxlError::SYNC_READ_FAIL; + return DxlError::BULK_READ_FAIL; } fprintf(stderr, "Success to set BulkRead handler using indirect address\n"); @@ -936,42 +926,122 @@ DxlError Dynamixel::SetBulkReadHandler(std::vector id_arr) return DxlError::OK; } -DxlError Dynamixel::GetDxlValueFromBulkRead() +DxlError Dynamixel::GetDxlValueFromBulkRead(double period_ms) { - int dxl_comm_result = group_bulk_read_->txRxPacket(); - if (dxl_comm_result != COMM_SUCCESS) { - fprintf(stderr, "BulkRead TxRx Fail [Error code : %d]\n", dxl_comm_result); - return DxlError::BULK_READ_FAIL; + DxlError comm_result = ProcessReadCommunication( + group_sync_read_, group_bulk_read_, port_handler_, period_ms, false); + if (comm_result != DxlError::OK) { + return comm_result; } for (auto it_read_data : read_data_list_) { - uint8_t ID = it_read_data.id; - uint16_t IN_ADDR = indirect_info_read_[ID].indirect_data_addr; - - for (size_t item_index = 0; item_index < indirect_info_read_[ID].cnt; item_index++) { - uint8_t SIZE = indirect_info_read_[ID].item_size.at(item_index); - if (item_index > 0) {IN_ADDR += indirect_info_read_[ID].item_size.at(item_index - 1);} - - uint32_t dxl_getdata = group_bulk_read_->getData(ID, IN_ADDR, SIZE); - - if (indirect_info_read_[ID].item_name.at(item_index) == "Present Position") { - *it_read_data.item_data_ptr_vec.at(item_index) = - dxl_info_.ConvertValueToRadian(ID, static_cast(dxl_getdata)); - } else if (indirect_info_read_[ID].item_name.at(item_index) == "Present Velocity") { - *it_read_data.item_data_ptr_vec.at(item_index) = - dxl_info_.ConvertValueRPMToVelocityRPS(ID, static_cast(dxl_getdata)); - } else if (indirect_info_read_[ID].item_name.at(item_index) == "Present Current") { - *it_read_data.item_data_ptr_vec.at(item_index) = - dxl_info_.ConvertCurrentToEffort(ID, static_cast(dxl_getdata)); - } else { - *it_read_data.item_data_ptr_vec.at(item_index) = - static_cast(dxl_getdata); - } + uint8_t id = it_read_data.id; + uint16_t indirect_addr = indirect_info_read_[id].indirect_data_addr; + + ProcessReadData( + id, + indirect_addr, + indirect_info_read_[id].item_name, + indirect_info_read_[id].item_size, + it_read_data.item_data_ptr_vec, + [this](uint8_t id, uint16_t addr, uint8_t size) { + return group_bulk_read_->getData(id, addr, size); + }); + } + return DxlError::OK; +} + +DxlError Dynamixel::ProcessReadCommunication( + dynamixel::GroupFastSyncRead * group_sync_read, + dynamixel::GroupFastBulkRead * group_bulk_read, + dynamixel::PortHandler * port_handler, + double period_ms, + bool is_sync) +{ + int dxl_comm_result; + + // Send packet + if (is_sync) { + 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", + 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", + read_data_list_.size(), dxl_comm_result); + return DxlError::BULK_READ_FAIL; + } + } + + // Set timeout if period_ms is specified + if (period_ms > 0) { + port_handler->setPacketTimeout(period_ms); + } + + // Receive packet + if (is_sync) { + 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", + 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", + read_data_list_.size(), dxl_comm_result); + return DxlError::BULK_READ_FAIL; + } + } + + return DxlError::OK; +} + +DxlError Dynamixel::ProcessReadData( + uint8_t id, + uint16_t indirect_addr, + const std::vector & item_names, + const std::vector & item_sizes, + const std::vector> & data_ptrs, + std::function get_data_func) +{ + uint16_t current_addr = indirect_addr; + + for (size_t item_index = 0; item_index < item_names.size(); item_index++) { + uint8_t size = item_sizes[item_index]; + if (item_index > 0) {current_addr += item_sizes[item_index - 1];} + + uint32_t dxl_getdata = get_data_func(id, current_addr, size); + + if (item_names[item_index] == "Present Position") { + *data_ptrs[item_index] = dxl_info_.ConvertValueToRadian( + 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)); + } else if (item_names[item_index] == "Present Current") { + *data_ptrs[item_index] = dxl_info_.ConvertCurrentToEffort( + id, + static_cast(dxl_getdata)); + } else { + *data_ptrs[item_index] = static_cast(dxl_getdata); } } return DxlError::OK; } + void Dynamixel::ResetIndirectRead(std::vector id_arr) { IndirectInfo temp; diff --git a/src/dynamixel_hardware_interface.cpp b/src/dynamixel_hardware_interface.cpp index e3822b8..21e75fc 100644 --- a/src/dynamixel_hardware_interface.cpp +++ b/src/dynamixel_hardware_interface.cpp @@ -76,6 +76,17 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( e.what()); } + try { + global_torque_enable_ = + std::stoi(info_.hardware_parameters["torque_enable"]) != 0; + RCLCPP_INFO_STREAM( + logger_, "Torque enable parameter: " << global_torque_enable_); + } catch (const std::exception & e) { + RCLCPP_ERROR( + logger_, "Failed to parse torque_enable parameter: %s, using default value", + e.what()); + } + RCLCPP_INFO_STREAM( logger_, "port_name " << port_name_.c_str() << " / baudrate " << baud_rate_.c_str()); @@ -105,6 +116,8 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( dxl_id_.push_back(static_cast(stoi(gpio.parameters.at("ID")))); } else if (gpio.parameters.at("type") == "sensor") { sensor_id_.push_back(static_cast(stoi(gpio.parameters.at("ID")))); + } else if (gpio.parameters.at("type") == "controller") { + controller_id_.push_back(static_cast(stoi(gpio.parameters.at("ID")))); } else { RCLCPP_ERROR_STREAM(logger_, "Invalid DXL / Sensor type"); exit(-1); @@ -115,6 +128,35 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( int trying_cnt = 60; int cnt = 0; + if (controller_id_.size() > 0) { + while (trying_connect) { + std::vector id_arr; + for (auto controller : controller_id_) { + id_arr.push_back(controller); + } + if (dxl_comm_->InitDxlComm(id_arr, port_name_, baud_rate_) == DxlError::OK) { + RCLCPP_INFO_STREAM(logger_, "Trying to connect to the communication port..."); + trying_connect = false; + } else { + sleep(1); + cnt++; + if (cnt > trying_cnt) { + RCLCPP_ERROR_STREAM(logger_, "Cannot connect communication port! :("); + cnt = 0; + } + } + } + } + + if (!InitControllerItems()) { + RCLCPP_ERROR_STREAM(logger_, "Error: InitControllerItems"); + return hardware_interface::CallbackReturn::ERROR; + } + + trying_connect = true; + trying_cnt = 60; + cnt = 0; + while (trying_connect) { std::vector id_arr; for (auto dxl : dxl_id_) { @@ -347,7 +389,7 @@ hardware_interface::CallbackReturn DynamixelHardware::on_deactivate( hardware_interface::CallbackReturn DynamixelHardware::start() { - dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData()); + dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData(0.0)); if (dxl_comm_err_ != DxlError::OK) { RCLCPP_ERROR_STREAM( logger_, @@ -377,7 +419,11 @@ hardware_interface::CallbackReturn DynamixelHardware::start() } usleep(500 * 1000); - dxl_comm_->DynamixelEnable(dxl_id_); + if (global_torque_enable_) { + dxl_comm_->DynamixelEnable(dxl_id_); + } else { + RCLCPP_INFO_STREAM(logger_, "Global Torque is disabled!"); + } RCLCPP_INFO_STREAM(logger_, "Dynamixel Hardware Start!"); @@ -396,11 +442,12 @@ hardware_interface::CallbackReturn DynamixelHardware::stop() hardware_interface::return_type DynamixelHardware::read( const rclcpp::Time & time, const rclcpp::Duration & period) { + double period_ms = period.seconds() * 1000; if (dxl_status_ == REBOOTING) { RCLCPP_ERROR_STREAM(logger_, "Dynamixel Read Fail : REBOOTING"); return hardware_interface::return_type::ERROR; } else if (dxl_status_ == DXL_OK || dxl_status_ == COMM_ERROR) { - dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData()); + dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData(period_ms)); if (dxl_comm_err_ != DxlError::OK) { if (!is_read_in_error_) { is_read_in_error_ = true; @@ -421,7 +468,7 @@ hardware_interface::return_type DynamixelHardware::read( is_read_in_error_ = false; read_error_duration_ = rclcpp::Duration(0, 0); } else if (dxl_status_ == HW_ERROR) { - dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData()); + dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData(period_ms)); if (dxl_comm_err_ != DxlError::OK) { RCLCPP_ERROR_STREAM( logger_, @@ -566,6 +613,7 @@ bool DynamixelHardware::CommReset() usleep(200 * 1000); } if (!result) {continue;} + if (!InitControllerItems()) {continue;} if (!InitDxlItems()) {continue;} if (!InitDxlReadItems()) {continue;} if (!InitDxlWriteItems()) {continue;} @@ -582,54 +630,79 @@ bool DynamixelHardware::CommReset() return false; } -bool DynamixelHardware::InitDxlItems() +bool DynamixelHardware::retryWriteItem(uint8_t id, const std::string & item_name, uint32_t value) { - RCLCPP_INFO_STREAM(logger_, "$$$$$ Init Dxl Items"); + rclcpp::Time start_time = this->now(); + rclcpp::Duration error_duration(0, 0); + + while (true) { + if (dxl_comm_->WriteItem(id, item_name, value) == DxlError::OK) { + RCLCPP_INFO_STREAM( + logger_, + "[ID:" << std::to_string(id) << "] item_name:" << item_name.c_str() << "\tdata:" << value); + return true; + } + + error_duration = this->now() - start_time; + if (error_duration.seconds() * 1000 >= err_timeout_ms_) { + RCLCPP_ERROR_STREAM( + logger_, + "[ID:" << std::to_string(id) << "] Write Item error (Timeout: " << + error_duration.seconds() * 1000 << "ms/" << err_timeout_ms_ << "ms)"); + return false; + } + RCLCPP_WARN_STREAM( + logger_, + "[ID:" << std::to_string(id) << "] Write Item retry..."); + } +} + +bool DynamixelHardware::initItems(const std::string & type_filter) +{ + RCLCPP_INFO_STREAM(logger_, "$$$$$ Init Items for type: " << type_filter); for (const hardware_interface::ComponentInfo & gpio : info_.gpios) { + if (gpio.parameters.at("type") != type_filter) { + continue; + } uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); + // First write items containing "Limit" for (auto it : gpio.parameters) { if (it.first != "ID" && it.first != "type" && it.first.find("Limit") != std::string::npos) { - if (dxl_comm_->WriteItem( - id, it.first, - static_cast(stoi(it.second))) != DxlError::OK) - { - RCLCPP_ERROR_STREAM(logger_, "[ID:" << std::to_string(id) << "] Write Item error"); + if (!retryWriteItem(id, it.first, static_cast(stoi(it.second)))) { return false; } - RCLCPP_INFO_STREAM( - logger_, - "[ID:" << std::to_string(id) << "] item_name:" << it.first.c_str() << "\tdata:" << - stoi(it.second)); } } // Then write the remaining items for (auto it : gpio.parameters) { if (it.first != "ID" && it.first != "type" && it.first.find("Limit") == std::string::npos) { - if (dxl_comm_->WriteItem( - id, it.first, - static_cast(stoi(it.second))) != DxlError::OK) - { - RCLCPP_ERROR_STREAM(logger_, "[ID:" << std::to_string(id) << "] Write Item error"); + if (!retryWriteItem(id, it.first, static_cast(stoi(it.second)))) { return false; } - RCLCPP_INFO_STREAM( - logger_, - "[ID:" << std::to_string(id) << "] item_name:" << it.first.c_str() << "\tdata:" << - stoi(it.second)); } } } return true; } +bool DynamixelHardware::InitControllerItems() +{ + return initItems("controller"); +} + +bool DynamixelHardware::InitDxlItems() +{ + return initItems("dxl") && initItems("sensor"); +} + bool DynamixelHardware::InitDxlReadItems() { RCLCPP_INFO_STREAM(logger_, "$$$$$ Init Dxl Read Items"); - static bool is_set_hdl = false; + is_set_hdl_ = false; - if (!is_set_hdl) { + if (!is_set_hdl_) { hdl_trans_states_.clear(); hdl_gpio_sensor_states_.clear(); for (const hardware_interface::ComponentInfo & gpio : info_.gpios) { @@ -685,7 +758,7 @@ bool DynamixelHardware::InitDxlReadItems() hdl_gpio_sensor_states_.push_back(temp_sensor); } } - is_set_hdl = true; + is_set_hdl_ = true; } for (auto it : hdl_trans_states_) { if (dxl_comm_->SetDxlReadItems( @@ -704,9 +777,9 @@ bool DynamixelHardware::InitDxlReadItems() bool DynamixelHardware::InitDxlWriteItems() { RCLCPP_INFO_STREAM(logger_, "$$$$$ Init Dxl Write Items"); - static bool is_set_hdl = false; + is_set_hdl_ = false; - if (!is_set_hdl) { + if (!is_set_hdl_) { hdl_trans_commands_.clear(); for (const hardware_interface::ComponentInfo & gpio : info_.gpios) { if (gpio.command_interfaces.size()) { @@ -728,7 +801,7 @@ bool DynamixelHardware::InitDxlWriteItems() hdl_trans_commands_.push_back(temp_write); } } - is_set_hdl = true; + is_set_hdl_ = true; } for (auto it : hdl_trans_commands_) {