サーボの速度、電流、入力電圧、温度を取得する機能を追加 (#3)

* for-loopをconst 参照に書き換えて、不要なコピーと値の書き換えを防ぐ

* サーボの速度、電流、電圧、温度をsync_readする機能を実装

* READMEの更新

* S17のread_preset_valuesサンプル追加

* ライブラリ：関数名の誤記修正

* サンプル：ID、ジョイント名で��ラメータを読み取るサンプルを追加

Author: Shota Aoki

rt_manipulators_lib/include/hardware.hpp

@@ -47,6 +47,18 @@ class Hardware {
   bool get_position(const uint8_t id, double& position);
   bool get_position(const std::string& joint_name, double& position);
   bool get_positions(const std::string& group_name, std::vector<double>& positions);
+  bool get_velocity(const uint8_t id, double& velocity);
+  bool get_velocity(const std::string& joint_name, double& velocity);
+  bool get_velocities(const std::string& group_name, std::vector<double>& velocities);
+  bool get_current(const uint8_t id, double& current);
+  bool get_current(const std::string& joint_name, double& current);
+  bool get_currents(const std::string& group_name, std::vector<double>& currents);
+  bool get_voltage(const uint8_t id, double& voltage);
+  bool get_voltage(const std::string& joint_name, double& voltage);
+  bool get_voltages(const std::string& group_name, std::vector<double>& voltages);
+  bool get_temperature(const uint8_t id, int8_t& temperature);
+  bool get_temperature(const std::string& joint_name, int8_t& temperature);
+  bool get_temperatures(const std::string& group_name, std::vector<int8_t>& temperatures);
   bool set_position(const uint8_t id, const double position);
   bool set_position(const std::string& joint_name, const double position);
   bool set_positions(const std::string& group_name, std::vector<double>& positions);
@@ -86,6 +98,9 @@ class Hardware {
                        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;
+  double dxl_voltage_to_volt(const int16_t voltage) const;
   uint32_t radian_to_dxl_pos(const double position);
   uint32_t to_dxl_acceleration(const double acceleration_rpss);
   uint32_t to_dxl_velocity(const double velocity_rps);
@@ -97,7 +112,11 @@ class Hardware {
   std::map<uint8_t, std::shared_ptr<joint::Joint>> all_joints_ref_from_id_;
   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> address_indirect_present_position_;
+  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_;
   std::map<JointGroupName, uint16_t> address_indirect_goal_position_;
   bool thread_enable_;
   std::shared_ptr<std::thread> read_write_thread_;

rt_manipulators_lib/include/joint.hpp

@@ -27,14 +27,26 @@ class Joint {
   uint8_t id() const;
   uint8_t operating_mode() const;
   void set_present_position(const double position_radian);
+  void set_present_velocity(const double velocity_rps);
+  void set_present_current(const double current_ampere);
+  void set_present_voltage(const double voltage_volt);
+  void set_present_temperature(const int8_t temperature_degree);
   double get_present_position() const;
+  double get_present_velocity() const;
+  double get_present_current() const;
+  double get_present_voltage() const;
+  int8_t get_present_temperature() const;
   void set_goal_position(const double position_radian);
   double get_goal_position() const;
 
  private:
   uint8_t id_;
   uint8_t operating_mode_;
   double present_position_;
+  double present_velocity_;
+  double present_current_;
+  double present_voltage_;
+  int8_t present_temperature_;
   double goal_position_;
 };
 
@@ -47,6 +59,7 @@ class JointGroup {
   bool sync_read_position_enabled() const;
   bool sync_read_velocity_enabled() const;
   bool sync_read_current_enabled() const;
+  bool sync_read_voltage_enabled() const;
   bool sync_read_temperature_enabled() const;
   bool sync_write_position_enabled() const;
   bool sync_write_velocity_enabled() const;
@@ -57,6 +70,7 @@ class JointGroup {
   bool sync_read_position_enabled_;
   bool sync_read_velocity_enabled_;
   bool sync_read_current_enabled_;
+  bool sync_read_voltage_enabled_;
   bool sync_read_temperature_enabled_;
   bool sync_write_position_enabled_;
   bool sync_write_velocity_enabled_;