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

* 可変長配列が初期化できていなかったので修正

* ジョイント情報を集約したhardware_jointsクラスを作成

* 型のエイリアス名を変更

* get, set関数をhardware_jointsに移植

* 不要になった関数と変数を削除

* HardwareJointsクラスの名前空間をhardware_jointsに修正

Author: Shota Aoki

rt_manipulators_lib/include/hardware.hpp

@@ -24,6 +24,7 @@
 #include <vector>
 
 #include "joint.hpp"
+#include "hardware_joints.hpp"
 
 namespace rt_manipulators_cpp {
 
@@ -94,9 +95,6 @@ class Hardware {
 
  private:
   bool parse_config_file(const std::string& config_yaml);
-  bool joint_groups_contain(const std::string& group_name);
-  bool all_joints_contain(const std::string& joint_name);
-  bool all_joints_contain_id(const uint8_t id);
   bool write_operating_mode(const std::string& group_name);
   bool limit_goal_velocity_by_present_position(const std::string& group_name);
   bool create_sync_read_group(const std::string& group_name);
@@ -119,9 +117,7 @@ class Hardware {
 
   std::shared_ptr<dynamixel::PortHandler> port_handler_;
   std::shared_ptr<dynamixel::PacketHandler> packet_handler_;
-  std::map<JointGroupName, std::shared_ptr<joint::JointGroup>> joint_groups_;
-  std::map<JointName, std::shared_ptr<joint::Joint>> all_joints_;
-  std::map<uint8_t, std::shared_ptr<joint::Joint>> all_joints_ref_from_id_;
+  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_;

rt_manipulators_lib/include/hardware_joints.hpp

@@ -0,0 +1,82 @@
+// 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_JOINTS_HPP_
+#define RT_MANIPULATORS_LIB_INCLUDE_HARDWARE_JOINTS_HPP_
+
+#include <map>
+#include <memory>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include "joint.hpp"
+
+namespace hardware_joints {
+
+using group_name_t = std::string;
+using joint_name_t = std::string;
+using group_map_t = std::map<group_name_t, std::shared_ptr<joint::JointGroup>>;
+using dxl_id_t = uint8_t;
+using position_t = double;
+using velocity_t = double;
+using current_t = double;
+using voltage_t = double;
+using temperature_t = int8_t;
+
+// ハードウェアのジョイント情報を持つクラス
+class Joints{
+ public:
+  Joints() {}
+  ~Joints() {}
+  const group_map_t groups() const;
+  void append_group(const group_name_t & group_name, const joint::JointGroup & group);
+  void append_joint(const joint_name_t & joint_name, const joint::Joint & joint);
+  std::shared_ptr<joint::JointGroup> group(const group_name_t & name);
+  std::shared_ptr<joint::Joint> joint(const joint_name_t & name);
+  std::shared_ptr<joint::Joint> joint(const dxl_id_t & id);
+  bool has_group(const group_name_t & name);
+  bool has_joint(const joint_name_t & name);
+  bool has_joint(const dxl_id_t & id);
+  bool get_position(const dxl_id_t & id, position_t & position);
+  bool get_position(const joint_name_t & joint_name, position_t & position);
+  bool get_positions(const group_name_t & group_name, std::vector<position_t> & positions);
+  bool get_velocity(const dxl_id_t & id, velocity_t & velocity);
+  bool get_velocity(const joint_name_t & joint_name, velocity_t & velocity);
+  bool get_velocities(const group_name_t & group_name, std::vector<velocity_t> & velocities);
+  bool get_current(const dxl_id_t & id, current_t & current);
+  bool get_current(const joint_name_t & joint_name, current_t & current);
+  bool get_currents(const group_name_t & group_name, std::vector<current_t>& currents);
+  bool get_voltage(const dxl_id_t id, voltage_t & voltage);
+  bool get_voltage(const joint_name_t & joint_name, voltage_t & voltage);
+  bool get_voltages(const group_name_t & group_name, std::vector<voltage_t>& voltages);
+  bool get_temperature(const dxl_id_t & id, temperature_t & temperature);
+  bool get_temperature(const joint_name_t & joint_name, temperature_t & temperature);
+  bool get_temperatures(const group_name_t & group_name, std::vector<temperature_t>& temperatures);
+  bool set_position(const dxl_id_t & id, const position_t & position);
+  bool set_position(const joint_name_t & joint_name, const position_t & position);
+  bool set_positions(const group_name_t & group_name, const std::vector<position_t> & positions);
+  bool set_velocity(const dxl_id_t & id, const velocity_t & velocity);
+  bool set_velocity(const joint_name_t & joint_name, const velocity_t & velocity);
+  bool set_velocities(const group_name_t & group_name, const std::vector<velocity_t>& velocities);
+
+ private:
+  group_map_t joint_groups_;
+  std::map<joint_name_t, std::shared_ptr<joint::Joint>> all_joints_;
+  std::map<dxl_id_t, std::shared_ptr<joint::Joint>> all_joints_ref_from_id_;
+};
+
+}  // namespace hardware_joints
+
+#endif  // RT_MANIPULATORS_LIB_INCLUDE_HARDWARE_JOINTS_HPP_

rt_manipulators_lib/src/CMakeLists.txt

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