diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 4427334..9853e5c 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package dynamixel_hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.4.4 (2025-05-28) +------------------ +* Added proper command interface support with ROS2-Dynamixel interface mapping +* Improved error handling and robustness throughout the codebase +* Implemented per-device torque enable control (replacing global control) +* Added support for new sensor model (sensorxel_joy) +* Enhanced joint state-command synchronization +* Improved parameter initialization organization +* Added robust error handling for model file reading +* Contributors: Woojin Wie + 1.4.3 (2025-04-10) ------------------ * Fixed build errors diff --git a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp index 4459205..ee73695 100644 --- a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp +++ b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// Authors: Hye-Jong KIM, Sungho Woo +// Authors: Hye-Jong KIM, Sungho Woo, Woojin Wie #ifndef DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_HPP_ #define DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_HPP_ @@ -110,8 +110,8 @@ class Dynamixel { private: // dxl communication variable - dynamixel::PortHandler * port_handler_; - dynamixel::PacketHandler * packet_handler_; + dynamixel::PortHandler * port_handler_ = nullptr; + dynamixel::PacketHandler * packet_handler_ = nullptr; // dxl info variable from dxl_model file DynamixelInfo dxl_info_; @@ -126,24 +126,35 @@ class Dynamixel std::vector read_data_list_; // sync read - dynamixel::GroupFastSyncRead * group_sync_read_; + dynamixel::GroupSyncRead * group_sync_read_ = nullptr; + // bulk read + dynamixel::GroupBulkRead * group_bulk_read_ = nullptr; + // fast sync read + dynamixel::GroupFastSyncRead * group_fast_sync_read_ = nullptr; + // fast bulk read + dynamixel::GroupFastBulkRead * group_fast_bulk_read_ = nullptr; + + // fast read protocol state (applies to both sync and bulk) + bool use_fast_read_protocol_ = true; + bool fast_read_permanent_ = false; + int fast_read_fail_count_ = 0; + // indirect inform for sync read std::map indirect_info_read_; - // bulk read - dynamixel::GroupFastBulkRead * group_bulk_read_; - // write item (sync or bulk) variable bool write_type_; std::vector write_data_list_; // sync write - dynamixel::GroupSyncWrite * group_sync_write_; + dynamixel::GroupSyncWrite * group_sync_write_ = nullptr; // indirect inform for sync write std::map indirect_info_write_; // bulk write - dynamixel::GroupBulkWrite * group_bulk_write_; + dynamixel::GroupBulkWrite * group_bulk_write_ = nullptr; + // direct inform for bulk write + std::map direct_info_write_; public: explicit Dynamixel(const char * path); @@ -202,11 +213,19 @@ class Dynamixel DxlError SetSyncReadItemAndHandler(); DxlError SetSyncReadHandler(std::vector id_arr); DxlError GetDxlValueFromSyncRead(double period_ms); + DxlError SetFastSyncReadHandler(std::vector id_arr); // BulkRead DxlError SetBulkReadItemAndHandler(); DxlError SetBulkReadHandler(std::vector id_arr); DxlError GetDxlValueFromBulkRead(double period_ms); + DxlError SetFastBulkReadHandler(std::vector id_arr); + + // DirectRead for BulkRead + DxlError AddDirectRead(uint8_t id, std::string item_name, uint16_t item_addr, uint8_t item_size); + + // Check Indirect Read + DxlError CheckIndirectReadAvailable(uint8_t id); // Read - Indirect Address void ResetIndirectRead(std::vector id_arr); @@ -225,13 +244,19 @@ class Dynamixel const std::vector> & data_ptrs, std::function get_data_func); + DxlError ProcessDirectReadData( + uint8_t id, + const std::vector & item_addrs, + 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); + bool is_sync, + bool is_fast); // SyncWrite DxlError SetSyncWriteItemAndHandler(); @@ -243,8 +268,12 @@ class Dynamixel DxlError SetBulkWriteHandler(std::vector id_arr); DxlError SetDxlValueToBulkWrite(); + // Check Indirect Write + DxlError CheckIndirectWriteAvailable(uint8_t id); + // Write - Indirect Address void ResetIndirectWrite(std::vector id_arr); + void ResetDirectWrite(std::vector id_arr); DxlError AddIndirectWrite( uint8_t id, std::string item_name, diff --git a/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp b/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp index 106304d..d0abe52 100644 --- a/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp +++ b/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// Authors: Hye-Jong KIM, Sungho Woo +// Authors: Hye-Jong KIM, Sungho Woo, Woojin Wie #ifndef DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_INFO_HPP_ #define DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_INFO_HPP_ diff --git a/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp b/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp index 365b437..44c5ca8 100644 --- a/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp +++ b/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// Authors: Hye-Jong KIM, Sungho Woo +// Authors: Hye-Jong KIM, Sungho Woo, Woojin Wie #ifndef DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL_HARDWARE_INTERFACE_HPP_ #define DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL_HARDWARE_INTERFACE_HPP_ @@ -21,6 +21,8 @@ #include #include #include +#include +#include #include "rclcpp/rclcpp.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" @@ -29,6 +31,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "dynamixel_hardware_interface/visibility_control.h" #include "dynamixel_hardware_interface/dynamixel/dynamixel.hpp" @@ -43,13 +46,6 @@ #include "std_srvs/srv/set_bool.hpp" -#define PRESENT_POSITION_INDEX 0 -#define PRESENT_VELOCITY_INDEX 1 -#define PRESENT_EFFORT_INDEX 2 - -#define GOAL_POSITION_INDEX 0 -// #define GOAL_VELOCITY_INDEX 1 // TODO: to be implemented -#define GOAL_CURRENT_INDEX 1 namespace dynamixel_hardware_interface { @@ -185,14 +181,13 @@ class DynamixelHardware : public std::map dxl_hw_err_; DxlTorqueStatus dxl_torque_status_; std::map dxl_torque_state_; + std::map dxl_torque_enable_; double err_timeout_ms_; rclcpp::Duration read_error_duration_{0, 0}; rclcpp::Duration write_error_duration_{0, 0}; 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_{""}; @@ -229,7 +224,6 @@ class DynamixelHardware : public bool CommReset(); ///// dxl variable - std::shared_ptr dxl_comm_; std::string port_name_; std::string baud_rate_; std::vector dxl_id_; @@ -250,6 +244,9 @@ class DynamixelHardware : public std::vector hdl_gpio_sensor_states_; std::vector hdl_sensor_states_; + ///// handler controller variable + std::vector hdl_gpio_controller_commands_; + bool is_set_hdl_{false}; // joint <-> transmission matrix @@ -360,6 +357,41 @@ class DynamixelHardware : public double revoluteToPrismatic(double revolute_value); double prismaticToRevolute(double prismatic_value); + + void MapInterfaces( + size_t outer_size, + size_t inner_size, + std::vector & outer_handlers, + const std::vector & inner_handlers, + double ** matrix, + const std::unordered_map> & iface_map, + const std::string & conversion_iface = "", + const std::string & conversion_name = "", + std::function conversion = nullptr); + + // Move dxl_comm_ to the end for safe destruction order + std::shared_ptr dxl_comm_; +}; + +// Conversion maps between ROS2 and Dynamixel interface names +inline const std::unordered_map> ros2_to_dxl_cmd_map = { + {hardware_interface::HW_IF_POSITION, {"Goal Position"}}, + {hardware_interface::HW_IF_VELOCITY, {"Goal Velocity"}}, + {hardware_interface::HW_IF_EFFORT, {"Goal Current"}} +}; + +// Mapping for Dynamixel command interface names to ROS2 state interface names +inline const std::unordered_map> dxl_to_ros2_cmd_map = { + {"Goal Position", {hardware_interface::HW_IF_POSITION}}, + {"Goal Velocity", {hardware_interface::HW_IF_VELOCITY}}, + {"Goal Current", {hardware_interface::HW_IF_EFFORT}} +}; + +// Mapping for ROS2 state interface names to Dynamixel state interface names +inline const std::unordered_map> ros2_to_dxl_state_map = { + {hardware_interface::HW_IF_POSITION, {"Present Position"}}, + {hardware_interface::HW_IF_VELOCITY, {"Present Velocity"}}, + {hardware_interface::HW_IF_EFFORT, {"Present Current", "Present Load"}} }; } // namespace dynamixel_hardware_interface diff --git a/package.xml b/package.xml index 08d8def..a4ff2a1 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ dynamixel_hardware_interface - 1.4.3 + 1.4.4 ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework. diff --git a/param/dxl_model/dynamixel.model b/param/dxl_model/dynamixel.model index 725444d..2d9354a 100644 --- a/param/dxl_model/dynamixel.model +++ b/param/dxl_model/dynamixel.model @@ -46,3 +46,5 @@ Number Name 35074 rh_p12_rn.model 220 omy_hat.model 230 omy_end.model +536 sensorxel_joy.model +600 sensorxel_joy.model diff --git a/param/dxl_model/ph42_020_s300.model b/param/dxl_model/ph42_020_s300.model index 4a05469..e9dd55c 100644 --- a/param/dxl_model/ph42_020_s300.model +++ b/param/dxl_model/ph42_020_s300.model @@ -62,7 +62,9 @@ Address Size Data Name 592 2 Present Input Voltage 594 1 Present Temperature 878 1 Backup Ready +168 2 Indirect Address 1 +634 1 Indirect Data 1 168 2 Indirect Address Write 634 1 Indirect Data Write -350 2 Indirect Address Read +296 2 Indirect Address Read 698 1 Indirect Data Read diff --git a/param/dxl_model/sensorxel_joy.model b/param/dxl_model/sensorxel_joy.model new file mode 100644 index 0000000..9bd9f54 --- /dev/null +++ b/param/dxl_model/sensorxel_joy.model @@ -0,0 +1,21 @@ +[control table] +Address Size Data Name +0 2 Model Number +2 1 ROBOT_Generation +3 1 Board_Number +5 1 Firmware_Version_Major +6 1 Firmware_Version_Minor +7 1 ID +8 1 Baud Rate +10 1 Boot_Version_Major +11 1 Boot_Version_MInor +12 4 Error_Code +20 4 Micros +24 4 Millis +28 1 JOYSTICK TACT SWITCH +29 2 JOYSTICK X VALUE +31 2 JOYSTICK Y VALUE +40 2 IMU ACC X +42 2 IMU ACC Y +44 2 IMU ACC Z +46 2 IMU ACC SUM diff --git a/src/dynamixel/dynamixel.cpp b/src/dynamixel/dynamixel.cpp index f80fd69..17eb3c3 100644 --- a/src/dynamixel/dynamixel.cpp +++ b/src/dynamixel/dynamixel.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// Authors: Hye-Jong KIM, Sungho Woo +// Authors: Hye-Jong KIM, Sungho Woo, Woojin Wie #include "dynamixel_hardware_interface/dynamixel/dynamixel.hpp" @@ -26,6 +26,14 @@ namespace dynamixel_hardware_interface { Dynamixel::Dynamixel(const char * path) +: port_handler_(nullptr), + packet_handler_(nullptr), + group_sync_read_(nullptr), + group_bulk_read_(nullptr), + group_fast_sync_read_(nullptr), + group_fast_bulk_read_(nullptr), + group_sync_write_(nullptr), + group_bulk_write_(nullptr) { dxl_info_.SetDxlModelFolderPath(path); dxl_info_.InitDxlModelInfo(); @@ -36,8 +44,38 @@ Dynamixel::Dynamixel(const char * path) Dynamixel::~Dynamixel() { - port_handler_->closePort(); - fprintf(stderr, "closed port\n"); + fprintf(stderr, "Dynamixel destructor start\n"); + if (group_sync_read_) { + delete group_sync_read_; + group_sync_read_ = nullptr; + } + if (group_fast_sync_read_) { + delete group_fast_sync_read_; + group_fast_sync_read_ = nullptr; + } + if (group_bulk_read_) { + delete group_bulk_read_; + group_bulk_read_ = nullptr; + } + if (group_fast_bulk_read_) { + delete group_fast_bulk_read_; + group_fast_bulk_read_ = nullptr; + } + if (group_sync_write_) { + delete group_sync_write_; + group_sync_write_ = nullptr; + } + if (group_bulk_write_) { + delete group_bulk_write_; + group_bulk_write_ = nullptr; + } + if (port_handler_) { + port_handler_->closePort(); + delete port_handler_; + port_handler_ = nullptr; + } + packet_handler_ = nullptr; + fprintf(stderr, "Dynamixel destructor end\n"); } DxlError Dynamixel::InitDxlComm( @@ -82,15 +120,25 @@ DxlError Dynamixel::InitDxlComm( fprintf(stderr, " - Ping succeeded. Dynamixel model number : %d\n", dxl_model_number); } - dxl_info_.ReadDxlModelFile(it_id, dxl_model_number); + try { + dxl_info_.ReadDxlModelFile(it_id, dxl_model_number); + } catch (const std::exception & e) { + fprintf(stderr, "Error reading model file for ID %d: %s\n", it_id, e.what()); + return DxlError::CANNOT_FIND_CONTROL_ITEM; + } } read_data_list_.clear(); write_data_list_.clear(); for (auto it_id : id_arr) { - if (dxl_info_.CheckDxlControlItem(it_id, "Torque Enable")) { - torque_state_[it_id] = TORQUE_OFF; + try { + if (dxl_info_.CheckDxlControlItem(it_id, "Torque Enable")) { + torque_state_[it_id] = TORQUE_OFF; + } + } catch (const std::exception & e) { + fprintf(stderr, "Error checking control item for ID %d: %s\n", it_id, e.what()); + return DxlError::CANNOT_FIND_CONTROL_ITEM; } } @@ -259,9 +307,12 @@ DxlError Dynamixel::SetMultiDxlWrite() } fprintf(stderr, "\n"); fprintf(stderr, "Write items : "); - - for (auto it_name : write_data_list_.at(0).item_name) { - fprintf(stderr, "\t%s", it_name.c_str()); + if (!write_data_list_.empty()) { + for (auto it_name : write_data_list_.at(0).item_name) { + fprintf(stderr, "\t%s", it_name.c_str()); + } + } else { + fprintf(stderr, "(none)"); } fprintf(stderr, "\n"); } else { @@ -665,6 +716,10 @@ bool Dynamixel::checkReadType() uint16_t indirect_addr[2]; // [i-1], [i] uint8_t indirect_size[2]; // [i-1], [i] + if (CheckIndirectReadAvailable(read_data_list_.at(dxl_index - 1).id) != DxlError::OK) { + return BULK; + } + if (!dxl_info_.GetDxlControlItem( read_data_list_.at(dxl_index).id, "Indirect Data Read", indirect_addr[1], indirect_size[1]) || @@ -706,6 +761,11 @@ bool Dynamixel::checkWriteType() // Check if Indirect Data Write address and size are different uint16_t indirect_addr[2]; // [i-1], [i] uint8_t indirect_size[2]; // [i-1], [i] + + if (CheckIndirectWriteAvailable(write_data_list_.at(dxl_index - 1).id) != DxlError::OK) { + return BULK; + } + if (!dxl_info_.GetDxlControlItem( write_data_list_.at(dxl_index).id, "Indirect Data Write", indirect_addr[1], indirect_size[1]) || @@ -741,6 +801,19 @@ bool Dynamixel::checkWriteType() return SYNC; } +DxlError Dynamixel::CheckIndirectWriteAvailable(uint8_t id) +{ + uint16_t INDIRECT_ADDR; + uint8_t INDIRECT_SIZE; + if (dxl_info_.GetDxlControlItem( + id, "Indirect Address Write", + INDIRECT_ADDR, INDIRECT_SIZE) == false) + { + return DxlError::CANNOT_FIND_CONTROL_ITEM; + } + return DxlError::OK; +} + DxlError Dynamixel::SetSyncReadItemAndHandler() { std::vector id_arr; @@ -779,9 +852,58 @@ DxlError Dynamixel::SetSyncReadItemAndHandler() return DxlError::OK; } +DxlError Dynamixel::SetFastSyncReadHandler(std::vector id_arr) +{ + uint16_t IN_ADDR = 0; + uint8_t IN_SIZE = 0; + + for (auto it_id : id_arr) { + if (dxl_info_.GetDxlControlItem(it_id, "Indirect Data Read", IN_ADDR, IN_SIZE) == false) { + fprintf( + stderr, + "Fail to set indirect address fast sync read. " + "the dxl unincluding indirect address in control table are being used.\n"); + return DxlError::SET_SYNC_READ_FAIL; + } + indirect_info_read_[it_id].indirect_data_addr = IN_ADDR; + } + fprintf( + stderr, + "set fast sync read (indirect addr) : addr %d, size %d\n", + IN_ADDR, indirect_info_read_[id_arr.at(0)].size); + + if (group_fast_sync_read_) { + delete group_fast_sync_read_; + group_fast_sync_read_ = nullptr; + } + group_fast_sync_read_ = + new dynamixel::GroupFastSyncRead( + port_handler_, packet_handler_, + IN_ADDR, indirect_info_read_[id_arr.at(0)].size); + + for (auto it_id : id_arr) { + if (group_fast_sync_read_->addParam(it_id) != true) { + fprintf(stderr, "[ID:%03d] groupFastSyncRead addparam failed", it_id); + return DxlError::SET_SYNC_READ_FAIL; + } + } + return DxlError::OK; +} DxlError Dynamixel::SetSyncReadHandler(std::vector id_arr) { + // Try to set up fast sync read first + if (use_fast_read_protocol_) { + DxlError fast_result = SetFastSyncReadHandler(id_arr); + if (fast_result == DxlError::OK) { + fprintf(stderr, "FastSyncRead handler set up successfully.\n"); + return DxlError::OK; + } else { + fprintf(stderr, "FastSyncRead handler failed, falling back to normal SyncRead.\n"); + use_fast_read_protocol_ = false; + } + } + // Fallback to normal sync read uint16_t IN_ADDR = 0; uint8_t IN_SIZE = 0; @@ -802,8 +924,12 @@ DxlError Dynamixel::SetSyncReadHandler(std::vector id_arr) "set sync read (indirect addr) : addr %d, size %d\n", IN_ADDR, indirect_info_read_[id_arr.at(0)].size); + if (group_sync_read_) { + delete group_sync_read_; + group_sync_read_ = nullptr; + } group_sync_read_ = - new dynamixel::GroupFastSyncRead( + new dynamixel::GroupSyncRead( port_handler_, packet_handler_, IN_ADDR, indirect_info_read_[id_arr.at(0)].size); @@ -819,16 +945,63 @@ DxlError Dynamixel::SetSyncReadHandler(std::vector id_arr) DxlError Dynamixel::GetDxlValueFromSyncRead(double period_ms) { - DxlError comm_result = ProcessReadCommunication( - group_sync_read_, group_bulk_read_, port_handler_, period_ms, true); + // Try fast sync read for the first 10 attempts after startup/handler setup. + // If any of the first 10 attempts succeeds, use fast sync read permanently. + // If all 10 attempts fail, permanently fallback to normal sync read. + if (use_fast_read_protocol_ && group_fast_sync_read_ && + (fast_read_permanent_ || fast_read_fail_count_ < 10)) + { + DxlError comm_result = ProcessReadCommunication(port_handler_, period_ms, true, true); + if (comm_result == DxlError::OK) { + // Success, process data, and use fast sync read permanently + for (auto it_read_data : read_data_list_) { + 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_fast_sync_read_->getData(id, addr, size); + }); + } + // Mark as permanently using fast sync read after first success + fast_read_permanent_ = true; + return DxlError::OK; + } else if (!fast_read_permanent_) { + // Only increment fail count and fallback if not yet permanent + ++fast_read_fail_count_; + fprintf(stderr, "FastSyncRead TxRx failed (attempt %d/10)\n", fast_read_fail_count_); + if (fast_read_fail_count_ >= 10) { + // Permanently switch to normal sync read + fprintf( + stderr, "FastSyncRead failed 10 times, switching to normal SyncRead " + "permanently.\n"); + use_fast_read_protocol_ = false; + // Set up normal sync read handler + std::vector id_arr; + for (auto it_read_data : read_data_list_) { + id_arr.push_back(it_read_data.id); + } + SetSyncReadHandler(id_arr); + } + // Return error for this attempt + return DxlError::SYNC_READ_FAIL; + } else { + // If permanent, ignore failures and keep using fast sync read + return comm_result; + } + } + // Use normal sync read + DxlError comm_result = ProcessReadCommunication(port_handler_, period_ms, true, 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 indirect_addr = indirect_info_read_[id].indirect_data_addr; - ProcessReadData( id, indirect_addr, @@ -844,15 +1017,74 @@ DxlError Dynamixel::GetDxlValueFromSyncRead(double period_ms) DxlError Dynamixel::SetBulkReadItemAndHandler() { - std::vector id_arr; - for (auto it_read_data : read_data_list_) { - id_arr.push_back(it_read_data.id); + if (group_fast_bulk_read_) { + delete group_fast_bulk_read_; + group_fast_bulk_read_ = nullptr; } + group_fast_bulk_read_ = new dynamixel::GroupFastBulkRead(port_handler_, packet_handler_); - DynamixelDisable(id_arr); - ResetIndirectRead(id_arr); + if (group_bulk_read_) { + delete group_bulk_read_; + group_bulk_read_ = nullptr; + } + group_bulk_read_ = new dynamixel::GroupBulkRead(port_handler_, packet_handler_); + + std::vector indirect_id_arr; + std::vector direct_id_arr; + + std::vector indirect_read_data_list; + std::vector direct_read_data_list; for (auto it_read_data : read_data_list_) { + if (CheckIndirectReadAvailable(it_read_data.id) == DxlError::OK) { + indirect_id_arr.push_back(it_read_data.id); + indirect_read_data_list.push_back(it_read_data); + } else { + direct_id_arr.push_back(it_read_data.id); + direct_read_data_list.push_back(it_read_data); + } + } + + for (auto it_read_data : direct_read_data_list) { + if (it_read_data.item_addr.empty() || it_read_data.item_size.empty()) { + continue; + } + // Calculate min address and max end address + uint16_t min_addr = it_read_data.item_addr[0]; + uint16_t max_end_addr = it_read_data.item_addr[0] + it_read_data.item_size[0]; + for (size_t item_index = 1; item_index < it_read_data.item_addr.size(); ++item_index) { + uint16_t addr = it_read_data.item_addr[item_index]; + uint16_t size = it_read_data.item_size[item_index]; + if (addr < min_addr) {min_addr = addr;} + if (addr + size > max_end_addr) {max_end_addr = addr + size;} + } + uint8_t total_size = max_end_addr - min_addr; + // Concatenate all item names with '+' + std::string group_item_names; + for (size_t i = 0; i < it_read_data.item_name.size(); ++i) { + group_item_names += it_read_data.item_name[i]; + if (i + 1 < it_read_data.item_name.size()) {group_item_names += " + ";} + } + // Call AddDirectRead once per id + if (AddDirectRead( + it_read_data.id, + group_item_names, + min_addr, + total_size) != DxlError::OK) + { + fprintf(stderr, "Cannot set the BulkRead handler.\n"); + return DxlError::BULK_READ_FAIL; + } + fprintf( + stderr, + "set bulk read (direct addr) : addr %d, size %d\n", + min_addr, total_size); + } + + DynamixelDisable(indirect_id_arr); + ResetIndirectRead(indirect_id_arr); + + for (auto it_read_data : indirect_read_data_list) { for (size_t item_index = 0; item_index < it_read_data.item_name.size(); item_index++) { @@ -867,17 +1099,23 @@ DxlError Dynamixel::SetBulkReadItemAndHandler() stderr, "[ID:%03d] Add Indirect Address Read Item : [%s]\n", it_read_data.id, it_read_data.item_name.at(item_index).c_str()); - } else { + } else if (result == DxlError::SET_BULK_READ_FAIL) { fprintf( stderr, "[ID:%03d] Failed to Indirect Address Read Item : [%s], %d\n", it_read_data.id, it_read_data.item_name.at(item_index).c_str(), result); + } else if (result == DxlError::CANNOT_FIND_CONTROL_ITEM) { + fprintf( + stderr, "[ID:%03d] 'Indirect Address Read' is not defined in control table, " + "Cannot set Indirect Address Read for : [%s]\n", + it_read_data.id, + it_read_data.item_name.at(item_index).c_str()); } } } - if (SetBulkReadHandler(id_arr) != DxlError::OK) { + if (SetBulkReadHandler(indirect_id_arr) != DxlError::OK) { fprintf(stderr, "Cannot set the BulkRead handler.\n"); return DxlError::BULK_READ_FAIL; } @@ -886,8 +1124,60 @@ DxlError Dynamixel::SetBulkReadItemAndHandler() return DxlError::OK; } +DxlError Dynamixel::SetFastBulkReadHandler(std::vector id_arr) +{ + uint16_t IN_ADDR = 0; + uint8_t IN_SIZE = 0; + + for (auto it_id : id_arr) { + // Get the indirect addr. + if (dxl_info_.GetDxlControlItem(it_id, "Indirect Data Read", IN_ADDR, IN_SIZE) == false) { + fprintf( + stderr, + "Fail to set indirect address fast bulk read. " + "the dxl unincluding indirect address in control table are being used.\n"); + return DxlError::SET_BULK_READ_FAIL; + } + // Set indirect addr. + indirect_info_read_[it_id].indirect_data_addr = IN_ADDR; + + fprintf( + stderr, + "set fast bulk read (indirect addr) : addr %d, size %d\n", + IN_ADDR, indirect_info_read_[it_id].size); + } + + for (auto it_id : id_arr) { + uint8_t ID = it_id; + uint16_t ADDR = indirect_info_read_[ID].indirect_data_addr; + uint8_t SIZE = indirect_info_read_[ID].size; + auto addParamResult = group_fast_bulk_read_->addParam(ID, ADDR, SIZE); + if (addParamResult) { // success + fprintf( + stderr, "[ID:%03d] Add BulkRead item : [Indirect Item Data][%d][%d]\n", ID, ADDR, SIZE); + } else { + fprintf( + stderr, "[ID:%03d] Failed to BulkRead item : [Indirect Item Data]]\n", ID); + return DxlError::SET_BULK_READ_FAIL; + } + } + return DxlError::OK; +} + DxlError Dynamixel::SetBulkReadHandler(std::vector id_arr) { + // Try to set up fast bulk read first + if (use_fast_read_protocol_) { + DxlError fast_result = SetFastBulkReadHandler(id_arr); + if (fast_result == DxlError::OK) { + fprintf(stderr, "FastBulkRead handler set up successfully.\n"); + return DxlError::OK; + } else { + fprintf(stderr, "FastBulkRead handler failed, falling back to normal BulkRead.\n"); + use_fast_read_protocol_ = false; + } + } + uint16_t IN_ADDR = 0; uint8_t IN_SIZE = 0; @@ -906,11 +1196,9 @@ DxlError Dynamixel::SetBulkReadHandler(std::vector id_arr) fprintf( stderr, "set bulk read (indirect addr) : addr %d, size %d\n", - IN_ADDR, indirect_info_read_[id_arr.at(0)].size); + IN_ADDR, indirect_info_read_[it_id].size); } - group_bulk_read_ = new dynamixel::GroupFastBulkRead(port_handler_, packet_handler_); - for (auto it_id : id_arr) { uint8_t ID = it_id; uint16_t ADDR = indirect_info_read_[ID].indirect_data_addr; @@ -928,10 +1216,109 @@ DxlError Dynamixel::SetBulkReadHandler(std::vector id_arr) return DxlError::OK; } +DxlError Dynamixel::AddDirectRead( + uint8_t id, std::string item_name, uint16_t item_addr, uint8_t item_size) +{ + if (group_bulk_read_->addParam(id, item_addr, item_size) == true) { + fprintf( + stderr, "[ID:%03d] Add BulkRead item : [%s][%d][%d]\n", + id, item_name.c_str(), item_addr, item_size); + } else { + fprintf( + stderr, "[ID:%03d] Failed to BulkRead item : [%s][%d][%d]\n", + id, item_name.c_str(), item_addr, item_size); + return DxlError::SET_BULK_READ_FAIL; + } + + if (group_fast_bulk_read_->addParam(id, item_addr, item_size) == true) { + fprintf( + stderr, "[ID:%03d] Add FastBulkRead item : [%s][%d][%d]\n", + id, item_name.c_str(), item_addr, item_size); + } else { + fprintf( + stderr, "[ID:%03d] Failed to FastBulkRead item : [%s][%d][%d]\n", + id, item_name.c_str(), item_addr, item_size); + return DxlError::SET_BULK_READ_FAIL; + } + return DxlError::OK; +} + DxlError Dynamixel::GetDxlValueFromBulkRead(double period_ms) { - DxlError comm_result = ProcessReadCommunication( - group_sync_read_, group_bulk_read_, port_handler_, period_ms, false); + // Try fast bulk read for the first 10 attempts after startup/handler setup. + // If any of the first 10 attempts succeeds, use fast bulk read permanently. + // If all 10 attempts fail, permanently fallback to normal bulk read. + if (use_fast_read_protocol_ && group_fast_bulk_read_ && + (fast_read_permanent_ || fast_read_fail_count_ < 10)) + { + DxlError comm_result = ProcessReadCommunication(port_handler_, period_ms, false, true); + if (comm_result == DxlError::OK) { + // Success, process data, and use fast bulk read permanently + if (group_bulk_read_) { + delete group_bulk_read_; + group_bulk_read_ = nullptr; + } + + for (auto it_read_data : read_data_list_) { + uint8_t id = it_read_data.id; + uint16_t indirect_addr = indirect_info_read_[id].indirect_data_addr; + if (CheckIndirectReadAvailable(id) != DxlError::OK) { + ProcessDirectReadData( + id, + it_read_data.item_addr, + it_read_data.item_size, + it_read_data.item_data_ptr_vec, + [this](uint8_t id, uint16_t addr, uint8_t size) { + return group_fast_bulk_read_->getData(id, addr, size); + }); + } else { + 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_fast_bulk_read_->getData(id, addr, size); + }); + } + } + // Mark as permanently using fast bulk read after first success + fast_read_permanent_ = true; + return DxlError::OK; + } else if (!fast_read_permanent_) { + // Only increment fail count and fallback if not yet permanent + ++fast_read_fail_count_; + fprintf(stderr, "FastBulkRead TxRx failed (attempt %d/10)\n", fast_read_fail_count_); + if (fast_read_fail_count_ >= 10) { + if (group_fast_bulk_read_) { + delete group_fast_bulk_read_; + group_fast_bulk_read_ = nullptr; + } + // Permanently switch to normal bulk read + fprintf( + stderr, + "FastBulkRead failed 10 times, switching to normal BulkRead permanently.\n"); + use_fast_read_protocol_ = false; + // Set up normal bulk read handler + std::vector indirect_id_arr; + + for (auto it_read_data : read_data_list_) { + if (CheckIndirectReadAvailable(it_read_data.id) == DxlError::OK) { + indirect_id_arr.push_back(it_read_data.id); + } + } + + SetBulkReadHandler(indirect_id_arr); + } + // Return error for this attempt + return DxlError::BULK_READ_FAIL; + } else { + // If permanent, ignore failures and keep using fast bulk read + return comm_result; + } + } + DxlError comm_result = ProcessReadCommunication(port_handler_, period_ms, false, false); if (comm_result != DxlError::OK) { return comm_result; } @@ -939,43 +1326,67 @@ DxlError Dynamixel::GetDxlValueFromBulkRead(double period_ms) for (auto it_read_data : read_data_list_) { 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); - }); + if (CheckIndirectReadAvailable(id) != DxlError::OK) { + ProcessDirectReadData( + id, + it_read_data.item_addr, + it_read_data.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); + }); + } else { + 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) + bool is_sync, + bool is_fast) { int dxl_comm_result; // Send packet if (is_sync) { - dxl_comm_result = group_sync_read->txPacket(); + if (is_fast && group_fast_sync_read_) { + dxl_comm_result = group_fast_sync_read_->txPacket(); + } else if (group_sync_read_) { + dxl_comm_result = group_sync_read_->txPacket(); + } else { + return DxlError::SYNC_READ_FAIL; + } if (dxl_comm_result != COMM_SUCCESS) { fprintf( - stderr, "SyncRead Tx Fail [Dxl Size : %ld] [Error code : %d]\n", + stderr, "%s Tx Fail [Dxl Size : %ld] [Error code : %d]\n", + is_fast ? "FastSyncRead" : "SyncRead", read_data_list_.size(), dxl_comm_result); return DxlError::SYNC_READ_FAIL; } } else { - dxl_comm_result = group_bulk_read->txPacket(); + // Bulk read + if (is_fast && group_fast_bulk_read_) { + dxl_comm_result = group_fast_bulk_read_->txPacket(); + } else if (group_bulk_read_) { + dxl_comm_result = group_bulk_read_->txPacket(); + } else { + return DxlError::BULK_READ_FAIL; + } if (dxl_comm_result != COMM_SUCCESS) { fprintf( - stderr, "BulkRead Tx Fail [Dxl Size : %ld] [Error code : %d]\n", + stderr, "%s Tx Fail [Dxl Size : %ld] [Error code : %d]\n", + is_fast ? "FastBulkRead" : "BulkRead", read_data_list_.size(), dxl_comm_result); return DxlError::BULK_READ_FAIL; } @@ -988,18 +1399,33 @@ DxlError Dynamixel::ProcessReadCommunication( // Receive packet if (is_sync) { - dxl_comm_result = group_sync_read->rxPacket(); + if (is_fast && group_fast_sync_read_) { + dxl_comm_result = group_fast_sync_read_->rxPacket(); + } else if (group_sync_read_) { + dxl_comm_result = group_sync_read_->rxPacket(); + } else { + return DxlError::SYNC_READ_FAIL; + } if (dxl_comm_result != COMM_SUCCESS) { fprintf( - stderr, "SyncRead Rx Fail [Dxl Size : %ld] [Error code : %d]\n", + stderr, "%s Rx Fail [Dxl Size : %ld] [Error code : %d]\n", + is_fast ? "FastSyncRead" : "SyncRead", read_data_list_.size(), dxl_comm_result); return DxlError::SYNC_READ_FAIL; } } else { - dxl_comm_result = group_bulk_read->rxPacket(); + // Bulk read + if (is_fast && group_fast_bulk_read_) { + dxl_comm_result = group_fast_bulk_read_->rxPacket(); + } else if (group_bulk_read_) { + dxl_comm_result = group_bulk_read_->rxPacket(); + } else { + return DxlError::BULK_READ_FAIL; + } if (dxl_comm_result != COMM_SUCCESS) { fprintf( - stderr, "BulkRead Rx Fail [Dxl Size : %ld] [Error code : %d]\n", + stderr, "%s Rx Fail [Dxl Size : %ld] [Error code : %d]\n", + is_fast ? "FastBulkRead" : "BulkRead", read_data_list_.size(), dxl_comm_result); return DxlError::BULK_READ_FAIL; } @@ -1042,11 +1468,30 @@ DxlError Dynamixel::ProcessReadData( return DxlError::OK; } +DxlError Dynamixel::ProcessDirectReadData( + uint8_t id, + const std::vector & item_addrs, + const std::vector & item_sizes, + const std::vector> & data_ptrs, + std::function get_data_func) +{ + for (size_t item_index = 0; item_index < item_addrs.size(); item_index++) { + uint16_t current_addr = item_addrs[item_index]; + uint8_t size = item_sizes[item_index]; + + uint32_t dxl_getdata = get_data_func(id, current_addr, size); + + *data_ptrs[item_index] = static_cast(dxl_getdata); + } + return DxlError::OK; +} void Dynamixel::ResetIndirectRead(std::vector id_arr) { IndirectInfo temp; - temp.cnt = temp.size = 0; + temp.indirect_data_addr = 0; + temp.size = 0; + temp.cnt = 0; temp.item_name.clear(); temp.item_size.clear(); for (auto it_id : id_arr) { @@ -1054,6 +1499,19 @@ void Dynamixel::ResetIndirectRead(std::vector id_arr) } } +DxlError Dynamixel::CheckIndirectReadAvailable(uint8_t id) +{ + uint16_t INDIRECT_ADDR; + uint8_t INDIRECT_SIZE; + if (dxl_info_.GetDxlControlItem( + id, "Indirect Address Read", + INDIRECT_ADDR, INDIRECT_SIZE) == false) + { + return DxlError::CANNOT_FIND_CONTROL_ITEM; + } + return DxlError::OK; +} + DxlError Dynamixel::AddIndirectRead( uint8_t id, std::string item_name, @@ -1169,22 +1627,22 @@ DxlError Dynamixel::SetDxlValueToSyncWrite() param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(goal_position)); param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_position)); param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(goal_position)); - } else if (indirect_info_write_[ID].item_name.at(item_index) == "Goal Current") { - int16_t goal_current = dxl_info_.ConvertEffortToCurrent(ID, data); - 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(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)); param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(goal_velocity)); + } else if (indirect_info_write_[ID].item_name.at(item_index) == "Goal Current") { + int16_t goal_current = dxl_info_.ConvertEffortToCurrent(ID, data); + param_write_value[added_byte + 0] = DXL_LOBYTE(goal_current); + param_write_value[added_byte + 1] = DXL_HIBYTE(goal_current); } added_byte += indirect_info_write_[ID].item_size.at(item_index); } if (group_sync_write_->addParam(ID, param_write_value) != true) { - printf("[ID:%03d] groupSyncWrite addparam failed\n", ID); + fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed\n", ID); return DxlError::SYNC_WRITE_FAIL; } } @@ -1193,7 +1651,7 @@ DxlError Dynamixel::SetDxlValueToSyncWrite() group_sync_write_->clearParam(); if (dxl_comm_result != COMM_SUCCESS) { - printf("%s\n", packet_handler_->getTxRxResult(dxl_comm_result)); + fprintf(stderr, "%s\n", packet_handler_->getTxRxResult(dxl_comm_result)); return DxlError::SYNC_WRITE_FAIL; } else { return DxlError::OK; @@ -1207,10 +1665,80 @@ DxlError Dynamixel::SetBulkWriteItemAndHandler() id_arr.push_back(it_write_data.id); } - DynamixelDisable(id_arr); - ResetIndirectWrite(id_arr); + group_bulk_write_ = new dynamixel::GroupBulkWrite(port_handler_, packet_handler_); + + std::vector indirect_id_arr; + std::vector direct_id_arr; + + std::vector indirect_write_data_list; + std::vector direct_write_data_list; for (auto it_write_data : write_data_list_) { + if (CheckIndirectWriteAvailable(it_write_data.id) == DxlError::OK) { + indirect_id_arr.push_back(it_write_data.id); + indirect_write_data_list.push_back(it_write_data); + } else { + direct_id_arr.push_back(it_write_data.id); + direct_write_data_list.push_back(it_write_data); + } + } + + ResetDirectWrite(direct_id_arr); + + // Handle direct writes + for (auto it_write_data : direct_write_data_list) { + if (it_write_data.item_addr.empty() || it_write_data.item_size.empty()) { + continue; + } + // Calculate min address and max end address + uint16_t min_addr = it_write_data.item_addr[0]; + uint16_t max_end_addr = it_write_data.item_addr[0] + it_write_data.item_size[0]; + for (size_t item_index = 1; item_index < it_write_data.item_addr.size(); ++item_index) { + uint16_t addr = it_write_data.item_addr[item_index]; + uint16_t size = it_write_data.item_size[item_index]; + if (addr < min_addr) {min_addr = addr;} + if (addr + size > max_end_addr) {max_end_addr = addr + size;} + } + uint8_t total_size = max_end_addr - min_addr; + + // Check for gaps between items + std::vector> addr_ranges; + for (size_t item_index = 0; item_index < it_write_data.item_addr.size(); ++item_index) { + addr_ranges.push_back( + { + it_write_data.item_addr[item_index], + it_write_data.item_addr[item_index] + it_write_data.item_size[item_index] + }); + } + std::sort(addr_ranges.begin(), addr_ranges.end()); + + for (size_t i = 0; i < addr_ranges.size() - 1; ++i) { + if (addr_ranges[i].second != addr_ranges[i + 1].first) { + fprintf( + stderr, "[ID:%03d] Error: Gap detected between items at addresses %d and %d\n", + it_write_data.id, addr_ranges[i].second, addr_ranges[i + 1].first); + return DxlError::BULK_WRITE_FAIL; + } + } + + // Store direct write info + direct_info_write_[it_write_data.id].indirect_data_addr = min_addr; + direct_info_write_[it_write_data.id].size = total_size; + direct_info_write_[it_write_data.id].cnt = it_write_data.item_name.size(); + direct_info_write_[it_write_data.id].item_name = it_write_data.item_name; + direct_info_write_[it_write_data.id].item_size = it_write_data.item_size; + + fprintf( + stderr, + "set bulk write (direct addr) : addr %d, size %d\n", + min_addr, total_size); + } + + // Handle indirect writes + DynamixelDisable(indirect_id_arr); + ResetIndirectWrite(indirect_id_arr); + + for (auto it_write_data : indirect_write_data_list) { for (size_t item_index = 0; item_index < it_write_data.item_name.size(); item_index++) { @@ -1231,7 +1759,7 @@ DxlError Dynamixel::SetBulkWriteItemAndHandler() } } - if (SetBulkWriteHandler(id_arr) < 0) { + if (SetBulkWriteHandler(indirect_id_arr) < 0) { fprintf(stderr, "Cannot set the BulkWrite handler.\n"); return DxlError::BULK_WRITE_FAIL; } @@ -1260,11 +1788,9 @@ DxlError Dynamixel::SetBulkWriteHandler(std::vector id_arr) fprintf( stderr, "set bulk write (indirect addr) : addr %d, size %d\n", - IN_ADDR, indirect_info_write_[id_arr.at(0)].size); + IN_ADDR, indirect_info_write_[it_id].size); } - group_bulk_write_ = new dynamixel::GroupBulkWrite(port_handler_, packet_handler_); - return DxlError::OK; } @@ -1272,40 +1798,77 @@ DxlError Dynamixel::SetDxlValueToBulkWrite() { for (auto it_write_data : write_data_list_) { uint8_t ID = it_write_data.id; - uint8_t * param_write_value = new uint8_t[indirect_info_write_[ID].size]; + uint8_t * param_write_value; uint8_t added_byte = 0; + // Check if this is a direct write + if (direct_info_write_.find(ID) != direct_info_write_.end()) { + param_write_value = new uint8_t[direct_info_write_[ID].size]; + + for (uint16_t item_index = 0; item_index < direct_info_write_[ID].cnt; item_index++) { + double data = *it_write_data.item_data_ptr_vec.at(item_index); + uint8_t size = direct_info_write_[ID].item_size.at(item_index); + + if (size == 4) { + int32_t value = static_cast(data); + param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(value)); + param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(value)); + param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(value)); + param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(value)); + } else if (size == 2) { + int16_t value = static_cast(data); + param_write_value[added_byte + 0] = DXL_LOBYTE(value); + param_write_value[added_byte + 1] = DXL_HIBYTE(value); + } else if (size == 1) { + param_write_value[added_byte] = static_cast(data); + } + added_byte += size; + } - for (uint16_t item_index = 0; item_index < indirect_info_write_[ID].cnt; item_index++) { - double data = *it_write_data.item_data_ptr_vec.at(item_index); - if (indirect_info_write_[ID].item_name.at(item_index) == "Goal Position") { - int32_t goal_position = dxl_info_.ConvertRadianToValue(ID, data); - param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(goal_position)); - param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(goal_position)); - param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_position)); - param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(goal_position)); - } else if (indirect_info_write_[ID].item_name.at(item_index) == "Goal Current") { - int16_t goal_current = dxl_info_.ConvertEffortToCurrent(ID, data); - 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(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)); - param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(goal_velocity)); + if (group_bulk_write_->addParam( + ID, + direct_info_write_[ID].indirect_data_addr, + direct_info_write_[ID].size, + param_write_value) != true) + { + fprintf(stderr, "[ID:%03d] groupBulkWrite addparam failed\n", ID); + return DxlError::BULK_WRITE_FAIL; + } + } else { + // Handle indirect write + param_write_value = new uint8_t[indirect_info_write_[ID].size]; + + for (uint16_t item_index = 0; item_index < indirect_info_write_[ID].cnt; item_index++) { + double data = *it_write_data.item_data_ptr_vec.at(item_index); + if (indirect_info_write_[ID].item_name.at(item_index) == "Goal Position") { + int32_t goal_position = dxl_info_.ConvertRadianToValue(ID, data); + param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(goal_position)); + param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(goal_position)); + param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_position)); + param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(goal_position)); + } else if (indirect_info_write_[ID].item_name.at(item_index) == "Goal Velocity") { + 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)); + param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(goal_velocity)); + } else if (indirect_info_write_[ID].item_name.at(item_index) == "Goal Current") { + int16_t goal_current = dxl_info_.ConvertEffortToCurrent(ID, data); + param_write_value[added_byte + 0] = DXL_LOBYTE(goal_current); + param_write_value[added_byte + 1] = DXL_HIBYTE(goal_current); + } + added_byte += indirect_info_write_[ID].item_size.at(item_index); } - added_byte += indirect_info_write_[ID].item_size.at(item_index); - } - if (group_bulk_write_->addParam( - ID, - indirect_info_write_[ID].indirect_data_addr, - indirect_info_write_[ID].size, - param_write_value) != true) - { - printf("[ID:%03d] groupBulkWrite addparam failed\n", ID); - return DxlError::BULK_WRITE_FAIL; + if (group_bulk_write_->addParam( + ID, + indirect_info_write_[ID].indirect_data_addr, + indirect_info_write_[ID].size, + param_write_value) != true) + { + fprintf(stderr, "[ID:%03d] groupBulkWrite addparam failed\n", ID); + return DxlError::BULK_WRITE_FAIL; + } } } @@ -1313,7 +1876,7 @@ DxlError Dynamixel::SetDxlValueToBulkWrite() group_bulk_write_->clearParam(); if (dxl_comm_result != COMM_SUCCESS) { - printf("%s\n", packet_handler_->getTxRxResult(dxl_comm_result)); + fprintf(stderr, "%s\n", packet_handler_->getTxRxResult(dxl_comm_result)); return DxlError::BULK_WRITE_FAIL; } else { return DxlError::OK; @@ -1323,7 +1886,9 @@ DxlError Dynamixel::SetDxlValueToBulkWrite() void Dynamixel::ResetIndirectWrite(std::vector id_arr) { IndirectInfo temp; - temp.cnt = temp.size = 0; + temp.indirect_data_addr = 0; + temp.size = 0; + temp.cnt = 0; temp.item_name.clear(); temp.item_size.clear(); for (auto it_id : id_arr) { @@ -1357,4 +1922,17 @@ DxlError Dynamixel::AddIndirectWrite( return DxlError::OK; } + +void Dynamixel::ResetDirectWrite(std::vector id_arr) +{ + IndirectInfo temp; + temp.indirect_data_addr = 0; + temp.size = 0; + temp.cnt = 0; + temp.item_name.clear(); + temp.item_size.clear(); + for (auto it_id : id_arr) { + direct_info_write_[it_id] = temp; + } +} } // namespace dynamixel_hardware_interface diff --git a/src/dynamixel/dynamixel_info.cpp b/src/dynamixel/dynamixel_info.cpp index c252632..33543e0 100644 --- a/src/dynamixel/dynamixel_info.cpp +++ b/src/dynamixel/dynamixel_info.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// Authors: Hye-Jong KIM, Sungho Woo +// Authors: Hye-Jong KIM, Sungho Woo, Woojin Wie #include "dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp" #include @@ -60,13 +60,13 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num) path += it->second; } else { fprintf(stderr, "[ERROR] CANNOT FIND THE DXL MODEL FROM FILE LIST.\n"); - return; + throw std::runtime_error("Cannot find the DXL model from file list"); } std::ifstream open_file(path); if (open_file.is_open() != 1) { fprintf(stderr, "[ERROR] CANNOT FIND DXL [%s] MODEL FILE.\n", path.c_str()); - exit(-1); + throw std::runtime_error("Cannot find DXL model file"); } DxlInfo temp_dxl_info; @@ -83,18 +83,28 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num) std::vector strs; boost::split(strs, line, boost::is_any_of("\t")); - if (strs.at(0) == "value_of_zero_radian_position") { - temp_dxl_info.value_of_zero_radian_position = static_cast(stoi(strs.at(1))); - } else if (strs.at(0) == "value_of_max_radian_position") { - temp_dxl_info.value_of_max_radian_position = static_cast(stoi(strs.at(1))); - } else if (strs.at(0) == "value_of_min_radian_position") { - temp_dxl_info.value_of_min_radian_position = static_cast(stoi(strs.at(1))); - } else if (strs.at(0) == "min_radian") { - temp_dxl_info.min_radian = static_cast(stod(strs.at(1))); - } else if (strs.at(0) == "max_radian") { - temp_dxl_info.max_radian = static_cast(stod(strs.at(1))); - } else if (strs.at(0) == "torque_constant") { - temp_dxl_info.torque_constant = static_cast(stod(strs.at(1))); + if (strs.size() < 2) { + continue; + } + + try { + if (strs.at(0) == "value_of_zero_radian_position") { + temp_dxl_info.value_of_zero_radian_position = static_cast(stoi(strs.at(1))); + } else if (strs.at(0) == "value_of_max_radian_position") { + temp_dxl_info.value_of_max_radian_position = static_cast(stoi(strs.at(1))); + } else if (strs.at(0) == "value_of_min_radian_position") { + temp_dxl_info.value_of_min_radian_position = static_cast(stoi(strs.at(1))); + } else if (strs.at(0) == "min_radian") { + temp_dxl_info.min_radian = static_cast(stod(strs.at(1))); + } else if (strs.at(0) == "max_radian") { + temp_dxl_info.max_radian = static_cast(stod(strs.at(1))); + } else if (strs.at(0) == "torque_constant") { + temp_dxl_info.torque_constant = static_cast(stod(strs.at(1))); + } + } catch (const std::exception & e) { + std::string error_msg = "Error processing line in model file: " + line + + "\nError: " + e.what(); + throw std::runtime_error(error_msg); } } @@ -108,11 +118,28 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num) std::vector strs; boost::split(strs, line, boost::is_any_of("\t")); - ControlItem temp; - temp.address = static_cast(stoi(strs.at(0))); - temp.size = static_cast(stoi(strs.at(1))); - temp.item_name = strs.at(2); - temp_dxl_info.item.push_back(temp); + if (strs.size() < 3) { + std::string error_msg = "Malformed control table line: " + line; + throw std::runtime_error(error_msg); + } + + try { + ControlItem temp; + temp.address = static_cast(stoi(strs.at(0))); + temp.size = static_cast(stoi(strs.at(1))); + temp.item_name = strs.at(2); + temp_dxl_info.item.push_back(temp); + } catch (const std::exception & e) { + std::string error_msg = "Error processing control table line: " + line + + "\nError: " + e.what(); + throw std::runtime_error(error_msg); + } + } + + if (temp_dxl_info.item.empty()) { + std::string error_msg = "No control table items found in model file for ID " + + std::to_string(id); + throw std::runtime_error(error_msg); } dxl_info_[id] = temp_dxl_info; diff --git a/src/dynamixel_hardware_interface.cpp b/src/dynamixel_hardware_interface.cpp index 091e7fd..f81d4a9 100644 --- a/src/dynamixel_hardware_interface.cpp +++ b/src/dynamixel_hardware_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// Authors: Hye-Jong KIM, Sungho Woo +// Authors: Hye-Jong KIM, Sungho Woo, Woojin Wie #include "dynamixel_hardware_interface/dynamixel_hardware_interface.hpp" @@ -22,6 +22,8 @@ #include #include #include +#include +#include #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" @@ -76,17 +78,6 @@ 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()); @@ -209,15 +200,6 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( HandlerVarType temp_state; temp_state.name = joint.name; - temp_state.interface_name_vec.push_back(hardware_interface::HW_IF_POSITION); - temp_state.value_ptr_vec.push_back(std::make_shared(0.0)); - - temp_state.interface_name_vec.push_back(hardware_interface::HW_IF_VELOCITY); - temp_state.value_ptr_vec.push_back(std::make_shared(0.0)); - - temp_state.interface_name_vec.push_back(hardware_interface::HW_IF_EFFORT); - temp_state.value_ptr_vec.push_back(std::make_shared(0.0)); - for (auto it : joint.state_interfaces) { if (hardware_interface::HW_IF_POSITION != it.name && hardware_interface::HW_IF_VELOCITY != it.name && @@ -230,15 +212,8 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( logger_, "Error: invalid joint state interface " << it.name); return hardware_interface::CallbackReturn::ERROR; } - if (it.name != hardware_interface::HW_IF_POSITION && - it.name != hardware_interface::HW_IF_VELOCITY && - it.name != hardware_interface::HW_IF_EFFORT) - { - RCLCPP_ERROR_STREAM( - logger_, "Error: invalid joint command interface " << it.name); - temp_state.interface_name_vec.push_back(it.name); - temp_state.value_ptr_vec.push_back(std::make_shared(0.0)); - } + temp_state.interface_name_vec.push_back(it.name); + temp_state.value_ptr_vec.push_back(std::make_shared(0.0)); } hdl_joint_states_.push_back(temp_state); } @@ -254,6 +229,8 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( hardware_interface::HW_IF_ACCELERATION != it.name && hardware_interface::HW_IF_EFFORT != it.name) { + RCLCPP_ERROR_STREAM( + logger_, "Error: invalid joint command interface " << it.name); return hardware_interface::CallbackReturn::ERROR; } temp_cmd.interface_name_vec.push_back(it.name); @@ -329,27 +306,54 @@ DynamixelHardware::export_state_interfaces() { std::vector state_interfaces; - for (auto it : hdl_trans_states_) { - for (size_t i = 0; i < it.value_ptr_vec.size(); i++) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - it.name, it.interface_name_vec.at(i), it.value_ptr_vec.at(i).get())); + try { + for (auto it : hdl_trans_states_) { + for (size_t i = 0; i < it.value_ptr_vec.size(); i++) { + if (i >= it.interface_name_vec.size()) { + RCLCPP_ERROR_STREAM( + logger_, + "Interface name vector size mismatch for " << it.name << + ". Expected size: " << it.value_ptr_vec.size() << + ", Actual size: " << it.interface_name_vec.size()); + continue; + } + state_interfaces.emplace_back( + hardware_interface::StateInterface( + it.name, it.interface_name_vec.at(i), it.value_ptr_vec.at(i).get())); + } } - } - for (auto it : hdl_joint_states_) { - for (size_t i = 0; i < it.value_ptr_vec.size(); i++) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - it.name, it.interface_name_vec.at(i), it.value_ptr_vec.at(i).get())); + for (auto it : hdl_joint_states_) { + for (size_t i = 0; i < it.value_ptr_vec.size(); i++) { + if (i >= it.interface_name_vec.size()) { + RCLCPP_ERROR_STREAM( + logger_, "Interface name vector size mismatch for joint " << it.name << + ". Expected size: " << it.value_ptr_vec.size() << + ", Actual size: " << it.interface_name_vec.size()); + continue; + } + state_interfaces.emplace_back( + hardware_interface::StateInterface( + it.name, it.interface_name_vec.at(i), it.value_ptr_vec.at(i).get())); + } } - } - for (auto it : hdl_sensor_states_) { - for (size_t i = 0; i < it.value_ptr_vec.size(); i++) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - it.name, it.interface_name_vec.at(i), it.value_ptr_vec.at(i).get())); + for (auto it : hdl_sensor_states_) { + for (size_t i = 0; i < it.value_ptr_vec.size(); i++) { + if (i >= it.interface_name_vec.size()) { + RCLCPP_ERROR_STREAM( + logger_, "Interface name vector size mismatch for sensor " << it.name << + ". Expected size: " << it.value_ptr_vec.size() << + ", Actual size: " << it.interface_name_vec.size()); + continue; + } + state_interfaces.emplace_back( + hardware_interface::StateInterface( + it.name, it.interface_name_vec.at(i), it.value_ptr_vec.at(i).get())); + } } + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM(logger_, "Error in export_state_interfaces: " << e.what()); } + return state_interfaces; } @@ -358,20 +362,53 @@ DynamixelHardware::export_command_interfaces() { std::vector command_interfaces; - for (auto it : hdl_trans_commands_) { - for (size_t i = 0; i < it.value_ptr_vec.size(); i++) { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - it.name, it.interface_name_vec.at(i), it.value_ptr_vec.at(i).get())); + try { + for (auto it : hdl_trans_commands_) { + for (size_t i = 0; i < it.value_ptr_vec.size(); i++) { + if (i >= it.interface_name_vec.size()) { + RCLCPP_ERROR_STREAM( + logger_, "Interface name vector size mismatch for " << it.name << + ". Expected size: " << it.value_ptr_vec.size() << + ", Actual size: " << it.interface_name_vec.size()); + continue; + } + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + it.name, it.interface_name_vec.at(i), it.value_ptr_vec.at(i).get())); + } } - } - for (auto it : hdl_joint_commands_) { - for (size_t i = 0; i < it.value_ptr_vec.size(); i++) { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - it.name, it.interface_name_vec.at(i), it.value_ptr_vec.at(i).get())); + for (auto it : hdl_joint_commands_) { + for (size_t i = 0; i < it.value_ptr_vec.size(); i++) { + if (i >= it.interface_name_vec.size()) { + RCLCPP_ERROR_STREAM( + logger_, "Interface name vector size mismatch for joint " << it.name << + ". Expected size: " << it.value_ptr_vec.size() << + ", Actual size: " << it.interface_name_vec.size()); + continue; + } + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + it.name, it.interface_name_vec.at(i), it.value_ptr_vec.at(i).get())); + } + } + for (auto it : hdl_gpio_controller_commands_) { + for (size_t i = 0; i < it.value_ptr_vec.size(); i++) { + if (i >= it.interface_name_vec.size()) { + RCLCPP_ERROR_STREAM( + logger_, "Interface name vector size mismatch for gpio controller " << + it.name << ". Expected size: " << it.value_ptr_vec.size() << + ", Actual size: " << it.interface_name_vec.size()); + continue; + } + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + it.name, it.interface_name_vec.at(i), it.value_ptr_vec.at(i).get())); + } } + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM(logger_, "Error in export_command_interfaces: " << e.what()); } + return command_interfaces; } @@ -389,40 +426,46 @@ hardware_interface::CallbackReturn DynamixelHardware::on_deactivate( hardware_interface::CallbackReturn DynamixelHardware::start() { - dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData(0.0)); - if (dxl_comm_err_ != DxlError::OK) { - RCLCPP_ERROR_STREAM( - logger_, - "Dynamixel Start Fail :" << Dynamixel::DxlErrorToString(dxl_comm_err_)); - return hardware_interface::CallbackReturn::ERROR; + rclcpp::Time start_time = this->now(); + rclcpp::Duration error_duration(0, 0); + while (true) { + dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData(0.0)); + if (dxl_comm_err_ == DxlError::OK) { + break; + } + error_duration = this->now() - start_time; + if (error_duration.seconds() * 1000 >= err_timeout_ms_) { + RCLCPP_ERROR_STREAM( + logger_, + "Dynamixel Start Fail (Timeout: " << error_duration.seconds() * 1000 << "ms/" << + err_timeout_ms_ << "ms): " << Dynamixel::DxlErrorToString(dxl_comm_err_)); + return hardware_interface::CallbackReturn::ERROR; + } } CalcTransmissionToJoint(); // sync commands = states joint - for (auto it_states : hdl_joint_states_) { - for (auto it_commands : hdl_joint_commands_) { - if (it_states.name == it_commands.name) { - for (size_t i = 0; i < it_states.interface_name_vec.size(); i++) { - if (it_commands.interface_name_vec.at(0) == it_states.interface_name_vec.at(i)) { - *it_commands.value_ptr_vec.at(0) = *it_states.value_ptr_vec.at(i); - RCLCPP_INFO_STREAM( - logger_, "Sync joint state to command (" << - it_commands.interface_name_vec.at(0).c_str() << ", " << - *it_commands.value_ptr_vec.at(0) * 180.0 / 3.141592 << " <- " << - it_states.interface_name_vec.at(i).c_str() << ", " << - *it_states.value_ptr_vec.at(i) * 180.0 / 3.141592); - } - } - } + SyncJointCommandWithStates(); + usleep(500 * 1000); + + // Enable torque only for Dynamixels that have torque enabled in their parameters + std::vector torque_enabled_ids; + for (const auto & [id, enabled] : dxl_torque_enable_) { + if (enabled) { + torque_enabled_ids.push_back(id); } } - usleep(500 * 1000); - if (global_torque_enable_) { - dxl_comm_->DynamixelEnable(dxl_id_); - } else { - RCLCPP_INFO_STREAM(logger_, "Global Torque is disabled!"); + if (torque_enabled_ids.size() > 0) { + RCLCPP_INFO_STREAM(logger_, "Enabling torque for Dynamixels"); + for (int i = 0; i < 10; i++) { + if (dxl_comm_->DynamixelEnable(torque_enabled_ids) == DxlError::OK) { + break; + } + RCLCPP_ERROR_STREAM(logger_, "Failed to enable torque for Dynamixels, retry..."); + usleep(100 * 1000); + } } RCLCPP_INFO_STREAM(logger_, "Dynamixel Hardware Start!"); @@ -432,7 +475,12 @@ hardware_interface::CallbackReturn DynamixelHardware::start() hardware_interface::CallbackReturn DynamixelHardware::stop() { - dxl_comm_->DynamixelDisable(dxl_id_); + if (dxl_comm_) { + dxl_comm_->DynamixelDisable(dxl_id_); + } else { + RCLCPP_ERROR_STREAM(logger_, "Dynamixel Hardware Stop Fail : dxl_comm_ is nullptr"); + return hardware_interface::CallbackReturn::ERROR; + } RCLCPP_INFO_STREAM(logger_, "Dynamixel Hardware Stop!"); @@ -443,6 +491,7 @@ hardware_interface::return_type DynamixelHardware::read( [[maybe_unused]] 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; @@ -666,23 +715,51 @@ bool DynamixelHardware::initItems(const std::string & type_filter) } 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 (!retryWriteItem(id, it.first, static_cast(stoi(it.second)))) { + // Handle torque enable parameter + bool torque_enabled = type_filter == "dxl"; + if (gpio.parameters.find("Torque Enable") != gpio.parameters.end()) { + torque_enabled = std::stoi(gpio.parameters.at("Torque Enable")) != 0; + } + dxl_torque_enable_[id] = torque_enabled; + + // 1. First pass: Write Operating Mode parameters + for (const auto & param : gpio.parameters) { + const std::string & param_name = param.first; + if (param_name == "Operating Mode") { + if (!retryWriteItem(id, param_name, static_cast(stoi(param.second)))) { return false; } } } - // 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 (!retryWriteItem(id, it.first, static_cast(stoi(it.second)))) { + // 2. Second pass: Write all Limit parameters + for (const auto & param : gpio.parameters) { + const std::string & param_name = param.first; + if (param_name == "ID" || param_name == "type" || + param_name == "Torque Enable" || param_name == "Operating Mode") + { + continue; + } + if (param_name.find("Limit") != std::string::npos) { + if (!retryWriteItem(id, param_name, static_cast(stoi(param.second)))) { return false; } } } + + // 3. Third pass: Write all other parameters (excluding already written ones) + for (const auto & param : gpio.parameters) { + const std::string & param_name = param.first; + if (param_name == "ID" || param_name == "type" || + param_name == "Torque Enable" || param_name == "Operating Mode" || + param_name.find("Limit") != std::string::npos) + { + continue; + } + if (!retryWriteItem(id, param_name, static_cast(stoi(param.second)))) { + return false; + } + } } return true; } @@ -709,51 +786,29 @@ bool DynamixelHardware::InitDxlReadItems() if (gpio.state_interfaces.size() && gpio.parameters.at("type") == "dxl") { uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); HandlerVarType temp_read; - temp_read.id = id; temp_read.name = gpio.name; - // Present Position - temp_read.interface_name_vec.push_back("Present Position"); - temp_read.value_ptr_vec.push_back(std::make_shared(0.0)); - - // Present Velocity - temp_read.interface_name_vec.push_back("Present Velocity"); - temp_read.value_ptr_vec.push_back(std::make_shared(0.0)); - - // effort third - for (auto it : gpio.state_interfaces) { - if (it.name == "Present Current" || it.name == "Present Load") { - temp_read.interface_name_vec.push_back(it.name); - temp_read.value_ptr_vec.push_back(std::make_shared(0.0)); - } - } - for (auto it : gpio.state_interfaces) { - if (it.name != "Present Position" && it.name != "Present Velocity" && - it.name != "Present Current" && it.name != "Present Load") - { - temp_read.interface_name_vec.push_back(it.name); - temp_read.value_ptr_vec.push_back(std::make_shared(0.0)); + temp_read.interface_name_vec.push_back(it.name); + temp_read.value_ptr_vec.push_back(std::make_shared(0.0)); - if (it.name == "Hardware Error Status") { - dxl_hw_err_[id] = 0x00; - } + if (it.name == "Hardware Error Status") { + dxl_hw_err_[id] = 0x00; } } + hdl_trans_states_.push_back(temp_read); } else if (gpio.state_interfaces.size() && gpio.parameters.at("type") == "sensor") { + uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); HandlerVarType temp_sensor; - for (auto it : gpio.state_interfaces) { - uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); + temp_sensor.id = id; + temp_sensor.name = gpio.name; - temp_sensor.id = id; - temp_sensor.name = gpio.name; - for (auto it : gpio.state_interfaces) { - temp_sensor.interface_name_vec.push_back(it.name); - temp_sensor.value_ptr_vec.push_back(std::make_shared(0.0)); - } + for (auto it : gpio.state_interfaces) { + temp_sensor.interface_name_vec.push_back(it.name); + temp_sensor.value_ptr_vec.push_back(std::make_shared(0.0)); } hdl_gpio_sensor_states_.push_back(temp_sensor); } @@ -768,6 +823,14 @@ bool DynamixelHardware::InitDxlReadItems() return false; } } + for (auto it : hdl_gpio_sensor_states_) { + if (dxl_comm_->SetDxlReadItems( + it.id, it.interface_name_vec, + it.value_ptr_vec) != DxlError::OK) + { + return false; + } + } if (dxl_comm_->SetMultiDxlRead() != DxlError::OK) { return false; } @@ -781,24 +844,37 @@ bool DynamixelHardware::InitDxlWriteItems() if (!is_set_hdl_) { hdl_trans_commands_.clear(); + hdl_gpio_controller_commands_.clear(); for (const hardware_interface::ComponentInfo & gpio : info_.gpios) { - if (gpio.command_interfaces.size()) { + if (gpio.parameters.at("type") == "dxl") { uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); HandlerVarType temp_write; temp_write.id = id; temp_write.name = gpio.name; - temp_write.interface_name_vec.push_back("Goal Position"); - temp_write.value_ptr_vec.push_back(std::make_shared(0.0)); - for (auto it : gpio.command_interfaces) { - if (it.name == "Goal Current") { - temp_write.interface_name_vec.push_back("Goal Current"); - temp_write.value_ptr_vec.push_back(std::make_shared(0.0)); + if (it.name != "Goal Position" && + it.name != "Goal Velocity" && + it.name != "Goal Current") + { + continue; } + temp_write.interface_name_vec.push_back(it.name); + temp_write.value_ptr_vec.push_back(std::make_shared(0.0)); } hdl_trans_commands_.push_back(temp_write); + } else if (gpio.parameters.at("type") == "controller") { + uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); + HandlerVarType temp_controller; + temp_controller.id = id; + temp_controller.name = gpio.name; + + for (auto it : gpio.command_interfaces) { + temp_controller.interface_name_vec.push_back(it.name); + temp_controller.value_ptr_vec.push_back(std::make_shared(0.0)); + } + hdl_gpio_controller_commands_.push_back(temp_controller); } } is_set_hdl_ = true; @@ -813,6 +889,15 @@ bool DynamixelHardware::InitDxlWriteItems() } } + for (auto it : hdl_gpio_controller_commands_) { + if (dxl_comm_->SetDxlWriteItems( + it.id, it.interface_name_vec, + it.value_ptr_vec) != DxlError::OK) + { + return false; + } + } + if (dxl_comm_->SetMultiDxlWrite() != DxlError::OK) { return false; } @@ -823,14 +908,12 @@ bool DynamixelHardware::InitDxlWriteItems() void DynamixelHardware::ReadSensorData(const HandlerVarType & sensor) { for (auto item : sensor.interface_name_vec) { - uint32_t data; - dxl_comm_->ReadItem(sensor.id, item, data); for (size_t i = 0; i < hdl_sensor_states_.size(); i++) { for (size_t j = 0; j < hdl_sensor_states_.at(i).interface_name_vec.size(); j++) { if (hdl_sensor_states_.at(i).name == sensor.name && hdl_sensor_states_.at(i).interface_name_vec.at(j) == item) { - *hdl_sensor_states_.at(i).value_ptr_vec.at(j) = data; + *hdl_sensor_states_.at(i).value_ptr_vec.at(j) = *sensor.value_ptr_vec.at(j); } } } @@ -892,92 +975,141 @@ void DynamixelHardware::SetMatrix() fprintf(stderr, "\n"); } } -void DynamixelHardware::CalcTransmissionToJoint() -{ - for (size_t i = 0; i < num_of_joints_; i++) { - double value = 0.0; - for (size_t j = 0; j < num_of_transmissions_; j++) { - value += transmission_to_joint_matrix_[i][j] * - (*hdl_trans_states_.at(j).value_ptr_vec.at(PRESENT_POSITION_INDEX)); - } - if (hdl_joint_states_.at(i).name == conversion_joint_name_) { - value = revoluteToPrismatic(value); - } - *hdl_joint_states_.at(i).value_ptr_vec.at(PRESENT_POSITION_INDEX) = value; - } - - for (size_t i = 0; i < num_of_joints_; i++) { - double value = 0.0; - for (size_t j = 0; j < num_of_transmissions_; j++) { - value += transmission_to_joint_matrix_[i][j] * - (*hdl_trans_states_.at(j).value_ptr_vec.at(PRESENT_VELOCITY_INDEX)); +void DynamixelHardware::MapInterfaces( + size_t outer_size, + size_t inner_size, + std::vector & outer_handlers, + const std::vector & inner_handlers, + double ** matrix, + const std::unordered_map> & iface_map, + const std::string & conversion_iface, + const std::string & conversion_name, + std::function conversion) +{ + for (size_t i = 0; i < outer_size; ++i) { + for (size_t k = 0; k < outer_handlers.at(i).interface_name_vec.size(); ++k) { + double value = 0.0; + const std::string & outer_iface = outer_handlers.at(i).interface_name_vec.at(k); + auto map_it = iface_map.find(outer_iface); + if (map_it == iface_map.end()) { + std::ostringstream oss; + oss << "No mapping found for '" << outer_handlers.at(i).name + << "', interface '" << outer_iface + << "'. Skipping. Available mapping keys: ["; + size_t key_count = 0; + for (const auto & pair : iface_map) { + oss << pair.first; + if (++key_count < iface_map.size()) {oss << ", ";} + } + oss << "]"; + RCLCPP_WARN_STREAM(logger_, oss.str()); + continue; + } + const std::vector & mapped_ifaces = map_it->second; + for (size_t j = 0; j < inner_size; ++j) { + for (const auto & mapped_iface : mapped_ifaces) { + auto it = std::find( + inner_handlers.at(j).interface_name_vec.begin(), + inner_handlers.at(j).interface_name_vec.end(), + mapped_iface); + if (it != inner_handlers.at(j).interface_name_vec.end()) { + size_t idx = std::distance(inner_handlers.at(j).interface_name_vec.begin(), it); + value += matrix[i][j] * (*inner_handlers.at(j).value_ptr_vec.at(idx)); + break; + } + } + } + if (!conversion_iface.empty() && !conversion_name.empty() && + outer_iface == conversion_iface && + outer_handlers.at(i).name == conversion_name && + conversion) + { + value = conversion(value); + } + *outer_handlers.at(i).value_ptr_vec.at(k) = value; } - *hdl_joint_states_.at(i).value_ptr_vec.at(PRESENT_VELOCITY_INDEX) = value; } +} - for (size_t i = 0; i < num_of_joints_; i++) { - double value = 0.0; - for (size_t j = 0; j < num_of_transmissions_; j++) { - value += transmission_to_joint_matrix_[i][j] * - (*hdl_trans_states_.at(j).value_ptr_vec.at(PRESENT_EFFORT_INDEX)); - } - *hdl_joint_states_.at(i).value_ptr_vec.at(PRESENT_EFFORT_INDEX) = value; - } +void DynamixelHardware::CalcTransmissionToJoint() +{ + std::function conv = use_revolute_to_prismatic_ ? + std::function( + std::bind(&DynamixelHardware::revoluteToPrismatic, this, std::placeholders::_1)) : + std::function(); + this->MapInterfaces( + num_of_joints_, + num_of_transmissions_, + hdl_joint_states_, + hdl_trans_states_, + transmission_to_joint_matrix_, + dynamixel_hardware_interface::ros2_to_dxl_state_map, + "Present Position", + conversion_joint_name_, + conv + ); } void DynamixelHardware::CalcJointToTransmission() { - for (size_t i = 0; i < num_of_transmissions_; i++) { - double value = 0.0; - for (size_t j = 0; j < num_of_joints_; j++) { - value += joint_to_transmission_matrix_[i][j] * - (*hdl_joint_commands_.at(j).value_ptr_vec.at(GOAL_POSITION_INDEX)); - } - - if (hdl_trans_commands_.at(i).name == conversion_dxl_name_) { - value = prismaticToRevolute(value); - } - *hdl_trans_commands_.at(i).value_ptr_vec.at(GOAL_POSITION_INDEX) = value; - } - - for (size_t i = 0; i < num_of_transmissions_; i++) { - if (hdl_trans_commands_.at(i).interface_name_vec.size() > GOAL_CURRENT_INDEX && - hdl_trans_commands_.at(i).interface_name_vec.at(GOAL_CURRENT_INDEX) == "Goal Current") - { - for (size_t j = 0; j < hdl_joint_commands_.size(); j++) { - if (hdl_joint_commands_.at(j).interface_name_vec.size() > GOAL_CURRENT_INDEX && - hdl_joint_commands_.at(j).interface_name_vec.at(GOAL_CURRENT_INDEX) == - hardware_interface::HW_IF_EFFORT) - { - double value = 0.0; - for (size_t k = 0; k < num_of_joints_; k++) { - value += joint_to_transmission_matrix_[i][k] * - (*hdl_joint_commands_.at(k).value_ptr_vec.at(GOAL_CURRENT_INDEX)); - } - *hdl_trans_commands_.at(i).value_ptr_vec.at(GOAL_CURRENT_INDEX) = value; - } - } - } - } + std::function conv = use_revolute_to_prismatic_ ? + std::function( + std::bind(&DynamixelHardware::prismaticToRevolute, this, std::placeholders::_1)) : + std::function(); + this->MapInterfaces( + num_of_transmissions_, + num_of_joints_, + hdl_trans_commands_, + hdl_joint_commands_, + joint_to_transmission_matrix_, + dynamixel_hardware_interface::dxl_to_ros2_cmd_map, + "Goal Position", + conversion_dxl_name_, + conv + ); } void DynamixelHardware::SyncJointCommandWithStates() { - for (auto it_states : hdl_joint_states_) { - for (auto it_commands : hdl_joint_commands_) { + for (auto & it_states : hdl_joint_states_) { + for (auto & it_commands : hdl_joint_commands_) { if (it_states.name == it_commands.name) { - for (size_t i = 0; i < it_states.interface_name_vec.size(); i++) { - if (it_commands.interface_name_vec.at(0) == it_states.interface_name_vec.at(i)) { - *it_commands.value_ptr_vec.at(0) = *it_states.value_ptr_vec.at(i); - RCLCPP_INFO_STREAM( - logger_, "Sync joint state to command (" << - it_commands.interface_name_vec.at(0).c_str() << ", " << - *it_commands.value_ptr_vec.at(0) << " <- " << - it_states.interface_name_vec.at(i).c_str() << ", " << - *it_states.value_ptr_vec.at(i)); - } + std::string pos_cmd_name = hardware_interface::HW_IF_POSITION; + // Find index in command interfaces + auto cmd_it = std::find( + it_commands.interface_name_vec.begin(), + it_commands.interface_name_vec.end(), + pos_cmd_name); + if (cmd_it == it_commands.interface_name_vec.end()) { + RCLCPP_WARN_STREAM( + logger_, + "No position interface found in command interfaces for joint '" << + it_commands.name << "'. Skipping sync!"); + continue; + } + size_t cmd_idx = std::distance(it_commands.interface_name_vec.begin(), cmd_it); + // Find index in state interfaces + auto state_it = std::find( + it_states.interface_name_vec.begin(), + it_states.interface_name_vec.end(), + pos_cmd_name); + if (state_it == it_states.interface_name_vec.end()) { + RCLCPP_WARN_STREAM( + logger_, + "No position interface found in state interfaces for joint '" << + it_states.name << "'. Skipping sync!"); + continue; } + size_t state_idx = std::distance(it_states.interface_name_vec.begin(), state_it); + // Sync the value + *it_commands.value_ptr_vec.at(cmd_idx) = *it_states.value_ptr_vec.at(state_idx); + RCLCPP_INFO_STREAM( + logger_, "Sync joint state to command (joint: " << it_states.name << ", " << + it_commands.interface_name_vec.at(cmd_idx).c_str() << ", " << + *it_commands.value_ptr_vec.at(cmd_idx) << " <- " << + it_states.interface_name_vec.at(state_idx).c_str() << ", " << + *it_states.value_ptr_vec.at(state_idx)); } } }