From da576e1a8a3fa12a1316bbec59491e720cd0fba2 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 7 Apr 2026 17:54:26 +0900 Subject: [PATCH 01/10] apply raii code style --- c++/include/dynamixel_sdk/group_bulk_read.h | 4 +- .../dynamixel_sdk/group_fast_bulk_read.h | 1 + .../dynamixel_sdk/group_fast_sync_read.h | 3 + c++/include/dynamixel_sdk/group_handler.h | 4 +- c++/include/dynamixel_sdk/group_sync_read.h | 4 +- c++/include/dynamixel_sdk/packet_handler.h | 12 +- c++/src/dynamixel_sdk/group_bulk_read.cpp | 51 ++--- c++/src/dynamixel_sdk/group_bulk_write.cpp | 49 ++-- .../dynamixel_sdk/group_fast_bulk_read.cpp | 31 ++- .../dynamixel_sdk/group_fast_sync_read.cpp | 13 +- c++/src/dynamixel_sdk/group_handler.cpp | 5 +- c++/src/dynamixel_sdk/group_sync_read.cpp | 33 +-- c++/src/dynamixel_sdk/group_sync_write.cpp | 41 ++-- c++/src/dynamixel_sdk/packet_handler.cpp | 6 +- c++/src/dynamixel_sdk/port_handler.cpp | 8 +- .../dynamixel_sdk/port_handler_arduino.cpp | 10 +- c++/src/dynamixel_sdk/port_handler_linux.cpp | 10 +- c++/src/dynamixel_sdk/port_handler_mac.cpp | 10 +- .../dynamixel_sdk/port_handler_windows.cpp | 14 +- .../protocol1_packet_handler.cpp | 140 ++++++------ .../protocol2_packet_handler.cpp | 213 ++++++++---------- 21 files changed, 287 insertions(+), 375 deletions(-) diff --git a/c++/include/dynamixel_sdk/group_bulk_read.h b/c++/include/dynamixel_sdk/group_bulk_read.h index f2041e45a..b55ae316c 100644 --- a/c++/include/dynamixel_sdk/group_bulk_read.h +++ b/c++/include/dynamixel_sdk/group_bulk_read.h @@ -38,7 +38,7 @@ class WINDECLSPEC GroupBulkRead : public GroupHandler protected: std::map address_list_; // std::map length_list_; // - std::map error_list_; // + std::map> error_list_; // bool last_result_; @@ -135,7 +135,7 @@ class WINDECLSPEC GroupBulkRead : public GroupHandler /// @error error of Dynamixel /// @return true /// @return when Dynamixel returned specific error byte - /// @return or false + /// @return or false //////////////////////////////////////////////////////////////////////////////// bool getError (uint8_t id, uint8_t* error); }; diff --git a/c++/include/dynamixel_sdk/group_fast_bulk_read.h b/c++/include/dynamixel_sdk/group_fast_bulk_read.h index 78ffe2b4a..7045a6f11 100644 --- a/c++/include/dynamixel_sdk/group_fast_bulk_read.h +++ b/c++/include/dynamixel_sdk/group_fast_bulk_read.h @@ -40,6 +40,7 @@ class WINDECLSPEC GroupFastBulkRead : public GroupBulkRead private: void makeParam(); + std::vector rxpacket_; }; } diff --git a/c++/include/dynamixel_sdk/group_fast_sync_read.h b/c++/include/dynamixel_sdk/group_fast_sync_read.h index a5b56a07c..450153aeb 100644 --- a/c++/include/dynamixel_sdk/group_fast_sync_read.h +++ b/c++/include/dynamixel_sdk/group_fast_sync_read.h @@ -37,6 +37,9 @@ class WINDECLSPEC GroupFastSyncRead : public GroupSyncRead int txPacket(); int rxPacket(); int txRxPacket(); + +private: + std::vector rxpacket_; }; } diff --git a/c++/include/dynamixel_sdk/group_handler.h b/c++/include/dynamixel_sdk/group_handler.h index 1ad5e711e..1c543ee06 100644 --- a/c++/include/dynamixel_sdk/group_handler.h +++ b/c++/include/dynamixel_sdk/group_handler.h @@ -44,11 +44,11 @@ class WINDECLSPEC GroupHandler PacketHandler *ph_; std::vector id_list_; - std::map data_list_; // + std::map> data_list_; // bool is_param_changed_; - uint8_t *param_; + std::vector param_; }; } diff --git a/c++/include/dynamixel_sdk/group_sync_read.h b/c++/include/dynamixel_sdk/group_sync_read.h index 19a36dd56..58b83d4db 100644 --- a/c++/include/dynamixel_sdk/group_sync_read.h +++ b/c++/include/dynamixel_sdk/group_sync_read.h @@ -36,7 +36,7 @@ namespace dynamixel class WINDECLSPEC GroupSyncRead : public GroupHandler { protected: - std::map error_list_; // + std::map> error_list_; // bool last_result_; @@ -140,7 +140,7 @@ class WINDECLSPEC GroupSyncRead : public GroupHandler /// @error error of Dynamixel /// @return true /// @return when Dynamixel returned specific error byte - /// @return or false + /// @return or false //////////////////////////////////////////////////////////////////////////////// bool getError (uint8_t id, uint8_t* error); }; diff --git a/c++/include/dynamixel_sdk/packet_handler.h b/c++/include/dynamixel_sdk/packet_handler.h index f3b7e4fb3..da8b2856e 100644 --- a/c++/include/dynamixel_sdk/packet_handler.h +++ b/c++/include/dynamixel_sdk/packet_handler.h @@ -39,12 +39,12 @@ #define MAX_ID 0xFC // 252 /* Macro for Control Table Value */ -#define DXL_MAKEWORD(a, b) ((uint16_t)(((uint8_t)(((uint64_t)(a)) & 0xff)) | ((uint16_t)((uint8_t)(((uint64_t)(b)) & 0xff))) << 8)) -#define DXL_MAKEDWORD(a, b) ((uint32_t)(((uint16_t)(((uint64_t)(a)) & 0xffff)) | ((uint32_t)((uint16_t)(((uint64_t)(b)) & 0xffff))) << 16)) -#define DXL_LOWORD(l) ((uint16_t)(((uint64_t)(l)) & 0xffff)) -#define DXL_HIWORD(l) ((uint16_t)((((uint64_t)(l)) >> 16) & 0xffff)) -#define DXL_LOBYTE(w) ((uint8_t)(((uint64_t)(w)) & 0xff)) -#define DXL_HIBYTE(w) ((uint8_t)((((uint64_t)(w)) >> 8) & 0xff)) +#define DXL_MAKEWORD(a, b) (static_cast((static_cast(static_cast(a) & 0xff)) | (static_cast(static_cast(static_cast(b) & 0xff))) << 8)) +#define DXL_MAKEDWORD(a, b) (static_cast((static_cast(static_cast(a) & 0xffff)) | (static_cast(static_cast(static_cast(b) & 0xffff))) << 16)) +#define DXL_LOWORD(l) (static_cast(static_cast(l) & 0xffff)) +#define DXL_HIWORD(l) (static_cast((static_cast(l) >> 16) & 0xffff)) +#define DXL_LOBYTE(w) (static_cast(static_cast(w) & 0xff)) +#define DXL_HIBYTE(w) (static_cast((static_cast(w) >> 8) & 0xff)) /* Instruction for DXL Protocol */ #define INST_PING 1 diff --git a/c++/src/dynamixel_sdk/group_bulk_read.cpp b/c++/src/dynamixel_sdk/group_bulk_read.cpp index 32feefaa3..ca464b682 100644 --- a/c++/src/dynamixel_sdk/group_bulk_read.cpp +++ b/c++/src/dynamixel_sdk/group_bulk_read.cpp @@ -41,39 +41,36 @@ GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph) void GroupBulkRead::makeParam() { - if (id_list_.size() == 0) + if (id_list_.empty()) return; - if (param_ != 0) - delete[] param_; - param_ = 0; + param_.clear(); if (ph_->getProtocolVersion() == 1.0) { - param_ = new uint8_t[id_list_.size() * 3]; // ID(1) + ADDR(1) + LENGTH(1) + param_.reserve(id_list_.size() * 3); // ID(1) + ADDR(1) + LENGTH(1) } else // 2.0 { - param_ = new uint8_t[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) + param_.reserve(id_list_.size() * 5); // ID(1) + ADDR(2) + LENGTH(2) } - int idx = 0; for (unsigned int i = 0; i < id_list_.size(); i++) { uint8_t id = id_list_[i]; if (ph_->getProtocolVersion() == 1.0) { - param_[idx++] = (uint8_t)length_list_[id]; // LEN - param_[idx++] = id; // ID - param_[idx++] = (uint8_t)address_list_[id]; // ADDR + param_.push_back(static_cast(length_list_[id])); // LEN + param_.push_back(id); // ID + param_.push_back(static_cast(address_list_[id])); // ADDR } else // 2.0 { - param_[idx++] = id; // ID - param_[idx++] = DXL_LOBYTE(address_list_[id]); // ADDR_L - param_[idx++] = DXL_HIBYTE(address_list_[id]); // ADDR_H - param_[idx++] = DXL_LOBYTE(length_list_[id]); // LEN_L - param_[idx++] = DXL_HIBYTE(length_list_[id]); // LEN_H + param_.push_back(id); // ID + param_.push_back(DXL_LOBYTE(address_list_[id])); // ADDR_L + param_.push_back(DXL_HIBYTE(address_list_[id])); // ADDR_H + param_.push_back(DXL_LOBYTE(length_list_[id])); // LEN_L + param_.push_back(DXL_HIBYTE(length_list_[id])); // LEN_H } } } @@ -86,8 +83,8 @@ bool GroupBulkRead::addParam(uint8_t id, uint16_t start_address, uint16_t data_l id_list_.push_back(id); length_list_[id] = data_length; address_list_[id] = start_address; - data_list_[id] = new uint8_t[data_length]; - error_list_[id] = new uint8_t[1]; + data_list_[id] = std::vector(data_length); + error_list_[id] = std::vector(1); is_param_changed_ = true; return true; @@ -102,8 +99,6 @@ void GroupBulkRead::removeParam(uint8_t id) id_list_.erase(it); address_list_.erase(id); length_list_.erase(id); - delete[] data_list_[id]; - delete[] error_list_[id]; data_list_.erase(id); error_list_.erase(id); @@ -115,20 +110,12 @@ void GroupBulkRead::clearParam() if (id_list_.size() == 0) return; - for (unsigned int i = 0; i < id_list_.size(); i++) - { - delete[] data_list_[id_list_[i]]; - delete[] error_list_[id_list_[i]]; - } - id_list_.clear(); address_list_.clear(); length_list_.clear(); data_list_.clear(); error_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; + param_.clear(); } int GroupBulkRead::txPacket() @@ -136,16 +123,16 @@ int GroupBulkRead::txPacket() if (id_list_.size() == 0) return COMM_NOT_AVAILABLE; - if (is_param_changed_ == true || param_ == 0) + if (is_param_changed_ == true || param_.empty()) makeParam(); if (ph_->getProtocolVersion() == 1.0) { - return ph_->bulkReadTx(port_, param_, id_list_.size() * 3); + return ph_->bulkReadTx(port_, param_.data(), id_list_.size() * 3); } else // 2.0 { - return ph_->bulkReadTx(port_, param_, id_list_.size() * 5); + return ph_->bulkReadTx(port_, param_.data(), id_list_.size() * 5); } } @@ -163,7 +150,7 @@ int GroupBulkRead::rxPacket() { uint8_t id = id_list_[i]; - result = ph_->readRx(port_, id, length_list_[id], data_list_[id], error_list_[id]); + result = ph_->readRx(port_, id, length_list_[id], data_list_[id].data(), error_list_[id].data()); if (result != COMM_SUCCESS) return result; } diff --git a/c++/src/dynamixel_sdk/group_bulk_write.cpp b/c++/src/dynamixel_sdk/group_bulk_write.cpp index 49af43bcf..f54647af0 100644 --- a/c++/src/dynamixel_sdk/group_bulk_write.cpp +++ b/c++/src/dynamixel_sdk/group_bulk_write.cpp @@ -40,33 +40,29 @@ GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph) void GroupBulkWrite::makeParam() { - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + if (ph_->getProtocolVersion() == 1.0 || id_list_.empty()) return; - if (param_ != 0) - delete[] param_; - param_ = 0; - param_length_ = 0; for (unsigned int i = 0; i < id_list_.size(); i++) param_length_ += 1 + 2 + 2 + length_list_[id_list_[i]]; - param_ = new uint8_t[param_length_]; + param_.clear(); + param_.reserve(param_length_); - int idx = 0; for (unsigned int i = 0; i < id_list_.size(); i++) { uint8_t id = id_list_[i]; - if (data_list_[id] == 0) + if (data_list_.find(id) == data_list_.end()) return; - param_[idx++] = id; - param_[idx++] = DXL_LOBYTE(address_list_[id]); - param_[idx++] = DXL_HIBYTE(address_list_[id]); - param_[idx++] = DXL_LOBYTE(length_list_[id]); - param_[idx++] = DXL_HIBYTE(length_list_[id]); + param_.push_back(id); + param_.push_back(DXL_LOBYTE(address_list_[id])); + param_.push_back(DXL_HIBYTE(address_list_[id])); + param_.push_back(DXL_LOBYTE(length_list_[id])); + param_.push_back(DXL_HIBYTE(length_list_[id])); for (int c = 0; c < length_list_[id]; c++) - param_[idx++] = (data_list_[id])[c]; + param_.push_back((data_list_[id])[c]); } } @@ -81,9 +77,7 @@ bool GroupBulkWrite::addParam(uint8_t id, uint16_t start_address, uint16_t data_ id_list_.push_back(id); address_list_[id] = start_address; length_list_[id] = data_length; - data_list_[id] = new uint8_t[data_length]; - for (int c = 0; c < data_length; c++) - data_list_[id][c] = data[c]; + data_list_[id].assign(data, data + data_length); is_param_changed_ = true; return true; @@ -100,7 +94,6 @@ void GroupBulkWrite::removeParam(uint8_t id) id_list_.erase(it); address_list_.erase(id); length_list_.erase(id); - delete[] data_list_[id]; data_list_.erase(id); is_param_changed_ = true; @@ -116,37 +109,29 @@ bool GroupBulkWrite::changeParam(uint8_t id, uint16_t start_address, uint16_t da address_list_[id] = start_address; length_list_[id] = data_length; - delete[] data_list_[id]; - data_list_[id] = new uint8_t[data_length]; - for (int c = 0; c < data_length; c++) - data_list_[id][c] = data[c]; + data_list_[id].assign(data, data + data_length); is_param_changed_ = true; return true; } void GroupBulkWrite::clearParam() { - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + if (ph_->getProtocolVersion() == 1.0 || id_list_.empty()) return; - for (unsigned int i = 0; i < id_list_.size(); i++) - delete[] data_list_[id_list_[i]]; - id_list_.clear(); address_list_.clear(); length_list_.clear(); data_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; + param_.clear(); } int GroupBulkWrite::txPacket() { - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + if (ph_->getProtocolVersion() == 1.0 || id_list_.empty()) return COMM_NOT_AVAILABLE; - if (is_param_changed_ == true || param_ == 0) + if (is_param_changed_ == true || param_.empty()) makeParam(); - return ph_->bulkWriteTxOnly(port_, param_, param_length_); + return ph_->bulkWriteTxOnly(port_, param_.data(), param_length_); } diff --git a/c++/src/dynamixel_sdk/group_fast_bulk_read.cpp b/c++/src/dynamixel_sdk/group_fast_bulk_read.cpp index 0b8d92b7e..df60fc136 100644 --- a/c++/src/dynamixel_sdk/group_fast_bulk_read.cpp +++ b/c++/src/dynamixel_sdk/group_fast_bulk_read.cpp @@ -46,20 +46,16 @@ void GroupFastBulkRead::makeParam() if ((1.0 == ph_->getProtocolVersion()) || (id_list_.empty())) return; - if (0 != param_) - delete[] param_; - param_ = 0; + param_.clear(); + param_.reserve(id_list_.size() * 5); - param_ = new uint8_t[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) - - int idx = 0; for (unsigned int i = 0; i < id_list_.size(); i++) { uint8_t id = id_list_[i]; - param_[idx++] = id; // ID - param_[idx++] = DXL_LOBYTE(address_list_[id]); // ADDR_L - param_[idx++] = DXL_HIBYTE(address_list_[id]); // ADDR_H - param_[idx++] = DXL_LOBYTE(length_list_[id]); // LEN_L - param_[idx++] = DXL_HIBYTE(length_list_[id]); // LEN_H + param_.push_back(id); // ID + param_.push_back(DXL_LOBYTE(address_list_[id])); // ADDR_L + param_.push_back(DXL_HIBYTE(address_list_[id])); // ADDR_H + param_.push_back(DXL_LOBYTE(length_list_[id])); // LEN_L + param_.push_back(DXL_HIBYTE(length_list_[id])); // LEN_H } } @@ -68,10 +64,10 @@ int GroupFastBulkRead::txPacket() if ((1.0 == ph_->getProtocolVersion()) || (id_list_.empty())) return COMM_NOT_AVAILABLE; - if ((true == is_param_changed_) || (0 == param_)) + if ((true == is_param_changed_) || (param_.empty())) makeParam(); - return ph_->fastBulkReadTx(port_, param_, id_list_.size() * 5); + return ph_->fastBulkReadTx(port_, param_.data(), id_list_.size() * 5); } int GroupFastBulkRead::rxPacket() @@ -83,9 +79,9 @@ int GroupFastBulkRead::rxPacket() int count = id_list_.size(); int result = COMM_RX_FAIL; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); - if (NULL == rxpacket) - return result; + if (rxpacket_.size() < static_cast(RXPACKET_MAX_LEN)) + rxpacket_.resize(static_cast(RXPACKET_MAX_LEN)); + uint8_t *rxpacket = rxpacket_.data(); do { result = ph_->rxPacket(port_, rxpacket, true); @@ -96,7 +92,7 @@ int GroupFastBulkRead::rxPacket() for (int i = 0; i < count; ++i) { uint8_t id = id_list_[i]; uint16_t length = length_list_[id]; - *error_list_[id] = (uint8_t)rxpacket[index]; + error_list_[id][0] = static_cast(rxpacket[index]); for (uint16_t s = 0; s < length; s++) { data_list_[id][s] = rxpacket[index + 2 + s]; } @@ -105,7 +101,6 @@ int GroupFastBulkRead::rxPacket() last_result_ = true; } - free(rxpacket); return result; } diff --git a/c++/src/dynamixel_sdk/group_fast_sync_read.cpp b/c++/src/dynamixel_sdk/group_fast_sync_read.cpp index ef3635eff..ca55d12e5 100644 --- a/c++/src/dynamixel_sdk/group_fast_sync_read.cpp +++ b/c++/src/dynamixel_sdk/group_fast_sync_read.cpp @@ -46,10 +46,10 @@ int GroupFastSyncRead::txPacket() if ((1.0 == ph_->getProtocolVersion()) || (id_list_.empty())) return COMM_NOT_AVAILABLE; - if ((true == is_param_changed_) || (0 == param_)) + if ((true == is_param_changed_) || (param_.empty())) makeParam(); - return ph_->fastSyncReadTx(port_, start_address_, data_length_, param_, (uint16_t)id_list_.size() * 1); + return ph_->fastSyncReadTx(port_, start_address_, data_length_, param_.data(), static_cast(id_list_.size() * 1)); } int GroupFastSyncRead::rxPacket() @@ -61,9 +61,9 @@ int GroupFastSyncRead::rxPacket() int count = id_list_.size(); int result = COMM_RX_FAIL; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); - if (NULL == rxpacket) - return result; + if (rxpacket_.size() < static_cast(RXPACKET_MAX_LEN)) + rxpacket_.resize(static_cast(RXPACKET_MAX_LEN)); + uint8_t *rxpacket = rxpacket_.data(); do { result = ph_->rxPacket(port_, rxpacket, true); @@ -73,7 +73,7 @@ int GroupFastSyncRead::rxPacket() int index = PKT_PARAMETER0; for (int i = 0; i < count; ++i) { uint8_t id = id_list_[i]; - *error_list_[id] = (uint8_t)rxpacket[index]; + error_list_[id][0] = static_cast(rxpacket[index]); for (uint16_t s = 0; s < data_length_; s++) { data_list_[id][s] = rxpacket[index + 2 + s]; } @@ -82,7 +82,6 @@ int GroupFastSyncRead::rxPacket() last_result_ = true; } - free(rxpacket); return result; } diff --git a/c++/src/dynamixel_sdk/group_handler.cpp b/c++/src/dynamixel_sdk/group_handler.cpp index 5998de536..116e01611 100644 --- a/c++/src/dynamixel_sdk/group_handler.cpp +++ b/c++/src/dynamixel_sdk/group_handler.cpp @@ -32,8 +32,7 @@ using namespace dynamixel; GroupHandler::GroupHandler(PortHandler *port, PacketHandler *ph) : port_(port), ph_(ph), - is_param_changed_(false), - param_(0) + is_param_changed_(false) { - + param_.clear(); } diff --git a/c++/src/dynamixel_sdk/group_sync_read.cpp b/c++/src/dynamixel_sdk/group_sync_read.cpp index af067dc04..0196f1479 100644 --- a/c++/src/dynamixel_sdk/group_sync_read.cpp +++ b/c++/src/dynamixel_sdk/group_sync_read.cpp @@ -42,18 +42,15 @@ GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t star void GroupSyncRead::makeParam() { - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + if (ph_->getProtocolVersion() == 1.0 || id_list_.empty()) return; - if (param_ != 0) - delete[] param_; - param_ = 0; + param_.clear(); - param_ = new uint8_t[id_list_.size() * 1]; // ID(1) + param_.reserve(id_list_.size()); // ID(1) - int idx = 0; for (unsigned int i = 0; i < id_list_.size(); i++) - param_[idx++] = id_list_[i]; + param_.push_back(id_list_[i]); } bool GroupSyncRead::addParam(uint8_t id) @@ -65,8 +62,8 @@ bool GroupSyncRead::addParam(uint8_t id) return false; id_list_.push_back(id); - data_list_[id] = new uint8_t[data_length_]; - error_list_[id] = new uint8_t[1]; + data_list_[id] = std::vector(data_length_); + error_list_[id] = std::vector(1); is_param_changed_ = true; return true; @@ -81,8 +78,6 @@ void GroupSyncRead::removeParam(uint8_t id) return; id_list_.erase(it); - delete[] data_list_[id]; - delete[] error_list_[id]; data_list_.erase(id); error_list_.erase(id); @@ -93,18 +88,10 @@ void GroupSyncRead::clearParam() if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) return; - for (unsigned int i = 0; i < id_list_.size(); i++) - { - delete[] data_list_[id_list_[i]]; - delete[] error_list_[id_list_[i]]; - } - id_list_.clear(); data_list_.clear(); error_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; + param_.clear(); } int GroupSyncRead::txPacket() @@ -112,10 +99,10 @@ int GroupSyncRead::txPacket() if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) return COMM_NOT_AVAILABLE; - if (is_param_changed_ == true || param_ == 0) + if (is_param_changed_ == true || param_.empty()) makeParam(); - return ph_->syncReadTx(port_, start_address_, data_length_, param_, (uint16_t)id_list_.size() * 1); + return ph_->syncReadTx(port_, start_address_, data_length_, param_.data(), static_cast(id_list_.size() * 1)); } int GroupSyncRead::rxPacket() @@ -135,7 +122,7 @@ int GroupSyncRead::rxPacket() { uint8_t id = id_list_[i]; - result = ph_->readRx(port_, id, data_length_, data_list_[id], error_list_[id]); + result = ph_->readRx(port_, id, data_length_, data_list_[id].data(), error_list_[id].data()); if (result != COMM_SUCCESS) return result; } diff --git a/c++/src/dynamixel_sdk/group_sync_write.cpp b/c++/src/dynamixel_sdk/group_sync_write.cpp index 2a7bdaaaf..cf3a2ae6b 100644 --- a/c++/src/dynamixel_sdk/group_sync_write.cpp +++ b/c++/src/dynamixel_sdk/group_sync_write.cpp @@ -41,24 +41,20 @@ GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t st void GroupSyncWrite::makeParam() { - if (id_list_.size() == 0) return; + if (id_list_.empty()) return; - if (param_ != 0) - delete[] param_; - param_ = 0; + param_.clear(); + param_.reserve(id_list_.size() * (1 + data_length_)); // ID(1) + DATA(data_length) - param_ = new uint8_t[id_list_.size() * (1 + data_length_)]; // ID(1) + DATA(data_length) - - int idx = 0; for (unsigned int i = 0; i < id_list_.size(); i++) { uint8_t id = id_list_[i]; - if (data_list_[id] == 0) + if (data_list_.find(id) == data_list_.end()) return; - param_[idx++] = id; + param_.push_back(id); for (int c = 0; c < data_length_; c++) - param_[idx++] = (data_list_[id])[c]; + param_.push_back((data_list_[id])[c]); } } @@ -68,9 +64,7 @@ bool GroupSyncWrite::addParam(uint8_t id, uint8_t *data) return false; id_list_.push_back(id); - data_list_[id] = new uint8_t[data_length_]; - for (int c = 0; c < data_length_; c++) - data_list_[id][c] = data[c]; + data_list_[id].assign(data, data + data_length_); is_param_changed_ = true; return true; @@ -83,7 +77,6 @@ void GroupSyncWrite::removeParam(uint8_t id) return; id_list_.erase(it); - delete[] data_list_[id]; data_list_.erase(id); is_param_changed_ = true; @@ -95,10 +88,7 @@ bool GroupSyncWrite::changeParam(uint8_t id, uint8_t *data) if (it == id_list_.end()) // NOT exist return false; - delete[] data_list_[id]; - data_list_[id] = new uint8_t[data_length_]; - for (int c = 0; c < data_length_; c++) - data_list_[id][c] = data[c]; + data_list_[id].assign(data, data + data_length_); is_param_changed_ = true; return true; @@ -106,26 +96,21 @@ bool GroupSyncWrite::changeParam(uint8_t id, uint8_t *data) void GroupSyncWrite::clearParam() { - if (id_list_.size() == 0) + if (id_list_.empty()) return; - for (unsigned int i = 0; i < id_list_.size(); i++) - delete[] data_list_[id_list_[i]]; - id_list_.clear(); data_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; + param_.clear(); } int GroupSyncWrite::txPacket() { - if (id_list_.size() == 0) + if (id_list_.empty()) return COMM_NOT_AVAILABLE; - if (is_param_changed_ == true || param_ == 0) + if (is_param_changed_ == true || param_.empty()) makeParam(); - return ph_->syncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_)); + return ph_->syncWriteTxOnly(port_, start_address_, data_length_, param_.data(), id_list_.size() * (1 + data_length_)); } diff --git a/c++/src/dynamixel_sdk/packet_handler.cpp b/c++/src/dynamixel_sdk/packet_handler.cpp index 4fcf890b8..3aef8283c 100644 --- a/c++/src/dynamixel_sdk/packet_handler.cpp +++ b/c++/src/dynamixel_sdk/packet_handler.cpp @@ -41,12 +41,12 @@ PacketHandler *PacketHandler::getPacketHandler(float protocol_version) { if (protocol_version == 1.0) { - return (PacketHandler *)(Protocol1PacketHandler::getInstance()); + return Protocol1PacketHandler::getInstance(); } else if (protocol_version == 2.0) { - return (PacketHandler *)(Protocol2PacketHandler::getInstance()); + return Protocol2PacketHandler::getInstance(); } - return (PacketHandler *)(Protocol2PacketHandler::getInstance()); + return Protocol2PacketHandler::getInstance(); } diff --git a/c++/src/dynamixel_sdk/port_handler.cpp b/c++/src/dynamixel_sdk/port_handler.cpp index 7dc1ec4ec..8450d0433 100644 --- a/c++/src/dynamixel_sdk/port_handler.cpp +++ b/c++/src/dynamixel_sdk/port_handler.cpp @@ -36,12 +36,12 @@ using namespace dynamixel; PortHandler *PortHandler::getPortHandler(const char *port_name) { #if defined(__linux__) - return (PortHandler *)(new PortHandlerLinux(port_name)); + return static_cast(new PortHandlerLinux(port_name)); #elif defined(__APPLE__) - return (PortHandler *)(new PortHandlerMac(port_name)); + return static_cast(new PortHandlerMac(port_name)); #elif defined(_WIN32) || defined(_WIN64) - return (PortHandler *)(new PortHandlerWindows(port_name)); + return static_cast(new PortHandlerWindows(port_name)); #elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) || defined(ARDUINO_OpenRB) - return (PortHandler *)(new PortHandlerArduino(port_name)); + return static_cast(new PortHandlerArduino(port_name)); #endif } diff --git a/c++/src/dynamixel_sdk/port_handler_arduino.cpp b/c++/src/dynamixel_sdk/port_handler_arduino.cpp index 9542ec54f..86eac0ddf 100644 --- a/c++/src/dynamixel_sdk/port_handler_arduino.cpp +++ b/c++/src/dynamixel_sdk/port_handler_arduino.cpp @@ -97,12 +97,12 @@ void PortHandlerArduino::clearPort() { int temp __attribute__((unused)); #if defined(__OPENCR__) || defined(ARDUINO_OpenRB) - while (DYNAMIXEL_SERIAL.available()) + while (DYNAMIXEL_SERIAL.available()) { temp = DYNAMIXEL_SERIAL.read(); } #elif defined(__OPENCM904__) - while (p_dxl_serial->available()) + while (p_dxl_serial->available()) { temp = p_dxl_serial->read(); } @@ -194,7 +194,7 @@ int PortHandlerArduino::writePort(uint8_t *packet, int length) void PortHandlerArduino::setPacketTimeout(uint16_t packet_length) { packet_start_time_ = getCurrentTime(); - packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; + packet_timeout_ = (tx_time_per_byte * static_cast(packet_length)) + (LATENCY_TIMER * 2.0) + 2.0; } void PortHandlerArduino::setPacketTimeout(double msec) @@ -216,7 +216,7 @@ bool PortHandlerArduino::isPacketTimeout() double PortHandlerArduino::getCurrentTime() { - return (double)millis(); + return static_cast(millis()); } double PortHandlerArduino::getTimeSinceStart() @@ -241,7 +241,7 @@ bool PortHandlerArduino::setupPort(int baudrate) delay(100); - tx_time_per_byte = (1000.0 / (double)baudrate) * 10.0; + tx_time_per_byte = (1000.0 / static_cast(baudrate)) * 10.0; return true; } diff --git a/c++/src/dynamixel_sdk/port_handler_linux.cpp b/c++/src/dynamixel_sdk/port_handler_linux.cpp index 5ba4091da..778c0125d 100644 --- a/c++/src/dynamixel_sdk/port_handler_linux.cpp +++ b/c++/src/dynamixel_sdk/port_handler_linux.cpp @@ -161,7 +161,7 @@ int PortHandlerLinux::writePort(uint8_t *packet, int length) void PortHandlerLinux::setPacketTimeout(uint16_t packet_length) { packet_start_time_ = getCurrentTime(); - packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; + packet_timeout_ = (tx_time_per_byte * static_cast(packet_length)) + (LATENCY_TIMER * 2.0) + 2.0; } void PortHandlerLinux::setPacketTimeout(double msec) @@ -184,7 +184,7 @@ double PortHandlerLinux::getCurrentTime() { struct timespec tv; clock_gettime(CLOCK_REALTIME, &tv); - return ((double)tv.tv_sec * 1000.0 + (double)tv.tv_nsec * 0.001 * 0.001); + return (static_cast(tv.tv_sec) * 1000.0 + static_cast(tv.tv_nsec) * 0.001 * 0.001); } double PortHandlerLinux::getTimeSinceStart() @@ -209,7 +209,7 @@ bool PortHandlerLinux::setupPort(int cflag_baud) return false; } - bzero(&newtio, sizeof(newtio)); // clear struct for new port settings + memset(&newtio, 0, sizeof(newtio)); // clear struct for new port settings newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD; newtio.c_iflag = IGNPAR; @@ -222,7 +222,7 @@ bool PortHandlerLinux::setupPort(int cflag_baud) tcflush(socket_fd_, TCIFLUSH); tcsetattr(socket_fd_, TCSANOW, &newtio); - tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; + tx_time_per_byte = (1000.0 / static_cast(baudrate_)) * 10.0; return true; } @@ -265,7 +265,7 @@ bool PortHandlerLinux::setCustomBaudrate(int speed) return false; } - tx_time_per_byte = (1000.0 / (double)speed) * 10.0; + tx_time_per_byte = (1000.0 / static_cast(speed)) * 10.0; return true; } diff --git a/c++/src/dynamixel_sdk/port_handler_mac.cpp b/c++/src/dynamixel_sdk/port_handler_mac.cpp index bb4eef229..fe36511d3 100644 --- a/c++/src/dynamixel_sdk/port_handler_mac.cpp +++ b/c++/src/dynamixel_sdk/port_handler_mac.cpp @@ -129,8 +129,8 @@ int PortHandlerMac::writePort(uint8_t *packet, int length) while (unsent > 0) { ioctl(socket_fd_, TIOCOUTQ, &unsent); } - unsigned int tx_time = ((float)(length * 10) / baudrate_) * 1000000000; - struct timespec delay = {0, tx_time}; + unsigned int tx_time = (static_cast(length * 10) / baudrate_) * 1000000000; + struct timespec delay = {0, static_cast(tx_time)}; nanosleep(&delay, NULL); return result; @@ -139,7 +139,7 @@ int PortHandlerMac::writePort(uint8_t *packet, int length) void PortHandlerMac::setPacketTimeout(uint16_t packet_length) { packet_start_time_ = getCurrentTime(); - packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; + packet_timeout_ = (tx_time_per_byte * static_cast(packet_length)) + (LATENCY_TIMER * 2.0) + 2.0; } void PortHandlerMac::setPacketTimeout(double msec) @@ -172,7 +172,7 @@ double PortHandlerMac::getCurrentTime() #else clock_gettime(CLOCK_REALTIME, &tv); #endif - return ((double)tv.tv_sec * 1000.0 + (double)tv.tv_nsec * 0.001 * 0.001); + return (static_cast(tv.tv_sec) * 1000.0 + static_cast(tv.tv_nsec) * 0.001 * 0.001); } double PortHandlerMac::getTimeSinceStart() @@ -212,7 +212,7 @@ bool PortHandlerMac::setupPort(int cflag_baud) tcflush(socket_fd_, TCIFLUSH); tcsetattr(socket_fd_, TCSANOW, &newtio); - tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; + tx_time_per_byte = (1000.0 / static_cast(baudrate_)) * 10.0; return true; } diff --git a/c++/src/dynamixel_sdk/port_handler_windows.cpp b/c++/src/dynamixel_sdk/port_handler_windows.cpp index d9b0ed55c..20a2c2b89 100644 --- a/c++/src/dynamixel_sdk/port_handler_windows.cpp +++ b/c++/src/dynamixel_sdk/port_handler_windows.cpp @@ -98,8 +98,8 @@ int PortHandlerWindows::getBytesAvailable() DWORD retbyte = 2; BOOL res = DeviceIoControl(serial_handle_, GENERIC_READ | GENERIC_WRITE, NULL, 0, 0, 0, &retbyte, (LPOVERLAPPED)NULL); - printf("%d", (int)res); - return (int)retbyte; + printf("%d", static_cast(res)); + return static_cast(retbyte); } int PortHandlerWindows::readPort(uint8_t *packet, int length) @@ -109,7 +109,7 @@ int PortHandlerWindows::readPort(uint8_t *packet, int length) if (ReadFile(serial_handle_, packet, (DWORD)length, &dwRead, NULL) == FALSE) return -1; - return (int)dwRead; + return static_cast(dwRead); } int PortHandlerWindows::writePort(uint8_t *packet, int length) @@ -119,13 +119,13 @@ int PortHandlerWindows::writePort(uint8_t *packet, int length) if (WriteFile(serial_handle_, packet, (DWORD)length, &dwWrite, NULL) == FALSE) return -1; - return (int)dwWrite; + return static_cast(dwWrite); } void PortHandlerWindows::setPacketTimeout(uint16_t packet_length) { packet_start_time_ = getCurrentTime(); - packet_timeout_ = (tx_time_per_byte_ * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; + packet_timeout_ = (tx_time_per_byte_ * static_cast(packet_length)) + (LATENCY_TIMER * 2.0) + 2.0; } void PortHandlerWindows::setPacketTimeout(double msec) @@ -148,7 +148,7 @@ double PortHandlerWindows::getCurrentTime() { QueryPerformanceCounter(&counter_); QueryPerformanceFrequency(&freq_); - return (double)counter_.QuadPart / (double)freq_.QuadPart * 1000.0; + return static_cast(counter_.QuadPart) / static_cast(freq_.QuadPart) * 1000.0; } double PortHandlerWindows::getTimeSinceStart() @@ -224,7 +224,7 @@ bool PortHandlerWindows::setupPort(int baudrate) if (SetCommTimeouts(serial_handle_, &timeouts) == FALSE) goto DXL_HAL_OPEN_ERROR; - tx_time_per_byte_ = (1000.0 / (double)baudrate_) * 10.0; + tx_time_per_byte_ = (1000.0 / static_cast(baudrate_)) * 10.0; return true; DXL_HAL_OPEN_ERROR: diff --git a/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp b/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp index 849075844..e110b508e 100644 --- a/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp +++ b/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp @@ -16,6 +16,9 @@ /* Author: zerom, Ryu Woon Jung (Leon) */ +#include +#include + #if defined(__linux__) #include "protocol1_packet_handler.h" #elif defined(__APPLE__) @@ -30,6 +33,11 @@ #include #include +namespace { + thread_local std::vector g_txpacket_buffer; + thread_local std::vector g_rxpacket_buffer; +} + #define TXPACKET_MAX_LEN (250) #define RXPACKET_MAX_LEN (250) @@ -293,11 +301,11 @@ int Protocol1PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uin // set packet timeout if (txpacket[PKT_INSTRUCTION] == INST_READ) { - port->setPacketTimeout((uint16_t)(txpacket[PKT_PARAMETER0+1] + 6)); + port->setPacketTimeout(static_cast(txpacket[PKT_PARAMETER0+1] + 6)); } else { - port->setPacketTimeout((uint16_t)6); // HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM + port->setPacketTimeout(static_cast(6)); // HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM } // rx packet @@ -308,7 +316,7 @@ int Protocol1PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uin if (result == COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID]) { if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; + *error = rxpacket[PKT_ERROR]; } return result; @@ -399,14 +407,14 @@ int Protocol1PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t addre txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = 4; txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)address; - txpacket[PKT_PARAMETER0+1] = (uint8_t)length; + txpacket[PKT_PARAMETER0+0] = static_cast(address); + txpacket[PKT_PARAMETER0+1] = static_cast(length); result = txPacket(port, txpacket); // set packet timeout if (result == COMM_SUCCESS) - port->setPacketTimeout((uint16_t)(length+6)); + port->setPacketTimeout(static_cast(length+6)); return result; } @@ -414,21 +422,21 @@ int Protocol1PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t addre int Protocol1PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); //(length+6); - //uint8_t *rxpacket = new uint8_t[length+6]; - if (rxpacket == NULL) - return result; + if (g_rxpacket_buffer.capacity() < RXPACKET_MAX_LEN) + g_rxpacket_buffer.reserve(RXPACKET_MAX_LEN); + g_rxpacket_buffer.resize(RXPACKET_MAX_LEN); + std::vector& rxpacket = g_rxpacket_buffer; do { - result = rxPacket(port, rxpacket); + result = rxPacket(port, rxpacket.data()); } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); if (result == COMM_SUCCESS && rxpacket[PKT_ID] == id) { if (error != 0) { - *error = (uint8_t)rxpacket[PKT_ERROR]; + *error = rxpacket[PKT_ERROR]; } for (uint16_t s = 0; s < length; s++) { @@ -437,8 +445,6 @@ int Protocol1PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t lengt //memcpy(data, &rxpacket[PKT_PARAMETER0], length); } - free(rxpacket); - //delete[] rxpacket; return result; } @@ -447,29 +453,29 @@ int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add int result = COMM_TX_FAIL; uint8_t txpacket[8] = {0}; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6); - if (rxpacket == NULL) - return result; + if (g_rxpacket_buffer.capacity() < RXPACKET_MAX_LEN) + g_rxpacket_buffer.reserve(RXPACKET_MAX_LEN); + g_rxpacket_buffer.resize(RXPACKET_MAX_LEN); + std::vector& rxpacket = g_rxpacket_buffer; if (id >= BROADCAST_ID) { - free(rxpacket); return COMM_NOT_AVAILABLE; } txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = 4; txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)address; - txpacket[PKT_PARAMETER0+1] = (uint8_t)length; + txpacket[PKT_PARAMETER0+0] = static_cast(address); + txpacket[PKT_PARAMETER0+1] = static_cast(length); - result = txRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket, rxpacket.data(), error); if (result == COMM_SUCCESS) { if (error != 0) { - *error = (uint8_t)rxpacket[PKT_ERROR]; + *error = rxpacket[PKT_ERROR]; } for (uint16_t s = 0; s < length; s++) { @@ -478,8 +484,6 @@ int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add //memcpy(data, &rxpacket[PKT_PARAMETER0], length); } - free(rxpacket); - //delete[] rxpacket; return result; } @@ -550,26 +554,23 @@ int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length+7); - //uint8_t *txpacket = new uint8_t[length+7]; - - if (txpacket == NULL) - return result; + if (g_txpacket_buffer.capacity() < static_cast(length + 7)) + g_txpacket_buffer.reserve(length + 7); + g_txpacket_buffer.resize(length + 7); + std::vector& txpacket = g_txpacket_buffer; txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; + txpacket[PKT_PARAMETER0] = static_cast(address); for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+1+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); port->is_using_ = false; - free(txpacket); - //delete[] txpacket; return result; } @@ -577,26 +578,23 @@ int Protocol1PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t ad { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length+7); //#6->7 - //uint8_t *txpacket = new uint8_t[length+7]; + if (g_txpacket_buffer.capacity() < static_cast(length + 7)) + g_txpacket_buffer.reserve(length + 7); + g_txpacket_buffer.resize(length + 7); + std::vector& txpacket = g_txpacket_buffer; uint8_t rxpacket[6] = {0}; - if (txpacket == NULL) - return result; - txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; + txpacket[PKT_PARAMETER0] = static_cast(address); for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+1+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - result = txRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket.data(), rxpacket, error); - free(txpacket); - //delete[] txpacket; return result; } @@ -637,26 +635,23 @@ int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16 { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length+6); - //uint8_t *txpacket = new uint8_t[length+6]; - - if (txpacket == NULL) - return result; + if (g_txpacket_buffer.capacity() < static_cast(length + 6)) + g_txpacket_buffer.reserve(length + 6); + g_txpacket_buffer.resize(length + 6); + std::vector& txpacket = g_txpacket_buffer; txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; + txpacket[PKT_PARAMETER0] = static_cast(address); for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+1+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); port->is_using_ = false; - free(txpacket); - //delete[] txpacket; return result; } @@ -664,26 +659,23 @@ int Protocol1PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length+6); - //uint8_t *txpacket = new uint8_t[length+6]; + if (g_txpacket_buffer.capacity() < static_cast(length + 6)) + g_txpacket_buffer.reserve(length + 6); + g_txpacket_buffer.resize(length + 6); + std::vector& txpacket = g_txpacket_buffer; uint8_t rxpacket[6] = {0}; - if (txpacket == NULL) - return result; - txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; + txpacket[PKT_PARAMETER0] = static_cast(address); for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+1+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - result = txRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket.data(), rxpacket, error); - free(txpacket); - //delete[] txpacket; return result; } @@ -696,12 +688,11 @@ int Protocol1PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length+8); // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM - //uint8_t *txpacket = new uint8_t[param_length + 8]; - - if (txpacket == NULL) - return result; + if (g_txpacket_buffer.capacity() < static_cast(param_length + 8)) + g_txpacket_buffer.reserve(param_length + 8); + g_txpacket_buffer.resize(param_length + 8); + std::vector& txpacket = g_txpacket_buffer; txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM @@ -713,10 +704,8 @@ int Protocol1PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad txpacket[PKT_PARAMETER0+2+s] = param[s]; //memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length); - result = txRxPacket(port, txpacket, 0, 0); + result = txRxPacket(port, txpacket.data(), 0, 0); - free(txpacket); - //delete[] txpacket; return result; } @@ -724,12 +713,11 @@ int Protocol1PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length+7); // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM - //uint8_t *txpacket = new uint8_t[param_length + 7]; - - if (txpacket == NULL) - return result; + if (g_txpacket_buffer.capacity() < static_cast(param_length + 7)) + g_txpacket_buffer.reserve(param_length + 7); + g_txpacket_buffer.resize(param_length + 7); + std::vector& txpacket = g_txpacket_buffer; txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM @@ -740,17 +728,15 @@ int Protocol1PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 txpacket[PKT_PARAMETER0+1+s] = param[s]; //memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); if (result == COMM_SUCCESS) { int wait_length = 0; for (uint16_t i = 0; i < param_length; i += 3) wait_length += param[i] + 7; - port->setPacketTimeout((uint16_t)wait_length); + port->setPacketTimeout(static_cast(wait_length)); } - free(txpacket); - //delete[] txpacket; return result; } diff --git a/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp b/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp index ef48bd185..eee853204 100644 --- a/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ b/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp @@ -16,6 +16,9 @@ /* Author: zerom, Ryu Woon Jung (Leon) */ +#include +#include + #if defined(__linux__) #include #include "protocol2_packet_handler.h" @@ -34,6 +37,11 @@ #include #include +namespace { + thread_local std::vector g_txpacket_buffer; + thread_local std::vector g_rxpacket_buffer; +} + #define TXPACKET_MAX_LEN (1*1024) #define RXPACKET_MAX_LEN (1*1024) @@ -184,7 +192,7 @@ unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *da for (uint16_t j = 0; j < data_blk_size; j++) { - i = ((uint16_t)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF; + i = (static_cast(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF; crc_accum = (crc_accum << 8) ^ crc_table[i]; } @@ -444,15 +452,15 @@ int Protocol2PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uin // set packet timeout if (txpacket[PKT_INSTRUCTION] == INST_READ) { - port->setPacketTimeout((uint16_t)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11)); + port->setPacketTimeout(static_cast(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11)); } else if (txpacket[PKT_INSTRUCTION] == INST_CLEAR) { - port->setPacketTimeout((double)10000); + port->setPacketTimeout(10000.0); } else { - port->setPacketTimeout((uint16_t)11); + port->setPacketTimeout(static_cast(11)); // HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H } @@ -464,7 +472,7 @@ int Protocol2PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uin if (result == COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID]) { if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; + *error = rxpacket[PKT_ERROR]; } return result; @@ -510,7 +518,7 @@ int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vectorgetBaudRate()) * 10.0; + double tx_time_per_byte = (1000.0 / static_cast(port->getBaudRate())) * 10.0; txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = 3; @@ -526,7 +534,7 @@ int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vectorsetPacketTimeout((uint16_t)(wait_length * 30)); - port->setPacketTimeout(((double)wait_length * tx_time_per_byte) + (3.0 * (double)MAX_ID) + 16.0); + port->setPacketTimeout((static_cast(wait_length) * tx_time_per_byte) + (3.0 * static_cast(MAX_ID)) + 16.0); while(1) { @@ -682,16 +690,16 @@ int Protocol2PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t addre txpacket[PKT_LENGTH_L] = 7; txpacket[PKT_LENGTH_H] = 0; txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (uint8_t)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (uint8_t)DXL_HIBYTE(length); + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(length); + txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(length); result = txPacket(port, txpacket); // set packet timeout if (result == COMM_SUCCESS) - port->setPacketTimeout((uint16_t)(length + 11)); + port->setPacketTimeout(static_cast(length + 11)); return result; } @@ -699,20 +707,20 @@ int Protocol2PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t addre int Protocol2PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); - //(length + 11 + (length/3)); // (length/3): consider stuffing - if (rxpacket == NULL) - return result; + if (g_rxpacket_buffer.capacity() < RXPACKET_MAX_LEN) + g_rxpacket_buffer.reserve(RXPACKET_MAX_LEN); + g_rxpacket_buffer.resize(RXPACKET_MAX_LEN); + std::vector& rxpacket = g_rxpacket_buffer; do { - result = rxPacket(port, rxpacket); + result = rxPacket(port, rxpacket.data()); } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); if (result == COMM_SUCCESS && rxpacket[PKT_ID] == id) { if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; + *error = rxpacket[PKT_ERROR]; for (uint16_t s = 0; s < length; s++) { @@ -721,8 +729,6 @@ int Protocol2PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t lengt //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); } - free(rxpacket); - //delete[] rxpacket; return result; } @@ -731,15 +737,14 @@ int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add int result = COMM_TX_FAIL; uint8_t txpacket[14] = {0}; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); - //(length + 11 + (length/3)); // (length/3): consider stuffing - - if (rxpacket == NULL) - return result; + if (g_rxpacket_buffer.capacity() < RXPACKET_MAX_LEN) + g_rxpacket_buffer.reserve(RXPACKET_MAX_LEN); + g_rxpacket_buffer.resize(RXPACKET_MAX_LEN); + std::vector& rxpacket = g_rxpacket_buffer; + if (id >= BROADCAST_ID) { - free(rxpacket); return COMM_NOT_AVAILABLE; } @@ -747,16 +752,16 @@ int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add txpacket[PKT_LENGTH_L] = 7; txpacket[PKT_LENGTH_H] = 0; txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (uint8_t)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (uint8_t)DXL_HIBYTE(length); + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(length); + txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(length); - result = txRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket, rxpacket.data(), error); if (result == COMM_SUCCESS) { if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; + *error = rxpacket[PKT_ERROR]; for (uint16_t s = 0; s < length; s++) { @@ -765,8 +770,6 @@ int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); } - free(rxpacket); - //delete[] rxpacket; return result; } @@ -838,27 +841,25 @@ int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); - - if (txpacket == NULL) - return result; + if (g_txpacket_buffer.capacity() < static_cast(length + 12 + (length / 3))) + g_txpacket_buffer.reserve(length + 12 + (length / 3)); + g_txpacket_buffer.resize(length + 12 + (length / 3)); + std::vector& txpacket = g_txpacket_buffer; txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(address); for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+2+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); port->is_using_ = false; - free(txpacket); - //delete[] txpacket; return result; } @@ -866,27 +867,25 @@ int Protocol2PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t ad { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + if (g_txpacket_buffer.capacity() < static_cast(length + 12 + (length / 3))) + g_txpacket_buffer.reserve(length + 12 + (length / 3)); + g_txpacket_buffer.resize(length + 12 + (length / 3)); + std::vector& txpacket = g_txpacket_buffer; uint8_t rxpacket[11] = {0}; - if (txpacket == NULL) - return result; - txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(address); for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+2+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - result = txRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket.data(), rxpacket, error); - free(txpacket); - //delete[] txpacket; return result; } @@ -927,27 +926,25 @@ int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16 { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); - - if (txpacket == NULL) - return result; + if (g_txpacket_buffer.capacity() < static_cast(length + 12 + (length / 3))) + g_txpacket_buffer.reserve(length + 12 + (length / 3)); + g_txpacket_buffer.resize(length + 12 + (length / 3)); + std::vector& txpacket = g_txpacket_buffer; txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(address); for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+2+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); port->is_using_ = false; - free(txpacket); - //delete[] txpacket; return result; } @@ -955,27 +952,25 @@ int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + if (g_txpacket_buffer.capacity() < static_cast(length + 12 + (length / 3))) + g_txpacket_buffer.reserve(length + 12 + (length / 3)); + g_txpacket_buffer.resize(length + 12 + (length / 3)); + std::vector& txpacket = g_txpacket_buffer; uint8_t rxpacket[11] = {0}; - if (txpacket == NULL) - return result; - txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(address); for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+2+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - result = txRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket.data(), rxpacket, error); - free(txpacket); - //delete[] txpacket; return result; } @@ -983,12 +978,12 @@ int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + if (g_txpacket_buffer.capacity() < static_cast(param_length + 14 + (param_length / 3))) + g_txpacket_buffer.reserve(param_length + 14 + (param_length / 3)); + g_txpacket_buffer.resize(param_length + 14 + (param_length / 3)); + std::vector& txpacket = g_txpacket_buffer; - if (txpacket == NULL) - return result; - txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -1002,11 +997,10 @@ int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address txpacket[PKT_PARAMETER0+4+s] = param[s]; //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); if (result == COMM_SUCCESS) - port->setPacketTimeout((uint16_t)((11 + data_length) * param_length)); + port->setPacketTimeout(static_cast((11 + data_length) * param_length)); - free(txpacket); return result; } @@ -1014,12 +1008,12 @@ int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + if (g_txpacket_buffer.capacity() < static_cast(param_length + 14 + (param_length / 3))) + g_txpacket_buffer.reserve(param_length + 14 + (param_length / 3)); + g_txpacket_buffer.resize(param_length + 14 + (param_length / 3)); + std::vector& txpacket = g_txpacket_buffer; - if (txpacket == NULL) - return result; - txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -1033,10 +1027,8 @@ int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad txpacket[PKT_PARAMETER0+4+s] = param[s]; //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - result = txRxPacket(port, txpacket, 0, 0); + result = txRxPacket(port, txpacket.data(), 0, 0); - free(txpacket); - //delete[] txpacket; return result; } @@ -1044,12 +1036,12 @@ int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + if (g_txpacket_buffer.capacity() < static_cast(param_length + 10 + (param_length / 3))) + g_txpacket_buffer.reserve(param_length + 10 + (param_length / 3)); + g_txpacket_buffer.resize(param_length + 10 + (param_length / 3)); + std::vector& txpacket = g_txpacket_buffer; - if (txpacket == NULL) - return result; - txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H @@ -1059,17 +1051,15 @@ int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 txpacket[PKT_PARAMETER0+s] = param[s]; //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); if (result == COMM_SUCCESS) { int wait_length = 0; for (uint16_t i = 0; i < param_length; i += 5) wait_length += DXL_MAKEWORD(param[i+3], param[i+4]) + 10; - port->setPacketTimeout((uint16_t)wait_length); + port->setPacketTimeout(static_cast(wait_length)); } - free(txpacket); - //delete[] txpacket; return result; } @@ -1077,12 +1067,12 @@ int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, u { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + if (g_txpacket_buffer.capacity() < static_cast(param_length + 10 + (param_length / 3))) + g_txpacket_buffer.reserve(param_length + 10 + (param_length / 3)); + g_txpacket_buffer.resize(param_length + 10 + (param_length / 3)); + std::vector& txpacket = g_txpacket_buffer; - if (txpacket == NULL) - return result; - txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H @@ -1092,10 +1082,8 @@ int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, u txpacket[PKT_PARAMETER0+s] = param[s]; //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - result = txRxPacket(port, txpacket, 0, 0); + result = txRxPacket(port, txpacket.data(), 0, 0); - free(txpacket); - //delete[] txpacket; return result; } @@ -1103,11 +1091,11 @@ int Protocol2PacketHandler::fastSyncReadTx(PortHandler *port, uint16_t start_add { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - - if (NULL == txpacket) - return result; + if (g_txpacket_buffer.capacity() < static_cast(param_length + 14 + (param_length / 3))) + g_txpacket_buffer.reserve(param_length + 14 + (param_length / 3)); + g_txpacket_buffer.resize(param_length + 14 + (param_length / 3)); + std::vector& txpacket = g_txpacket_buffer; txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -1122,11 +1110,10 @@ int Protocol2PacketHandler::fastSyncReadTx(PortHandler *port, uint16_t start_add txpacket[PKT_PARAMETER0 + 4 + s] = param[s]; //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); if (COMM_SUCCESS == result) - port->setPacketTimeout((uint16_t)((11 + data_length) * param_length)); + port->setPacketTimeout(static_cast((11 + data_length) * param_length)); - free(txpacket); return result; } @@ -1134,11 +1121,11 @@ int Protocol2PacketHandler::fastBulkReadTx(PortHandler *port, uint8_t *param, ui { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - - if (NULL == txpacket) - return result; + if (g_txpacket_buffer.capacity() < static_cast(param_length + 10 + (param_length / 3))) + g_txpacket_buffer.reserve(param_length + 10 + (param_length / 3)); + g_txpacket_buffer.resize(param_length + 10 + (param_length / 3)); + std::vector& txpacket = g_txpacket_buffer; txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H @@ -1149,15 +1136,13 @@ int Protocol2PacketHandler::fastBulkReadTx(PortHandler *port, uint8_t *param, ui txpacket[PKT_PARAMETER0 + s] = param[s]; //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); if (result == COMM_SUCCESS) { int wait_length = 0; for (uint16_t i = 0; i < param_length; i += 5) wait_length += DXL_MAKEWORD(param[i + 3], param[i + 4]) + 10; - port->setPacketTimeout((uint16_t)wait_length); + port->setPacketTimeout(static_cast(wait_length)); } - free(txpacket); - //delete[] txpacket; return result; } From 94e70e3c8752235277cbedb6a2361067eebcd97d Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Thu, 9 Apr 2026 08:14:19 +0900 Subject: [PATCH 02/10] applied raii update to ros package --- .../include/dynamixel_sdk/group_bulk_read.h | 4 +- .../dynamixel_sdk/group_fast_bulk_read.h | 1 + .../dynamixel_sdk/group_fast_sync_read.h | 3 + .../include/dynamixel_sdk/group_handler.h | 4 +- .../include/dynamixel_sdk/group_sync_read.h | 4 +- .../include/dynamixel_sdk/packet_handler.h | 12 +- .../src/dynamixel_sdk/group_bulk_read.cpp | 51 ++--- .../src/dynamixel_sdk/group_bulk_write.cpp | 49 ++-- .../dynamixel_sdk/group_fast_bulk_read.cpp | 31 ++- .../dynamixel_sdk/group_fast_sync_read.cpp | 13 +- .../src/dynamixel_sdk/group_handler.cpp | 5 +- .../src/dynamixel_sdk/group_sync_read.cpp | 33 +-- .../src/dynamixel_sdk/group_sync_write.cpp | 41 ++-- .../src/dynamixel_sdk/packet_handler.cpp | 6 +- .../src/dynamixel_sdk/port_handler.cpp | 8 +- .../dynamixel_sdk/port_handler_arduino.cpp | 10 +- .../src/dynamixel_sdk/port_handler_linux.cpp | 10 +- .../src/dynamixel_sdk/port_handler_mac.cpp | 10 +- .../dynamixel_sdk/port_handler_windows.cpp | 14 +- .../protocol1_packet_handler.cpp | 140 ++++++------ .../protocol2_packet_handler.cpp | 213 ++++++++---------- 21 files changed, 287 insertions(+), 375 deletions(-) diff --git a/ros/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h b/ros/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h index f2041e45a..b55ae316c 100644 --- a/ros/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h +++ b/ros/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h @@ -38,7 +38,7 @@ class WINDECLSPEC GroupBulkRead : public GroupHandler protected: std::map address_list_; // std::map length_list_; // - std::map error_list_; // + std::map> error_list_; // bool last_result_; @@ -135,7 +135,7 @@ class WINDECLSPEC GroupBulkRead : public GroupHandler /// @error error of Dynamixel /// @return true /// @return when Dynamixel returned specific error byte - /// @return or false + /// @return or false //////////////////////////////////////////////////////////////////////////////// bool getError (uint8_t id, uint8_t* error); }; diff --git a/ros/dynamixel_sdk/include/dynamixel_sdk/group_fast_bulk_read.h b/ros/dynamixel_sdk/include/dynamixel_sdk/group_fast_bulk_read.h index 78ffe2b4a..7045a6f11 100644 --- a/ros/dynamixel_sdk/include/dynamixel_sdk/group_fast_bulk_read.h +++ b/ros/dynamixel_sdk/include/dynamixel_sdk/group_fast_bulk_read.h @@ -40,6 +40,7 @@ class WINDECLSPEC GroupFastBulkRead : public GroupBulkRead private: void makeParam(); + std::vector rxpacket_; }; } diff --git a/ros/dynamixel_sdk/include/dynamixel_sdk/group_fast_sync_read.h b/ros/dynamixel_sdk/include/dynamixel_sdk/group_fast_sync_read.h index a5b56a07c..450153aeb 100644 --- a/ros/dynamixel_sdk/include/dynamixel_sdk/group_fast_sync_read.h +++ b/ros/dynamixel_sdk/include/dynamixel_sdk/group_fast_sync_read.h @@ -37,6 +37,9 @@ class WINDECLSPEC GroupFastSyncRead : public GroupSyncRead int txPacket(); int rxPacket(); int txRxPacket(); + +private: + std::vector rxpacket_; }; } diff --git a/ros/dynamixel_sdk/include/dynamixel_sdk/group_handler.h b/ros/dynamixel_sdk/include/dynamixel_sdk/group_handler.h index 1ad5e711e..1c543ee06 100644 --- a/ros/dynamixel_sdk/include/dynamixel_sdk/group_handler.h +++ b/ros/dynamixel_sdk/include/dynamixel_sdk/group_handler.h @@ -44,11 +44,11 @@ class WINDECLSPEC GroupHandler PacketHandler *ph_; std::vector id_list_; - std::map data_list_; // + std::map> data_list_; // bool is_param_changed_; - uint8_t *param_; + std::vector param_; }; } diff --git a/ros/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h b/ros/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h index 19a36dd56..58b83d4db 100644 --- a/ros/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h +++ b/ros/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h @@ -36,7 +36,7 @@ namespace dynamixel class WINDECLSPEC GroupSyncRead : public GroupHandler { protected: - std::map error_list_; // + std::map> error_list_; // bool last_result_; @@ -140,7 +140,7 @@ class WINDECLSPEC GroupSyncRead : public GroupHandler /// @error error of Dynamixel /// @return true /// @return when Dynamixel returned specific error byte - /// @return or false + /// @return or false //////////////////////////////////////////////////////////////////////////////// bool getError (uint8_t id, uint8_t* error); }; diff --git a/ros/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h b/ros/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h index f3b7e4fb3..da8b2856e 100644 --- a/ros/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h +++ b/ros/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h @@ -39,12 +39,12 @@ #define MAX_ID 0xFC // 252 /* Macro for Control Table Value */ -#define DXL_MAKEWORD(a, b) ((uint16_t)(((uint8_t)(((uint64_t)(a)) & 0xff)) | ((uint16_t)((uint8_t)(((uint64_t)(b)) & 0xff))) << 8)) -#define DXL_MAKEDWORD(a, b) ((uint32_t)(((uint16_t)(((uint64_t)(a)) & 0xffff)) | ((uint32_t)((uint16_t)(((uint64_t)(b)) & 0xffff))) << 16)) -#define DXL_LOWORD(l) ((uint16_t)(((uint64_t)(l)) & 0xffff)) -#define DXL_HIWORD(l) ((uint16_t)((((uint64_t)(l)) >> 16) & 0xffff)) -#define DXL_LOBYTE(w) ((uint8_t)(((uint64_t)(w)) & 0xff)) -#define DXL_HIBYTE(w) ((uint8_t)((((uint64_t)(w)) >> 8) & 0xff)) +#define DXL_MAKEWORD(a, b) (static_cast((static_cast(static_cast(a) & 0xff)) | (static_cast(static_cast(static_cast(b) & 0xff))) << 8)) +#define DXL_MAKEDWORD(a, b) (static_cast((static_cast(static_cast(a) & 0xffff)) | (static_cast(static_cast(static_cast(b) & 0xffff))) << 16)) +#define DXL_LOWORD(l) (static_cast(static_cast(l) & 0xffff)) +#define DXL_HIWORD(l) (static_cast((static_cast(l) >> 16) & 0xffff)) +#define DXL_LOBYTE(w) (static_cast(static_cast(w) & 0xff)) +#define DXL_HIBYTE(w) (static_cast((static_cast(w) >> 8) & 0xff)) /* Instruction for DXL Protocol */ #define INST_PING 1 diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp index 32feefaa3..ca464b682 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp @@ -41,39 +41,36 @@ GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph) void GroupBulkRead::makeParam() { - if (id_list_.size() == 0) + if (id_list_.empty()) return; - if (param_ != 0) - delete[] param_; - param_ = 0; + param_.clear(); if (ph_->getProtocolVersion() == 1.0) { - param_ = new uint8_t[id_list_.size() * 3]; // ID(1) + ADDR(1) + LENGTH(1) + param_.reserve(id_list_.size() * 3); // ID(1) + ADDR(1) + LENGTH(1) } else // 2.0 { - param_ = new uint8_t[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) + param_.reserve(id_list_.size() * 5); // ID(1) + ADDR(2) + LENGTH(2) } - int idx = 0; for (unsigned int i = 0; i < id_list_.size(); i++) { uint8_t id = id_list_[i]; if (ph_->getProtocolVersion() == 1.0) { - param_[idx++] = (uint8_t)length_list_[id]; // LEN - param_[idx++] = id; // ID - param_[idx++] = (uint8_t)address_list_[id]; // ADDR + param_.push_back(static_cast(length_list_[id])); // LEN + param_.push_back(id); // ID + param_.push_back(static_cast(address_list_[id])); // ADDR } else // 2.0 { - param_[idx++] = id; // ID - param_[idx++] = DXL_LOBYTE(address_list_[id]); // ADDR_L - param_[idx++] = DXL_HIBYTE(address_list_[id]); // ADDR_H - param_[idx++] = DXL_LOBYTE(length_list_[id]); // LEN_L - param_[idx++] = DXL_HIBYTE(length_list_[id]); // LEN_H + param_.push_back(id); // ID + param_.push_back(DXL_LOBYTE(address_list_[id])); // ADDR_L + param_.push_back(DXL_HIBYTE(address_list_[id])); // ADDR_H + param_.push_back(DXL_LOBYTE(length_list_[id])); // LEN_L + param_.push_back(DXL_HIBYTE(length_list_[id])); // LEN_H } } } @@ -86,8 +83,8 @@ bool GroupBulkRead::addParam(uint8_t id, uint16_t start_address, uint16_t data_l id_list_.push_back(id); length_list_[id] = data_length; address_list_[id] = start_address; - data_list_[id] = new uint8_t[data_length]; - error_list_[id] = new uint8_t[1]; + data_list_[id] = std::vector(data_length); + error_list_[id] = std::vector(1); is_param_changed_ = true; return true; @@ -102,8 +99,6 @@ void GroupBulkRead::removeParam(uint8_t id) id_list_.erase(it); address_list_.erase(id); length_list_.erase(id); - delete[] data_list_[id]; - delete[] error_list_[id]; data_list_.erase(id); error_list_.erase(id); @@ -115,20 +110,12 @@ void GroupBulkRead::clearParam() if (id_list_.size() == 0) return; - for (unsigned int i = 0; i < id_list_.size(); i++) - { - delete[] data_list_[id_list_[i]]; - delete[] error_list_[id_list_[i]]; - } - id_list_.clear(); address_list_.clear(); length_list_.clear(); data_list_.clear(); error_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; + param_.clear(); } int GroupBulkRead::txPacket() @@ -136,16 +123,16 @@ int GroupBulkRead::txPacket() if (id_list_.size() == 0) return COMM_NOT_AVAILABLE; - if (is_param_changed_ == true || param_ == 0) + if (is_param_changed_ == true || param_.empty()) makeParam(); if (ph_->getProtocolVersion() == 1.0) { - return ph_->bulkReadTx(port_, param_, id_list_.size() * 3); + return ph_->bulkReadTx(port_, param_.data(), id_list_.size() * 3); } else // 2.0 { - return ph_->bulkReadTx(port_, param_, id_list_.size() * 5); + return ph_->bulkReadTx(port_, param_.data(), id_list_.size() * 5); } } @@ -163,7 +150,7 @@ int GroupBulkRead::rxPacket() { uint8_t id = id_list_[i]; - result = ph_->readRx(port_, id, length_list_[id], data_list_[id], error_list_[id]); + result = ph_->readRx(port_, id, length_list_[id], data_list_[id].data(), error_list_[id].data()); if (result != COMM_SUCCESS) return result; } diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp index 49af43bcf..f54647af0 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp @@ -40,33 +40,29 @@ GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph) void GroupBulkWrite::makeParam() { - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + if (ph_->getProtocolVersion() == 1.0 || id_list_.empty()) return; - if (param_ != 0) - delete[] param_; - param_ = 0; - param_length_ = 0; for (unsigned int i = 0; i < id_list_.size(); i++) param_length_ += 1 + 2 + 2 + length_list_[id_list_[i]]; - param_ = new uint8_t[param_length_]; + param_.clear(); + param_.reserve(param_length_); - int idx = 0; for (unsigned int i = 0; i < id_list_.size(); i++) { uint8_t id = id_list_[i]; - if (data_list_[id] == 0) + if (data_list_.find(id) == data_list_.end()) return; - param_[idx++] = id; - param_[idx++] = DXL_LOBYTE(address_list_[id]); - param_[idx++] = DXL_HIBYTE(address_list_[id]); - param_[idx++] = DXL_LOBYTE(length_list_[id]); - param_[idx++] = DXL_HIBYTE(length_list_[id]); + param_.push_back(id); + param_.push_back(DXL_LOBYTE(address_list_[id])); + param_.push_back(DXL_HIBYTE(address_list_[id])); + param_.push_back(DXL_LOBYTE(length_list_[id])); + param_.push_back(DXL_HIBYTE(length_list_[id])); for (int c = 0; c < length_list_[id]; c++) - param_[idx++] = (data_list_[id])[c]; + param_.push_back((data_list_[id])[c]); } } @@ -81,9 +77,7 @@ bool GroupBulkWrite::addParam(uint8_t id, uint16_t start_address, uint16_t data_ id_list_.push_back(id); address_list_[id] = start_address; length_list_[id] = data_length; - data_list_[id] = new uint8_t[data_length]; - for (int c = 0; c < data_length; c++) - data_list_[id][c] = data[c]; + data_list_[id].assign(data, data + data_length); is_param_changed_ = true; return true; @@ -100,7 +94,6 @@ void GroupBulkWrite::removeParam(uint8_t id) id_list_.erase(it); address_list_.erase(id); length_list_.erase(id); - delete[] data_list_[id]; data_list_.erase(id); is_param_changed_ = true; @@ -116,37 +109,29 @@ bool GroupBulkWrite::changeParam(uint8_t id, uint16_t start_address, uint16_t da address_list_[id] = start_address; length_list_[id] = data_length; - delete[] data_list_[id]; - data_list_[id] = new uint8_t[data_length]; - for (int c = 0; c < data_length; c++) - data_list_[id][c] = data[c]; + data_list_[id].assign(data, data + data_length); is_param_changed_ = true; return true; } void GroupBulkWrite::clearParam() { - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + if (ph_->getProtocolVersion() == 1.0 || id_list_.empty()) return; - for (unsigned int i = 0; i < id_list_.size(); i++) - delete[] data_list_[id_list_[i]]; - id_list_.clear(); address_list_.clear(); length_list_.clear(); data_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; + param_.clear(); } int GroupBulkWrite::txPacket() { - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + if (ph_->getProtocolVersion() == 1.0 || id_list_.empty()) return COMM_NOT_AVAILABLE; - if (is_param_changed_ == true || param_ == 0) + if (is_param_changed_ == true || param_.empty()) makeParam(); - return ph_->bulkWriteTxOnly(port_, param_, param_length_); + return ph_->bulkWriteTxOnly(port_, param_.data(), param_length_); } diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/group_fast_bulk_read.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/group_fast_bulk_read.cpp index 0b8d92b7e..df60fc136 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/group_fast_bulk_read.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/group_fast_bulk_read.cpp @@ -46,20 +46,16 @@ void GroupFastBulkRead::makeParam() if ((1.0 == ph_->getProtocolVersion()) || (id_list_.empty())) return; - if (0 != param_) - delete[] param_; - param_ = 0; + param_.clear(); + param_.reserve(id_list_.size() * 5); - param_ = new uint8_t[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) - - int idx = 0; for (unsigned int i = 0; i < id_list_.size(); i++) { uint8_t id = id_list_[i]; - param_[idx++] = id; // ID - param_[idx++] = DXL_LOBYTE(address_list_[id]); // ADDR_L - param_[idx++] = DXL_HIBYTE(address_list_[id]); // ADDR_H - param_[idx++] = DXL_LOBYTE(length_list_[id]); // LEN_L - param_[idx++] = DXL_HIBYTE(length_list_[id]); // LEN_H + param_.push_back(id); // ID + param_.push_back(DXL_LOBYTE(address_list_[id])); // ADDR_L + param_.push_back(DXL_HIBYTE(address_list_[id])); // ADDR_H + param_.push_back(DXL_LOBYTE(length_list_[id])); // LEN_L + param_.push_back(DXL_HIBYTE(length_list_[id])); // LEN_H } } @@ -68,10 +64,10 @@ int GroupFastBulkRead::txPacket() if ((1.0 == ph_->getProtocolVersion()) || (id_list_.empty())) return COMM_NOT_AVAILABLE; - if ((true == is_param_changed_) || (0 == param_)) + if ((true == is_param_changed_) || (param_.empty())) makeParam(); - return ph_->fastBulkReadTx(port_, param_, id_list_.size() * 5); + return ph_->fastBulkReadTx(port_, param_.data(), id_list_.size() * 5); } int GroupFastBulkRead::rxPacket() @@ -83,9 +79,9 @@ int GroupFastBulkRead::rxPacket() int count = id_list_.size(); int result = COMM_RX_FAIL; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); - if (NULL == rxpacket) - return result; + if (rxpacket_.size() < static_cast(RXPACKET_MAX_LEN)) + rxpacket_.resize(static_cast(RXPACKET_MAX_LEN)); + uint8_t *rxpacket = rxpacket_.data(); do { result = ph_->rxPacket(port_, rxpacket, true); @@ -96,7 +92,7 @@ int GroupFastBulkRead::rxPacket() for (int i = 0; i < count; ++i) { uint8_t id = id_list_[i]; uint16_t length = length_list_[id]; - *error_list_[id] = (uint8_t)rxpacket[index]; + error_list_[id][0] = static_cast(rxpacket[index]); for (uint16_t s = 0; s < length; s++) { data_list_[id][s] = rxpacket[index + 2 + s]; } @@ -105,7 +101,6 @@ int GroupFastBulkRead::rxPacket() last_result_ = true; } - free(rxpacket); return result; } diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/group_fast_sync_read.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/group_fast_sync_read.cpp index ef3635eff..ca55d12e5 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/group_fast_sync_read.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/group_fast_sync_read.cpp @@ -46,10 +46,10 @@ int GroupFastSyncRead::txPacket() if ((1.0 == ph_->getProtocolVersion()) || (id_list_.empty())) return COMM_NOT_AVAILABLE; - if ((true == is_param_changed_) || (0 == param_)) + if ((true == is_param_changed_) || (param_.empty())) makeParam(); - return ph_->fastSyncReadTx(port_, start_address_, data_length_, param_, (uint16_t)id_list_.size() * 1); + return ph_->fastSyncReadTx(port_, start_address_, data_length_, param_.data(), static_cast(id_list_.size() * 1)); } int GroupFastSyncRead::rxPacket() @@ -61,9 +61,9 @@ int GroupFastSyncRead::rxPacket() int count = id_list_.size(); int result = COMM_RX_FAIL; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); - if (NULL == rxpacket) - return result; + if (rxpacket_.size() < static_cast(RXPACKET_MAX_LEN)) + rxpacket_.resize(static_cast(RXPACKET_MAX_LEN)); + uint8_t *rxpacket = rxpacket_.data(); do { result = ph_->rxPacket(port_, rxpacket, true); @@ -73,7 +73,7 @@ int GroupFastSyncRead::rxPacket() int index = PKT_PARAMETER0; for (int i = 0; i < count; ++i) { uint8_t id = id_list_[i]; - *error_list_[id] = (uint8_t)rxpacket[index]; + error_list_[id][0] = static_cast(rxpacket[index]); for (uint16_t s = 0; s < data_length_; s++) { data_list_[id][s] = rxpacket[index + 2 + s]; } @@ -82,7 +82,6 @@ int GroupFastSyncRead::rxPacket() last_result_ = true; } - free(rxpacket); return result; } diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/group_handler.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/group_handler.cpp index 5998de536..116e01611 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/group_handler.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/group_handler.cpp @@ -32,8 +32,7 @@ using namespace dynamixel; GroupHandler::GroupHandler(PortHandler *port, PacketHandler *ph) : port_(port), ph_(ph), - is_param_changed_(false), - param_(0) + is_param_changed_(false) { - + param_.clear(); } diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp index af067dc04..0196f1479 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp @@ -42,18 +42,15 @@ GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t star void GroupSyncRead::makeParam() { - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + if (ph_->getProtocolVersion() == 1.0 || id_list_.empty()) return; - if (param_ != 0) - delete[] param_; - param_ = 0; + param_.clear(); - param_ = new uint8_t[id_list_.size() * 1]; // ID(1) + param_.reserve(id_list_.size()); // ID(1) - int idx = 0; for (unsigned int i = 0; i < id_list_.size(); i++) - param_[idx++] = id_list_[i]; + param_.push_back(id_list_[i]); } bool GroupSyncRead::addParam(uint8_t id) @@ -65,8 +62,8 @@ bool GroupSyncRead::addParam(uint8_t id) return false; id_list_.push_back(id); - data_list_[id] = new uint8_t[data_length_]; - error_list_[id] = new uint8_t[1]; + data_list_[id] = std::vector(data_length_); + error_list_[id] = std::vector(1); is_param_changed_ = true; return true; @@ -81,8 +78,6 @@ void GroupSyncRead::removeParam(uint8_t id) return; id_list_.erase(it); - delete[] data_list_[id]; - delete[] error_list_[id]; data_list_.erase(id); error_list_.erase(id); @@ -93,18 +88,10 @@ void GroupSyncRead::clearParam() if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) return; - for (unsigned int i = 0; i < id_list_.size(); i++) - { - delete[] data_list_[id_list_[i]]; - delete[] error_list_[id_list_[i]]; - } - id_list_.clear(); data_list_.clear(); error_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; + param_.clear(); } int GroupSyncRead::txPacket() @@ -112,10 +99,10 @@ int GroupSyncRead::txPacket() if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) return COMM_NOT_AVAILABLE; - if (is_param_changed_ == true || param_ == 0) + if (is_param_changed_ == true || param_.empty()) makeParam(); - return ph_->syncReadTx(port_, start_address_, data_length_, param_, (uint16_t)id_list_.size() * 1); + return ph_->syncReadTx(port_, start_address_, data_length_, param_.data(), static_cast(id_list_.size() * 1)); } int GroupSyncRead::rxPacket() @@ -135,7 +122,7 @@ int GroupSyncRead::rxPacket() { uint8_t id = id_list_[i]; - result = ph_->readRx(port_, id, data_length_, data_list_[id], error_list_[id]); + result = ph_->readRx(port_, id, data_length_, data_list_[id].data(), error_list_[id].data()); if (result != COMM_SUCCESS) return result; } diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp index 2a7bdaaaf..cf3a2ae6b 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp @@ -41,24 +41,20 @@ GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t st void GroupSyncWrite::makeParam() { - if (id_list_.size() == 0) return; + if (id_list_.empty()) return; - if (param_ != 0) - delete[] param_; - param_ = 0; + param_.clear(); + param_.reserve(id_list_.size() * (1 + data_length_)); // ID(1) + DATA(data_length) - param_ = new uint8_t[id_list_.size() * (1 + data_length_)]; // ID(1) + DATA(data_length) - - int idx = 0; for (unsigned int i = 0; i < id_list_.size(); i++) { uint8_t id = id_list_[i]; - if (data_list_[id] == 0) + if (data_list_.find(id) == data_list_.end()) return; - param_[idx++] = id; + param_.push_back(id); for (int c = 0; c < data_length_; c++) - param_[idx++] = (data_list_[id])[c]; + param_.push_back((data_list_[id])[c]); } } @@ -68,9 +64,7 @@ bool GroupSyncWrite::addParam(uint8_t id, uint8_t *data) return false; id_list_.push_back(id); - data_list_[id] = new uint8_t[data_length_]; - for (int c = 0; c < data_length_; c++) - data_list_[id][c] = data[c]; + data_list_[id].assign(data, data + data_length_); is_param_changed_ = true; return true; @@ -83,7 +77,6 @@ void GroupSyncWrite::removeParam(uint8_t id) return; id_list_.erase(it); - delete[] data_list_[id]; data_list_.erase(id); is_param_changed_ = true; @@ -95,10 +88,7 @@ bool GroupSyncWrite::changeParam(uint8_t id, uint8_t *data) if (it == id_list_.end()) // NOT exist return false; - delete[] data_list_[id]; - data_list_[id] = new uint8_t[data_length_]; - for (int c = 0; c < data_length_; c++) - data_list_[id][c] = data[c]; + data_list_[id].assign(data, data + data_length_); is_param_changed_ = true; return true; @@ -106,26 +96,21 @@ bool GroupSyncWrite::changeParam(uint8_t id, uint8_t *data) void GroupSyncWrite::clearParam() { - if (id_list_.size() == 0) + if (id_list_.empty()) return; - for (unsigned int i = 0; i < id_list_.size(); i++) - delete[] data_list_[id_list_[i]]; - id_list_.clear(); data_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; + param_.clear(); } int GroupSyncWrite::txPacket() { - if (id_list_.size() == 0) + if (id_list_.empty()) return COMM_NOT_AVAILABLE; - if (is_param_changed_ == true || param_ == 0) + if (is_param_changed_ == true || param_.empty()) makeParam(); - return ph_->syncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_)); + return ph_->syncWriteTxOnly(port_, start_address_, data_length_, param_.data(), id_list_.size() * (1 + data_length_)); } diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp index 4fcf890b8..3aef8283c 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp @@ -41,12 +41,12 @@ PacketHandler *PacketHandler::getPacketHandler(float protocol_version) { if (protocol_version == 1.0) { - return (PacketHandler *)(Protocol1PacketHandler::getInstance()); + return Protocol1PacketHandler::getInstance(); } else if (protocol_version == 2.0) { - return (PacketHandler *)(Protocol2PacketHandler::getInstance()); + return Protocol2PacketHandler::getInstance(); } - return (PacketHandler *)(Protocol2PacketHandler::getInstance()); + return Protocol2PacketHandler::getInstance(); } diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp index 7dc1ec4ec..8450d0433 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp @@ -36,12 +36,12 @@ using namespace dynamixel; PortHandler *PortHandler::getPortHandler(const char *port_name) { #if defined(__linux__) - return (PortHandler *)(new PortHandlerLinux(port_name)); + return static_cast(new PortHandlerLinux(port_name)); #elif defined(__APPLE__) - return (PortHandler *)(new PortHandlerMac(port_name)); + return static_cast(new PortHandlerMac(port_name)); #elif defined(_WIN32) || defined(_WIN64) - return (PortHandler *)(new PortHandlerWindows(port_name)); + return static_cast(new PortHandlerWindows(port_name)); #elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) || defined(ARDUINO_OpenRB) - return (PortHandler *)(new PortHandlerArduino(port_name)); + return static_cast(new PortHandlerArduino(port_name)); #endif } diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_arduino.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_arduino.cpp index 9542ec54f..86eac0ddf 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_arduino.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_arduino.cpp @@ -97,12 +97,12 @@ void PortHandlerArduino::clearPort() { int temp __attribute__((unused)); #if defined(__OPENCR__) || defined(ARDUINO_OpenRB) - while (DYNAMIXEL_SERIAL.available()) + while (DYNAMIXEL_SERIAL.available()) { temp = DYNAMIXEL_SERIAL.read(); } #elif defined(__OPENCM904__) - while (p_dxl_serial->available()) + while (p_dxl_serial->available()) { temp = p_dxl_serial->read(); } @@ -194,7 +194,7 @@ int PortHandlerArduino::writePort(uint8_t *packet, int length) void PortHandlerArduino::setPacketTimeout(uint16_t packet_length) { packet_start_time_ = getCurrentTime(); - packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; + packet_timeout_ = (tx_time_per_byte * static_cast(packet_length)) + (LATENCY_TIMER * 2.0) + 2.0; } void PortHandlerArduino::setPacketTimeout(double msec) @@ -216,7 +216,7 @@ bool PortHandlerArduino::isPacketTimeout() double PortHandlerArduino::getCurrentTime() { - return (double)millis(); + return static_cast(millis()); } double PortHandlerArduino::getTimeSinceStart() @@ -241,7 +241,7 @@ bool PortHandlerArduino::setupPort(int baudrate) delay(100); - tx_time_per_byte = (1000.0 / (double)baudrate) * 10.0; + tx_time_per_byte = (1000.0 / static_cast(baudrate)) * 10.0; return true; } diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp index 5ba4091da..778c0125d 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp @@ -161,7 +161,7 @@ int PortHandlerLinux::writePort(uint8_t *packet, int length) void PortHandlerLinux::setPacketTimeout(uint16_t packet_length) { packet_start_time_ = getCurrentTime(); - packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; + packet_timeout_ = (tx_time_per_byte * static_cast(packet_length)) + (LATENCY_TIMER * 2.0) + 2.0; } void PortHandlerLinux::setPacketTimeout(double msec) @@ -184,7 +184,7 @@ double PortHandlerLinux::getCurrentTime() { struct timespec tv; clock_gettime(CLOCK_REALTIME, &tv); - return ((double)tv.tv_sec * 1000.0 + (double)tv.tv_nsec * 0.001 * 0.001); + return (static_cast(tv.tv_sec) * 1000.0 + static_cast(tv.tv_nsec) * 0.001 * 0.001); } double PortHandlerLinux::getTimeSinceStart() @@ -209,7 +209,7 @@ bool PortHandlerLinux::setupPort(int cflag_baud) return false; } - bzero(&newtio, sizeof(newtio)); // clear struct for new port settings + memset(&newtio, 0, sizeof(newtio)); // clear struct for new port settings newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD; newtio.c_iflag = IGNPAR; @@ -222,7 +222,7 @@ bool PortHandlerLinux::setupPort(int cflag_baud) tcflush(socket_fd_, TCIFLUSH); tcsetattr(socket_fd_, TCSANOW, &newtio); - tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; + tx_time_per_byte = (1000.0 / static_cast(baudrate_)) * 10.0; return true; } @@ -265,7 +265,7 @@ bool PortHandlerLinux::setCustomBaudrate(int speed) return false; } - tx_time_per_byte = (1000.0 / (double)speed) * 10.0; + tx_time_per_byte = (1000.0 / static_cast(speed)) * 10.0; return true; } diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_mac.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_mac.cpp index bb4eef229..fe36511d3 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_mac.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_mac.cpp @@ -129,8 +129,8 @@ int PortHandlerMac::writePort(uint8_t *packet, int length) while (unsent > 0) { ioctl(socket_fd_, TIOCOUTQ, &unsent); } - unsigned int tx_time = ((float)(length * 10) / baudrate_) * 1000000000; - struct timespec delay = {0, tx_time}; + unsigned int tx_time = (static_cast(length * 10) / baudrate_) * 1000000000; + struct timespec delay = {0, static_cast(tx_time)}; nanosleep(&delay, NULL); return result; @@ -139,7 +139,7 @@ int PortHandlerMac::writePort(uint8_t *packet, int length) void PortHandlerMac::setPacketTimeout(uint16_t packet_length) { packet_start_time_ = getCurrentTime(); - packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; + packet_timeout_ = (tx_time_per_byte * static_cast(packet_length)) + (LATENCY_TIMER * 2.0) + 2.0; } void PortHandlerMac::setPacketTimeout(double msec) @@ -172,7 +172,7 @@ double PortHandlerMac::getCurrentTime() #else clock_gettime(CLOCK_REALTIME, &tv); #endif - return ((double)tv.tv_sec * 1000.0 + (double)tv.tv_nsec * 0.001 * 0.001); + return (static_cast(tv.tv_sec) * 1000.0 + static_cast(tv.tv_nsec) * 0.001 * 0.001); } double PortHandlerMac::getTimeSinceStart() @@ -212,7 +212,7 @@ bool PortHandlerMac::setupPort(int cflag_baud) tcflush(socket_fd_, TCIFLUSH); tcsetattr(socket_fd_, TCSANOW, &newtio); - tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; + tx_time_per_byte = (1000.0 / static_cast(baudrate_)) * 10.0; return true; } diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_windows.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_windows.cpp index d9b0ed55c..20a2c2b89 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_windows.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_windows.cpp @@ -98,8 +98,8 @@ int PortHandlerWindows::getBytesAvailable() DWORD retbyte = 2; BOOL res = DeviceIoControl(serial_handle_, GENERIC_READ | GENERIC_WRITE, NULL, 0, 0, 0, &retbyte, (LPOVERLAPPED)NULL); - printf("%d", (int)res); - return (int)retbyte; + printf("%d", static_cast(res)); + return static_cast(retbyte); } int PortHandlerWindows::readPort(uint8_t *packet, int length) @@ -109,7 +109,7 @@ int PortHandlerWindows::readPort(uint8_t *packet, int length) if (ReadFile(serial_handle_, packet, (DWORD)length, &dwRead, NULL) == FALSE) return -1; - return (int)dwRead; + return static_cast(dwRead); } int PortHandlerWindows::writePort(uint8_t *packet, int length) @@ -119,13 +119,13 @@ int PortHandlerWindows::writePort(uint8_t *packet, int length) if (WriteFile(serial_handle_, packet, (DWORD)length, &dwWrite, NULL) == FALSE) return -1; - return (int)dwWrite; + return static_cast(dwWrite); } void PortHandlerWindows::setPacketTimeout(uint16_t packet_length) { packet_start_time_ = getCurrentTime(); - packet_timeout_ = (tx_time_per_byte_ * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; + packet_timeout_ = (tx_time_per_byte_ * static_cast(packet_length)) + (LATENCY_TIMER * 2.0) + 2.0; } void PortHandlerWindows::setPacketTimeout(double msec) @@ -148,7 +148,7 @@ double PortHandlerWindows::getCurrentTime() { QueryPerformanceCounter(&counter_); QueryPerformanceFrequency(&freq_); - return (double)counter_.QuadPart / (double)freq_.QuadPart * 1000.0; + return static_cast(counter_.QuadPart) / static_cast(freq_.QuadPart) * 1000.0; } double PortHandlerWindows::getTimeSinceStart() @@ -224,7 +224,7 @@ bool PortHandlerWindows::setupPort(int baudrate) if (SetCommTimeouts(serial_handle_, &timeouts) == FALSE) goto DXL_HAL_OPEN_ERROR; - tx_time_per_byte_ = (1000.0 / (double)baudrate_) * 10.0; + tx_time_per_byte_ = (1000.0 / static_cast(baudrate_)) * 10.0; return true; DXL_HAL_OPEN_ERROR: diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp index 849075844..e110b508e 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp @@ -16,6 +16,9 @@ /* Author: zerom, Ryu Woon Jung (Leon) */ +#include +#include + #if defined(__linux__) #include "protocol1_packet_handler.h" #elif defined(__APPLE__) @@ -30,6 +33,11 @@ #include #include +namespace { + thread_local std::vector g_txpacket_buffer; + thread_local std::vector g_rxpacket_buffer; +} + #define TXPACKET_MAX_LEN (250) #define RXPACKET_MAX_LEN (250) @@ -293,11 +301,11 @@ int Protocol1PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uin // set packet timeout if (txpacket[PKT_INSTRUCTION] == INST_READ) { - port->setPacketTimeout((uint16_t)(txpacket[PKT_PARAMETER0+1] + 6)); + port->setPacketTimeout(static_cast(txpacket[PKT_PARAMETER0+1] + 6)); } else { - port->setPacketTimeout((uint16_t)6); // HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM + port->setPacketTimeout(static_cast(6)); // HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM } // rx packet @@ -308,7 +316,7 @@ int Protocol1PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uin if (result == COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID]) { if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; + *error = rxpacket[PKT_ERROR]; } return result; @@ -399,14 +407,14 @@ int Protocol1PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t addre txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = 4; txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)address; - txpacket[PKT_PARAMETER0+1] = (uint8_t)length; + txpacket[PKT_PARAMETER0+0] = static_cast(address); + txpacket[PKT_PARAMETER0+1] = static_cast(length); result = txPacket(port, txpacket); // set packet timeout if (result == COMM_SUCCESS) - port->setPacketTimeout((uint16_t)(length+6)); + port->setPacketTimeout(static_cast(length+6)); return result; } @@ -414,21 +422,21 @@ int Protocol1PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t addre int Protocol1PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); //(length+6); - //uint8_t *rxpacket = new uint8_t[length+6]; - if (rxpacket == NULL) - return result; + if (g_rxpacket_buffer.capacity() < RXPACKET_MAX_LEN) + g_rxpacket_buffer.reserve(RXPACKET_MAX_LEN); + g_rxpacket_buffer.resize(RXPACKET_MAX_LEN); + std::vector& rxpacket = g_rxpacket_buffer; do { - result = rxPacket(port, rxpacket); + result = rxPacket(port, rxpacket.data()); } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); if (result == COMM_SUCCESS && rxpacket[PKT_ID] == id) { if (error != 0) { - *error = (uint8_t)rxpacket[PKT_ERROR]; + *error = rxpacket[PKT_ERROR]; } for (uint16_t s = 0; s < length; s++) { @@ -437,8 +445,6 @@ int Protocol1PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t lengt //memcpy(data, &rxpacket[PKT_PARAMETER0], length); } - free(rxpacket); - //delete[] rxpacket; return result; } @@ -447,29 +453,29 @@ int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add int result = COMM_TX_FAIL; uint8_t txpacket[8] = {0}; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6); - if (rxpacket == NULL) - return result; + if (g_rxpacket_buffer.capacity() < RXPACKET_MAX_LEN) + g_rxpacket_buffer.reserve(RXPACKET_MAX_LEN); + g_rxpacket_buffer.resize(RXPACKET_MAX_LEN); + std::vector& rxpacket = g_rxpacket_buffer; if (id >= BROADCAST_ID) { - free(rxpacket); return COMM_NOT_AVAILABLE; } txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = 4; txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)address; - txpacket[PKT_PARAMETER0+1] = (uint8_t)length; + txpacket[PKT_PARAMETER0+0] = static_cast(address); + txpacket[PKT_PARAMETER0+1] = static_cast(length); - result = txRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket, rxpacket.data(), error); if (result == COMM_SUCCESS) { if (error != 0) { - *error = (uint8_t)rxpacket[PKT_ERROR]; + *error = rxpacket[PKT_ERROR]; } for (uint16_t s = 0; s < length; s++) { @@ -478,8 +484,6 @@ int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add //memcpy(data, &rxpacket[PKT_PARAMETER0], length); } - free(rxpacket); - //delete[] rxpacket; return result; } @@ -550,26 +554,23 @@ int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length+7); - //uint8_t *txpacket = new uint8_t[length+7]; - - if (txpacket == NULL) - return result; + if (g_txpacket_buffer.capacity() < static_cast(length + 7)) + g_txpacket_buffer.reserve(length + 7); + g_txpacket_buffer.resize(length + 7); + std::vector& txpacket = g_txpacket_buffer; txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; + txpacket[PKT_PARAMETER0] = static_cast(address); for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+1+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); port->is_using_ = false; - free(txpacket); - //delete[] txpacket; return result; } @@ -577,26 +578,23 @@ int Protocol1PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t ad { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length+7); //#6->7 - //uint8_t *txpacket = new uint8_t[length+7]; + if (g_txpacket_buffer.capacity() < static_cast(length + 7)) + g_txpacket_buffer.reserve(length + 7); + g_txpacket_buffer.resize(length + 7); + std::vector& txpacket = g_txpacket_buffer; uint8_t rxpacket[6] = {0}; - if (txpacket == NULL) - return result; - txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; + txpacket[PKT_PARAMETER0] = static_cast(address); for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+1+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - result = txRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket.data(), rxpacket, error); - free(txpacket); - //delete[] txpacket; return result; } @@ -637,26 +635,23 @@ int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16 { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length+6); - //uint8_t *txpacket = new uint8_t[length+6]; - - if (txpacket == NULL) - return result; + if (g_txpacket_buffer.capacity() < static_cast(length + 6)) + g_txpacket_buffer.reserve(length + 6); + g_txpacket_buffer.resize(length + 6); + std::vector& txpacket = g_txpacket_buffer; txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; + txpacket[PKT_PARAMETER0] = static_cast(address); for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+1+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); port->is_using_ = false; - free(txpacket); - //delete[] txpacket; return result; } @@ -664,26 +659,23 @@ int Protocol1PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length+6); - //uint8_t *txpacket = new uint8_t[length+6]; + if (g_txpacket_buffer.capacity() < static_cast(length + 6)) + g_txpacket_buffer.reserve(length + 6); + g_txpacket_buffer.resize(length + 6); + std::vector& txpacket = g_txpacket_buffer; uint8_t rxpacket[6] = {0}; - if (txpacket == NULL) - return result; - txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; + txpacket[PKT_PARAMETER0] = static_cast(address); for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+1+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - result = txRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket.data(), rxpacket, error); - free(txpacket); - //delete[] txpacket; return result; } @@ -696,12 +688,11 @@ int Protocol1PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length+8); // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM - //uint8_t *txpacket = new uint8_t[param_length + 8]; - - if (txpacket == NULL) - return result; + if (g_txpacket_buffer.capacity() < static_cast(param_length + 8)) + g_txpacket_buffer.reserve(param_length + 8); + g_txpacket_buffer.resize(param_length + 8); + std::vector& txpacket = g_txpacket_buffer; txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM @@ -713,10 +704,8 @@ int Protocol1PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad txpacket[PKT_PARAMETER0+2+s] = param[s]; //memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length); - result = txRxPacket(port, txpacket, 0, 0); + result = txRxPacket(port, txpacket.data(), 0, 0); - free(txpacket); - //delete[] txpacket; return result; } @@ -724,12 +713,11 @@ int Protocol1PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length+7); // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM - //uint8_t *txpacket = new uint8_t[param_length + 7]; - - if (txpacket == NULL) - return result; + if (g_txpacket_buffer.capacity() < static_cast(param_length + 7)) + g_txpacket_buffer.reserve(param_length + 7); + g_txpacket_buffer.resize(param_length + 7); + std::vector& txpacket = g_txpacket_buffer; txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM @@ -740,17 +728,15 @@ int Protocol1PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 txpacket[PKT_PARAMETER0+1+s] = param[s]; //memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); if (result == COMM_SUCCESS) { int wait_length = 0; for (uint16_t i = 0; i < param_length; i += 3) wait_length += param[i] + 7; - port->setPacketTimeout((uint16_t)wait_length); + port->setPacketTimeout(static_cast(wait_length)); } - free(txpacket); - //delete[] txpacket; return result; } diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp index ef48bd185..eee853204 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp @@ -16,6 +16,9 @@ /* Author: zerom, Ryu Woon Jung (Leon) */ +#include +#include + #if defined(__linux__) #include #include "protocol2_packet_handler.h" @@ -34,6 +37,11 @@ #include #include +namespace { + thread_local std::vector g_txpacket_buffer; + thread_local std::vector g_rxpacket_buffer; +} + #define TXPACKET_MAX_LEN (1*1024) #define RXPACKET_MAX_LEN (1*1024) @@ -184,7 +192,7 @@ unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *da for (uint16_t j = 0; j < data_blk_size; j++) { - i = ((uint16_t)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF; + i = (static_cast(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF; crc_accum = (crc_accum << 8) ^ crc_table[i]; } @@ -444,15 +452,15 @@ int Protocol2PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uin // set packet timeout if (txpacket[PKT_INSTRUCTION] == INST_READ) { - port->setPacketTimeout((uint16_t)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11)); + port->setPacketTimeout(static_cast(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11)); } else if (txpacket[PKT_INSTRUCTION] == INST_CLEAR) { - port->setPacketTimeout((double)10000); + port->setPacketTimeout(10000.0); } else { - port->setPacketTimeout((uint16_t)11); + port->setPacketTimeout(static_cast(11)); // HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H } @@ -464,7 +472,7 @@ int Protocol2PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uin if (result == COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID]) { if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; + *error = rxpacket[PKT_ERROR]; } return result; @@ -510,7 +518,7 @@ int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vectorgetBaudRate()) * 10.0; + double tx_time_per_byte = (1000.0 / static_cast(port->getBaudRate())) * 10.0; txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = 3; @@ -526,7 +534,7 @@ int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vectorsetPacketTimeout((uint16_t)(wait_length * 30)); - port->setPacketTimeout(((double)wait_length * tx_time_per_byte) + (3.0 * (double)MAX_ID) + 16.0); + port->setPacketTimeout((static_cast(wait_length) * tx_time_per_byte) + (3.0 * static_cast(MAX_ID)) + 16.0); while(1) { @@ -682,16 +690,16 @@ int Protocol2PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t addre txpacket[PKT_LENGTH_L] = 7; txpacket[PKT_LENGTH_H] = 0; txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (uint8_t)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (uint8_t)DXL_HIBYTE(length); + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(length); + txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(length); result = txPacket(port, txpacket); // set packet timeout if (result == COMM_SUCCESS) - port->setPacketTimeout((uint16_t)(length + 11)); + port->setPacketTimeout(static_cast(length + 11)); return result; } @@ -699,20 +707,20 @@ int Protocol2PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t addre int Protocol2PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); - //(length + 11 + (length/3)); // (length/3): consider stuffing - if (rxpacket == NULL) - return result; + if (g_rxpacket_buffer.capacity() < RXPACKET_MAX_LEN) + g_rxpacket_buffer.reserve(RXPACKET_MAX_LEN); + g_rxpacket_buffer.resize(RXPACKET_MAX_LEN); + std::vector& rxpacket = g_rxpacket_buffer; do { - result = rxPacket(port, rxpacket); + result = rxPacket(port, rxpacket.data()); } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); if (result == COMM_SUCCESS && rxpacket[PKT_ID] == id) { if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; + *error = rxpacket[PKT_ERROR]; for (uint16_t s = 0; s < length; s++) { @@ -721,8 +729,6 @@ int Protocol2PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t lengt //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); } - free(rxpacket); - //delete[] rxpacket; return result; } @@ -731,15 +737,14 @@ int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add int result = COMM_TX_FAIL; uint8_t txpacket[14] = {0}; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); - //(length + 11 + (length/3)); // (length/3): consider stuffing - - if (rxpacket == NULL) - return result; + if (g_rxpacket_buffer.capacity() < RXPACKET_MAX_LEN) + g_rxpacket_buffer.reserve(RXPACKET_MAX_LEN); + g_rxpacket_buffer.resize(RXPACKET_MAX_LEN); + std::vector& rxpacket = g_rxpacket_buffer; + if (id >= BROADCAST_ID) { - free(rxpacket); return COMM_NOT_AVAILABLE; } @@ -747,16 +752,16 @@ int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add txpacket[PKT_LENGTH_L] = 7; txpacket[PKT_LENGTH_H] = 0; txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (uint8_t)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (uint8_t)DXL_HIBYTE(length); + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(length); + txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(length); - result = txRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket, rxpacket.data(), error); if (result == COMM_SUCCESS) { if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; + *error = rxpacket[PKT_ERROR]; for (uint16_t s = 0; s < length; s++) { @@ -765,8 +770,6 @@ int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); } - free(rxpacket); - //delete[] rxpacket; return result; } @@ -838,27 +841,25 @@ int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); - - if (txpacket == NULL) - return result; + if (g_txpacket_buffer.capacity() < static_cast(length + 12 + (length / 3))) + g_txpacket_buffer.reserve(length + 12 + (length / 3)); + g_txpacket_buffer.resize(length + 12 + (length / 3)); + std::vector& txpacket = g_txpacket_buffer; txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(address); for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+2+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); port->is_using_ = false; - free(txpacket); - //delete[] txpacket; return result; } @@ -866,27 +867,25 @@ int Protocol2PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t ad { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + if (g_txpacket_buffer.capacity() < static_cast(length + 12 + (length / 3))) + g_txpacket_buffer.reserve(length + 12 + (length / 3)); + g_txpacket_buffer.resize(length + 12 + (length / 3)); + std::vector& txpacket = g_txpacket_buffer; uint8_t rxpacket[11] = {0}; - if (txpacket == NULL) - return result; - txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(address); for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+2+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - result = txRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket.data(), rxpacket, error); - free(txpacket); - //delete[] txpacket; return result; } @@ -927,27 +926,25 @@ int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16 { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); - - if (txpacket == NULL) - return result; + if (g_txpacket_buffer.capacity() < static_cast(length + 12 + (length / 3))) + g_txpacket_buffer.reserve(length + 12 + (length / 3)); + g_txpacket_buffer.resize(length + 12 + (length / 3)); + std::vector& txpacket = g_txpacket_buffer; txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(address); for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+2+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); port->is_using_ = false; - free(txpacket); - //delete[] txpacket; return result; } @@ -955,27 +952,25 @@ int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + if (g_txpacket_buffer.capacity() < static_cast(length + 12 + (length / 3))) + g_txpacket_buffer.reserve(length + 12 + (length / 3)); + g_txpacket_buffer.resize(length + 12 + (length / 3)); + std::vector& txpacket = g_txpacket_buffer; uint8_t rxpacket[11] = {0}; - if (txpacket == NULL) - return result; - txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(address); for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+2+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - result = txRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket.data(), rxpacket, error); - free(txpacket); - //delete[] txpacket; return result; } @@ -983,12 +978,12 @@ int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + if (g_txpacket_buffer.capacity() < static_cast(param_length + 14 + (param_length / 3))) + g_txpacket_buffer.reserve(param_length + 14 + (param_length / 3)); + g_txpacket_buffer.resize(param_length + 14 + (param_length / 3)); + std::vector& txpacket = g_txpacket_buffer; - if (txpacket == NULL) - return result; - txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -1002,11 +997,10 @@ int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address txpacket[PKT_PARAMETER0+4+s] = param[s]; //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); if (result == COMM_SUCCESS) - port->setPacketTimeout((uint16_t)((11 + data_length) * param_length)); + port->setPacketTimeout(static_cast((11 + data_length) * param_length)); - free(txpacket); return result; } @@ -1014,12 +1008,12 @@ int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + if (g_txpacket_buffer.capacity() < static_cast(param_length + 14 + (param_length / 3))) + g_txpacket_buffer.reserve(param_length + 14 + (param_length / 3)); + g_txpacket_buffer.resize(param_length + 14 + (param_length / 3)); + std::vector& txpacket = g_txpacket_buffer; - if (txpacket == NULL) - return result; - txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -1033,10 +1027,8 @@ int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad txpacket[PKT_PARAMETER0+4+s] = param[s]; //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - result = txRxPacket(port, txpacket, 0, 0); + result = txRxPacket(port, txpacket.data(), 0, 0); - free(txpacket); - //delete[] txpacket; return result; } @@ -1044,12 +1036,12 @@ int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + if (g_txpacket_buffer.capacity() < static_cast(param_length + 10 + (param_length / 3))) + g_txpacket_buffer.reserve(param_length + 10 + (param_length / 3)); + g_txpacket_buffer.resize(param_length + 10 + (param_length / 3)); + std::vector& txpacket = g_txpacket_buffer; - if (txpacket == NULL) - return result; - txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H @@ -1059,17 +1051,15 @@ int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 txpacket[PKT_PARAMETER0+s] = param[s]; //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); if (result == COMM_SUCCESS) { int wait_length = 0; for (uint16_t i = 0; i < param_length; i += 5) wait_length += DXL_MAKEWORD(param[i+3], param[i+4]) + 10; - port->setPacketTimeout((uint16_t)wait_length); + port->setPacketTimeout(static_cast(wait_length)); } - free(txpacket); - //delete[] txpacket; return result; } @@ -1077,12 +1067,12 @@ int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, u { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + if (g_txpacket_buffer.capacity() < static_cast(param_length + 10 + (param_length / 3))) + g_txpacket_buffer.reserve(param_length + 10 + (param_length / 3)); + g_txpacket_buffer.resize(param_length + 10 + (param_length / 3)); + std::vector& txpacket = g_txpacket_buffer; - if (txpacket == NULL) - return result; - txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H @@ -1092,10 +1082,8 @@ int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, u txpacket[PKT_PARAMETER0+s] = param[s]; //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - result = txRxPacket(port, txpacket, 0, 0); + result = txRxPacket(port, txpacket.data(), 0, 0); - free(txpacket); - //delete[] txpacket; return result; } @@ -1103,11 +1091,11 @@ int Protocol2PacketHandler::fastSyncReadTx(PortHandler *port, uint16_t start_add { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - - if (NULL == txpacket) - return result; + if (g_txpacket_buffer.capacity() < static_cast(param_length + 14 + (param_length / 3))) + g_txpacket_buffer.reserve(param_length + 14 + (param_length / 3)); + g_txpacket_buffer.resize(param_length + 14 + (param_length / 3)); + std::vector& txpacket = g_txpacket_buffer; txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -1122,11 +1110,10 @@ int Protocol2PacketHandler::fastSyncReadTx(PortHandler *port, uint16_t start_add txpacket[PKT_PARAMETER0 + 4 + s] = param[s]; //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); if (COMM_SUCCESS == result) - port->setPacketTimeout((uint16_t)((11 + data_length) * param_length)); + port->setPacketTimeout(static_cast((11 + data_length) * param_length)); - free(txpacket); return result; } @@ -1134,11 +1121,11 @@ int Protocol2PacketHandler::fastBulkReadTx(PortHandler *port, uint8_t *param, ui { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - - if (NULL == txpacket) - return result; + if (g_txpacket_buffer.capacity() < static_cast(param_length + 10 + (param_length / 3))) + g_txpacket_buffer.reserve(param_length + 10 + (param_length / 3)); + g_txpacket_buffer.resize(param_length + 10 + (param_length / 3)); + std::vector& txpacket = g_txpacket_buffer; txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H @@ -1149,15 +1136,13 @@ int Protocol2PacketHandler::fastBulkReadTx(PortHandler *port, uint8_t *param, ui txpacket[PKT_PARAMETER0 + s] = param[s]; //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket.data()); if (result == COMM_SUCCESS) { int wait_length = 0; for (uint16_t i = 0; i < param_length; i += 5) wait_length += DXL_MAKEWORD(param[i + 3], param[i + 4]) + 10; - port->setPacketTimeout((uint16_t)wait_length); + port->setPacketTimeout(static_cast(wait_length)); } - free(txpacket); - //delete[] txpacket; return result; } From afa3b9672f2a37e046f26cc2a09bb07a58667ef5 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Thu, 9 Apr 2026 10:45:31 +0900 Subject: [PATCH 03/10] Use static path when uninstalling the package --- c++/CMakeLists.txt | 19 ++++-- c++/cmake/cmake_uninstall.cmake.in | 93 +++++++++++++++--------------- 2 files changed, 63 insertions(+), 49 deletions(-) diff --git a/c++/CMakeLists.txt b/c++/CMakeLists.txt index ef1f6bfcf..d98644a14 100644 --- a/c++/CMakeLists.txt +++ b/c++/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.10) # Standalone C++ SDK: configure/build from the c++/ directory (paths below are relative to c++/). -project(dynamixel_sdk VERSION 4.0.4 LANGUAGES CXX) +project(dynamixel_sdk VERSION 5.0.0 LANGUAGES CXX) set(_dxl_pkg "dynamixel_sdk") @@ -73,7 +73,7 @@ target_include_directories(dynamixel_sdk PRIVATE set_target_properties(dynamixel_sdk PROPERTIES VERSION ${PROJECT_VERSION} - SOVERSION 2 + SOVERSION 3 CXX_STANDARD 17 CXX_STANDARD_REQUIRED ON OUTPUT_NAME "dxl_cpp" @@ -159,12 +159,23 @@ if(DYNAMIXEL_SDK_RUN_LDCONFIG AND UNIX AND NOT APPLE) endif() # -------------------------------------------------------- -# 4. Uninstall / Reinstall Targets +# 4. Uninstall / Reinstall Targets (name-based; no install_manifest.txt required) # -------------------------------------------------------- +if(UNIX AND NOT APPLE) + set(DXL_UNINSTALL_EASY_SDK 1) +else() + set(DXL_UNINSTALL_EASY_SDK 0) +endif() +if(WIN32) + set(DXL_UNINSTALL_WIN32 1) +else() + set(DXL_UNINSTALL_WIN32 0) +endif() + configure_file( "${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake_uninstall.cmake.in" "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake" - IMMEDIATE @ONLY + @ONLY ) if(NOT TARGET uninstall) diff --git a/c++/cmake/cmake_uninstall.cmake.in b/c++/cmake/cmake_uninstall.cmake.in index ef75962ab..00f9d7431 100644 --- a/c++/cmake/cmake_uninstall.cmake.in +++ b/c++/cmake/cmake_uninstall.cmake.in @@ -1,53 +1,56 @@ -# Uninstall files listed in install_manifest.txt (created by cmake --install). -# Configured from cmake_uninstall.cmake.in via configure_file(... IMMEDIATE @ONLY). - -if(NOT EXISTS "@CMAKE_BINARY_DIR@/install_manifest.txt") - message(FATAL_ERROR - "Cannot find @CMAKE_BINARY_DIR@/install_manifest.txt\n" - "Run: cmake --install (or make install) at least once so the manifest exists." - ) -endif() +# Name-based uninstall for Dynamixel SDK (C++), under CMAKE_INSTALL_PREFIX. +# Removes the same layout as install(): headers, libdxl_cpp.*, CMake package config, +# share/dynamixel_sdk (incl. control_table), and dynamixel_easy_sdk headers on Linux. -file(STRINGS "@CMAKE_BINARY_DIR@/install_manifest.txt" _dxl_installed_files) +set(_dxl_prefix "@CMAKE_INSTALL_PREFIX@") +string(REGEX REPLACE "/+$" "" _dxl_prefix "${_dxl_prefix}") -foreach(_dxl_file IN LISTS _dxl_installed_files) - string(STRIP "${_dxl_file}" _dxl_file) - if(_dxl_file STREQUAL "") - continue() - endif() - message(STATUS "Uninstalling: ${_dxl_file}") - # EXISTS is false for broken symlinks, so also check IS_SYMLINK. - if(EXISTS "${_dxl_file}" OR IS_SYMLINK "${_dxl_file}") - file(REMOVE "${_dxl_file}") - endif() -endforeach() - -# Remove installed directories (manifest only removes files). -# Derive prefix from install_manifest.txt paths so --prefix overrides work. -set(_dxl_prefix "") -foreach(_dxl_file IN LISTS _dxl_installed_files) - string(STRIP "${_dxl_file}" _dxl_file_stripped) - if(_dxl_file_stripped STREQUAL "") - continue() - endif() +set(_dxl_inc "@CMAKE_INSTALL_INCLUDEDIR@") +set(_dxl_lib "@CMAKE_INSTALL_LIBDIR@") +set(_dxl_share "@CMAKE_INSTALL_DATAROOTDIR@") +set(_dxl_bin "@CMAKE_INSTALL_BINDIR@") - # Example: - # ${prefix}/include/dynamixel_sdk/dynamixel_sdk.h - if(_dxl_file_stripped MATCHES "^(.*)/@CMAKE_INSTALL_INCLUDEDIR@/dynamixel_sdk/.*$") - set(_dxl_prefix "${CMAKE_MATCH_1}") - break() - endif() -endforeach() +message(STATUS "Dynamixel SDK (C++) uninstall from prefix: ${_dxl_prefix}") -if(NOT _dxl_prefix STREQUAL "") - file(REMOVE_RECURSE "${_dxl_prefix}/@CMAKE_INSTALL_INCLUDEDIR@/dynamixel_sdk") - if(NOT APPLE) - file(REMOVE_RECURSE "${_dxl_prefix}/@CMAKE_INSTALL_INCLUDEDIR@/dynamixel_easy_sdk") +macro(_dxl_remove_tree _path) + if(EXISTS "${_path}") + file(REMOVE_RECURSE "${_path}") + message(STATUS "Removed ${_path}") endif() +endmacro() - if(NOT APPLE) - file(REMOVE_RECURSE "${_dxl_prefix}/@CMAKE_INSTALL_DATAROOTDIR@/dynamixel_sdk/control_table") - endif() +# Headers: dynamixel_sdk (all platforms) +_dxl_remove_tree("${_dxl_prefix}/${_dxl_inc}/dynamixel_sdk") + +# dynamixel_easy_sdk: same condition as install() (Linux, not Apple) +if(@DXL_UNINSTALL_EASY_SDK@) + _dxl_remove_tree("${_dxl_prefix}/${_dxl_inc}/dynamixel_easy_sdk") +endif() - file(REMOVE_RECURSE "${_dxl_prefix}/@CMAKE_INSTALL_LIBDIR@/cmake/dynamixel_sdk") +# Shared library / import lib +if(@DXL_UNINSTALL_WIN32@) + file(GLOB _dxl_dlls LIST_DIRECTORIES false "${_dxl_prefix}/${_dxl_bin}/dxl_cpp.dll") + foreach(_f IN LISTS _dxl_dlls) + message(STATUS "Uninstalling: ${_f}") + file(REMOVE "${_f}") + endforeach() + foreach(_name "dxl_cpp.lib" "dxl_cpp.exp") + set(_f "${_dxl_prefix}/${_dxl_lib}/${_name}") + if(EXISTS "${_f}") + message(STATUS "Uninstalling: ${_f}") + file(REMOVE "${_f}") + endif() + endforeach() +else() + file(GLOB _dxl_libs LIST_DIRECTORIES false "${_dxl_prefix}/${_dxl_lib}/libdxl_cpp.*") + foreach(_f IN LISTS _dxl_libs) + message(STATUS "Uninstalling: ${_f}") + file(REMOVE "${_f}") + endforeach() endif() + +# CMake package files +_dxl_remove_tree("${_dxl_prefix}/${_dxl_lib}/cmake/dynamixel_sdk") + +# share (e.g. control_table) +_dxl_remove_tree("${_dxl_prefix}/${_dxl_share}/dynamixel_sdk") From 6e10654647025f7a66d242c690bddd9e266843a7 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Thu, 9 Apr 2026 10:46:57 +0900 Subject: [PATCH 04/10] Update version to 5.0.0 across all packages and documentation, aligning with the new release. Enhancements include safer dynamic storage in C++ SDK and unified versioning. --- Doxyfile | 2 +- ReleaseNote.md | 7 +++++++ c++/build/linux32/Makefile | 2 +- c++/build/linux64/Makefile | 2 +- c++/build/linux_sbc/Makefile | 2 +- c++/build/mac/Makefile | 2 +- c++/library.properties | 2 +- c/CMakeLists.txt | 3 +-- python/pyproject.toml | 2 +- python/setup.py | 2 +- ros/dynamixel_sdk/CHANGELOG.rst | 8 ++++++++ ros/dynamixel_sdk/CMakeLists.txt | 7 ++++++- ros/dynamixel_sdk/package.xml | 2 +- ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst | 5 +++++ ros/dynamixel_sdk_custom_interfaces/package.xml | 2 +- ros/dynamixel_sdk_examples/CHANGELOG.rst | 5 +++++ ros/dynamixel_sdk_examples/package.xml | 2 +- 17 files changed, 43 insertions(+), 14 deletions(-) diff --git a/Doxyfile b/Doxyfile index 5e72aa9be..b38532e67 100644 --- a/Doxyfile +++ b/Doxyfile @@ -38,7 +38,7 @@ PROJECT_NAME = "DynamixelSDK c++" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = 4.0.4 +PROJECT_NUMBER = 5.0.0 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/ReleaseNote.md b/ReleaseNote.md index 8bf7d3f15..e11df517e 100644 --- a/ReleaseNote.md +++ b/ReleaseNote.md @@ -1,5 +1,12 @@ # Dynamixel SDK Release Notes +5.0.0 (2026-04-09) +------------------ +* Updated the standalone C++ SDK and ROS 2 C++ wrappers to use safer dynamic storage for group and packet buffers +* Bumped the C++ shared library ABI to SOVERSION 3 due to public C++ class layout changes +* Unified package versioning to 5.0.0 across C, C++, Python, and ROS 2 packaging metadata +* Contributors: Hyungyu Kim + 4.0.4 (2026-03-27) ------------------ * Added CMakeLists.txt for unified build system in c, c++ diff --git a/c++/build/linux32/Makefile b/c++/build/linux32/Makefile index cb026b270..0c87e17d9 100644 --- a/c++/build/linux32/Makefile +++ b/c++/build/linux32/Makefile @@ -10,7 +10,7 @@ DIR_OBJS = ./.objects INSTALL_ROOT = /usr/local -MAJ_VERSION = 2 +MAJ_VERSION = 3 MIN_VERSION = 0 REV_VERSION = 0 diff --git a/c++/build/linux64/Makefile b/c++/build/linux64/Makefile index ea9ad8244..f139ad582 100644 --- a/c++/build/linux64/Makefile +++ b/c++/build/linux64/Makefile @@ -10,7 +10,7 @@ DIR_OBJS = ./.objects INSTALL_ROOT = /usr/local -MAJ_VERSION = 2 +MAJ_VERSION = 3 MIN_VERSION = 0 REV_VERSION = 0 diff --git a/c++/build/linux_sbc/Makefile b/c++/build/linux_sbc/Makefile index 79eecde26..f96ce88e3 100644 --- a/c++/build/linux_sbc/Makefile +++ b/c++/build/linux_sbc/Makefile @@ -10,7 +10,7 @@ DIR_OBJS = ./.objects INSTALL_ROOT = /usr/local -MAJ_VERSION = 2 +MAJ_VERSION = 3 MIN_VERSION = 0 REV_VERSION = 0 diff --git a/c++/build/mac/Makefile b/c++/build/mac/Makefile index 83abdee67..2a2c567b0 100644 --- a/c++/build/mac/Makefile +++ b/c++/build/mac/Makefile @@ -10,7 +10,7 @@ DIR_OBJS = ./.objects INSTALL_ROOT = /usr/local -MAJ_VERSION = 2 +MAJ_VERSION = 3 MIN_VERSION = 0 REV_VERSION = 0 diff --git a/c++/library.properties b/c++/library.properties index 8bc4695ef..a7434b862 100644 --- a/c++/library.properties +++ b/c++/library.properties @@ -1,5 +1,5 @@ name=DynamixelSDK -version=4.0.4 +version=5.0.0 author=ROBOTIS maintainer=ROBOTIS sentence=DynamixelSDK for Arduino diff --git a/c/CMakeLists.txt b/c/CMakeLists.txt index 660740f3b..345c43cf4 100644 --- a/c/CMakeLists.txt +++ b/c/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.10) # Standalone C SDK: configure/build from the c/ directory (paths below are relative to c/). -project(dynamixel_sdk_c VERSION 4.0.4 LANGUAGES C) +project(dynamixel_sdk_c VERSION 5.0.0 LANGUAGES C) # -------------------------------------------------------- # 1. Build Environment Configuration @@ -148,4 +148,3 @@ if(NOT TARGET reinstall) COMMENT "Reinstalling DynamixelSDK (C)..." ) endif() - diff --git a/python/pyproject.toml b/python/pyproject.toml index 65ae0c760..22f8836bf 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "dynamixel-sdk" -version = "4.0.4" +version = "5.0.0" description = "Dynamixel SDK 4. python package" readme = "README.txt" license = { text = "Apache 2.0" } diff --git a/python/setup.py b/python/setup.py index cc73e51af..9d2906970 100644 --- a/python/setup.py +++ b/python/setup.py @@ -14,7 +14,7 @@ setup( name='dynamixel_sdk', - version='4.0.4', + version='5.0.0', packages=['dynamixel_sdk'], package_dir={'': 'src'}, license='Apache 2.0', diff --git a/ros/dynamixel_sdk/CHANGELOG.rst b/ros/dynamixel_sdk/CHANGELOG.rst index 9354d39ca..a2b514db9 100644 --- a/ros/dynamixel_sdk/CHANGELOG.rst +++ b/ros/dynamixel_sdk/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package dynamixel_sdk ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +5.0.0 (2026-04-09) +------------------ +* Updated the standalone C++ SDK and ROS 2 C++ wrappers to use safer dynamic storage for group and packet buffers +* Bumped the C++ shared library ABI to SOVERSION 3 due to public C++ class layout changes +* Added VERSION 5.0.0 and SOVERSION 3 to the ROS 2 shared library target for consistent ABI versioning +* Unified package versioning to 5.0.0 +* Contributors: Hyungyu Kim + 4.0.4 (2026-03-27) ------------------ * None diff --git a/ros/dynamixel_sdk/CMakeLists.txt b/ros/dynamixel_sdk/CMakeLists.txt index 74838eece..448856f39 100644 --- a/ros/dynamixel_sdk/CMakeLists.txt +++ b/ros/dynamixel_sdk/CMakeLists.txt @@ -2,7 +2,7 @@ # Set minimum required version of cmake, project name and compile options ################################################################################ cmake_minimum_required(VERSION 3.5) -project(dynamixel_sdk) +project(dynamixel_sdk VERSION 5.0.0) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) @@ -66,6 +66,11 @@ else() ) endif() +set_target_properties(dynamixel_sdk PROPERTIES + VERSION ${PROJECT_VERSION} + SOVERSION 3 +) + ################################################################################ # Install ################################################################################ diff --git a/ros/dynamixel_sdk/package.xml b/ros/dynamixel_sdk/package.xml index 1ef11f19f..e45d789af 100644 --- a/ros/dynamixel_sdk/package.xml +++ b/ros/dynamixel_sdk/package.xml @@ -2,7 +2,7 @@ dynamixel_sdk - 4.0.4 + 5.0.0 This package is wrapping version of ROBOTIS Dynamixel SDK for ROS 2. The ROBOTIS Dynamixel SDK, or SDK, is a software development library that provides Dynamixel control functions for packet communication. The API is designed for Dynamixel actuators and Dynamixel-based platforms. diff --git a/ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst b/ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst index 5014c4728..036a1b33f 100644 --- a/ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst +++ b/ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package dynamixel_sdk_custom_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +5.0.0 (2026-04-09) +------------------ +* Updated package versioning to 5.0.0 to align with the Dynamixel SDK 5.0.0 release +* Contributors: Hyungyu Kim + 4.0.4 (2026-03-27) ------------------ * None diff --git a/ros/dynamixel_sdk_custom_interfaces/package.xml b/ros/dynamixel_sdk_custom_interfaces/package.xml index 74dc24821..c1a294299 100644 --- a/ros/dynamixel_sdk_custom_interfaces/package.xml +++ b/ros/dynamixel_sdk_custom_interfaces/package.xml @@ -2,7 +2,7 @@ dynamixel_sdk_custom_interfaces - 4.0.4 + 5.0.0 ROS 2 custom interface examples using ROBOTIS DYNAMIXEL SDK Pyo Apache 2.0 diff --git a/ros/dynamixel_sdk_examples/CHANGELOG.rst b/ros/dynamixel_sdk_examples/CHANGELOG.rst index 475553a30..2ca98d54f 100644 --- a/ros/dynamixel_sdk_examples/CHANGELOG.rst +++ b/ros/dynamixel_sdk_examples/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package dynamixel_sdk_examples ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +5.0.0 (2026-04-09) +------------------ +* Updated package versioning to 5.0.0 to align with the Dynamixel SDK 5.0.0 release +* Contributors: Hyungyu Kim + 4.0.4 (2026-03-27) ------------------ * None diff --git a/ros/dynamixel_sdk_examples/package.xml b/ros/dynamixel_sdk_examples/package.xml index e02e99d37..3e9275658 100644 --- a/ros/dynamixel_sdk_examples/package.xml +++ b/ros/dynamixel_sdk_examples/package.xml @@ -1,7 +1,7 @@ dynamixel_sdk_examples - 4.0.4 + 5.0.0 ROS 2 examples using ROBOTIS DYNAMIXEL SDK Apache 2.0 Pyo From 6efd6d2b03f186d691b444619a43000fa78bb266 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Thu, 9 Apr 2026 11:40:08 +0900 Subject: [PATCH 05/10] Refactor CMakeLists.txt by removing redundant comment on uninstall/reinstall targets for clarity. --- c++/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/c++/CMakeLists.txt b/c++/CMakeLists.txt index d98644a14..ea0b63374 100644 --- a/c++/CMakeLists.txt +++ b/c++/CMakeLists.txt @@ -159,7 +159,7 @@ if(DYNAMIXEL_SDK_RUN_LDCONFIG AND UNIX AND NOT APPLE) endif() # -------------------------------------------------------- -# 4. Uninstall / Reinstall Targets (name-based; no install_manifest.txt required) +# 4. Uninstall / Reinstall Targets # -------------------------------------------------------- if(UNIX AND NOT APPLE) set(DXL_UNINSTALL_EASY_SDK 1) From 9e61bcaa69b05c4449927d3e796316543d7a7f2c Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Thu, 9 Apr 2026 11:46:31 +0900 Subject: [PATCH 06/10] Enhance ReleaseNote.md with new versioning details: added VERSION 5.0.0 and SOVERSION 3 for ROS 2 shared library, and updated uninstall process to remove SDK-owned paths by name. --- ReleaseNote.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ReleaseNote.md b/ReleaseNote.md index e11df517e..80a0dcee1 100644 --- a/ReleaseNote.md +++ b/ReleaseNote.md @@ -4,6 +4,8 @@ ------------------ * Updated the standalone C++ SDK and ROS 2 C++ wrappers to use safer dynamic storage for group and packet buffers * Bumped the C++ shared library ABI to SOVERSION 3 due to public C++ class layout changes +* Added VERSION 5.0.0 and SOVERSION 3 to the ROS 2 shared library target for consistent ABI versioning +* Changed the C++ uninstall step to remove SDK-owned install paths by name instead of relying on the install manifest * Unified package versioning to 5.0.0 across C, C++, Python, and ROS 2 packaging metadata * Contributors: Hyungyu Kim From 60ed1518a6078c87915ca16db4e21d4a81ef0d66 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Mon, 13 Apr 2026 17:57:48 +0900 Subject: [PATCH 07/10] Refactor Protocol1 and Protocol2 packet handlers to eliminate thread-local buffers, replacing them with stack-allocated vectors for improved memory management and performance. --- .../protocol1_packet_handler.cpp | 51 +++---------- .../protocol2_packet_handler.cpp | 71 ++++--------------- .../protocol1_packet_handler.cpp | 51 +++---------- .../protocol2_packet_handler.cpp | 71 ++++--------------- 4 files changed, 40 insertions(+), 204 deletions(-) diff --git a/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp b/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp index e110b508e..cfa14862d 100644 --- a/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp +++ b/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp @@ -33,11 +33,6 @@ #include #include -namespace { - thread_local std::vector g_txpacket_buffer; - thread_local std::vector g_rxpacket_buffer; -} - #define TXPACKET_MAX_LEN (250) #define RXPACKET_MAX_LEN (250) @@ -422,11 +417,7 @@ int Protocol1PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t addre int Protocol1PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - - if (g_rxpacket_buffer.capacity() < RXPACKET_MAX_LEN) - g_rxpacket_buffer.reserve(RXPACKET_MAX_LEN); - g_rxpacket_buffer.resize(RXPACKET_MAX_LEN); - std::vector& rxpacket = g_rxpacket_buffer; + std::vector rxpacket(RXPACKET_MAX_LEN); do { result = rxPacket(port, rxpacket.data()); @@ -453,11 +444,7 @@ int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add int result = COMM_TX_FAIL; uint8_t txpacket[8] = {0}; - - if (g_rxpacket_buffer.capacity() < RXPACKET_MAX_LEN) - g_rxpacket_buffer.reserve(RXPACKET_MAX_LEN); - g_rxpacket_buffer.resize(RXPACKET_MAX_LEN); - std::vector& rxpacket = g_rxpacket_buffer; + std::vector rxpacket(RXPACKET_MAX_LEN); if (id >= BROADCAST_ID) { @@ -553,11 +540,7 @@ int Protocol1PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_ int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { int result = COMM_TX_FAIL; - - if (g_txpacket_buffer.capacity() < static_cast(length + 7)) - g_txpacket_buffer.reserve(length + 7); - g_txpacket_buffer.resize(length + 7); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(length + 7); txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; @@ -577,11 +560,7 @@ int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t int Protocol1PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - - if (g_txpacket_buffer.capacity() < static_cast(length + 7)) - g_txpacket_buffer.reserve(length + 7); - g_txpacket_buffer.resize(length + 7); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(length + 7); uint8_t rxpacket[6] = {0}; txpacket[PKT_ID] = id; @@ -634,11 +613,7 @@ int Protocol1PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16 int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { int result = COMM_TX_FAIL; - - if (g_txpacket_buffer.capacity() < static_cast(length + 6)) - g_txpacket_buffer.reserve(length + 6); - g_txpacket_buffer.resize(length + 6); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(length + 6); txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; @@ -658,11 +633,7 @@ int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16 int Protocol1PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - - if (g_txpacket_buffer.capacity() < static_cast(length + 6)) - g_txpacket_buffer.reserve(length + 6); - g_txpacket_buffer.resize(length + 6); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(length + 6); uint8_t rxpacket[6] = {0}; txpacket[PKT_ID] = id; @@ -689,10 +660,7 @@ int Protocol1PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad int result = COMM_TX_FAIL; // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM - if (g_txpacket_buffer.capacity() < static_cast(param_length + 8)) - g_txpacket_buffer.reserve(param_length + 8); - g_txpacket_buffer.resize(param_length + 8); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(param_length + 8); txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM @@ -714,10 +682,7 @@ int Protocol1PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 int result = COMM_TX_FAIL; // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM - if (g_txpacket_buffer.capacity() < static_cast(param_length + 7)) - g_txpacket_buffer.reserve(param_length + 7); - g_txpacket_buffer.resize(param_length + 7); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(param_length + 7); txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM diff --git a/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp b/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp index eee853204..d4f3da9ac 100644 --- a/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ b/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp @@ -37,11 +37,6 @@ #include #include -namespace { - thread_local std::vector g_txpacket_buffer; - thread_local std::vector g_rxpacket_buffer; -} - #define TXPACKET_MAX_LEN (1*1024) #define RXPACKET_MAX_LEN (1*1024) @@ -707,11 +702,7 @@ int Protocol2PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t addre int Protocol2PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - - if (g_rxpacket_buffer.capacity() < RXPACKET_MAX_LEN) - g_rxpacket_buffer.reserve(RXPACKET_MAX_LEN); - g_rxpacket_buffer.resize(RXPACKET_MAX_LEN); - std::vector& rxpacket = g_rxpacket_buffer; + std::vector rxpacket(RXPACKET_MAX_LEN); do { result = rxPacket(port, rxpacket.data()); @@ -737,11 +728,7 @@ int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add int result = COMM_TX_FAIL; uint8_t txpacket[14] = {0}; - - if (g_rxpacket_buffer.capacity() < RXPACKET_MAX_LEN) - g_rxpacket_buffer.reserve(RXPACKET_MAX_LEN); - g_rxpacket_buffer.resize(RXPACKET_MAX_LEN); - std::vector& rxpacket = g_rxpacket_buffer; + std::vector rxpacket(RXPACKET_MAX_LEN); if (id >= BROADCAST_ID) { @@ -840,11 +827,7 @@ int Protocol2PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_ int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { int result = COMM_TX_FAIL; - - if (g_txpacket_buffer.capacity() < static_cast(length + 12 + (length / 3))) - g_txpacket_buffer.reserve(length + 12 + (length / 3)); - g_txpacket_buffer.resize(length + 12 + (length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(length + 12 + (length / 3)); txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); @@ -866,11 +849,7 @@ int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t int Protocol2PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - - if (g_txpacket_buffer.capacity() < static_cast(length + 12 + (length / 3))) - g_txpacket_buffer.reserve(length + 12 + (length / 3)); - g_txpacket_buffer.resize(length + 12 + (length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(length + 12 + (length / 3)); uint8_t rxpacket[11] = {0}; txpacket[PKT_ID] = id; @@ -925,11 +904,7 @@ int Protocol2PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16 int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { int result = COMM_TX_FAIL; - - if (g_txpacket_buffer.capacity() < static_cast(length + 12 + (length / 3))) - g_txpacket_buffer.reserve(length + 12 + (length / 3)); - g_txpacket_buffer.resize(length + 12 + (length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(length + 12 + (length / 3)); txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); @@ -951,11 +926,7 @@ int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16 int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - - if (g_txpacket_buffer.capacity() < static_cast(length + 12 + (length / 3))) - g_txpacket_buffer.reserve(length + 12 + (length / 3)); - g_txpacket_buffer.resize(length + 12 + (length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(length + 12 + (length / 3)); uint8_t rxpacket[11] = {0}; txpacket[PKT_ID] = id; @@ -979,10 +950,7 @@ int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address int result = COMM_TX_FAIL; // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - if (g_txpacket_buffer.capacity() < static_cast(param_length + 14 + (param_length / 3))) - g_txpacket_buffer.reserve(param_length + 14 + (param_length / 3)); - g_txpacket_buffer.resize(param_length + 14 + (param_length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(param_length + 14 + (param_length / 3)); txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -1009,10 +977,7 @@ int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad int result = COMM_TX_FAIL; // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - if (g_txpacket_buffer.capacity() < static_cast(param_length + 14 + (param_length / 3))) - g_txpacket_buffer.reserve(param_length + 14 + (param_length / 3)); - g_txpacket_buffer.resize(param_length + 14 + (param_length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(param_length + 14 + (param_length / 3)); txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -1037,10 +1002,7 @@ int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 int result = COMM_TX_FAIL; // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - if (g_txpacket_buffer.capacity() < static_cast(param_length + 10 + (param_length / 3))) - g_txpacket_buffer.reserve(param_length + 10 + (param_length / 3)); - g_txpacket_buffer.resize(param_length + 10 + (param_length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(param_length + 10 + (param_length / 3)); txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H @@ -1068,10 +1030,7 @@ int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, u int result = COMM_TX_FAIL; // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - if (g_txpacket_buffer.capacity() < static_cast(param_length + 10 + (param_length / 3))) - g_txpacket_buffer.reserve(param_length + 10 + (param_length / 3)); - g_txpacket_buffer.resize(param_length + 10 + (param_length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(param_length + 10 + (param_length / 3)); txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H @@ -1092,10 +1051,7 @@ int Protocol2PacketHandler::fastSyncReadTx(PortHandler *port, uint16_t start_add int result = COMM_TX_FAIL; // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - if (g_txpacket_buffer.capacity() < static_cast(param_length + 14 + (param_length / 3))) - g_txpacket_buffer.reserve(param_length + 14 + (param_length / 3)); - g_txpacket_buffer.resize(param_length + 14 + (param_length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(param_length + 14 + (param_length / 3)); txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -1122,10 +1078,7 @@ int Protocol2PacketHandler::fastBulkReadTx(PortHandler *port, uint8_t *param, ui int result = COMM_TX_FAIL; // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - if (g_txpacket_buffer.capacity() < static_cast(param_length + 10 + (param_length / 3))) - g_txpacket_buffer.reserve(param_length + 10 + (param_length / 3)); - g_txpacket_buffer.resize(param_length + 10 + (param_length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(param_length + 10 + (param_length / 3)); txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp index e110b508e..cfa14862d 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp @@ -33,11 +33,6 @@ #include #include -namespace { - thread_local std::vector g_txpacket_buffer; - thread_local std::vector g_rxpacket_buffer; -} - #define TXPACKET_MAX_LEN (250) #define RXPACKET_MAX_LEN (250) @@ -422,11 +417,7 @@ int Protocol1PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t addre int Protocol1PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - - if (g_rxpacket_buffer.capacity() < RXPACKET_MAX_LEN) - g_rxpacket_buffer.reserve(RXPACKET_MAX_LEN); - g_rxpacket_buffer.resize(RXPACKET_MAX_LEN); - std::vector& rxpacket = g_rxpacket_buffer; + std::vector rxpacket(RXPACKET_MAX_LEN); do { result = rxPacket(port, rxpacket.data()); @@ -453,11 +444,7 @@ int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add int result = COMM_TX_FAIL; uint8_t txpacket[8] = {0}; - - if (g_rxpacket_buffer.capacity() < RXPACKET_MAX_LEN) - g_rxpacket_buffer.reserve(RXPACKET_MAX_LEN); - g_rxpacket_buffer.resize(RXPACKET_MAX_LEN); - std::vector& rxpacket = g_rxpacket_buffer; + std::vector rxpacket(RXPACKET_MAX_LEN); if (id >= BROADCAST_ID) { @@ -553,11 +540,7 @@ int Protocol1PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_ int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { int result = COMM_TX_FAIL; - - if (g_txpacket_buffer.capacity() < static_cast(length + 7)) - g_txpacket_buffer.reserve(length + 7); - g_txpacket_buffer.resize(length + 7); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(length + 7); txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; @@ -577,11 +560,7 @@ int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t int Protocol1PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - - if (g_txpacket_buffer.capacity() < static_cast(length + 7)) - g_txpacket_buffer.reserve(length + 7); - g_txpacket_buffer.resize(length + 7); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(length + 7); uint8_t rxpacket[6] = {0}; txpacket[PKT_ID] = id; @@ -634,11 +613,7 @@ int Protocol1PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16 int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { int result = COMM_TX_FAIL; - - if (g_txpacket_buffer.capacity() < static_cast(length + 6)) - g_txpacket_buffer.reserve(length + 6); - g_txpacket_buffer.resize(length + 6); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(length + 6); txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; @@ -658,11 +633,7 @@ int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16 int Protocol1PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - - if (g_txpacket_buffer.capacity() < static_cast(length + 6)) - g_txpacket_buffer.reserve(length + 6); - g_txpacket_buffer.resize(length + 6); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(length + 6); uint8_t rxpacket[6] = {0}; txpacket[PKT_ID] = id; @@ -689,10 +660,7 @@ int Protocol1PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad int result = COMM_TX_FAIL; // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM - if (g_txpacket_buffer.capacity() < static_cast(param_length + 8)) - g_txpacket_buffer.reserve(param_length + 8); - g_txpacket_buffer.resize(param_length + 8); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(param_length + 8); txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM @@ -714,10 +682,7 @@ int Protocol1PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 int result = COMM_TX_FAIL; // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM - if (g_txpacket_buffer.capacity() < static_cast(param_length + 7)) - g_txpacket_buffer.reserve(param_length + 7); - g_txpacket_buffer.resize(param_length + 7); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(param_length + 7); txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp index eee853204..d4f3da9ac 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp @@ -37,11 +37,6 @@ #include #include -namespace { - thread_local std::vector g_txpacket_buffer; - thread_local std::vector g_rxpacket_buffer; -} - #define TXPACKET_MAX_LEN (1*1024) #define RXPACKET_MAX_LEN (1*1024) @@ -707,11 +702,7 @@ int Protocol2PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t addre int Protocol2PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - - if (g_rxpacket_buffer.capacity() < RXPACKET_MAX_LEN) - g_rxpacket_buffer.reserve(RXPACKET_MAX_LEN); - g_rxpacket_buffer.resize(RXPACKET_MAX_LEN); - std::vector& rxpacket = g_rxpacket_buffer; + std::vector rxpacket(RXPACKET_MAX_LEN); do { result = rxPacket(port, rxpacket.data()); @@ -737,11 +728,7 @@ int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add int result = COMM_TX_FAIL; uint8_t txpacket[14] = {0}; - - if (g_rxpacket_buffer.capacity() < RXPACKET_MAX_LEN) - g_rxpacket_buffer.reserve(RXPACKET_MAX_LEN); - g_rxpacket_buffer.resize(RXPACKET_MAX_LEN); - std::vector& rxpacket = g_rxpacket_buffer; + std::vector rxpacket(RXPACKET_MAX_LEN); if (id >= BROADCAST_ID) { @@ -840,11 +827,7 @@ int Protocol2PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_ int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { int result = COMM_TX_FAIL; - - if (g_txpacket_buffer.capacity() < static_cast(length + 12 + (length / 3))) - g_txpacket_buffer.reserve(length + 12 + (length / 3)); - g_txpacket_buffer.resize(length + 12 + (length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(length + 12 + (length / 3)); txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); @@ -866,11 +849,7 @@ int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t int Protocol2PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - - if (g_txpacket_buffer.capacity() < static_cast(length + 12 + (length / 3))) - g_txpacket_buffer.reserve(length + 12 + (length / 3)); - g_txpacket_buffer.resize(length + 12 + (length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(length + 12 + (length / 3)); uint8_t rxpacket[11] = {0}; txpacket[PKT_ID] = id; @@ -925,11 +904,7 @@ int Protocol2PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16 int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { int result = COMM_TX_FAIL; - - if (g_txpacket_buffer.capacity() < static_cast(length + 12 + (length / 3))) - g_txpacket_buffer.reserve(length + 12 + (length / 3)); - g_txpacket_buffer.resize(length + 12 + (length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(length + 12 + (length / 3)); txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); @@ -951,11 +926,7 @@ int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16 int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { int result = COMM_TX_FAIL; - - if (g_txpacket_buffer.capacity() < static_cast(length + 12 + (length / 3))) - g_txpacket_buffer.reserve(length + 12 + (length / 3)); - g_txpacket_buffer.resize(length + 12 + (length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(length + 12 + (length / 3)); uint8_t rxpacket[11] = {0}; txpacket[PKT_ID] = id; @@ -979,10 +950,7 @@ int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address int result = COMM_TX_FAIL; // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - if (g_txpacket_buffer.capacity() < static_cast(param_length + 14 + (param_length / 3))) - g_txpacket_buffer.reserve(param_length + 14 + (param_length / 3)); - g_txpacket_buffer.resize(param_length + 14 + (param_length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(param_length + 14 + (param_length / 3)); txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -1009,10 +977,7 @@ int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad int result = COMM_TX_FAIL; // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - if (g_txpacket_buffer.capacity() < static_cast(param_length + 14 + (param_length / 3))) - g_txpacket_buffer.reserve(param_length + 14 + (param_length / 3)); - g_txpacket_buffer.resize(param_length + 14 + (param_length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(param_length + 14 + (param_length / 3)); txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -1037,10 +1002,7 @@ int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 int result = COMM_TX_FAIL; // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - if (g_txpacket_buffer.capacity() < static_cast(param_length + 10 + (param_length / 3))) - g_txpacket_buffer.reserve(param_length + 10 + (param_length / 3)); - g_txpacket_buffer.resize(param_length + 10 + (param_length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(param_length + 10 + (param_length / 3)); txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H @@ -1068,10 +1030,7 @@ int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, u int result = COMM_TX_FAIL; // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - if (g_txpacket_buffer.capacity() < static_cast(param_length + 10 + (param_length / 3))) - g_txpacket_buffer.reserve(param_length + 10 + (param_length / 3)); - g_txpacket_buffer.resize(param_length + 10 + (param_length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(param_length + 10 + (param_length / 3)); txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H @@ -1092,10 +1051,7 @@ int Protocol2PacketHandler::fastSyncReadTx(PortHandler *port, uint16_t start_add int result = COMM_TX_FAIL; // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - if (g_txpacket_buffer.capacity() < static_cast(param_length + 14 + (param_length / 3))) - g_txpacket_buffer.reserve(param_length + 14 + (param_length / 3)); - g_txpacket_buffer.resize(param_length + 14 + (param_length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(param_length + 14 + (param_length / 3)); txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -1122,10 +1078,7 @@ int Protocol2PacketHandler::fastBulkReadTx(PortHandler *port, uint8_t *param, ui int result = COMM_TX_FAIL; // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - if (g_txpacket_buffer.capacity() < static_cast(param_length + 10 + (param_length / 3))) - g_txpacket_buffer.reserve(param_length + 10 + (param_length / 3)); - g_txpacket_buffer.resize(param_length + 10 + (param_length / 3)); - std::vector& txpacket = g_txpacket_buffer; + std::vector txpacket(param_length + 10 + (param_length / 3)); txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H From fe2a2a70c4e7d7606faf5b29d7da68bb985d38f7 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 14 Apr 2026 15:37:23 +0900 Subject: [PATCH 08/10] Update clearPort method in PortHandler class to use reset_input_buffer for improved input handling. --- python/src/dynamixel_sdk/port_handler.py | 2 +- ros/dynamixel_sdk/src/dynamixel_sdk/port_handler.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/python/src/dynamixel_sdk/port_handler.py b/python/src/dynamixel_sdk/port_handler.py index 6bec0a935..a5f3fca33 100644 --- a/python/src/dynamixel_sdk/port_handler.py +++ b/python/src/dynamixel_sdk/port_handler.py @@ -48,7 +48,7 @@ def closePort(self): self.is_open = False def clearPort(self): - self.ser.flush() + self.ser.reset_input_buffer() def setPortName(self, port_name): self.port_name = port_name diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler.py b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler.py index 6bec0a935..a5f3fca33 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler.py +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler.py @@ -48,7 +48,7 @@ def closePort(self): self.is_open = False def clearPort(self): - self.ser.flush() + self.ser.reset_input_buffer() def setPortName(self, port_name): self.port_name = port_name From a70dc613610cb0f97d062de9818cb7db59b6f5a9 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 14 Apr 2026 16:47:36 +0900 Subject: [PATCH 09/10] Implement parameter size validation in GroupBulkWrite and GroupSyncWrite classes to prevent transmission errors. Return COMM_TX_ERROR for invalid parameter sizes before executing write operations. --- c++/src/dynamixel_sdk/group_bulk_write.cpp | 7 ++++++- c++/src/dynamixel_sdk/group_sync_write.cpp | 8 +++++++- ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp | 7 ++++++- ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp | 8 +++++++- 4 files changed, 26 insertions(+), 4 deletions(-) diff --git a/c++/src/dynamixel_sdk/group_bulk_write.cpp b/c++/src/dynamixel_sdk/group_bulk_write.cpp index f54647af0..8753ec055 100644 --- a/c++/src/dynamixel_sdk/group_bulk_write.cpp +++ b/c++/src/dynamixel_sdk/group_bulk_write.cpp @@ -133,5 +133,10 @@ int GroupBulkWrite::txPacket() if (is_param_changed_ == true || param_.empty()) makeParam(); - return ph_->bulkWriteTxOnly(port_, param_.data(), param_length_); + if (param_.size() != static_cast(param_length_) || + param_.size() > 0xFFFFu) + return COMM_TX_ERROR; + + return ph_->bulkWriteTxOnly( + port_, param_.data(), static_cast(param_.size())); } diff --git a/c++/src/dynamixel_sdk/group_sync_write.cpp b/c++/src/dynamixel_sdk/group_sync_write.cpp index cf3a2ae6b..e14519432 100644 --- a/c++/src/dynamixel_sdk/group_sync_write.cpp +++ b/c++/src/dynamixel_sdk/group_sync_write.cpp @@ -112,5 +112,11 @@ int GroupSyncWrite::txPacket() if (is_param_changed_ == true || param_.empty()) makeParam(); - return ph_->syncWriteTxOnly(port_, start_address_, data_length_, param_.data(), id_list_.size() * (1 + data_length_)); + if (param_.size() != id_list_.size() * (1 + data_length_) || + param_.size() > 0xFFFFu) + return COMM_TX_ERROR; + + return ph_->syncWriteTxOnly( + port_, start_address_, data_length_, param_.data(), + static_cast(param_.size())); } diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp index f54647af0..8753ec055 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp @@ -133,5 +133,10 @@ int GroupBulkWrite::txPacket() if (is_param_changed_ == true || param_.empty()) makeParam(); - return ph_->bulkWriteTxOnly(port_, param_.data(), param_length_); + if (param_.size() != static_cast(param_length_) || + param_.size() > 0xFFFFu) + return COMM_TX_ERROR; + + return ph_->bulkWriteTxOnly( + port_, param_.data(), static_cast(param_.size())); } diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp index cf3a2ae6b..e14519432 100644 --- a/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp @@ -112,5 +112,11 @@ int GroupSyncWrite::txPacket() if (is_param_changed_ == true || param_.empty()) makeParam(); - return ph_->syncWriteTxOnly(port_, start_address_, data_length_, param_.data(), id_list_.size() * (1 + data_length_)); + if (param_.size() != id_list_.size() * (1 + data_length_) || + param_.size() > 0xFFFFu) + return COMM_TX_ERROR; + + return ph_->syncWriteTxOnly( + port_, start_address_, data_length_, param_.data(), + static_cast(param_.size())); } From bc3232f4681e622011278c11ce09176c8a240a92 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Thu, 16 Apr 2026 13:19:27 +0900 Subject: [PATCH 10/10] Update versioning to 4.1.0 across all packages and documentation, reflecting the new release. Changes include enhancements in the C++ SDK and consistent versioning across C, C++, Python, and ROS 2 packages. --- Doxyfile | 2 +- ReleaseNote.md | 6 +++--- c++/CMakeLists.txt | 2 +- c++/library.properties | 2 +- c/CMakeLists.txt | 2 +- python/pyproject.toml | 2 +- python/setup.py | 2 +- ros/dynamixel_sdk/CHANGELOG.rst | 6 +++--- ros/dynamixel_sdk/CMakeLists.txt | 2 +- ros/dynamixel_sdk/package.xml | 2 +- ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst | 4 ++-- ros/dynamixel_sdk_custom_interfaces/package.xml | 2 +- ros/dynamixel_sdk_examples/CHANGELOG.rst | 4 ++-- ros/dynamixel_sdk_examples/package.xml | 2 +- 14 files changed, 20 insertions(+), 20 deletions(-) diff --git a/Doxyfile b/Doxyfile index b38532e67..b71245057 100644 --- a/Doxyfile +++ b/Doxyfile @@ -38,7 +38,7 @@ PROJECT_NAME = "DynamixelSDK c++" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = 5.0.0 +PROJECT_NUMBER = 4.1.0 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/ReleaseNote.md b/ReleaseNote.md index 80a0dcee1..ac34aa6e1 100644 --- a/ReleaseNote.md +++ b/ReleaseNote.md @@ -1,12 +1,12 @@ # Dynamixel SDK Release Notes -5.0.0 (2026-04-09) +4.1.0 (2026-04-14) ------------------ * Updated the standalone C++ SDK and ROS 2 C++ wrappers to use safer dynamic storage for group and packet buffers * Bumped the C++ shared library ABI to SOVERSION 3 due to public C++ class layout changes -* Added VERSION 5.0.0 and SOVERSION 3 to the ROS 2 shared library target for consistent ABI versioning +* Added VERSION 4.1.0 and SOVERSION 3 to the ROS 2 shared library target for consistent ABI versioning * Changed the C++ uninstall step to remove SDK-owned install paths by name instead of relying on the install manifest -* Unified package versioning to 5.0.0 across C, C++, Python, and ROS 2 packaging metadata +* Unified package versioning to 4.1.0 across C, C++, Python, and ROS 2 packaging metadata * Contributors: Hyungyu Kim 4.0.4 (2026-03-27) diff --git a/c++/CMakeLists.txt b/c++/CMakeLists.txt index ea0b63374..37d2e782d 100644 --- a/c++/CMakeLists.txt +++ b/c++/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.10) # Standalone C++ SDK: configure/build from the c++/ directory (paths below are relative to c++/). -project(dynamixel_sdk VERSION 5.0.0 LANGUAGES CXX) +project(dynamixel_sdk VERSION 4.1.0 LANGUAGES CXX) set(_dxl_pkg "dynamixel_sdk") diff --git a/c++/library.properties b/c++/library.properties index a7434b862..d52d373d2 100644 --- a/c++/library.properties +++ b/c++/library.properties @@ -1,5 +1,5 @@ name=DynamixelSDK -version=5.0.0 +version=4.1.0 author=ROBOTIS maintainer=ROBOTIS sentence=DynamixelSDK for Arduino diff --git a/c/CMakeLists.txt b/c/CMakeLists.txt index 345c43cf4..5f20c1c38 100644 --- a/c/CMakeLists.txt +++ b/c/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.10) # Standalone C SDK: configure/build from the c/ directory (paths below are relative to c/). -project(dynamixel_sdk_c VERSION 5.0.0 LANGUAGES C) +project(dynamixel_sdk_c VERSION 4.1.0 LANGUAGES C) # -------------------------------------------------------- # 1. Build Environment Configuration diff --git a/python/pyproject.toml b/python/pyproject.toml index 22f8836bf..76e01c829 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "dynamixel-sdk" -version = "5.0.0" +version = "4.1.0" description = "Dynamixel SDK 4. python package" readme = "README.txt" license = { text = "Apache 2.0" } diff --git a/python/setup.py b/python/setup.py index 9d2906970..8586bc32b 100644 --- a/python/setup.py +++ b/python/setup.py @@ -14,7 +14,7 @@ setup( name='dynamixel_sdk', - version='5.0.0', + version='4.1.0', packages=['dynamixel_sdk'], package_dir={'': 'src'}, license='Apache 2.0', diff --git a/ros/dynamixel_sdk/CHANGELOG.rst b/ros/dynamixel_sdk/CHANGELOG.rst index a2b514db9..6bb07bfdf 100644 --- a/ros/dynamixel_sdk/CHANGELOG.rst +++ b/ros/dynamixel_sdk/CHANGELOG.rst @@ -2,12 +2,12 @@ Changelog for package dynamixel_sdk ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -5.0.0 (2026-04-09) +4.1.0 (2026-04-14) ------------------ * Updated the standalone C++ SDK and ROS 2 C++ wrappers to use safer dynamic storage for group and packet buffers * Bumped the C++ shared library ABI to SOVERSION 3 due to public C++ class layout changes -* Added VERSION 5.0.0 and SOVERSION 3 to the ROS 2 shared library target for consistent ABI versioning -* Unified package versioning to 5.0.0 +* Added VERSION 4.1.0 and SOVERSION 3 to the ROS 2 shared library target for consistent ABI versioning +* Unified package versioning to 4.1.0 * Contributors: Hyungyu Kim 4.0.4 (2026-03-27) diff --git a/ros/dynamixel_sdk/CMakeLists.txt b/ros/dynamixel_sdk/CMakeLists.txt index 448856f39..cd950ea26 100644 --- a/ros/dynamixel_sdk/CMakeLists.txt +++ b/ros/dynamixel_sdk/CMakeLists.txt @@ -2,7 +2,7 @@ # Set minimum required version of cmake, project name and compile options ################################################################################ cmake_minimum_required(VERSION 3.5) -project(dynamixel_sdk VERSION 5.0.0) +project(dynamixel_sdk VERSION 4.1.0) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) diff --git a/ros/dynamixel_sdk/package.xml b/ros/dynamixel_sdk/package.xml index e45d789af..90efd94b8 100644 --- a/ros/dynamixel_sdk/package.xml +++ b/ros/dynamixel_sdk/package.xml @@ -2,7 +2,7 @@ dynamixel_sdk - 5.0.0 + 4.1.0 This package is wrapping version of ROBOTIS Dynamixel SDK for ROS 2. The ROBOTIS Dynamixel SDK, or SDK, is a software development library that provides Dynamixel control functions for packet communication. The API is designed for Dynamixel actuators and Dynamixel-based platforms. diff --git a/ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst b/ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst index 036a1b33f..c08e0019f 100644 --- a/ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst +++ b/ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst @@ -2,9 +2,9 @@ Changelog for package dynamixel_sdk_custom_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -5.0.0 (2026-04-09) +4.1.0 (2026-04-14) ------------------ -* Updated package versioning to 5.0.0 to align with the Dynamixel SDK 5.0.0 release +* Updated package versioning to 4.1.0 to align with the Dynamixel SDK 4.1.0 release * Contributors: Hyungyu Kim 4.0.4 (2026-03-27) diff --git a/ros/dynamixel_sdk_custom_interfaces/package.xml b/ros/dynamixel_sdk_custom_interfaces/package.xml index c1a294299..37a90dee7 100644 --- a/ros/dynamixel_sdk_custom_interfaces/package.xml +++ b/ros/dynamixel_sdk_custom_interfaces/package.xml @@ -2,7 +2,7 @@ dynamixel_sdk_custom_interfaces - 5.0.0 + 4.1.0 ROS 2 custom interface examples using ROBOTIS DYNAMIXEL SDK Pyo Apache 2.0 diff --git a/ros/dynamixel_sdk_examples/CHANGELOG.rst b/ros/dynamixel_sdk_examples/CHANGELOG.rst index 2ca98d54f..04884e8cf 100644 --- a/ros/dynamixel_sdk_examples/CHANGELOG.rst +++ b/ros/dynamixel_sdk_examples/CHANGELOG.rst @@ -2,9 +2,9 @@ Changelog for package dynamixel_sdk_examples ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -5.0.0 (2026-04-09) +4.1.0 (2026-04-14) ------------------ -* Updated package versioning to 5.0.0 to align with the Dynamixel SDK 5.0.0 release +* Updated package versioning to 4.1.0 to align with the Dynamixel SDK 4.1.0 release * Contributors: Hyungyu Kim 4.0.4 (2026-03-27) diff --git a/ros/dynamixel_sdk_examples/package.xml b/ros/dynamixel_sdk_examples/package.xml index 3e9275658..aeb69a093 100644 --- a/ros/dynamixel_sdk_examples/package.xml +++ b/ros/dynamixel_sdk_examples/package.xml @@ -1,7 +1,7 @@ dynamixel_sdk_examples - 5.0.0 + 4.1.0 ROS 2 examples using ROBOTIS DYNAMIXEL SDK Apache 2.0 Pyo