Skip to content

Commit 7bd884c

Browse files
Woojin-CriveWoojin-Crive
committed
refactor: improve code formatting and error handling in Dynamixel hardware interface
1 parent 3d8e13a commit 7bd884c

File tree

4 files changed

+59
-48
lines changed

4 files changed

+59
-48
lines changed

‎include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -360,14 +360,14 @@ class DynamixelHardware : public
360360

361361
// Conversion maps between ROS2 and Dynamixel interface names
362362
inline const std::unordered_map<std::string, std::string> ros2_to_dxl_cmd_map = {
363-
{hardware_interface::HW_IF_POSITION, "Goal Position"},
364-
{hardware_interface::HW_IF_VELOCITY, "Goal Velocity"},
365-
{hardware_interface::HW_IF_EFFORT, "Goal Current"}
363+
{hardware_interface::HW_IF_POSITION, "Goal Position"},
364+
{hardware_interface::HW_IF_VELOCITY, "Goal Velocity"},
365+
{hardware_interface::HW_IF_EFFORT, "Goal Current"}
366366
};
367367
inline const std::unordered_map<std::string, std::string> dxl_to_ros2_cmd_map = {
368-
{"Goal Position", hardware_interface::HW_IF_POSITION},
369-
{"Goal Velocity", hardware_interface::HW_IF_VELOCITY},
370-
{"Goal Current", hardware_interface::HW_IF_EFFORT}
368+
{"Goal Position", hardware_interface::HW_IF_POSITION},
369+
{"Goal Velocity", hardware_interface::HW_IF_VELOCITY},
370+
{"Goal Current", hardware_interface::HW_IF_EFFORT}
371371
};
372372

373373
} // namespace dynamixel_hardware_interface

‎src/dynamixel/dynamixel.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -84,7 +84,7 @@ DxlError Dynamixel::InitDxlComm(
8484

8585
try {
8686
dxl_info_.ReadDxlModelFile(it_id, dxl_model_number);
87-
} catch (const std::exception& e) {
87+
} catch (const std::exception & e) {
8888
fprintf(stderr, "Error reading model file for ID %d: %s\n", it_id, e.what());
8989
return DxlError::CANNOT_FIND_CONTROL_ITEM;
9090
}
@@ -98,7 +98,7 @@ DxlError Dynamixel::InitDxlComm(
9898
if (dxl_info_.CheckDxlControlItem(it_id, "Torque Enable")) {
9999
torque_state_[it_id] = TORQUE_OFF;
100100
}
101-
} catch (const std::exception& e) {
101+
} catch (const std::exception & e) {
102102
fprintf(stderr, "Error checking control item for ID %d: %s\n", it_id, e.what());
103103
return DxlError::CANNOT_FIND_CONTROL_ITEM;
104104
}

‎src/dynamixel/dynamixel_info.cpp‎

Lines changed: 8 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -101,9 +101,9 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num)
101101
} else if (strs.at(0) == "torque_constant") {
102102
temp_dxl_info.torque_constant = static_cast<double>(stod(strs.at(1)));
103103
}
104-
} catch (const std::exception& e) {
105-
std::string error_msg = "Error processing line in model file: " + line + "\nError: " + e.what();
106-
// fprintf(stderr, "[WARN] %s\n", error_msg.c_str());
104+
} catch (const std::exception & e) {
105+
std::string error_msg = "Error processing line in model file: " + line +
106+
"\nError: " + e.what();
107107
throw std::runtime_error(error_msg);
108108
}
109109
}
@@ -120,7 +120,6 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num)
120120

121121
if (strs.size() < 3) {
122122
std::string error_msg = "Malformed control table line: " + line;
123-
// fprintf(stderr, "[ERROR] %s\n", error_msg.c_str());
124123
throw std::runtime_error(error_msg);
125124
}
126125

@@ -130,16 +129,16 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num)
130129
temp.size = static_cast<uint8_t>(stoi(strs.at(1)));
131130
temp.item_name = strs.at(2);
132131
temp_dxl_info.item.push_back(temp);
133-
} catch (const std::exception& e) {
134-
std::string error_msg = "Error processing control table line: " + line + "\nError: " + e.what();
135-
// fprintf(stderr, "[ERROR] %s\n", error_msg.c_str());
132+
} catch (const std::exception & e) {
133+
std::string error_msg = "Error processing control table line: " + line +
134+
"\nError: " + e.what();
136135
throw std::runtime_error(error_msg);
137136
}
138137
}
139138

140139
if (temp_dxl_info.item.empty()) {
141-
std::string error_msg = "No control table items found in model file for ID " + std::to_string(id);
142-
// fprintf(stderr, "[ERROR] %s\n", error_msg.c_str());
140+
std::string error_msg = "No control table items found in model file for ID " +
141+
std::to_string(id);
143142
throw std::runtime_error(error_msg);
144143
}
145144

‎src/dynamixel_hardware_interface.cpp‎

Lines changed: 43 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -325,9 +325,10 @@ DynamixelHardware::export_state_interfaces()
325325
for (size_t i = 0; i < it.value_ptr_vec.size(); i++) {
326326
if (i >= it.interface_name_vec.size()) {
327327
RCLCPP_ERROR_STREAM(
328-
logger_, "Interface name vector size mismatch for " << it.name <<
329-
". Expected size: " << it.value_ptr_vec.size() <<
330-
", Actual size: " << it.interface_name_vec.size());
328+
logger_,
329+
"Interface name vector size mismatch for " << it.name <<
330+
". Expected size: " << it.value_ptr_vec.size() <<
331+
", Actual size: " << it.interface_name_vec.size());
331332
continue;
332333
}
333334
state_interfaces.emplace_back(
@@ -340,8 +341,8 @@ DynamixelHardware::export_state_interfaces()
340341
if (i >= it.interface_name_vec.size()) {
341342
RCLCPP_ERROR_STREAM(
342343
logger_, "Interface name vector size mismatch for joint " << it.name <<
343-
". Expected size: " << it.value_ptr_vec.size() <<
344-
", Actual size: " << it.interface_name_vec.size());
344+
". Expected size: " << it.value_ptr_vec.size() <<
345+
", Actual size: " << it.interface_name_vec.size());
345346
continue;
346347
}
347348
state_interfaces.emplace_back(
@@ -354,8 +355,8 @@ DynamixelHardware::export_state_interfaces()
354355
if (i >= it.interface_name_vec.size()) {
355356
RCLCPP_ERROR_STREAM(
356357
logger_, "Interface name vector size mismatch for sensor " << it.name <<
357-
". Expected size: " << it.value_ptr_vec.size() <<
358-
", Actual size: " << it.interface_name_vec.size());
358+
". Expected size: " << it.value_ptr_vec.size() <<
359+
", Actual size: " << it.interface_name_vec.size());
359360
continue;
360361
}
361362
state_interfaces.emplace_back(
@@ -381,8 +382,8 @@ DynamixelHardware::export_command_interfaces()
381382
if (i >= it.interface_name_vec.size()) {
382383
RCLCPP_ERROR_STREAM(
383384
logger_, "Interface name vector size mismatch for " << it.name <<
384-
". Expected size: " << it.value_ptr_vec.size() <<
385-
", Actual size: " << it.interface_name_vec.size());
385+
". Expected size: " << it.value_ptr_vec.size() <<
386+
", Actual size: " << it.interface_name_vec.size());
386387
continue;
387388
}
388389
command_interfaces.emplace_back(
@@ -395,8 +396,8 @@ DynamixelHardware::export_command_interfaces()
395396
if (i >= it.interface_name_vec.size()) {
396397
RCLCPP_ERROR_STREAM(
397398
logger_, "Interface name vector size mismatch for joint " << it.name <<
398-
". Expected size: " << it.value_ptr_vec.size() <<
399-
", Actual size: " << it.interface_name_vec.size());
399+
". Expected size: " << it.value_ptr_vec.size() <<
400+
", Actual size: " << it.interface_name_vec.size());
400401
continue;
401402
}
402403
command_interfaces.emplace_back(
@@ -441,7 +442,7 @@ hardware_interface::CallbackReturn DynamixelHardware::start()
441442

442443
// Enable torque only for Dynamixels that have torque enabled in their parameters
443444
std::vector<uint8_t> torque_enabled_ids;
444-
for (const auto& [id, enabled] : dxl_torque_enable_) {
445+
for (const auto & [id, enabled] : dxl_torque_enable_) {
445446
if (enabled) {
446447
torque_enabled_ids.push_back(id);
447448
}
@@ -699,8 +700,8 @@ bool DynamixelHardware::initItems(const std::string & type_filter)
699700
dxl_torque_enable_[id] = torque_enabled;
700701

701702
// 1. First pass: Write Operating Mode parameters
702-
for (const auto& param : gpio.parameters) {
703-
const std::string& param_name = param.first;
703+
for (const auto & param : gpio.parameters) {
704+
const std::string & param_name = param.first;
704705
if (param_name == "Operating Mode") {
705706
if (!retryWriteItem(id, param_name, static_cast<uint32_t>(stoi(param.second)))) {
706707
return false;
@@ -709,10 +710,11 @@ bool DynamixelHardware::initItems(const std::string & type_filter)
709710
}
710711

711712
// 2. Second pass: Write all Limit parameters
712-
for (const auto& param : gpio.parameters) {
713-
const std::string& param_name = param.first;
714-
// Skip special parameters
715-
if (param_name == "ID" || param_name == "type" || param_name == "Torque Enable" || param_name == "Operating Mode") {
713+
for (const auto & param : gpio.parameters) {
714+
const std::string & param_name = param.first;
715+
if (param_name == "ID" || param_name == "type" ||
716+
param_name == "Torque Enable" || param_name == "Operating Mode")
717+
{
716718
continue;
717719
}
718720
if (param_name.find("Limit") != std::string::npos) {
@@ -723,10 +725,12 @@ bool DynamixelHardware::initItems(const std::string & type_filter)
723725
}
724726

725727
// 3. Third pass: Write all other parameters (excluding already written ones)
726-
for (const auto& param : gpio.parameters) {
727-
const std::string& param_name = param.first;
728-
// Skip special and already written parameters
729-
if (param_name == "ID" || param_name == "type" || param_name == "Torque Enable" || param_name == "Operating Mode" || param_name.find("Limit") != std::string::npos) {
728+
for (const auto & param : gpio.parameters) {
729+
const std::string & param_name = param.first;
730+
if (param_name == "ID" || param_name == "type" ||
731+
param_name == "Torque Enable" || param_name == "Operating Mode" ||
732+
param_name.find("Limit") != std::string::npos)
733+
{
730734
continue;
731735
}
732736
if (!retryWriteItem(id, param_name, static_cast<uint32_t>(stoi(param.second)))) {
@@ -840,8 +844,8 @@ bool DynamixelHardware::InitDxlWriteItems()
840844

841845
for (auto it : gpio.command_interfaces) {
842846
if (it.name != "Goal Position" &&
843-
it.name != "Goal Velocity" &&
844-
it.name != "Goal Current")
847+
it.name != "Goal Velocity" &&
848+
it.name != "Goal Current")
845849
{
846850
continue;
847851
}
@@ -959,7 +963,8 @@ void DynamixelHardware::CalcTransmissionToJoint()
959963
}
960964

961965
if (state_idx == PRESENT_POSITION_INDEX &&
962-
hdl_joint_states_.at(i).name == conversion_joint_name_) {
966+
hdl_joint_states_.at(i).name == conversion_joint_name_)
967+
{
963968
value = revoluteToPrismatic(value);
964969
}
965970

@@ -981,7 +986,8 @@ void DynamixelHardware::CalcJointToTransmission()
981986

982987
for(size_t k = 0; k < hdl_trans_commands_.at(i).interface_name_vec.size(); k++) {
983988
if (hdl_trans_commands_.at(i).interface_name_vec.at(k) == "Goal Position" &&
984-
hdl_trans_commands_.at(i).name == conversion_dxl_name_) {
989+
hdl_trans_commands_.at(i).name == conversion_dxl_name_)
990+
{
985991
value = prismaticToRevolute(value);
986992
}
987993

@@ -993,18 +999,22 @@ void DynamixelHardware::CalcJointToTransmission()
993999

9941000
void DynamixelHardware::SyncJointCommandWithStates()
9951001
{
996-
for (auto& it_states : hdl_joint_states_) {
997-
for (auto& it_commands : hdl_joint_commands_) {
1002+
for (auto & it_states : hdl_joint_states_) {
1003+
for (auto & it_commands : hdl_joint_commands_) {
9981004
if (it_states.name == it_commands.name) {
9991005
std::string pos_cmd_name = hardware_interface::HW_IF_POSITION;
1000-
std::string pos_dxl_name = ros2_to_dxl_cmd_map.count(pos_cmd_name) ? ros2_to_dxl_cmd_map.at(pos_cmd_name) : pos_cmd_name;
1006+
std::string pos_dxl_name =
1007+
ros2_to_dxl_cmd_map.count(pos_cmd_name) ? ros2_to_dxl_cmd_map.at(pos_cmd_name) :
1008+
pos_cmd_name;
10011009
// Find index in command interfaces
10021010
auto cmd_it = std::find(
10031011
it_commands.interface_name_vec.begin(),
10041012
it_commands.interface_name_vec.end(),
10051013
pos_cmd_name);
10061014
if (cmd_it == it_commands.interface_name_vec.end()) {
1007-
RCLCPP_WARN_STREAM(logger_, "No position interface found in command interfaces for joint '" << it_commands.name << "'. Skipping sync!");
1015+
RCLCPP_WARN_STREAM(logger_,
1016+
"No position interface found in command interfaces for joint '" <<
1017+
it_commands.name << "'. Skipping sync!");
10081018
continue;
10091019
}
10101020
size_t cmd_idx = std::distance(it_commands.interface_name_vec.begin(), cmd_it);
@@ -1014,7 +1024,9 @@ void DynamixelHardware::SyncJointCommandWithStates()
10141024
it_states.interface_name_vec.end(),
10151025
pos_cmd_name);
10161026
if (state_it == it_states.interface_name_vec.end()) {
1017-
RCLCPP_WARN_STREAM(logger_, "No position interface found in state interfaces for joint '" << it_states.name << "'. Skipping sync!");
1027+
RCLCPP_WARN_STREAM(logger_,
1028+
"No position interface found in state interfaces for joint '" <<
1029+
it_states.name << "'. Skipping sync!");
10181030
continue;
10191031
}
10201032
size_t state_idx = std::distance(it_states.interface_name_vec.begin(), state_it);

0 commit comments

Comments
 (0)