Skip to content

Enhance ProcessDirectReadData to handle item names for specific conversions #64

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
wants to merge 2 commits into from
Closed

Enhance ProcessDirectReadData to handle item names for specific conversions #64

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5fe935a
feat: Enhance ProcessDirectReadData to handle item names for specific…
Woojin-Crive Jul 8, 2025
3dc9b00
feat: Introduce unit and sign type handling in DynamixelInfo and upda…
Woojin-Crive Jul 15, 2025
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
1 change: 1 addition & 0 deletions include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -260,6 +260,7 @@ class Dynamixel
DxlError ProcessDirectReadData(
uint8_t id,
const std::vector<uint16_t> & item_addrs,
const std::vector<std::string> & item_names,
const std::vector<uint8_t> & item_sizes,
const std::vector<std::shared_ptr<double>> & data_ptrs,
std::function<uint32_t(uint8_t, uint16_t, uint8_t)> get_data_func);
Expand Down
64 changes: 51 additions & 13 deletions src/dynamixel/dynamixel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1449,6 +1449,7 @@ DxlError Dynamixel::GetDxlValueFromBulkRead(double period_ms)
ProcessDirectReadData(
id,
it_read_data.item_addr,
it_read_data.item_name,
it_read_data.item_size,
it_read_data.item_data_ptr_vec,
[this](uint8_t id, uint16_t addr, uint8_t size) {
Expand Down Expand Up @@ -1514,6 +1515,7 @@ DxlError Dynamixel::GetDxlValueFromBulkRead(double period_ms)
ProcessDirectReadData(
id,
it_read_data.item_addr,
it_read_data.item_name,
it_read_data.item_size,
it_read_data.item_data_ptr_vec,
[this](uint8_t id, uint16_t addr, uint8_t size) {
Expand Down Expand Up @@ -1659,17 +1661,33 @@ DxlError Dynamixel::ProcessReadData(
DxlError Dynamixel::ProcessDirectReadData(
uint8_t comm_id,
const std::vector<uint16_t> & item_addrs,
const std::vector<std::string> & item_names,
const std::vector<uint8_t> & item_sizes,
const std::vector<std::shared_ptr<double>> & data_ptrs,
std::function<uint32_t(uint8_t, uint16_t, uint8_t)> get_data_func)
{
for (size_t item_index = 0; item_index < item_addrs.size(); item_index++) {
uint8_t ID = comm_id;
Copy link
Contributor

@gemini-code-assist gemini-code-assist bot Jul 8, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

The ID variable is redundant as it's just a copy of comm_id. You can remove this line and use comm_id directly in the conversion functions below (lines 1677, 1681, 1685). This will make the code slightly cleaner.

uint16_t current_addr = item_addrs[item_index];
uint8_t size = item_sizes[item_index];

uint32_t dxl_getdata = get_data_func(comm_id, current_addr, size);

*data_ptrs[item_index] = static_cast<double>(dxl_getdata);
if (item_names[item_index] == "Present Position") {
*data_ptrs[item_index] = dxl_info_.ConvertValueToRadian(
ID,
static_cast<int32_t>(dxl_getdata));
} else if (item_names[item_index] == "Present Velocity") {
*data_ptrs[item_index] = dxl_info_.ConvertValueRPMToVelocityRPS(
ID,
static_cast<int32_t>(dxl_getdata));
} else if (item_names[item_index] == "Present Current") {
*data_ptrs[item_index] = dxl_info_.ConvertCurrentToEffort(
ID,
static_cast<int16_t>(dxl_getdata));
} else {
*data_ptrs[item_index] = static_cast<double>(dxl_getdata);
}
Copy link
Contributor

@gemini-code-assist gemini-code-assist bot Jul 8, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

Using hardcoded strings like "Present Position", "Present Velocity", and "Present Current" can make the code harder to maintain and prone to typos. It's better to define these as static const string variables at a suitable scope (e.g., at the top of the file or in a shared header).

}
return DxlError::OK;
}
Expand Down Expand Up @@ -2021,19 +2039,39 @@ DxlError Dynamixel::SetDxlValueToBulkWrite()

for (uint16_t item_index = 0; item_index < direct_info_write_[comm_id].cnt; item_index++) {
double data = *it_write_data.item_data_ptr_vec.at(item_index);
uint8_t ID = comm_id;
Copy link
Contributor

@gemini-code-assist gemini-code-assist bot Jul 8, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

The ID variable is redundant as it's just a copy of comm_id. You can remove this line and use comm_id directly in the conversion functions below.

uint8_t size = direct_info_write_[comm_id].item_size.at(item_index);
if (size == 4) {
int32_t value = static_cast<int32_t>(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<int16_t>(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<uint8_t>(data);

if (direct_info_write_[comm_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 (direct_info_write_[comm_id].item_name.at(item_index) == "Goal Velocity") {
int32_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM(ID, 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 (direct_info_write_[comm_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 (size == 4) {
int32_t value = static_cast<int32_t>(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<int16_t>(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<uint8_t>(data);
}
}
Copy link
Contributor

@gemini-code-assist gemini-code-assist bot Jul 8, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

The strings "Goal Position", "Goal Velocity", and "Goal Current" are hardcoded. It's better to define these as static const string variables to avoid typos and improve maintainability.

added_byte += size;
}
Expand Down
Loading