Skip to content

Commit 68d0d28

Browse files
Woojin-CriveWoojin-Crive
authored
Merge pull request #93 from ROBOTIS-GIT/feature-major-update
Major Update for Dynamixel Hardware Interface
2 parents 34120da + 931e073 commit 68d0d28

File tree

114 files changed

+4564
-1552
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

114 files changed

+4564
-1552
lines changed

‎CHANGELOG.rst‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,15 @@
22
Changelog for package dynamixel_hardware_interface
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
44

5+
1.5.0 (2025-11-26)
6+
------------------
7+
* Added comm_id/id concept for virtual_* devices.
8+
* Added unit info system for Unified unit conversion logic.
9+
* Added sequential initialization logic.
10+
* Fixed memory leak of matrix malloc.
11+
* Refactored every type info based unit conversion to unit info based system.
12+
* Contributors: Woojin Wie
13+
514
1.4.16 (2025-10-14)
615
-------------------
716
* Added support for default unit information for Present Input Voltage to model files

‎include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp‎

Lines changed: 31 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828
#include <cstdarg>
2929
#include <memory>
3030
#include <functional>
31+
#include <utility>
3132

3233
namespace dynamixel_hardware_interface
3334
{
@@ -164,6 +165,7 @@ typedef struct
164165
*/
165166
typedef struct
166167
{
168+
uint8_t comm_id; ///< Communication ID used to reach the device.
167169
uint8_t id; ///< ID of the Dynamixel motor.
168170
ControlItem control_item; ///< Control item details.
169171
uint32_t data; ///< Data associated with the control item.
@@ -197,7 +199,7 @@ class Dynamixel
197199
// item write variable
198200
std::vector<RWItemBufInfo> write_item_buf_;
199201
std::vector<RWItemBufInfo> read_item_buf_;
200-
std::map<uint8_t /*id*/, bool> torque_state_;
202+
std::map<std::pair<uint8_t /*comm_id*/, uint8_t /*id*/>, bool> torque_state_;
201203

202204
// read item (sync or bulk) variable
203205
bool read_type_;
@@ -234,26 +236,25 @@ class Dynamixel
234236
// direct inform for bulk write
235237
std::map<uint8_t /*id*/, IndirectInfo> direct_info_write_;
236238

237-
std::map<uint8_t /*id*/, uint8_t> comm_id_;
238-
239239
public:
240240
explicit Dynamixel(const char * path);
241241
~Dynamixel();
242242

243243
// DXL Communication Setting
244-
DxlError InitDxlComm(std::vector<uint8_t> id_arr, std::string port_name, std::string baudrate);
244+
DxlError SetupPort(const std::string & port_name, const std::string & baudrate);
245+
DxlError InitDxlComm(uint8_t comm_id, uint8_t id);
245246
DxlError Reboot(uint8_t id);
246247
void RWDataReset();
247248

248249
// DXL Read Setting
249250
DxlError SetDxlReadItems(
250-
uint8_t id, uint8_t comm_id, std::vector<std::string> item_names,
251+
uint8_t comm_id, uint8_t id, std::vector<std::string> item_names,
251252
std::vector<std::shared_ptr<double>> data_vec_ptr);
252253
DxlError SetMultiDxlRead();
253254

254255
// DXL Write Setting
255256
DxlError SetDxlWriteItems(
256-
uint8_t id, uint8_t comm_id, std::vector<std::string> item_names,
257+
uint8_t comm_id, uint8_t id, std::vector<std::string> item_names,
257258
std::vector<std::shared_ptr<double>> data_vec_ptr);
258259
DxlError SetMultiDxlWrite();
259260

@@ -264,34 +265,44 @@ class Dynamixel
264265

265266
// Set Dxl Option
266267
// DxlError SetOperatingMode(uint8_t id, uint8_t dynamixel_mode);
267-
DxlError DynamixelEnable(std::vector<uint8_t> id_arr);
268-
DxlError DynamixelDisable(std::vector<uint8_t> id_arr);
268+
DxlError DynamixelEnable(const std::vector<std::pair<uint8_t, uint8_t>> & comm_id_id_arr);
269+
DxlError DynamixelDisable(const std::vector<std::pair<uint8_t, uint8_t>> & comm_id_id_arr);
269270

270271
// DXL Item Write
271-
DxlError WriteItem(uint8_t id, std::string item_name, uint32_t data);
272-
DxlError WriteItem(uint8_t id, uint16_t addr, uint8_t size, uint32_t data);
272+
DxlError WriteItem(uint8_t comm_id, uint8_t id, std::string item_name, uint32_t data);
273+
DxlError WriteItem(uint8_t comm_id, uint8_t id, uint16_t addr, uint8_t size, uint32_t data);
273274
DxlError InsertWriteItemBuf(uint8_t id, std::string item_name, uint32_t data);
274275
DxlError WriteItemBuf();
275276

276277
// DXL Item Read
277-
DxlError ReadItem(uint8_t id, std::string item_name, uint32_t & data);
278+
DxlError ReadItem(uint8_t comm_id, uint8_t id, std::string item_name, uint32_t & data);
278279
DxlError InsertReadItemBuf(uint8_t id, std::string item_name);
279280
DxlError ReadItemBuf();
280281
bool CheckReadItemBuf(uint8_t id, std::string item_name);
281282
uint32_t GetReadItemDataBuf(uint8_t id, std::string item_name);
282283

283284
DynamixelInfo GetDxlInfo() {return dxl_info_;}
284-
std::map<uint8_t, bool> GetDxlTorqueState() {return torque_state_;}
285+
std::map<std::pair<uint8_t, uint8_t>, bool> GetDxlTorqueState() {return torque_state_;}
285286

286287
static std::string DxlErrorToString(DxlError error_num);
287288

288-
DxlError ReadDxlModelFile(uint8_t id, uint16_t model_num);
289-
DxlError ReadDxlModelFile(uint8_t id, uint16_t model_num, uint8_t firmware_version);
290-
DxlError ReadFirmwareVersion(uint8_t id, uint8_t & firmware_version);
289+
DxlError ReadDxlModelFile(uint8_t comm_id, uint8_t id, uint16_t model_num);
290+
DxlError ReadDxlModelFile(
291+
uint8_t comm_id, uint8_t id, uint16_t model_num,
292+
uint8_t firmware_version);
293+
DxlError ReadFirmwareVersion(uint8_t comm_id, uint8_t id, uint8_t & firmware_version);
291294

292-
void SetCommId(uint8_t id, uint8_t comm_id) {comm_id_[id] = comm_id;}
295+
DxlError InitTorqueStates(
296+
std::vector<std::pair<uint8_t, uint8_t>> comm_id_id_arr,
297+
bool disable_torque = false);
293298

294-
DxlError InitTorqueStates(std::vector<uint8_t> id_arr, bool disable_torque = false);
299+
void OverrideUnitInfo(
300+
uint8_t comm_id,
301+
uint8_t id,
302+
const std::string & data_name,
303+
double unit_multiplier,
304+
bool is_signed,
305+
double offset_value);
295306

296307
private:
297308
bool checkReadType();
@@ -334,7 +345,7 @@ class Dynamixel
334345
std::function<uint32_t(uint8_t, uint16_t, uint8_t)> get_data_func);
335346

336347
DxlError ProcessDirectReadData(
337-
uint8_t id,
348+
uint8_t comm_id,
338349
const std::vector<uint16_t> & item_addrs,
339350
const std::vector<std::string> & item_names,
340351
const std::vector<uint8_t> & item_sizes,
@@ -372,6 +383,7 @@ class Dynamixel
372383

373384
// Helper function for value conversion with unit info
374385
double ConvertValueWithUnitInfo(
386+
uint8_t comm_id,
375387
uint8_t id,
376388
std::string item_name,
377389
uint32_t raw_value,
@@ -380,6 +392,7 @@ class Dynamixel
380392

381393
// Helper function for converting unit values to raw values
382394
uint32_t ConvertUnitValueToRawValue(
395+
uint8_t comm_id,
383396
uint8_t id,
384397
std::string item_name,
385398
double unit_value,

‎include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp‎

Lines changed: 41 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@ typedef struct
5252
std::vector<ControlItem> item;
5353
std::map<std::string, double> unit_map;
5454
std::map<std::string, bool> sign_type_map;
55+
std::map<std::string, double> offset_map;
5556
} DxlInfo;
5657

5758
class DynamixelInfo
@@ -69,56 +70,74 @@ class DynamixelInfo
6970
uint8_t ExtractFirmwareVersionFromFilename(const std::string & filename);
7071

7172
public:
72-
// Id, Control table
73-
std::map<uint8_t, DxlInfo> dxl_info_;
73+
// comm_id -> (id -> Control table)
74+
std::map<uint8_t, std::map<uint8_t, DxlInfo>> dxl_info_by_comm_;
7475

7576
DynamixelInfo() {}
7677
~DynamixelInfo() {}
7778

7879
void SetDxlModelFolderPath(const char * path);
7980
void InitDxlModelInfo();
8081

81-
void ReadDxlModelFile(uint8_t id, uint16_t model_num);
82-
void ReadDxlModelFile(uint8_t id, uint16_t model_num, uint8_t firmware_version);
83-
bool GetDxlControlItem(uint8_t id, std::string item_name, uint16_t & addr, uint8_t & size);
84-
bool CheckDxlControlItem(uint8_t id, std::string item_name);
82+
void ReadDxlModelFile(uint8_t comm_id, uint8_t id, uint16_t model_num);
83+
void ReadDxlModelFile(uint8_t comm_id, uint8_t id, uint16_t model_num, uint8_t firmware_version);
84+
bool GetDxlControlItem(
85+
uint8_t comm_id, uint8_t id, std::string item_name, uint16_t & addr,
86+
uint8_t & size);
87+
bool CheckDxlControlItem(uint8_t comm_id, uint8_t id, std::string item_name);
8588

86-
bool GetDxlUnitValue(uint8_t id, std::string data_name, double & unit_value);
87-
bool GetDxlSignType(uint8_t id, std::string data_name, bool & is_signed);
89+
bool GetDxlUnitValue(uint8_t comm_id, uint8_t id, std::string data_name, double & unit_value);
90+
bool GetDxlSignType(uint8_t comm_id, uint8_t id, std::string data_name, bool & is_signed);
91+
bool GetDxlOffsetValue(uint8_t comm_id, uint8_t id, std::string data_name, double & offset_value);
8892

89-
// Template-based conversion methods
9093
template<typename T>
91-
double ConvertValueToUnit(uint8_t id, std::string data_name, T value);
94+
double ConvertValueToUnit(uint8_t comm_id, uint8_t id, std::string data_name, T value);
9295

9396
template<typename T>
94-
T ConvertUnitToValue(uint8_t id, std::string data_name, double unit_value);
97+
T ConvertUnitToValue(uint8_t comm_id, uint8_t id, std::string data_name, double unit_value);
9598

9699
// Helper method for internal use
97-
double GetUnitMultiplier(uint8_t id, std::string data_name);
100+
double GetUnitMultiplier(uint8_t comm_id, uint8_t id, std::string data_name);
98101

99-
int32_t ConvertRadianToValue(uint8_t id, double radian);
100-
double ConvertValueToRadian(uint8_t id, int32_t value);
102+
int32_t ConvertRadianToValue(uint8_t comm_id, uint8_t id, double radian);
103+
double ConvertValueToRadian(uint8_t comm_id, uint8_t id, int32_t value);
101104

102105
std::string GetModelName(uint16_t model_number) const;
103106
};
104107

105108
// Template implementations
106109
template<typename T>
107-
double DynamixelInfo::ConvertValueToUnit(uint8_t id, std::string data_name, T value)
110+
double DynamixelInfo::ConvertValueToUnit(
111+
uint8_t comm_id, uint8_t id, std::string data_name,
112+
T value)
108113
{
109-
auto it = dxl_info_[id].unit_map.find(data_name);
110-
if (it != dxl_info_[id].unit_map.end()) {
111-
return static_cast<double>(value) * it->second;
114+
auto & info = dxl_info_by_comm_[comm_id][id];
115+
auto it = info.unit_map.find(data_name);
116+
if (it != info.unit_map.end()) {
117+
double converted_value = static_cast<double>(value) * it->second;
118+
auto offset_it = info.offset_map.find(data_name);
119+
if (offset_it != info.offset_map.end()) {
120+
converted_value += offset_it->second;
121+
}
122+
return converted_value;
112123
}
113124
return static_cast<double>(value);
114125
}
115126

116127
template<typename T>
117-
T DynamixelInfo::ConvertUnitToValue(uint8_t id, std::string data_name, double unit_value)
128+
T DynamixelInfo::ConvertUnitToValue(
129+
uint8_t comm_id, uint8_t id, std::string data_name,
130+
double unit_value)
118131
{
119-
auto it = dxl_info_[id].unit_map.find(data_name);
120-
if (it != dxl_info_[id].unit_map.end()) {
121-
return static_cast<T>(unit_value / it->second);
132+
auto & info = dxl_info_by_comm_[comm_id][id];
133+
auto it = info.unit_map.find(data_name);
134+
if (it != info.unit_map.end()) {
135+
double adjusted_value = unit_value;
136+
auto offset_it = info.offset_map.find(data_name);
137+
if (offset_it != info.offset_map.end()) {
138+
adjusted_value -= offset_it->second;
139+
}
140+
return static_cast<T>(adjusted_value / it->second);
122141
}
123142
return static_cast<T>(unit_value);
124143
}

‎include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp‎

Lines changed: 12 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#include <map>
2424
#include <unordered_map>
2525
#include <functional>
26+
#include <utility>
2627

2728
#include "rclcpp/rclcpp.hpp"
2829
#include "ament_index_cpp/get_package_share_directory.hpp"
@@ -182,8 +183,8 @@ class DynamixelHardware : public
182183
std::map<uint8_t /*id*/, uint8_t /*err*/> dxl_hw_err_;
183184
std::map<uint8_t /*id*/, uint8_t /*error code*/> dxl_error_code_;
184185
DxlTorqueStatus dxl_torque_status_;
185-
std::map<uint8_t /*id*/, bool /*enable*/> dxl_torque_state_;
186-
std::vector<uint8_t> torque_enabled_ids_;
186+
std::map<std::pair<uint8_t /*comm_id*/, uint8_t /*id*/>, bool /*enable*/> dxl_torque_state_;
187+
std::vector<std::pair<uint8_t, uint8_t>> torque_enabled_comm_id_id_;
187188
double err_timeout_ms_;
188189
rclcpp::Duration read_error_duration_{0, 0};
189190
rclcpp::Duration write_error_duration_{0, 0};
@@ -228,13 +229,13 @@ class DynamixelHardware : public
228229
///// dxl variable
229230
std::string port_name_;
230231
std::string baud_rate_;
231-
std::vector<uint8_t> dxl_id_;
232-
std::vector<uint8_t> virtual_dxl_id_;
232+
std::vector<std::pair<uint8_t, uint8_t>> dxl_comm_id_id_;
233+
std::vector<std::pair<uint8_t, uint8_t>> virtual_dxl_comm_id_id_;
233234

234-
std::vector<uint8_t> sensor_id_;
235+
std::vector<std::pair<uint8_t, uint8_t>> sensor_comm_id_id_;
235236
std::map<uint8_t /*id*/, std::string /*interface_name*/> sensor_item_;
236237

237-
std::vector<uint8_t> controller_id_;
238+
std::vector<std::pair<uint8_t, uint8_t>> controller_comm_id_id_;
238239
std::map<uint8_t /*id*/, std::string /*interface_name*/> controller_item_;
239240

240241
///// handler variable
@@ -256,27 +257,14 @@ class DynamixelHardware : public
256257
// joint <-> transmission matrix
257258
size_t num_of_joints_;
258259
size_t num_of_transmissions_;
259-
double ** transmission_to_joint_matrix_;
260-
double ** joint_to_transmission_matrix_;
260+
std::vector<std::vector<double>> transmission_to_joint_matrix_;
261+
std::vector<std::vector<double>> joint_to_transmission_matrix_;
261262

262263
/**
263-
* @brief Helper function to initialize items for a specific type.
264-
* @param type_filter The type of items to initialize ("controller" or "dxl" or "sensor").
264+
* @brief Helper function to initialize items
265265
* @return True if initialization was successful, false otherwise.
266266
*/
267-
bool initItems(const std::string & type_filter);
268-
269-
/**
270-
* @brief Initializes Dynamixel items.
271-
* @return True if initialization was successful, false otherwise.
272-
*/
273-
bool InitDxlItems();
274-
275-
/**
276-
* @brief Initializes the controller items.
277-
* @return True if initialization was successful, false otherwise.
278-
*/
279-
bool InitControllerItems();
267+
bool InitItem(const hardware_interface::ComponentInfo & gpio);
280268

281269
/**
282270
* @brief Initializes the read items for Dynamixel.
@@ -377,7 +365,7 @@ class DynamixelHardware : public
377365
size_t inner_size,
378366
std::vector<HandlerVarType> & outer_handlers,
379367
std::vector<HandlerVarType> & inner_handlers,
380-
double ** matrix,
368+
const std::vector<std::vector<double>> & matrix,
381369
const std::unordered_map<std::string, std::vector<std::string>> & iface_map,
382370
const std::string & conversion_iface = "",
383371
const std::string & conversion_name = "",

‎package.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
44
<name>dynamixel_hardware_interface</name>
5-
<version>1.4.16</version>
5+
<version>1.5.0</version>
66
<description>
77
ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework.
88
</description>

‎param/dxl_model/2xc430_w250.model‎

Lines changed: 6 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,10 @@
1-
[type info]
2-
name value
3-
value_of_zero_radian_position 2048
4-
value_of_max_radian_position 4095
5-
value_of_min_radian_position 0
6-
min_radian -3.14159265
7-
max_radian 3.14159265
8-
91
[unit info]
10-
Data Name value unit Sign Type
11-
Present Velocity 0.0239691227 rad/s signed
12-
Goal Velocity 0.0239691227 rad/s signed
13-
Present Input Voltage 0.1 V unsigned
2+
Data Name value unit Sign Type Offset
3+
Present Velocity 0.0239691227 rad/s signed 0.0
4+
Goal Velocity 0.0239691227 rad/s signed 0.0
5+
Present Position 0.0015339807878856412 rad signed -3.14159265359
6+
Goal Position 0.0015339807878856412 rad signed -3.14159265359
7+
Present Input Voltage 0.1 V unsigned 0.0
148

159
[control table]
1610
Address Size Data Name

‎param/dxl_model/2xl430_w250.model‎

Lines changed: 6 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,10 @@
1-
[type info]
2-
name value
3-
value_of_zero_radian_position 2048
4-
value_of_max_radian_position 4095
5-
value_of_min_radian_position 0
6-
min_radian -3.14159265
7-
max_radian 3.14159265
8-
91
[unit info]
10-
Data Name value unit Sign Type
11-
Present Velocity 0.0239691227 rad/s signed
12-
Goal Velocity 0.0239691227 rad/s signed
13-
Present Input Voltage 0.1 V unsigned
2+
Data Name value unit Sign Type Offset
3+
Present Velocity 0.0239691227 rad/s signed 0.0
4+
Goal Velocity 0.0239691227 rad/s signed 0.0
5+
Present Position 0.0015339807878856412 rad signed -3.14159265359
6+
Goal Position 0.0015339807878856412 rad signed -3.14159265359
7+
Present Input Voltage 0.1 V unsigned 0.0
148

159
[control table]
1610
Address Size Data Name

0 commit comments

Comments
 (0)