feat: Implemented automatic fast protocol selection logic for sync and bulk read

Author: Woojin-Crive

include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 //
-// Authors: Hye-Jong KIM, Sungho Woo
+// Authors: Hye-Jong KIM, Sungho Woo, Woojin Wie
 
 #ifndef DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_HPP_
 #define DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_HPP_
@@ -110,8 +110,8 @@ class Dynamixel
 {
 private:
   // dxl communication variable
-  dynamixel::PortHandler * port_handler_;
-  dynamixel::PacketHandler * packet_handler_;
+  dynamixel::PortHandler * port_handler_ = nullptr;
+  dynamixel::PacketHandler * packet_handler_ = nullptr;
 
   // dxl info variable from dxl_model file
   DynamixelInfo dxl_info_;
@@ -126,24 +126,33 @@ class Dynamixel
   std::vector<RWItemList> read_data_list_;
 
   // sync read
-  dynamixel::GroupSyncRead * group_sync_read_;
+  dynamixel::GroupSyncRead * group_sync_read_ = nullptr;
+  // bulk read
+  dynamixel::GroupBulkRead * group_bulk_read_ = nullptr;
+  // fast sync read
+  dynamixel::GroupFastSyncRead * group_fast_sync_read_ = nullptr;
+  // fast bulk read
+  dynamixel::GroupFastBulkRead * group_fast_bulk_read_ = nullptr;
+
+  // fast read protocol state (applies to both sync and bulk)
+  bool use_fast_read_protocol_ = true;
+  bool fast_read_permanent_ = false;
+  int fast_read_fail_count_ = 0;
+
   // indirect inform for sync read
   std::map<uint8_t /*id*/, IndirectInfo> indirect_info_read_;
 
-  // bulk read
-  dynamixel::GroupBulkRead * group_bulk_read_;
-
   // write item (sync or bulk) variable
   bool write_type_;
   std::vector<RWItemList> write_data_list_;
 
   // sync write
-  dynamixel::GroupSyncWrite * group_sync_write_;
+  dynamixel::GroupSyncWrite * group_sync_write_ = nullptr;
   // indirect inform for sync write
   std::map<uint8_t /*id*/, IndirectInfo> indirect_info_write_;
 
   // bulk write
-  dynamixel::GroupBulkWrite * group_bulk_write_;
+  dynamixel::GroupBulkWrite * group_bulk_write_ = nullptr;
   // direct inform for bulk write
   std::map<uint8_t /*id*/, IndirectInfo> direct_info_write_;
 
@@ -204,12 +213,16 @@ class Dynamixel
   DxlError SetSyncReadItemAndHandler();
   DxlError SetSyncReadHandler(std::vector<uint8_t> id_arr);
   DxlError GetDxlValueFromSyncRead(double period_ms);
+  DxlError SetFastSyncReadHandler(std::vector<uint8_t> id_arr);
 
   // BulkRead
   DxlError SetBulkReadItemAndHandler();
   DxlError SetBulkReadHandler(std::vector<uint8_t> id_arr);
-  DxlError AddDirectRead(uint8_t id, std::string item_name, uint16_t item_addr, uint8_t item_size);
   DxlError GetDxlValueFromBulkRead(double period_ms);
+  DxlError SetFastBulkReadHandler(std::vector<uint8_t> id_arr);
+
+  // DirectRead for BulkRead
+  DxlError AddDirectRead(uint8_t id, std::string item_name, uint16_t item_addr, uint8_t item_size);
 
   // Check Indirect Read
   DxlError CheckIndirectReadAvailable(uint8_t id);
@@ -240,11 +253,10 @@ class Dynamixel
 
   // Read - Communication
   DxlError ProcessReadCommunication(
-    dynamixel::GroupSyncRead * group_sync_read,
-    dynamixel::GroupBulkRead * group_bulk_read,
     dynamixel::PortHandler * port_handler,
     double period_ms,
-    bool is_sync);
+    bool is_sync,
+    bool is_fast);
 
   // SyncWrite
   DxlError SetSyncWriteItemAndHandler();

include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 //
-// Authors: Hye-Jong KIM, Sungho Woo
+// Authors: Hye-Jong KIM, Sungho Woo, Woojin Wie
 
 #ifndef DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_INFO_HPP_
 #define DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_INFO_HPP_

include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 //
-// Authors: Hye-Jong KIM, Sungho Woo
+// Authors: Hye-Jong KIM, Sungho Woo, Woojin Wie
 
 #ifndef DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL_HARDWARE_INTERFACE_HPP_
 #define DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL_HARDWARE_INTERFACE_HPP_