From cc00486274f93c0d76d18e7d62b007843dd02852 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Mon, 6 Feb 2023 09:08:25 +0100 Subject: [PATCH 1/2] Fix API for bulkWrite to allow multiple different values and addresses. --- .../example-06-bulk-write.cpp | 14 +++-- include/dynamixel++/Dynamixel.hpp | 10 ++-- include/dynamixel++/Dynamixel.ipp | 34 ------------ src/Dynamixel.cpp | 55 ++++++++++++++----- 4 files changed, 54 insertions(+), 59 deletions(-) diff --git a/examples/example-06-bulk-write/example-06-bulk-write.cpp b/examples/example-06-bulk-write/example-06-bulk-write.cpp index f6a3492..b1ebaeb 100644 --- a/examples/example-06-bulk-write/example-06-bulk-write.cpp +++ b/examples/example-06-bulk-write/example-06-bulk-write.cpp @@ -107,20 +107,22 @@ catch (dynamixelplusplus::StatusError const & e) void turnLedOn(Dynamixel & dynamixel_ctrl, Dynamixel::IdVect const & id_vect) { - std::map led_on_data_map; + Dynamixel::BulkWriteDataVect led_on_data_vect; + uint8_t const LED_ON_VAL = 1; for (auto id : id_vect) - led_on_data_map[id] = 1; + led_on_data_vect.push_back(std::make_tuple(id, MX28_ControlTable_LED, LED_ON_VAL)); - dynamixel_ctrl.bulkWrite(MX28_ControlTable_LED, led_on_data_map); + dynamixel_ctrl.bulkWrite(led_on_data_vect); } void turnLedOff(Dynamixel & dynamixel_ctrl, Dynamixel::IdVect const & id_vect) { - std::map led_off_data_map; + Dynamixel::BulkWriteDataVect led_off_data_vect; + uint8_t const LED_OFF_VAL = 0; for (auto id : id_vect) - led_off_data_map[id] = 0; + led_off_data_vect.push_back(std::make_tuple(id, MX28_ControlTable_LED, LED_OFF_VAL)); - dynamixel_ctrl.bulkWrite(MX28_ControlTable_LED, led_off_data_map); + dynamixel_ctrl.bulkWrite(led_off_data_vect); } diff --git a/include/dynamixel++/Dynamixel.hpp b/include/dynamixel++/Dynamixel.hpp index 9103f0e..8708931 100644 --- a/include/dynamixel++/Dynamixel.hpp +++ b/include/dynamixel++/Dynamixel.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include @@ -75,7 +76,6 @@ class Dynamixel typedef uint8_t Id; typedef std::vector IdVect; - IdVect broadcastPing(); void reboot(Id const id); @@ -85,9 +85,12 @@ class Dynamixel template std::map syncRead(uint16_t const start_address, IdVect const & id_vect); template void write (uint16_t const start_address, Id const id, T const val); - template void bulkWrite(uint16_t const start_address, std::map const & val_map); template void syncWrite(uint16_t const start_address, std::map const & val_map); + typedef std::tuple> BulkWriteData; + typedef std::vector BulkWriteDataVect; + void bulkWrite(BulkWriteDataVect const & bulk_write_data); + private: std::unique_ptr _port_handler; @@ -96,9 +99,6 @@ class Dynamixel typedef std::tuple SyncWriteData; typedef std::vector SyncWriteDataVect; void syncWrite(uint16_t const start_address, uint16_t const data_length, SyncWriteDataVect const & data); - typedef SyncWriteData BulkWriteData; - typedef SyncWriteDataVect BulkWriteDataVect; - void bulkWrite(uint16_t const start_address, uint16_t const data_length, BulkWriteDataVect const & data); typedef std::vector void Dynamixel::write(uint16_t const start_address, Id cons throw StatusError(_packet_handler.get(), error & 0x7F); } -template void Dynamixel::bulkWrite(uint16_t const start_address, std::map const & val_map) -{ - static_assert(std::is_same::value || - std::is_same::value || - std::is_same::value, "Only uint8_t, uint16_t and uint32_t are allowed parameters."); - - /* Convert the functions input data into the required - * format to feed to the Dynamixel SDK. - */ - std::vector id_vect; - std::vector value_vect; - for (auto [id, val] : val_map) - { - id_vect.push_back(id); - value_vect.push_back(val); - } - - /* This 2-step dance is necessary because we need to pass a pointer - * to a local variable which still needs to exist in scope until - * bulkWrite has been fully executed. - */ - BulkWriteDataVect data_vect; - for (size_t i = 0; i < id_vect.size(); i++) - { - BulkWriteData const d = std::make_tuple(id_vect[i], reinterpret_cast(&value_vect[i])); - data_vect.push_back(d); - } - - /* Call the actual sync write API invoking the underlying - * DynamixelSDK sync write API. - */ - bulkWrite(start_address, sizeof(T), data_vect); -} - template void Dynamixel::syncWrite(uint16_t const start_address, std::map const & val_map) { static_assert(std::is_same::value || diff --git a/src/Dynamixel.cpp b/src/Dynamixel.cpp index d8d1591..264e7ca 100644 --- a/src/Dynamixel.cpp +++ b/src/Dynamixel.cpp @@ -72,6 +72,47 @@ void Dynamixel::reboot(Id const id) throw StatusError(_packet_handler.get(), error & 0x7F); } +void Dynamixel::bulkWrite(BulkWriteDataVect const & bulk_write_data) +{ + dynamixel::GroupBulkWrite group_bulk_write(_port_handler.get(), _packet_handler.get()); + + /* Unfortunately we need to copy and hold the data until the bulk write + * has been completed. This works best by adding it temporarily to some + * std::vector. + */ + std::vector> tmp_val_uint8_t; + std::vector> tmp_val_uint16_t; + std::vector> tmp_val_uint32_t; + + for(auto [id, start_address, value] : bulk_write_data) + { + if (std::holds_alternative(value)) + { + auto tmp_val = std::make_shared(std::get(value)); + tmp_val_uint8_t.push_back(tmp_val); + group_bulk_write.addParam(id, start_address, sizeof(uint8_t), tmp_val.get()); + } + else if (std::holds_alternative(value)) + { + auto tmp_val = std::make_shared(std::get(value)); + tmp_val_uint16_t.push_back(tmp_val); + group_bulk_write.addParam(id, start_address, sizeof(uint16_t), reinterpret_cast(tmp_val.get())); + } + else if (std::holds_alternative(value)) + { + auto tmp_val = std::make_shared(std::get(value)); + tmp_val_uint32_t.push_back(tmp_val); + group_bulk_write.addParam(id, start_address, sizeof(uint32_t), reinterpret_cast(tmp_val.get())); + } + } + + if (int const res = group_bulk_write.txPacket(); + res != COMM_SUCCESS) { + throw CommunicationError(_packet_handler.get(), res); + } + group_bulk_write.clearParam(); +} + /************************************************************************************** * PRIVATE MEMBER FUNCTIONS **************************************************************************************/ @@ -90,20 +131,6 @@ void Dynamixel::syncWrite(uint16_t const start_address, uint16_t const data_leng group_sync_write.clearParam(); } -void Dynamixel::bulkWrite(uint16_t const start_address, uint16_t const data_length, BulkWriteDataVect const & data) -{ - dynamixel::GroupBulkWrite group_bulk_write(_port_handler.get(), _packet_handler.get()); - - for(auto [id, data_ptr] : data) - group_bulk_write.addParam(id, start_address, data_length, data_ptr); - - if (int const res = group_bulk_write.txPacket(); - res != COMM_SUCCESS) { - throw CommunicationError(_packet_handler.get(), res); - } - group_bulk_write.clearParam(); -} - Dynamixel::SyncReadDataVect Dynamixel::syncRead(uint16_t const start_address, uint16_t const data_length, IdVect const & id_vect) { SyncReadDataVect data_vect; From 1e8199db7e16ce085eba5dc6c40e304588528b0a Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Mon, 6 Feb 2023 09:25:03 +0100 Subject: [PATCH 2/2] Fix: Bulk read allows to read from different servos and different addresses. --- .../example-03-bulk-read.cpp | 10 ++- include/dynamixel++/Dynamixel.hpp | 16 ++-- include/dynamixel++/Dynamixel.ipp | 18 ----- src/Dynamixel.cpp | 81 ++++++++++--------- 4 files changed, 58 insertions(+), 67 deletions(-) diff --git a/examples/example-03-bulk-read/example-03-bulk-read.cpp b/examples/example-03-bulk-read/example-03-bulk-read.cpp index 1818ca5..829da67 100644 --- a/examples/example-03-bulk-read/example-03-bulk-read.cpp +++ b/examples/example-03-bulk-read/example-03-bulk-read.cpp @@ -58,10 +58,14 @@ int main(int argc, char **argv) try /* Read the current angle from all those servos * and print it to std::cout. */ - std::map position_map; + Dynamixel::BulkReadDataMap position_map; try { - position_map = dynamixel_ctrl.bulkRead(MX28_ControlTable_PresentPosition, id_vect); + Dynamixel::BulkReadRequestVect bulk_read_req; + for (auto id : id_vect) + bulk_read_req.push_back(std::make_tuple(id, MX28_ControlTable_PresentPosition, sizeof(uint32_t))); + + position_map = dynamixel_ctrl.bulkRead(bulk_read_req); } catch (dynamixelplusplus::HardwareAlert const & e) { @@ -74,7 +78,7 @@ int main(int argc, char **argv) try for (auto [id, position_raw] : position_map) { - float const position_deg = static_cast(position_raw) * 360.0f / 4096; + float const position_deg = static_cast(std::get(position_raw)) * 360.0f / 4096; std::cout << "Dynamixel MX28 servo #" << static_cast(id) << ": " diff --git a/include/dynamixel++/Dynamixel.hpp b/include/dynamixel++/Dynamixel.hpp index 8708931..b6069b1 100644 --- a/include/dynamixel++/Dynamixel.hpp +++ b/include/dynamixel++/Dynamixel.hpp @@ -80,17 +80,21 @@ class Dynamixel void reboot(Id const id); + + typedef std::map> BulkReadDataMap; + typedef std::vector> BulkReadRequestVect; + BulkReadDataMap bulkRead(BulkReadRequestVect const & bulk_read_req); + + typedef std::vector>> BulkWriteDataVect; + void bulkWrite(BulkWriteDataVect const & bulk_write_data); + + template T read (uint16_t const start_address, Id const id); - template std::map bulkRead(uint16_t const start_address, IdVect const & id_vect); template std::map syncRead(uint16_t const start_address, IdVect const & id_vect); template void write (uint16_t const start_address, Id const id, T const val); template void syncWrite(uint16_t const start_address, std::map const & val_map); - typedef std::tuple> BulkWriteData; - typedef std::vector BulkWriteDataVect; - void bulkWrite(BulkWriteDataVect const & bulk_write_data); - private: std::unique_ptr _port_handler; @@ -105,9 +109,7 @@ class Dynamixel std::optional > > SyncReadDataVect; - typedef SyncReadDataVect BulkReadDataVect; SyncReadDataVect syncRead(uint16_t const start_address, uint16_t const data_length, IdVect const & id_vect); - BulkReadDataVect bulkRead(uint16_t const start_address, uint16_t const data_length, IdVect const & id_vect); }; /************************************************************************************** diff --git a/include/dynamixel++/Dynamixel.ipp b/include/dynamixel++/Dynamixel.ipp index 2aff327..d5cbe48 100644 --- a/include/dynamixel++/Dynamixel.ipp +++ b/include/dynamixel++/Dynamixel.ipp @@ -71,24 +71,6 @@ template T Dynamixel::read(uint16_t const start_address, Id const id return val; } -template std::map Dynamixel::bulkRead(uint16_t const start_address, IdVect const & id_vect) -{ - static_assert(std::is_same::value || - std::is_same::value || - std::is_same::value, "Only uint8_t, uint16_t and uint32_t are allowed parameters."); - - auto bulk_read_data_vect = bulkRead(start_address, sizeof(T), id_vect); - - std::map val_map; - for (auto [id, opt_data] : bulk_read_data_vect) - { - if (opt_data.has_value()) - val_map[id] = static_cast(opt_data.value()); - } - - return val_map; -} - template std::map Dynamixel::syncRead(uint16_t const start_address, IdVect const & id_vect) { static_assert(std::is_same::value || diff --git a/src/Dynamixel.cpp b/src/Dynamixel.cpp index 264e7ca..68ca74b 100644 --- a/src/Dynamixel.cpp +++ b/src/Dynamixel.cpp @@ -72,6 +72,48 @@ void Dynamixel::reboot(Id const id) throw StatusError(_packet_handler.get(), error & 0x7F); } +Dynamixel::BulkReadDataMap Dynamixel::bulkRead(BulkReadRequestVect const & bulk_read_req) +{ + BulkReadDataMap data_map; + + dynamixel::GroupBulkRead group_bulk_read(_port_handler.get(), _packet_handler.get()); + + for(auto [id, start_address, data_length] : bulk_read_req) + group_bulk_read.addParam(id, start_address, data_length); + + if (int const res = group_bulk_read.txRxPacket(); + res != COMM_SUCCESS) { + throw CommunicationError(_packet_handler.get(), res); + } + + for(auto [id, start_address, data_length] : bulk_read_req) + { + uint8_t dxl_error = 0; + + if (group_bulk_read.getError(id, &dxl_error)) + { + if (dxl_error & 0x80) + throw HardwareAlert(id); + else if(dxl_error & 0x7F) + throw StatusError(_packet_handler.get(), dxl_error & 0x7F); + } + + if (group_bulk_read.isAvailable(id, start_address, data_length)) + { + if (data_length == 1) + data_map[id] = static_cast(group_bulk_read.getData(id, start_address, data_length)); + else if (data_length == 2) + data_map[id] = static_cast(group_bulk_read.getData(id, start_address, data_length)); + else if (data_length == 4) + data_map[id] = static_cast(group_bulk_read.getData(id, start_address, data_length)); + } + } + + group_bulk_read.clearParam(); + + return data_map; +} + void Dynamixel::bulkWrite(BulkWriteDataVect const & bulk_write_data) { dynamixel::GroupBulkWrite group_bulk_write(_port_handler.get(), _packet_handler.get()); @@ -170,45 +212,6 @@ Dynamixel::SyncReadDataVect Dynamixel::syncRead(uint16_t const start_address, ui return data_vect; } -Dynamixel::BulkReadDataVect Dynamixel::bulkRead(uint16_t const start_address, uint16_t const data_length, IdVect const & id_vect) -{ - BulkReadDataVect data_vect; - - dynamixel::GroupBulkRead group_bulk_read(_port_handler.get(), _packet_handler.get()); - - for(auto id : id_vect) - group_bulk_read.addParam(id, start_address, data_length); - - if (int const res = group_bulk_read.txRxPacket(); - res != COMM_SUCCESS) { - throw CommunicationError(_packet_handler.get(), res); - } - - for(auto id : id_vect) - { - uint8_t dxl_error = 0; - if (group_bulk_read.getError(id, &dxl_error)) - { - if (dxl_error & 0x80) - throw HardwareAlert(id); - else if(dxl_error & 0x7F) - throw StatusError(_packet_handler.get(), dxl_error & 0x7F); - } - } - - for(auto id : id_vect) - { - if (group_bulk_read.isAvailable(id, start_address, data_length)) - data_vect.push_back(std::make_tuple(id, group_bulk_read.getData(id, start_address, data_length))); - else - data_vect.push_back(std::make_tuple(id, std::nullopt)); - } - - group_bulk_read.clearParam(); - - return data_vect; -} - /************************************************************************************** * NAMESPACE **************************************************************************************/