Add Support for Various Command Interfaces

include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp

@@ -21,6 +21,7 @@
 #include <string>
 #include <vector>
 #include <map>
+#include <unordered_map>
 
 #include "rclcpp/rclcpp.hpp"
 #include "ament_index_cpp/get_package_share_directory.hpp"
@@ -47,10 +48,6 @@
 #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 +182,13 @@ class DynamixelHardware : public
   std::map<uint8_t /*id*/, uint8_t /*err*/> dxl_hw_err_;
   DxlTorqueStatus dxl_torque_status_;
   std::map<uint8_t /*id*/, bool /*enable*/> dxl_torque_state_;
+  std::map<uint8_t /*id*/, bool /*enable*/> 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_{""};
@@ -362,6 +358,18 @@ class DynamixelHardware : public
   double prismaticToRevolute(double prismatic_value);
 };
 
+// Conversion maps between ROS2 and Dynamixel interface names
+inline const std::unordered_map<std::string, std::string> 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"}
+};
+inline const std::unordered_map<std::string, std::string> 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}
+};
+
 }  // namespace dynamixel_hardware_interface
 
 #endif  // DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL_HARDWARE_INTERFACE_HPP_

param/dxl_model/dynamixel.model

@@ -46,3 +46,4 @@ Number	Name
 35074	rh_p12_rn.model
 220	omy_hat.model
 230	omy_end.model
+536 sensorxel_joy.model

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

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

src/dynamixel/dynamixel.cpp

@@ -82,15 +82,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 +269,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 {
@@ -1169,16 +1182,16 @@ 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);
     }
@@ -1284,16 +1297,16 @@ DxlError Dynamixel::SetDxlValueToBulkWrite()
         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));
+      } 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);
     }

src/dynamixel/dynamixel_info.cpp

@@ -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<std::string> 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<int32_t>(stoi(strs.at(1)));
-    } else if (strs.at(0) == "value_of_max_radian_position") {
-      temp_dxl_info.value_of_max_radian_position = static_cast<int32_t>(stoi(strs.at(1)));
-    } else if (strs.at(0) == "value_of_min_radian_position") {
-      temp_dxl_info.value_of_min_radian_position = static_cast<int32_t>(stoi(strs.at(1)));
-    } else if (strs.at(0) == "min_radian") {
-      temp_dxl_info.min_radian = static_cast<double>(stod(strs.at(1)));
-    } else if (strs.at(0) == "max_radian") {
-      temp_dxl_info.max_radian = static_cast<double>(stod(strs.at(1)));
-    } else if (strs.at(0) == "torque_constant") {
-      temp_dxl_info.torque_constant = static_cast<double>(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<int32_t>(stoi(strs.at(1)));
+      } else if (strs.at(0) == "value_of_max_radian_position") {
+        temp_dxl_info.value_of_max_radian_position = static_cast<int32_t>(stoi(strs.at(1)));
+      } else if (strs.at(0) == "value_of_min_radian_position") {
+        temp_dxl_info.value_of_min_radian_position = static_cast<int32_t>(stoi(strs.at(1)));
+      } else if (strs.at(0) == "min_radian") {
+        temp_dxl_info.min_radian = static_cast<double>(stod(strs.at(1)));
+      } else if (strs.at(0) == "max_radian") {
+        temp_dxl_info.max_radian = static_cast<double>(stod(strs.at(1)));
+      } else if (strs.at(0) == "torque_constant") {
+        temp_dxl_info.torque_constant = static_cast<double>(stod(strs.at(1)));
+      }
+    } catch (const std::exception& e) {
+      std::string error_msg = "Error processing line in model file: " + line + "\nError: " + e.what();
+      // fprintf(stderr, "[WARN] %s\n", error_msg.c_str());
+      throw std::runtime_error(error_msg);
     }
   }
 
@@ -108,11 +118,29 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num)
     std::vector<std::string> strs;
     boost::split(strs, line, boost::is_any_of("\t"));
 
-    ControlItem temp;
-    temp.address = static_cast<uint16_t>(stoi(strs.at(0)));
-    temp.size = static_cast<uint8_t>(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;
+      // fprintf(stderr, "[ERROR] %s\n", error_msg.c_str());
+      throw std::runtime_error(error_msg);
+    }
+
+    try {
+      ControlItem temp;
+      temp.address = static_cast<uint16_t>(stoi(strs.at(0)));
+      temp.size = static_cast<uint8_t>(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();
+      // fprintf(stderr, "[ERROR] %s\n", error_msg.c_str());
+      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);
+    // fprintf(stderr, "[ERROR] %s\n", error_msg.c_str());
+    throw std::runtime_error(error_msg);
   }
 
   dxl_info_[id] = temp_dxl_info;