Skip to content

Commit 8cb952d

Browse files
Woojin-CriveWoojin-Crive
committed
feat: Refactor read functions to accept period_ms for improved timeout control
- Updated ReadMultiDxlData, GetDxlValueFromSyncRead, and GetDxlValueFromBulkRead to accept a period_ms parameter. - Introduced ProcessReadCommunication and ProcessReadData functions for better data handling and communication management. - Enhanced error handling and logging for read operations.
1 parent 5283416 commit 8cb952d

File tree

3 files changed

+147
-66
lines changed

3 files changed

+147
-66
lines changed

‎include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp‎

Lines changed: 21 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727
#include <iostream>
2828
#include <cstdarg>
2929
#include <memory>
30+
#include <functional>
3031

3132
namespace dynamixel_hardware_interface
3233
{
@@ -166,7 +167,7 @@ class Dynamixel
166167
DxlError SetMultiDxlWrite();
167168

168169
// Read Item (sync or bulk)
169-
DxlError ReadMultiDxlData();
170+
DxlError ReadMultiDxlData(double period_ms);
170171
// Write Item (sync or bulk)
171172
DxlError WriteMultiDxlData();
172173

@@ -200,12 +201,12 @@ class Dynamixel
200201
// SyncRead
201202
DxlError SetSyncReadItemAndHandler();
202203
DxlError SetSyncReadHandler(std::vector<uint8_t> id_arr);
203-
DxlError GetDxlValueFromSyncRead();
204+
DxlError GetDxlValueFromSyncRead(double period_ms);
204205

205206
// BulkRead
206207
DxlError SetBulkReadItemAndHandler();
207208
DxlError SetBulkReadHandler(std::vector<uint8_t> id_arr);
208-
DxlError GetDxlValueFromBulkRead();
209+
DxlError GetDxlValueFromBulkRead(double period_ms);
209210

210211
// Read - Indirect Address
211212
void ResetIndirectRead(std::vector<uint8_t> id_arr);
@@ -215,6 +216,23 @@ class Dynamixel
215216
uint16_t item_addr,
216217
uint8_t item_size);
217218

219+
// Read - Data Processing
220+
DxlError ProcessReadData(
221+
uint8_t id,
222+
uint16_t indirect_addr,
223+
const std::vector<std::string> & item_names,
224+
const std::vector<uint8_t> & item_sizes,
225+
const std::vector<std::shared_ptr<double>> & data_ptrs,
226+
std::function<uint32_t(uint8_t, uint16_t, uint8_t)> get_data_func);
227+
228+
// Read - Communication
229+
DxlError ProcessReadCommunication(
230+
dynamixel::GroupFastSyncRead * group_sync_read,
231+
dynamixel::GroupFastBulkRead * group_bulk_read,
232+
dynamixel::PortHandler * port_handler,
233+
double period_ms,
234+
bool is_sync);
235+
218236
// SyncWrite
219237
DxlError SetSyncWriteItemAndHandler();
220238
DxlError SetSyncWriteHandler(std::vector<uint8_t> id_arr);

‎src/dynamixel/dynamixel.cpp‎

Lines changed: 122 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include <vector>
2121
#include <string>
2222
#include <memory>
23+
#include <functional>
2324

2425
namespace dynamixel_hardware_interface
2526
{
@@ -637,12 +638,12 @@ std::string Dynamixel::DxlErrorToString(DxlError error_num)
637638
}
638639
}
639640

640-
DxlError Dynamixel::ReadMultiDxlData()
641+
DxlError Dynamixel::ReadMultiDxlData(double period_ms)
641642
{
642643
if (read_type_ == SYNC) {
643-
return GetDxlValueFromSyncRead();
644+
return GetDxlValueFromSyncRead(period_ms);
644645
} else {
645-
return GetDxlValueFromBulkRead();
646+
return GetDxlValueFromBulkRead(period_ms);
646647
}
647648
}
648649

@@ -814,39 +815,27 @@ DxlError Dynamixel::SetSyncReadHandler(std::vector<uint8_t> id_arr)
814815
return DxlError::OK;
815816
}
816817

817-
DxlError Dynamixel::GetDxlValueFromSyncRead()
818+
DxlError Dynamixel::GetDxlValueFromSyncRead(double period_ms)
818819
{
819-
// SyncRead tx
820-
int dxl_comm_result = group_sync_read_->txRxPacket();
821-
if (dxl_comm_result != COMM_SUCCESS) {
822-
fprintf(stderr, "SyncRead TxRx Fail [Dxl Size : %d] [Error code : %d]\n", read_data_list_.size(), dxl_comm_result);
823-
return DxlError::SYNC_READ_FAIL;
820+
DxlError comm_result = ProcessReadCommunication(
821+
group_sync_read_, group_bulk_read_, port_handler_, period_ms, true);
822+
if (comm_result != DxlError::OK) {
823+
return comm_result;
824824
}
825825

826826
for (auto it_read_data : read_data_list_) {
827-
uint8_t ID = it_read_data.id;
828-
uint16_t IN_ADDR = indirect_info_read_[ID].indirect_data_addr;
829-
830-
for (size_t item_index = 0; item_index < indirect_info_read_[ID].cnt; item_index++) {
831-
uint8_t SIZE = indirect_info_read_[ID].item_size.at(item_index);
832-
if (item_index > 0) {IN_ADDR += indirect_info_read_[ID].item_size.at(item_index - 1);}
833-
834-
uint32_t dxl_getdata = group_sync_read_->getData(ID, IN_ADDR, SIZE);
835-
836-
if (indirect_info_read_[ID].item_name.at(item_index) == "Present Position") {
837-
*it_read_data.item_data_ptr_vec.at(item_index) =
838-
dxl_info_.ConvertValueToRadian(ID, static_cast<int32_t>(dxl_getdata));
839-
} else if (indirect_info_read_[ID].item_name.at(item_index) == "Present Velocity") {
840-
*it_read_data.item_data_ptr_vec.at(item_index) =
841-
dxl_info_.ConvertValueRPMToVelocityRPS(ID, static_cast<int32_t>(dxl_getdata));
842-
} else if (indirect_info_read_[ID].item_name.at(item_index) == "Present Current") {
843-
*it_read_data.item_data_ptr_vec.at(item_index) =
844-
dxl_info_.ConvertCurrentToEffort(ID, static_cast<int16_t>(dxl_getdata));
845-
} else {
846-
*it_read_data.item_data_ptr_vec.at(item_index) =
847-
static_cast<double>(dxl_getdata);
848-
}
849-
}
827+
uint8_t id = it_read_data.id;
828+
uint16_t indirect_addr = indirect_info_read_[id].indirect_data_addr;
829+
830+
ProcessReadData(
831+
id,
832+
indirect_addr,
833+
indirect_info_read_[id].item_name,
834+
indirect_info_read_[id].item_size,
835+
it_read_data.item_data_ptr_vec,
836+
[this](uint8_t id, uint16_t addr, uint8_t size) {
837+
return group_sync_read_->getData(id, addr, size);
838+
});
850839
}
851840
return DxlError::OK;
852841
}
@@ -888,7 +877,7 @@ DxlError Dynamixel::SetBulkReadItemAndHandler()
888877

889878
if (SetBulkReadHandler(id_arr) != DxlError::OK) {
890879
fprintf(stderr, "Cannot set the BulkRead handler.\n");
891-
return DxlError::SYNC_READ_FAIL;
880+
return DxlError::BULK_READ_FAIL;
892881
}
893882

894883
fprintf(stderr, "Success to set BulkRead handler using indirect address\n");
@@ -937,42 +926,115 @@ DxlError Dynamixel::SetBulkReadHandler(std::vector<uint8_t> id_arr)
937926
return DxlError::OK;
938927
}
939928

940-
DxlError Dynamixel::GetDxlValueFromBulkRead()
929+
DxlError Dynamixel::GetDxlValueFromBulkRead(double period_ms)
941930
{
942-
int dxl_comm_result = group_bulk_read_->txRxPacket();
943-
if (dxl_comm_result != COMM_SUCCESS) {
944-
fprintf(stderr, "BulkRead TxRx Fail [Dxl Size : %d] [Error code : %d]\n", read_data_list_.size(), dxl_comm_result);
945-
return DxlError::BULK_READ_FAIL;
931+
DxlError comm_result = ProcessReadCommunication(
932+
group_sync_read_, group_bulk_read_, port_handler_, period_ms, false);
933+
if (comm_result != DxlError::OK) {
934+
return comm_result;
946935
}
947936

948937
for (auto it_read_data : read_data_list_) {
949-
uint8_t ID = it_read_data.id;
950-
uint16_t IN_ADDR = indirect_info_read_[ID].indirect_data_addr;
951-
952-
for (size_t item_index = 0; item_index < indirect_info_read_[ID].cnt; item_index++) {
953-
uint8_t SIZE = indirect_info_read_[ID].item_size.at(item_index);
954-
if (item_index > 0) {IN_ADDR += indirect_info_read_[ID].item_size.at(item_index - 1);}
955-
956-
uint32_t dxl_getdata = group_bulk_read_->getData(ID, IN_ADDR, SIZE);
957-
958-
if (indirect_info_read_[ID].item_name.at(item_index) == "Present Position") {
959-
*it_read_data.item_data_ptr_vec.at(item_index) =
960-
dxl_info_.ConvertValueToRadian(ID, static_cast<int32_t>(dxl_getdata));
961-
} else if (indirect_info_read_[ID].item_name.at(item_index) == "Present Velocity") {
962-
*it_read_data.item_data_ptr_vec.at(item_index) =
963-
dxl_info_.ConvertValueRPMToVelocityRPS(ID, static_cast<int32_t>(dxl_getdata));
964-
} else if (indirect_info_read_[ID].item_name.at(item_index) == "Present Current") {
965-
*it_read_data.item_data_ptr_vec.at(item_index) =
966-
dxl_info_.ConvertCurrentToEffort(ID, static_cast<int16_t>(dxl_getdata));
967-
} else {
968-
*it_read_data.item_data_ptr_vec.at(item_index) =
969-
static_cast<double>(dxl_getdata);
970-
}
938+
uint8_t id = it_read_data.id;
939+
uint16_t indirect_addr = indirect_info_read_[id].indirect_data_addr;
940+
941+
ProcessReadData(
942+
id,
943+
indirect_addr,
944+
indirect_info_read_[id].item_name,
945+
indirect_info_read_[id].item_size,
946+
it_read_data.item_data_ptr_vec,
947+
[this](uint8_t id, uint16_t addr, uint8_t size) {
948+
return group_bulk_read_->getData(id, addr, size);
949+
});
950+
}
951+
return DxlError::OK;
952+
}
953+
954+
DxlError Dynamixel::ProcessReadCommunication(
955+
dynamixel::GroupFastSyncRead * group_sync_read,
956+
dynamixel::GroupFastBulkRead * group_bulk_read,
957+
dynamixel::PortHandler * port_handler,
958+
double period_ms,
959+
bool is_sync)
960+
{
961+
int dxl_comm_result;
962+
963+
// Send packet
964+
if (is_sync) {
965+
dxl_comm_result = group_sync_read->txPacket();
966+
if (dxl_comm_result != COMM_SUCCESS) {
967+
fprintf(stderr, "SyncRead Tx Fail [Dxl Size : %d] [Error code : %d]\n",
968+
read_data_list_.size(), dxl_comm_result);
969+
return DxlError::SYNC_READ_FAIL;
970+
}
971+
} else {
972+
dxl_comm_result = group_bulk_read->txPacket();
973+
if (dxl_comm_result != COMM_SUCCESS) {
974+
fprintf(stderr, "BulkRead Tx Fail [Dxl Size : %d] [Error code : %d]\n",
975+
read_data_list_.size(), dxl_comm_result);
976+
return DxlError::BULK_READ_FAIL;
977+
}
978+
}
979+
980+
// Set timeout if period_ms is specified
981+
if (period_ms > 0) {
982+
port_handler->setPacketTimeout(period_ms);
983+
}
984+
985+
// Receive packet
986+
if (is_sync) {
987+
dxl_comm_result = group_sync_read->rxPacket();
988+
if (dxl_comm_result != COMM_SUCCESS) {
989+
fprintf(stderr, "SyncRead Rx Fail [Dxl Size : %d] [Error code : %d]\n",
990+
read_data_list_.size(), dxl_comm_result);
991+
return DxlError::SYNC_READ_FAIL;
992+
}
993+
} else {
994+
dxl_comm_result = group_bulk_read->rxPacket();
995+
if (dxl_comm_result != COMM_SUCCESS) {
996+
fprintf(stderr, "BulkRead Rx Fail [Dxl Size : %d] [Error code : %d]\n",
997+
read_data_list_.size(), dxl_comm_result);
998+
return DxlError::BULK_READ_FAIL;
971999
}
9721000
}
1001+
9731002
return DxlError::OK;
9741003
}
9751004

1005+
DxlError Dynamixel::ProcessReadData(
1006+
uint8_t id,
1007+
uint16_t indirect_addr,
1008+
const std::vector<std::string> & item_names,
1009+
const std::vector<uint8_t> & item_sizes,
1010+
const std::vector<std::shared_ptr<double>> & data_ptrs,
1011+
std::function<uint32_t(uint8_t, uint16_t, uint8_t)> get_data_func)
1012+
{
1013+
uint16_t current_addr = indirect_addr;
1014+
1015+
for (size_t item_index = 0; item_index < item_names.size(); item_index++) {
1016+
uint8_t size = item_sizes[item_index];
1017+
if (item_index > 0) {current_addr += item_sizes[item_index - 1];}
1018+
1019+
uint32_t dxl_getdata = get_data_func(id, current_addr, size);
1020+
1021+
if (item_names[item_index] == "Present Position") {
1022+
*data_ptrs[item_index] = dxl_info_.ConvertValueToRadian(id,
1023+
static_cast<int32_t>(dxl_getdata));
1024+
} else if (item_names[item_index] == "Present Velocity") {
1025+
*data_ptrs[item_index] = dxl_info_.ConvertValueRPMToVelocityRPS(id,
1026+
static_cast<int32_t>(dxl_getdata));
1027+
} else if (item_names[item_index] == "Present Current") {
1028+
*data_ptrs[item_index] = dxl_info_.ConvertCurrentToEffort(id,
1029+
static_cast<int16_t>(dxl_getdata));
1030+
} else {
1031+
*data_ptrs[item_index] = static_cast<double>(dxl_getdata);
1032+
}
1033+
}
1034+
return DxlError::OK;
1035+
}
1036+
1037+
9761038
void Dynamixel::ResetIndirectRead(std::vector<uint8_t> id_arr)
9771039
{
9781040
IndirectInfo temp;

‎src/dynamixel_hardware_interface.cpp‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -378,7 +378,7 @@ hardware_interface::CallbackReturn DynamixelHardware::on_deactivate(
378378

379379
hardware_interface::CallbackReturn DynamixelHardware::start()
380380
{
381-
dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData());
381+
dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData(0.0));
382382
if (dxl_comm_err_ != DxlError::OK) {
383383
RCLCPP_ERROR_STREAM(
384384
logger_,
@@ -427,11 +427,12 @@ hardware_interface::CallbackReturn DynamixelHardware::stop()
427427
hardware_interface::return_type DynamixelHardware::read(
428428
const rclcpp::Time & time, const rclcpp::Duration & period)
429429
{
430+
double period_ms = period.seconds() * 1000;
430431
if (dxl_status_ == REBOOTING) {
431432
RCLCPP_ERROR_STREAM(logger_, "Dynamixel Read Fail : REBOOTING");
432433
return hardware_interface::return_type::ERROR;
433434
} else if (dxl_status_ == DXL_OK || dxl_status_ == COMM_ERROR) {
434-
dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData());
435+
dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData(period_ms));
435436
if (dxl_comm_err_ != DxlError::OK) {
436437
if (!is_read_in_error_) {
437438
is_read_in_error_ = true;
@@ -452,7 +453,7 @@ hardware_interface::return_type DynamixelHardware::read(
452453
is_read_in_error_ = false;
453454
read_error_duration_ = rclcpp::Duration(0, 0);
454455
} else if (dxl_status_ == HW_ERROR) {
455-
dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData());
456+
dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData(period_ms));
456457
if (dxl_comm_err_ != DxlError::OK) {
457458
RCLCPP_ERROR_STREAM(
458459
logger_,

0 commit comments

Comments
 (0)