リファクタリング：hardware_communicatorクラスの追加 (#6)

* hardware_communicatorクラスにhardwareクラスの通信部分を移植

* sync_write_groupのコードをcommunicatorに移植

* sync_readのデータ取得部分をhardware_communicatorに移植

* static_castで型変換を明示する

* sync_read_groupにIDを追加する関数をcommunicatorに移植

* sync_write_groupにIDを追加する関数をcommunicatorに移植

* 外部から使用されない関数をprivateに変更

* hardware_communicatorを派生クラスから使用できるように、protectedに変更

* make_sync_read/write_groupに関数名を変更

Author: Shota Aoki

rt_manipulators_lib/include/hardware.hpp

@@ -15,7 +15,6 @@
 #ifndef RT_MANIPULATORS_LIB_INCLUDE_HARDWARE_HPP_
 #define RT_MANIPULATORS_LIB_INCLUDE_HARDWARE_HPP_
 
-#include <dynamixel_sdk/dynamixel_sdk.h>
 #include <chrono>
 #include <map>
 #include <memory>
@@ -25,6 +24,7 @@
 
 #include "joint.hpp"
 #include "hardware_joints.hpp"
+#include "hardware_communicator.hpp"
 
 namespace rt_manipulators_cpp {
 
@@ -81,17 +81,14 @@ class Hardware {
                                        const uint16_t i);
 
  protected:
-  bool write_byte_data(const uint8_t id, const uint16_t address, const uint8_t write_data);
   bool write_byte_data_to_group(const std::string& group_name, const uint16_t address,
                                 const uint8_t write_data);
-  bool write_word_data(const uint8_t id, const uint16_t address, const uint16_t write_data);
   bool write_word_data_to_group(const std::string& group_name, const uint16_t address,
                                 const uint16_t write_data);
-  bool write_double_word_data(const uint8_t id, const uint16_t address, const uint32_t write_data);
   bool write_double_word_data_to_group(const std::string& group_name, const uint16_t address,
                                        const uint32_t write_data);
-  bool read_byte_data(const uint8_t id, const uint16_t address, uint8_t& read_data);
-  bool read_double_word_data(const uint8_t id, const uint16_t address, uint32_t& read_data);
+
+  std::shared_ptr<hardware_communicator::Communicator> comm_;
 
  private:
   bool parse_config_file(const std::string& config_yaml);
@@ -103,9 +100,6 @@ class Hardware {
                             const uint16_t addr_target, const uint16_t len_target);
   void read_write_thread(const std::vector<std::string>& group_names,
                          const std::chrono::milliseconds& update_cycle_ms);
-  bool parse_dxl_error(const std::string& func_name, const uint8_t id, const uint16_t address,
-                       const int dxl_comm_result, const uint8_t dxl_packet_error);
-  bool parse_dxl_error(const std::string& func_name, const int dxl_comm_result);
   double dxl_pos_to_radian(const int32_t position);
   double dxl_velocity_to_rps(const int32_t velocity) const;
   double dxl_current_to_ampere(const int16_t current) const;
@@ -115,17 +109,12 @@ class Hardware {
   uint32_t to_dxl_acceleration(const double acceleration_rpss);
   uint32_t to_dxl_profile_velocity(const double velocity_rps);
 
-  std::shared_ptr<dynamixel::PortHandler> port_handler_;
-  std::shared_ptr<dynamixel::PacketHandler> packet_handler_;
   hardware_joints::Joints joints_;
-  std::map<JointGroupName, std::shared_ptr<dynamixel::GroupSyncRead>> sync_read_groups_;
-  std::map<JointGroupName, std::shared_ptr<dynamixel::GroupSyncWrite>> sync_write_groups_;
   std::map<JointGroupName, uint16_t> addr_sync_read_position_;
   std::map<JointGroupName, uint16_t> addr_sync_read_velocity_;
   std::map<JointGroupName, uint16_t> addr_sync_read_current_;
   std::map<JointGroupName, uint16_t> addr_sync_read_voltage_;
   std::map<JointGroupName, uint16_t> addr_sync_read_temperature_;
-  bool is_connected_;
   bool thread_enable_;
   std::shared_ptr<std::thread> read_write_thread_;
 };

rt_manipulators_lib/include/hardware_communicator.hpp

@@ -0,0 +1,91 @@
+// Copyright 2021 RT Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RT_MANIPULATORS_LIB_INCLUDE_HARDWARE_COMMUNICATOR_HPP_
+#define RT_MANIPULATORS_LIB_INCLUDE_HARDWARE_COMMUNICATOR_HPP_
+
+#include <dynamixel_sdk/dynamixel_sdk.h>
+#include <string>
+#include <map>
+#include <memory>
+#include <vector>
+
+namespace hardware_communicator {
+
+using dxl_id_t = uint8_t;
+using dxl_address_t = uint16_t;
+using dxl_data_length_t = uint16_t;
+using dxl_error_t = uint8_t;
+using dxl_result_t = int;
+using dxl_byte_t = uint8_t;
+using dxl_word_t = uint16_t;
+using dxl_double_word_t = uint32_t;
+using group_name_t = std::string;
+using GroupSyncRead = dynamixel::GroupSyncRead;
+using GroupSyncWrite = dynamixel::GroupSyncWrite;
+
+// ハードウェアとの通信を担うクラス
+class Communicator{
+ public:
+  explicit Communicator(const std::string device_name);
+  ~Communicator();
+  bool is_connected();
+  bool connect(const int baudrate = 3000000);
+  void disconnect();
+  void make_sync_read_group(const group_name_t & group_name, const dxl_address_t & start_address,
+                              const dxl_data_length_t & data_length);
+  void make_sync_write_group(const group_name_t & group_name, const dxl_address_t & start_address,
+                               const dxl_data_length_t & data_length);
+  bool append_id_to_sync_read_group(const group_name_t & group_name, const dxl_id_t & id);
+  bool append_id_to_sync_write_group(const group_name_t & group_name, const dxl_id_t & id,
+                                     std::vector<dxl_byte_t> & init_data);
+  bool send_sync_read_packet(const group_name_t & group_name);
+  bool send_sync_write_packet(const group_name_t & group_name);
+  bool get_sync_read_data(const group_name_t & group_name, const dxl_id_t id,
+                          const dxl_address_t & address, const dxl_data_length_t & length,
+                          dxl_double_word_t & read_data);
+  bool set_sync_write_data(const group_name_t & group_name, const dxl_id_t id,
+                           std::vector<dxl_byte_t> & write_data);
+  bool write_byte_data(const dxl_id_t & id, const dxl_address_t & address,
+                       const dxl_byte_t & write_data);
+  bool write_word_data(const dxl_id_t & id, const dxl_address_t & address,
+                       const dxl_word_t & write_data);
+  bool write_double_word_data(const dxl_id_t & id, const dxl_address_t & address,
+                              const dxl_double_word_t & write_data);
+  bool read_byte_data(const dxl_id_t & id, const dxl_address_t & address, dxl_byte_t & read_data);
+  bool read_double_word_data(const dxl_id_t & id, const dxl_address_t & address,
+                             dxl_double_word_t & read_data);
+
+ private:
+  std::shared_ptr<GroupSyncRead> sync_read_group(const group_name_t & name);
+  std::shared_ptr<GroupSyncWrite> sync_write_group(const group_name_t & name);
+  bool has_sync_read_group(const group_name_t & name);
+  bool has_sync_write_group(const group_name_t & name);
+  bool parse_dxl_error(const std::string & func_name, const dxl_id_t & id,
+                       const dxl_address_t & address, const dxl_result_t & dxl_comm_result,
+                       const dxl_error_t & dxl_packet_error);
+  bool parse_dxl_error(const std::string & func_name, const dxl_result_t & dxl_comm_result);
+
+  bool is_connected_;
+  std::shared_ptr<dynamixel::PortHandler> port_handler_;
+  std::shared_ptr<dynamixel::PacketHandler> packet_handler_;
+  std::map<group_name_t, std::shared_ptr<GroupSyncRead>> sync_read_groups_;
+  std::map<group_name_t, std::shared_ptr<GroupSyncWrite>> sync_write_groups_;
+};
+
+}  // namespace hardware_communicator
+
+
+#endif  // RT_MANIPULATORS_LIB_INCLUDE_HARDWARE_COMMUNICATOR_HPP_
+

rt_manipulators_lib/src/CMakeLists.txt

@@ -8,6 +8,7 @@ add_library(${library_name}
     hardware.cpp
     joint.cpp
     hardware_joints.cpp
+    hardware_communicator.cpp
 )
 set_target_properties(${library_name} PROPERTIES VERSION 1.0.0 SOVERSION 1)