sync_read, sync_write関連をDynamixelクラスへ移植 (#19)

* readインダイレクトアドレスの設定関数をDynamixelクラスに作成

* テストするためnext_indirect_addr_read()をpublicに変更。extract_present_position_from_sync_readを実装

* sync_readの関数をdynamixelクラスへ移植

* sync_write関係の関数をdynamixelクラスに実装

* sync_write用のデータ加工関数をdynamixelクラスに移植

* dynamixelクラスのsync_write関係の関数を使用する

* 不要な関数を削除

* lintエラーを修正

Author: Shota Aoki

rt_manipulators_lib/include/dynamixel_base.hpp

@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "hardware_communicator.hpp"
 
@@ -72,6 +73,63 @@ class DynamixelBase {
   virtual unsigned int from_velocity_rps(const double velocity_rps) { return 0; }
   virtual unsigned int from_current_ampere(const double current_ampere) { return 0; }
 
+  virtual bool auto_set_indirect_address_of_present_position(
+    const dynamixel_base::comm_t & comm) { return false; }
+  virtual bool auto_set_indirect_address_of_present_velocity(
+    const dynamixel_base::comm_t & comm) { return false; }
+  virtual bool auto_set_indirect_address_of_present_current(
+    const dynamixel_base::comm_t & comm) { return false; }
+  virtual bool auto_set_indirect_address_of_present_input_voltage(
+    const dynamixel_base::comm_t & comm) { return false; }
+  virtual bool auto_set_indirect_address_of_present_temperature(
+    const dynamixel_base::comm_t & comm) { return false; }
+  virtual bool auto_set_indirect_address_of_goal_position(
+    const dynamixel_base::comm_t & comm) { return false; }
+  virtual bool auto_set_indirect_address_of_goal_velocity(
+    const dynamixel_base::comm_t & comm) { return false; }
+  virtual bool auto_set_indirect_address_of_goal_current(
+    const dynamixel_base::comm_t & comm) { return false; }
+
+  virtual unsigned int indirect_addr_of_present_position(void) { return 0; }
+  virtual unsigned int indirect_addr_of_present_velocity(void) { return 0; }
+  virtual unsigned int indirect_addr_of_present_current(void) { return 0; }
+  virtual unsigned int indirect_addr_of_present_input_voltage(void) { return 0; }
+  virtual unsigned int indirect_addr_of_present_temperature(void) { return 0; }
+  virtual unsigned int indirect_addr_of_goal_position(void) { return 0; }
+  virtual unsigned int indirect_addr_of_goal_velocity(void) { return 0; }
+  virtual unsigned int indirect_addr_of_goal_current(void) { return 0; }
+
+  virtual unsigned int start_address_for_indirect_read(void) { return 0; }
+  virtual unsigned int length_of_indirect_data_read(void) { return 0; }
+  virtual unsigned int next_indirect_addr_read(void) const { return 0; }
+
+  virtual unsigned int start_address_for_indirect_write(void) { return 0; }
+  virtual unsigned int length_of_indirect_data_write(void) { return 0; }
+  virtual unsigned int next_indirect_addr_write(void) const { return 0; }
+
+  virtual bool extract_present_position_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & position_rad) { return false; }
+  virtual bool extract_present_velocity_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & velocity_rps) { return false; }
+  virtual bool extract_present_current_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & current_ampere) { return false; }
+  virtual bool extract_present_input_voltage_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & voltage_volt) { return false; }
+  virtual bool extract_present_temperature_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    int & temperature_deg) { return false; }
+
+  virtual void push_back_position_for_sync_write(
+    const double position_rad, std::vector<uint8_t> & write_data) {}
+  virtual void push_back_velocity_for_sync_write(
+    const double velocity_rps, std::vector<uint8_t> & write_data) {}
+  virtual void push_back_current_for_sync_write(
+    const double current_ampere, std::vector<uint8_t> & write_data) {}
+
  protected:
   uint8_t id_;
   std::string name_;

rt_manipulators_lib/include/dynamixel_xm.hpp

@@ -15,6 +15,9 @@
 #ifndef RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XM_HPP_
 #define RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XM_HPP_
 
+#include <string>
+#include <vector>
+
 #include "dynamixel_base.hpp"
 
 namespace dynamixel_xm {
@@ -52,8 +55,74 @@ class DynamixelXM : public dynamixel_base::DynamixelBase  {
   unsigned int from_velocity_rps(const double velocity_rps);
   unsigned int from_current_ampere(const double current_ampere);
 
- private:
+  bool auto_set_indirect_address_of_present_position(const dynamixel_base::comm_t & comm);
+  bool auto_set_indirect_address_of_present_velocity(const dynamixel_base::comm_t & comm);
+  bool auto_set_indirect_address_of_present_current(const dynamixel_base::comm_t & comm);
+  bool auto_set_indirect_address_of_present_input_voltage(const dynamixel_base::comm_t & comm);
+  bool auto_set_indirect_address_of_present_temperature(const dynamixel_base::comm_t & comm);
+  bool auto_set_indirect_address_of_goal_position(const dynamixel_base::comm_t & comm);
+  bool auto_set_indirect_address_of_goal_velocity(const dynamixel_base::comm_t & comm);
+  bool auto_set_indirect_address_of_goal_current(const dynamixel_base::comm_t & comm);
+
+  unsigned int indirect_addr_of_present_position(void);
+  unsigned int indirect_addr_of_present_velocity(void);
+  unsigned int indirect_addr_of_present_current(void);
+  unsigned int indirect_addr_of_present_input_voltage(void);
+  unsigned int indirect_addr_of_present_temperature(void);
+  unsigned int indirect_addr_of_goal_position(void);
+  unsigned int indirect_addr_of_goal_velocity(void);
+  unsigned int indirect_addr_of_goal_current(void);
+
+  unsigned int start_address_for_indirect_read(void);
+  unsigned int length_of_indirect_data_read(void);
+  unsigned int next_indirect_addr_read(void) const;
+
+  unsigned int start_address_for_indirect_write(void);
+  unsigned int length_of_indirect_data_write(void);
+  unsigned int next_indirect_addr_write(void) const;
+
+  bool extract_present_position_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & position_rad);
+  bool extract_present_velocity_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & velocity_rps);
+  bool extract_present_current_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & current_ampere);
+  bool extract_present_input_voltage_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & voltage_volt);
+  bool extract_present_temperature_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    int & temperature_deg);
+
+  void push_back_position_for_sync_write(
+    const double position_rad, std::vector<uint8_t> & write_data);
+  void push_back_velocity_for_sync_write(
+    const double velocity_rps, std::vector<uint8_t> & write_data);
+  void push_back_current_for_sync_write(
+    const double current_ampere, std::vector<uint8_t> & write_data);
+
+ protected:
   int HOME_POSITION_;
+  unsigned int total_length_of_indirect_addr_read_;
+  unsigned int total_length_of_indirect_addr_write_;
+  uint16_t indirect_addr_of_present_position_;
+  uint16_t indirect_addr_of_present_velocity_;
+  uint16_t indirect_addr_of_present_current_;
+  uint16_t indirect_addr_of_present_input_voltage_;
+  uint16_t indirect_addr_of_present_temperature_;
+  uint16_t indirect_addr_of_goal_position_;
+  uint16_t indirect_addr_of_goal_velocity_;
+  uint16_t indirect_addr_of_goal_current_;
+
+  bool set_indirect_address_read(
+    const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len,
+    uint16_t & indirect_addr);
+  bool set_indirect_address_write(
+    const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len,
+    uint16_t & indirect_addr);
 };
 
 }  // namespace dynamixel_xm

rt_manipulators_lib/include/hardware.hpp

@@ -86,9 +86,6 @@ class Hardware {
                                        const uint16_t i);
 
  protected:
-  bool write_word_data_to_group(const std::string& group_name, const uint16_t address,
-                                const uint16_t write_data);
-
   std::shared_ptr<hardware_communicator::Communicator> comm_;
 
  private:
@@ -97,8 +94,6 @@ class Hardware {
   bool limit_goal_current_by_present_position(const std::string& group_name);
   bool create_sync_read_group(const std::string& group_name);
   bool create_sync_write_group(const std::string& group_name);
-  bool set_indirect_address(const std::string& group_name, const uint16_t addr_indirect_start,
-                            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);