Support ROS 2 Humble (#162)

* gazeboのコンフィグファイルをgazebo6のデフォルトファイルから移植

* gazebo動作確認のため一時的にコントローラyamlを移植
joint_state_broadcasterに変更
全ノードにuse_sim_timeを設定

* joint_state_broadcasterの更新

* controller_managerのoutputとspawnerを更新

* HardwareInterfaceのHumble対応

* CIのビルド環境修正

* .ci.rosinstallにgz_ros2_control追加

* ros_ign_gazeboをros_gz_simに更新

* joint_valuesの駆動速度を下げた

* スタイル修正

* Update README

* Update README

* Update README

* Update README.en.md

Co-authored-by: Shota Aoki <s.aoki@rt-net.jp>

* Update README.md

Co-authored-by: Shota Aoki <s.aoki@rt-net.jp>

* max_velocityを設定

* 不要になったファイルを削除

* 依存パッケージを更新

Co-authored-by: ShotaAk <s.aoki@rt-net.jp>

.ci.rosinstall

@@ -2,3 +2,7 @@
     uri: https://github.com/rt-net/crane_x7_description.git
     local-name: crane_x7_description
     version: ros2
+- git:
+    uri: https://github.com/ros-controls/gz_ros2_control.git
+    local-name: gz_ros2_control
+    version: master

.github/workflows/industrial_ci.yml

@@ -17,7 +17,7 @@ jobs:
     strategy:
       matrix:
         env:
-          - { ROS_DISTRO: foxy, ROS_REPO: ros }
+          - { ROS_DISTRO: humble, ROS_REPO: ros }
     runs-on: ubuntu-latest
     steps:
       - uses: actions/checkout@v3

README.en.md

@@ -10,18 +10,21 @@ ROS 2 package suite of CRANE-X7.
 
 ## Table of Contents
 
-- [Supported ROS 2 distributions](#supported-ros-2-distributions)
-  - [ROS](#ros)
-- [Requirements](#requirements)
-- [Installation](#installation)
-  - [Build from source](#build-from-source)
-- [Quick Start](#quick-start)
-- [Packages](#packages)
-- [License](#license)
+- [crane_x7_ros](#crane_x7_ros)
+  - [Table of Contents](#table-of-contents)
+  - [Supported ROS 2 distributions](#supported-ros-2-distributions)
+    - [ROS](#ros)
+  - [Requirements](#requirements)
+  - [Installation](#installation)
+    - [Build from source](#build-from-source)
+  - [Quick Start](#quick-start)
+  - [Packages](#packages)
+  - [License](#license)
 
 ## Supported ROS 2 distributions
 
-- Foxy
+- [Foxy](https://github.com/rt-net/crane_x7_ros/tree/foxy-devel)
+- Humble
 
 ### ROS
 
@@ -35,15 +38,15 @@ ROS 2 package suite of CRANE-X7.
 - Linux OS
   - Ubuntu 20.04
 - ROS
-  - [Foxy Fitzroy](https://docs.ros.org/en/foxy/Installation.html)
+  - [Humble Hawksbill](https://docs.ros.org/en/humble/Installation.html)
 
 ## Installation
 
 ### Build from source
 
 ```sh
 # Setup ROS environment
-$ source /opt/ros/foxy/setup.bash
+$ source /opt/ros/humble/setup.bash
 
 # Download crane_x7 repositories
 $ mkdir -p ~/ros2_ws/src
@@ -52,6 +55,7 @@ $ git clone -b ros2 https://github.com/rt-net/crane_x7_ros.git
 $ git clone -b ros2 https://github.com/rt-net/crane_x7_description.git
 
 # Install dependencies
+$ git clone https://github.com/ros-controls/gz_ros2_control.git
 $ rosdep install -r -y -i --from-paths .
 
 # Build & Install

README.md

@@ -10,19 +10,22 @@ ROS 2 package suite of CRANE-X7.
 
 ## Table of Contents
 
-- [Supported ROS 2 distributions](#supported-ros-2-distributions)
-  - [ROS](#ros)
-- [Requirements](#requirements)
-- [Installation](#installation)
-  - [Build from source](#build-from-source)
-- [Quick Start](#quick-start)
-- [Packages](#packages)
-- [ライセンス](#ライセンス)
-- [開発について](#開発について)
+- [crane_x7_ros](#crane_x7_ros)
+  - [Table of Contents](#table-of-contents)
+  - [Supported ROS 2 distributions](#supported-ros-2-distributions)
+    - [ROS](#ros)
+  - [Requirements](#requirements)
+  - [Installation](#installation)
+    - [Build from source](#build-from-source)
+  - [Quick Start](#quick-start)
+  - [Packages](#packages)
+  - [ライセンス](#ライセンス)
+  - [開発について](#開発について)
 
 ## Supported ROS 2 distributions
 
-- Foxy
+- [Foxy](https://github.com/rt-net/crane_x7_ros/tree/foxy-devel)
+- Humble
 
 ### ROS
 
@@ -37,15 +40,15 @@ ROS 2 package suite of CRANE-X7.
 - Linux OS
   - Ubuntu 20.04
 - ROS
-  - [Foxy Fitzroy](https://docs.ros.org/en/foxy/Installation.html)
+  - [Humble Hawksbill](https://docs.ros.org/en/humble/Installation.html)
 
 ## Installation
 
 ### Build from source
 
 ```sh
 # Setup ROS environment
-$ source /opt/ros/foxy/setup.bash
+$ source /opt/ros/humble/setup.bash
 
 # Download crane_x7 repositories
 $ mkdir -p ~/ros2_ws/src
@@ -54,6 +57,7 @@ $ git clone -b ros2 https://github.com/rt-net/crane_x7_ros.git
 $ git clone -b ros2 https://github.com/rt-net/crane_x7_description.git
 
 # Install dependencies
+$ git clone https://github.com/ros-controls/gz_ros2_control.git
 $ rosdep install -r -y -i --from-paths .
 
 # Build & Install

crane_x7_control/config/crane_x7_controllers.yaml

@@ -7,7 +7,7 @@ controller_manager:
     crane_x7_gripper_controller:
       type: position_controllers/GripperActionController
     joint_state_controller:
-      type: joint_state_controller/JointStateController
+      type: joint_state_broadcaster/JointStateBroadcaster
 
 crane_x7_arm_controller:
   ros__parameters:

crane_x7_control/include/crane_x7_control/crane_x7_hardware.hpp

@@ -20,18 +20,21 @@
 #include <vector>
 
 #include "crane_x7_control/visibility_control.h"
-#include "hardware_interface/base_interface.hpp"
 #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 crane_x7_control
 {
 class CraneX7Hardware : public
-  hardware_interface::BaseInterface<hardware_interface::SystemInterface>
+  hardware_interface::SystemInterface
 {
 public:
   RCLCPP_SHARED_PTR_DEFINITIONS(CraneX7Hardware)
@@ -40,7 +43,7 @@ class CraneX7Hardware : public
   ~CraneX7Hardware();
 
   CRANE_X7_CONTROL_PUBLIC
-  return_type configure(const hardware_interface::HardwareInfo & info) override;
+  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
 
   CRANE_X7_CONTROL_PUBLIC
   std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
@@ -49,16 +52,16 @@ class CraneX7Hardware : public
   std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
 
   CRANE_X7_CONTROL_PUBLIC
-  return_type start() override;
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
 
   CRANE_X7_CONTROL_PUBLIC
-  return_type stop() override;
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
 
   CRANE_X7_CONTROL_PUBLIC
-  return_type read() override;
+  return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
   CRANE_X7_CONTROL_PUBLIC
-  return_type write() override;
+  return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
 private:
   bool communication_timeout();

crane_x7_control/launch/crane_x7_control.launch.py

@@ -61,26 +61,23 @@ def generate_launch_description():
         executable='ros2_control_node',
         parameters=[{'robot_description': LaunchConfiguration('loaded_description')},
                     crane_x7_controllers],
-        output={
-          'stdout': 'screen',
-          'stderr': 'screen',
-          },
+        output='screen',
         )
 
     spawn_joint_state_controller = ExecuteProcess(
-                cmd=['ros2 run controller_manager spawner.py joint_state_controller'],
+                cmd=['ros2 run controller_manager spawner joint_state_controller'],
                 shell=True,
                 output='screen',
             )
 
     spawn_arm_controller = ExecuteProcess(
-                cmd=['ros2 run controller_manager spawner.py crane_x7_arm_controller'],
+                cmd=['ros2 run controller_manager spawner crane_x7_arm_controller'],
                 shell=True,
                 output='screen',
             )
 
     spawn_gripper_controller = ExecuteProcess(
-                cmd=['ros2 run controller_manager spawner.py crane_x7_gripper_controller'],
+                cmd=['ros2 run controller_manager spawner crane_x7_gripper_controller'],
                 shell=True,
                 output='screen',
             )

crane_x7_control/src/crane_x7_hardware.cpp

@@ -46,11 +46,11 @@ CraneX7Hardware::~CraneX7Hardware()
   }
 }
 
-return_type CraneX7Hardware::configure(
+CallbackReturn CraneX7Hardware::on_init(
   const hardware_interface::HardwareInfo & info)
 {
-  if (configure_default(info) != return_type::OK) {
-    return return_type::ERROR;
+  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
+    return CallbackReturn::ERROR;
   }
 
   hw_position_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
@@ -66,17 +66,17 @@ return_type CraneX7Hardware::configure(
       RCLCPP_ERROR(
         LOGGER, "Joint '%s' does not have 'current_to_effort' parameter.",
         joint.name.c_str());
-      return return_type::ERROR;
+      return CallbackReturn::ERROR;
     }
   }
 
   for (const hardware_interface::ComponentInfo & joint : info_.joints) {
     if (joint.command_interfaces.size() != 1) {
       RCLCPP_FATAL(
         LOGGER,
-        "Joint '%s' has %d command interfaces. 1 expected.",
+        "Joint '%s' has %ld command interfaces. 1 expected.",
         joint.name.c_str(), joint.command_interfaces.size());
-      return return_type::ERROR;
+      return CallbackReturn::ERROR;
     }
 
     if (!(joint.command_interfaces[0].name == hardware_interface::HW_IF_POSITION)) {
@@ -85,15 +85,15 @@ return_type CraneX7Hardware::configure(
         "Joint '%s' has %s command interface. Expected %s, %s, or %s.", joint.name.c_str(),
         joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION,
         hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION);
-      return return_type::ERROR;
+      return CallbackReturn::ERROR;
     }
 
     if (joint.state_interfaces.size() < 1) {
       RCLCPP_FATAL(
         LOGGER,
-        "Joint '%s'has %d state interfaces. At least 1 expected.",
+        "Joint '%s'has %ld state interfaces. At least 1 expected.",
         joint.name.c_str(), joint.state_interfaces.size());
-      return return_type::ERROR;
+      return CallbackReturn::ERROR;
     }
 
     for (auto state_interface : joint.state_interfaces) {
@@ -106,7 +106,7 @@ return_type CraneX7Hardware::configure(
           "Joint '%s' has %s state interface. Expected %s, %s, or %s.", joint.name.c_str(),
           joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION,
           hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_EFFORT);
-        return return_type::ERROR;
+        return CallbackReturn::ERROR;
       }
     }
   }
@@ -119,19 +119,18 @@ return_type CraneX7Hardware::configure(
 
   if (!hardware_->connect(baudrate)) {
     RCLCPP_ERROR(LOGGER, "Failed to connect a robot.");
-    return return_type::ERROR;
+    return CallbackReturn::ERROR;
   }
 
   if (!hardware_->load_config_file(config_file_path)) {
     RCLCPP_ERROR(LOGGER, "Failed to read a config file.");
-    return return_type::ERROR;
+    return CallbackReturn::ERROR;
   }
 
   timeout_seconds_ = std::stod(info_.hardware_parameters["timeout_seconds"]);
   steady_clock_ = rclcpp::Clock(RCL_STEADY_TIME);
 
-  status_ = hardware_interface::status::CONFIGURED;
-  return return_type::OK;
+  return CallbackReturn::SUCCESS;
 }
 
 std::vector<hardware_interface::StateInterface>
@@ -168,14 +167,14 @@ CraneX7Hardware::export_command_interfaces()
   return command_interfaces;
 }
 
-return_type CraneX7Hardware::start()
+CallbackReturn CraneX7Hardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
 {
   // Set current timestamp to disable the communication timeout.
   prev_comm_timestamp_ = steady_clock_.now();
   timeout_has_printed_ = false;
 
   // Set present joint positions to hw_position_commands for safe start-up.
-  read();
+  read(prev_comm_timestamp_, rclcpp::Duration::from_seconds(0));
   for (std::size_t i = 0; i < hw_position_commands_.size(); i++) {
     double present_position = hw_position_states_[i];
     double limit_min = present_position;
@@ -188,39 +187,38 @@ return_type CraneX7Hardware::start()
     }
     hw_position_commands_[i] = std::clamp(present_position, limit_min, limit_max);
   }
-  write();
+  write(prev_comm_timestamp_, rclcpp::Duration::from_seconds(0));
 
   if (!hardware_->write_position_pid_gain_to_group(
       GROUP_NAME, START_P_GAIN, START_I_GAIN, START_D_GAIN))
   {
     RCLCPP_ERROR(LOGGER, "Failed to set PID gains.");
-    return return_type::ERROR;
+    return CallbackReturn::ERROR;
   }
 
   if (!hardware_->torque_on(GROUP_NAME)) {
     RCLCPP_ERROR(LOGGER, "Failed to set torque on.");
-    return return_type::ERROR;
+    return CallbackReturn::ERROR;
   }
 
-  status_ = hardware_interface::status::STARTED;
-  return return_type::OK;
+  return CallbackReturn::SUCCESS;
 }
 
-return_type CraneX7Hardware::stop()
+CallbackReturn CraneX7Hardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/)
 {
   // Set low PID gains for safe stopping.
   if (!hardware_->write_position_pid_gain_to_group(
       GROUP_NAME, STOP_P_GAIN, STOP_I_GAIN, STOP_D_GAIN))
   {
     RCLCPP_ERROR(LOGGER, "Failed to set PID gains.");
-    return return_type::ERROR;
+    return CallbackReturn::ERROR;
   }
 
-  status_ = hardware_interface::status::STOPPED;
-  return return_type::OK;
+  return CallbackReturn::SUCCESS;
 }
 
-return_type CraneX7Hardware::read()
+return_type CraneX7Hardware::read(
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
 {
   if (communication_timeout()) {
     if (!timeout_has_printed_) {
@@ -269,7 +267,8 @@ return_type CraneX7Hardware::read()
   return return_type::OK;
 }
 
-return_type CraneX7Hardware::write()
+return_type CraneX7Hardware::write(
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
 {
   if (communication_timeout()) {
     if (!timeout_has_printed_) {

crane_x7_examples/README.md

@@ -155,7 +155,7 @@ ros2 launch crane_x7_examples example.launch.py example:='joint_values'
 
 ### cartesian_path
 
-[Cartesian Path](https://moveit.picknik.ai/foxy/doc/move_group_interface/move_group_interface_tutorial.html#cartesian-paths)
+[Cartesian Path](https://moveit.picknik.ai/humble/doc/examples/move_group_interface/move_group_interface_tutorial.html#cartesian-paths)
 を生成し、手先で円を描くコード例です。
 
 次のコマンドを実行します。