Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,11 @@
Changelog for package dynamixel_hardware_interface
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

1.4.3 (2025-04-09)
------------------
* Removed Boost dependency to reduce external dependencies
* Contributors: Woojin Wie

1.4.2 (2025-04-05)
------------------
* Added OM-Y dynamixel model files
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,15 +19,14 @@

#include <cmath>
#include <cstring>
#include <cstdint>

#include <fstream>
#include <iostream>
#include <map>
#include <string>
#include <vector>

#include <boost/algorithm/string.hpp>

namespace dynamixel_hardware_interface
{

Expand Down
2 changes: 1 addition & 1 deletion package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>dynamixel_hardware_interface</name>
<version>1.4.2</version>
<version>1.4.3</version>
<description>
ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework.
</description>
Expand Down
89 changes: 66 additions & 23 deletions src/dynamixel/dynamixel_info.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,10 +18,35 @@
#include <string>
#include <utility>
#include <vector>
#include <sstream>

namespace dynamixel_hardware_interface
{

// Helper function to split string by delimiter
std::vector<std::string> split_string(const std::string & str, char delimiter, bool trim = false)
{
std::vector<std::string> tokens;
std::string token;
std::istringstream token_stream(str);
while (std::getline(token_stream, token, delimiter)) {
if (trim) {
// Trim leading and trailing whitespace
size_t first = token.find_first_not_of(" \t\r\n");
size_t last = token.find_last_not_of(" \t\r\n");
if (first != std::string::npos && last != std::string::npos) {
token = token.substr(first, (last - first + 1));
} else {
token.clear();
}
}
if (!token.empty()) { // Skip empty tokens
tokens.push_back(token);
}
}
return tokens;
}

void DynamixelInfo::SetDxlModelFolderPath(const char * path)
{
dxl_model_file_dir = std::string(path);
Expand Down Expand Up @@ -80,21 +105,30 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num)
break;
}

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)));
std::vector<std::string> strs = split_string(line, '\t', true);

if (!strs.empty()) {
try {
if (strs.at(0) == "value_of_zero_radian_position") {
temp_dxl_info.value_of_zero_radian_position = static_cast<int32_t>(std::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>(std::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>(std::stoi(strs.at(1)));
} else if (strs.at(0) == "min_radian") {
temp_dxl_info.min_radian = static_cast<double>(std::stod(strs.at(1)));
} else if (strs.at(0) == "max_radian") {
temp_dxl_info.max_radian = static_cast<double>(std::stod(strs.at(1)));
} else if (strs.at(0) == "torque_constant") {
temp_dxl_info.torque_constant = static_cast<double>(std::stod(strs.at(1)));
}
} catch (const std::invalid_argument & e) {
fprintf(stderr, "[ERROR] Invalid argument in model file: %s\n", e.what());
continue;
} catch (const std::out_of_range & e) {
fprintf(stderr, "[ERROR] Out of range in model file: %s\n", e.what());
continue;
}
}
}

Expand All @@ -105,14 +139,23 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num)
break;
}

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);
std::vector<std::string> strs = split_string(line, '\t', true);

if (!strs.empty()) {
try {
ControlItem temp;
temp.address = static_cast<uint16_t>(std::stoi(strs.at(0)));
temp.size = static_cast<uint8_t>(std::stoi(strs.at(1)));
temp.item_name = strs.at(2);
temp_dxl_info.item.push_back(temp);
} catch (const std::invalid_argument & e) {
fprintf(stderr, "[ERROR] Invalid argument in control table: %s\n", e.what());
continue;
} catch (const std::out_of_range & e) {
fprintf(stderr, "[ERROR] Out of range in control table: %s\n", e.what());
continue;
}
}
}

dxl_info_[id] = temp_dxl_info;
Expand Down
Loading