sciurus17_controlのROS 2対応 (#142)

* パッケージ依存関係更新 * CRANE-X7のcontrollerをコピー * 腰と首のcontrol launch設定 * 動作確認のため首、腰以外のパラメータをコメントアウト * stateの格納先をジョイント名をキーにしたstd::mapクラスのメンバ変数に変更 * commandの格納先をジョイント名をキーにしたstd::mapクラスのメンバ変数に変更 * 制御周期変更 * 関節のコメントアウトを解除 * 両腕のコントローラのspawner追加 * 両腕のサーボグループに対応 * disconnectのタイミングを修正 * sync_write失敗時にエラーを返さない * Sync read失敗時にエラーを返さないように変更 * link3とlink5間の干渉判定を無効化 * 不要なファイルを削除 * ROS 2のビルドに対応 * demo.launch追加 * 不要なライブラリのインポートを削除 * 腰サーボ設定修正 * update_rateを100に変更 * C++17に変更 Co-authored-by: Shota Aoki <[email protected]> * コメント修正 Co-authored-by: Shota Aoki <[email protected]> * auto型推定の使用 Co-authored-by: Shota Aoki <[email protected]> * auto型推定の使用 Co-authored-by: Shota Aoki <[email protected]> * auto型推定の使用 Co-authored-by: Shota Aoki <[email protected]> * 左辺値参照に変更 Co-authored-by: Shota Aoki <[email protected]> * 左辺値参照に変更 Co-authored-by: Shota Aoki <[email protected]> * 左辺値参照に変更 Co-authored-by: Shota Aoki <[email protected]> * 左辺値参照に変更 Co-authored-by: Shota Aoki <[email protected]> * 左辺値参照に変更 Co-authored-by: Shota Aoki <[email protected]> * 左辺値参照に変更 Co-authored-by: Shota Aoki <[email protected]> * 左辺値参照に変更 Co-authored-by: Shota Aoki <[email protected]> * 左辺値参照に変更 Co-authored-by: Shota Aoki <[email protected]> * 左辺値参照に変更 Co-authored-by: Shota Aoki <[email protected]> * 左辺値参照に変更 Co-authored-by: Shota Aoki <[email protected]> * 左辺値参照に変更 Co-authored-by: Shota Aoki <[email protected]> * 定数化の解除 * 複数for文をまとめた * 定数化 * 変数のconst化 --------- Co-authored-by: Shota Aoki <[email protected]>
rt-net · Nov 6, 2023 · bbe86b8 · bbe86b8
1 parent a4533ee
commit bbe86b8
Show file tree

Hide file tree

Showing 30 changed files with 844 additions and 3,003 deletions.
diff --git a/sciurus17_control/CMakeLists.txt b/sciurus17_control/CMakeLists.txt
@@ -1,11 +1,78 @@
 cmake_minimum_required(VERSION 3.5)
 project(sciurus17_control)
 
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
 find_package(ament_cmake REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rt_manipulators_cpp REQUIRED)
+
+set(LIBRARY_NAME "sciurus17_hardware")
+add_library(
+  ${LIBRARY_NAME}
+  SHARED
+  src/sciurus17_hardware.cpp
+)
+target_include_directories(
+  ${LIBRARY_NAME}
+  PRIVATE
+  include
+)
+ament_target_dependencies(
+  ${LIBRARY_NAME}
+  hardware_interface
+  pluginlib
+  rclcpp
+  rt_manipulators_cpp
+)
+
+pluginlib_export_plugin_description_file(hardware_interface sciurus17_hardware.xml)
+
+install(
+  TARGETS ${LIBRARY_NAME}
+  DESTINATION lib
+)
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
 
 install(DIRECTORY
+  launch
   config
   DESTINATION share/${PROJECT_NAME}/
 )
 
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ${LIBRARY_NAME}
+)
+ament_export_dependencies(
+  hardware_interface
+  pluginlib
+  rclcpp
+  rt_manipulators_cpp
+)
+
 ament_package()
diff --git a/sciurus17_control/config/manipulator_config.yaml b/sciurus17_control/config/manipulator_config.yaml
@@ -0,0 +1,66 @@
+joint_groups:
+  right_arm:
+    joints:
+      - r_arm_joint1
+      - r_arm_joint2
+      - r_arm_joint3
+      - r_arm_joint4
+      - r_arm_joint5
+      - r_arm_joint6
+      - r_arm_joint7
+      - r_hand_joint
+    sync_read:
+      - position
+      - velocity
+      - current
+    sync_write:
+      - position
+
+  left_arm:
+    joints:
+      - l_arm_joint1
+      - l_arm_joint2
+      - l_arm_joint3
+      - l_arm_joint4
+      - l_arm_joint5
+      - l_arm_joint6
+      - l_arm_joint7
+      - l_hand_joint
+    sync_read:
+      - position
+      - velocity
+      - current
+    sync_write:
+      - position
+
+  torso:
+    joints:
+      - waist_yaw_joint
+      - neck_yaw_joint
+      - neck_pitch_joint
+    sync_read:
+      - position
+      - velocity
+      - current
+    sync_write:
+      - position
+
+r_arm_joint1: { id: 2, dynamixel: "XM540", operating_mode: 3 }
+r_arm_joint2: { id: 3, dynamixel: "XM540", operating_mode: 3 }
+r_arm_joint3: { id: 4, dynamixel: "XM430", operating_mode: 3 }
+r_arm_joint4: { id: 5, dynamixel: "XM540", operating_mode: 3 }
+r_arm_joint5: { id: 6, dynamixel: "XM430", operating_mode: 3 }
+r_arm_joint6: { id: 7, dynamixel: "XM430", operating_mode: 3 }
+r_arm_joint7: { id: 8, dynamixel: "XM430", operating_mode: 3 }
+r_hand_joint: { id: 9, dynamixel: "XM430", operating_mode: 3 }
+l_arm_joint1: { id: 10, dynamixel: "XM540", operating_mode: 3 }
+l_arm_joint2: { id: 11, dynamixel: "XM540", operating_mode: 3 }
+l_arm_joint3: { id: 12, dynamixel: "XM430", operating_mode: 3 }
+l_arm_joint4: { id: 13, dynamixel: "XM540", operating_mode: 3 }
+l_arm_joint5: { id: 14, dynamixel: "XM430", operating_mode: 3 }
+l_arm_joint6: { id: 15, dynamixel: "XM430", operating_mode: 3 }
+l_arm_joint7: { id: 16, dynamixel: "XM430", operating_mode: 3 }
+l_hand_joint: { id: 17, dynamixel: "XM430", operating_mode: 3 }
+waist_yaw_joint: { id: 18, dynamixel: "XM540", operating_mode: 3 }
+neck_yaw_joint: { id: 19, dynamixel: "XM430", operating_mode: 3 }
+neck_pitch_joint: { id: 20, dynamixel: "XM430", operating_mode: 3 }
diff --git a/sciurus17_control/config/sciurus17_controllers.yaml b/sciurus17_control/config/sciurus17_controllers.yaml
@@ -1,6 +1,6 @@
 controller_manager:
   ros__parameters:
-    update_rate: 500  # Hz
+    update_rate: 100  # Hz
 
     right_arm_controller:
       type: joint_trajectory_controller/JointTrajectoryController

diff --git a/sciurus17_control/include/sciurus17_control/sciurus17_hardware.hpp b/sciurus17_control/include/sciurus17_control/sciurus17_hardware.hpp
@@ -0,0 +1,87 @@
+// Copyright 2023 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 SCIURUS17_CONTROL__SCIURUS17_HARDWARE_HPP_
+#define SCIURUS17_CONTROL__SCIURUS17_HARDWARE_HPP_
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "sciurus17_control/visibility_control.h"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rt_manipulators_cpp/hardware.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+
+using hardware_interface::return_type;
+using hardware_interface::CallbackReturn;
+
+namespace sciurus17_control
+{
+class Sciurus17Hardware : public
+  hardware_interface::SystemInterface
+{
+public:
+  RCLCPP_SHARED_PTR_DEFINITIONS(Sciurus17Hardware)
+
+  SCIURUS17_CONTROL_PUBLIC
+  ~Sciurus17Hardware();
+
+  SCIURUS17_CONTROL_PUBLIC
+  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
+
+  SCIURUS17_CONTROL_PUBLIC
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  SCIURUS17_CONTROL_PUBLIC
+  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+  SCIURUS17_CONTROL_PUBLIC
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+  SCIURUS17_CONTROL_PUBLIC
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+
+  SCIURUS17_CONTROL_PUBLIC
+  return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  SCIURUS17_CONTROL_PUBLIC
+  return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+private:
+  bool communication_timeout();
+
+  std::shared_ptr<rt_manipulators_cpp::Hardware> hardware_;
+  double timeout_seconds_;
+
+  std::map<std::string, double> hw_position_commands_;
+  std::map<std::string, double> hw_position_states_;
+  std::map<std::string, double> hw_velocity_states_;
+  std::map<std::string, double> hw_effort_states_;
+
+  std::map<std::string, double> current_to_effort_;
+
+  rclcpp::Clock steady_clock_;
+  rclcpp::Time prev_comm_timestamp_;
+  bool timeout_has_printed_;
+};
+}  // namespace sciurus17_control
+
+#endif  // SCIURUS17_CONTROL__SCIURUS17_HARDWARE_HPP_
diff --git a/sciurus17_control/include/sciurus17_control/visibility_control.h b/sciurus17_control/include/sciurus17_control/visibility_control.h
@@ -0,0 +1,56 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// 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.
+
+/* This header must be included by all rclcpp headers which declare symbols
+ * which are defined in the rclcpp library. When not building the rclcpp
+ * library, i.e. when using the headers in other package's code, the contents
+ * of this header change the visibility of certain symbols which the rclcpp
+ * library cannot have, but the consuming code must have inorder to link.
+ */
+
+#ifndef SCIURUS17_CONTROL__VISIBILITY_CONTROL_H_
+#define SCIURUS17_CONTROL__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define SCIURUS17_CONTROL_EXPORT __attribute__((dllexport))
+#define SCIURUS17_CONTROL_IMPORT __attribute__((dllimport))
+#else
+#define SCIURUS17_CONTROL_EXPORT __declspec(dllexport)
+#define SCIURUS17_CONTROL_IMPORT __declspec(dllimport)
+#endif
+#ifdef SCIURUS17_CONTROL_BUILDING_DLL
+#define SCIURUS17_CONTROL_PUBLIC ROS2_CONTROL_DEMO_HARDWARE_EXPORT
+#else
+#define SCIURUS17_CONTROL_PUBLIC ROS2_CONTROL_DEMO_HARDWARE_IMPORT
+#endif
+#define SCIURUS17_CONTROL_PUBLIC_TYPE ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+#define SCIURUS17_CONTROL_LOCAL
+#else
+#define SCIURUS17_CONTROL_EXPORT __attribute__((visibility("default")))
+#define SCIURUS17_CONTROL_IMPORT
+#if __GNUC__ >= 4
+#define SCIURUS17_CONTROL_PUBLIC __attribute__((visibility("default")))
+#define SCIURUS17_CONTROL_LOCAL __attribute__((visibility("hidden")))
+#else
+#define SCIURUS17_CONTROL_PUBLIC
+#define SCIURUS17_CONTROL_LOCAL
+#endif
+#define SCIURUS17_CONTROL_PUBLIC_TYPE
+#endif
+
+#endif  // SCIURUS17_CONTROL__VISIBILITY_CONTROL_H_