MoveIt2上でRRT*を用いたプランナーを構築する手順

# はじめに

卒業研究が終わり，ひと段落したところで，卒論でやった技術的なところをまとめてみようと思います．卒業研究では，RRT\*をベースとして，把持物体の落下リスクを考えたロボットアームの運動計画を研究しました．手法としては，単純で，RRT*のコスト関数の中の距離だったところに把持物体の落下リスクを加えるというものでした．実験は，実機まで行けず，シミュレーションで有効性を示し，終わったという感じです．実験で必要なものととしては，落下リスクを考慮する必要があるため，独自のプランナーを構築することでした．ただ，MoveIt2で，独自のプランナーを構築する方法がよくわからず，以下のようなGitHubのIssueとStack Overflowがあったのですが，さっぱりわかりませんでした．

https://robotics.stackexchange.com/questions/113731/how-to-add-custom-planner-in-moveit2

https://github.com/moveit/moveit2/issues/3201


そこで，MoveIt2の中に，独自のプランナーを構築せず，MoveIt2の上に，独自のプランナーを構築し，コードを書くこととしました．また，研究では，KUKA LBR IIWA 7 R800を用いたため，事前にロボットに関する実装がされている[lbr_fri_ros2_stack](https://github.com/lbr-stack/lbr_fri_ros2_stack)を使用しました．この上で，RRT*アルゴリズムを用いて，独自のプランナーを構築する手順について説明します．

# ソフトウェア構成

実装方法について説明します．卒業研究で用いた運動計画アルゴリズムのベースとなるRRT*のコードは，[rrt-algorithms](https://github.com/motion-planning/rrt-algorithms)ベースに作成しました．そして，このrrt-algorithmsをlbr_fri_ros2_stackのパッケージのプランナーとして，動くように設定しました．またlbr_fri_ros2_stackはC++，rrt-algorithmsはPythonで実装し，service通信作成することで，値の受け渡しが可能なようにしました．以下は，この記事でのソフトウェア構成のイメージです．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2734306/d7bb7df9-fc87-4191-bebf-58abf942a3b3.png)


サービス通信を実装しているのは，卒論で落下リスクを考える際，エンドエフェクタの向きが必要であり，そこの値の受け渡しをするために実装しました，そのため，今回紹介するRRT*で運動計画を動作させるだけでは，サービス通信の実装は必要ありません．ただ，何か中身をいじりたい人がこの記事を見る方が多いと思うので（RRT*を動かすだけならたぶんデフォルトで可能），拡張が容易なこのソフトウェア構成で，説明していきます．

# 実行環境

実行は，WSL上で動かしたUbuntu22.04のDockerコンテナ上で行いました．

# 実装方法

実装したコードは，Githubにあげています．rrtstar-planner-on-moveit2が今回使用するコードです．

https://github.com/koki3141/article-code/tree/main/rrtstar-planner-on-moveit2

以下のコマンドで，クローンし，使用するディレクトリに移動してください．
```
git clone https://github.com/koki3141/article-code.git
cd article-code/rrtstar-planner-on-moveit2
```

※.srvディレクトリに，以下の内容を動作させたコードがあります．ささっと動かしたい人は，.srvをsrvに変更して，rrt-algorithmsをライブラリとしてインストールしてから，ビルドし，実行してみてください．

## コンテナの作成

まず，コンテナの作成を行うために，イメージの選定をしていきます．ROS2をインストールする際は，メタパッケージでインストールすることが多いかと思います．例えば，`sudo apt install -y ros-humble-ros-base` や，`sudo apt install -y ros-humble-desktop` のようなコマンドです．これらは，複数のパッケージを含んで，インストールするコマンドとなっています．以下，メタパッケージと主なパッケージの対応関係です．

表 **ROS 2 Humbleのメタパッケージと主なパッケージ（**[参照元](https://www.ros.org/reps/rep-2001.html#humble-hawksbill-may-2022-may-2027)**）**

| メタパッケージ | 継承/含まれるパッケージ | 主なパッケージ |
| --- | --- | --- |
| **ros_core** | - | ament_cmake, class_loader, common_interfaces, pluginlib, rclcpp, rclpy, ros2cli_common_extensions, etc. |
| **ros_base** | 継承: [ros_core] | geometry2, kdl_parser, robot_state_publisher,rosbag2, urdf |
| **desktop** | 継承: [ros_base] | action_tutorials_cpp, demo_nodes_cpp, joy, rviz2, turtlesim, teleop_twist_joy, teleop_twist_keyboard, etc. |
| **perception** | 継承: [ros_base] | image_common, image_pipeline, image_transport_plugins, laser_filters, vision_opencv |
| **simulation** | 継承: [ros_base] | ros_ign_bridge, ros_ign_gazebo, ros_ign_image, ros_ign_interfaces |
| **desktop_full** | 継承: [desktop] + [perception, simulation] | ros_ign_gazebo_demos |

ここでは，rviz2が必要である，`desktop`を使用します．また，これらのメタパッケージには，インストールに時間がかかります．そこで，Dockerのコンテナのイメージとして，これらをインストールし，高速化をはかります．
私は，よく，ROS公式と，ROSとGazeboシミュレータの開発を行っているOpen Source Robotics Foundation（OSRF）のDocker Imageを使用しています．以下はROS2 Humble のメタパッケージと対応するDocker Imageです．

表 ROS 2 Humble メタパッケージと対応するDocker Image（参照元: [ROS Official Docker Hub](https://hub.docker.com/_/ros), [OSRF ROS Docker Hub](https://hub.docker.com/r/osrf/ros)）

| メタパッケージ | Docker ベースイメージ |
| --- | --- |
| **ros_core** | `ros:humble-ros-core-jammy` |
| **ros_base** | `ros:humble-ros-base-jammy` |
| **desktop** | `osrf/ros:humble-desktop-jammy` |
| **perception** | `ros:humble-perception-jammy` |
| **simulation** | `osrf/humble-simulation-jammy` |
| **desktop_full** | `osrf/ros:humble-desktop-full-jammy` |

ここでは，ROS2 Humbleの`desktop`を使用するため，`osrf/ros:humble-desktop-jammy` をDocker Imageとして設定します．

次に，コンテナの設定である`Dockerfile`について説明していきます．前述した，`osrf/ros:humble-desktop-jammy` を使用し，lbr_fri_ros2_stackを使用するための，依存パッケージと，rrt-algorithms必要なライブラリ，開発に使用するいくつかのツールをコンテナ内に，インストールしています．以下，使用するDockerFileです．

- DockerFile
    
    ```docker
    FROM osrf/ros:humble-desktop-jammy
    
    # 環境変数を設定
    ENV DEBIAN_FRONTEND=noninteractive
    ENV LANG=C.UTF-8
    
    # 基本的なツールをインストール
    RUN apt-get update && apt-get install -y \
        sudo \
        lsb-release \
        gnupg \
        curl \
        wget \
        vim \
        git \
        python3-pip \
        # debugのためのツール
        gdb \
        build-essential
    
    # Gazeboのリポジトリキーとリポジトリを追加
    RUN curl https://packages.osrfoundation.org/gazebo.gpg --output /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg && \
        echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null && \
        apt-get update &&  apt-get install -y  gz-harmonic
    
    # install MoveIt2
    RUN apt-get update && apt-get install -y \
        ros-humble-moveit
    
    # install ROS2 Control
    RUN apt-get update && apt-get install -y \
        ros-humble-ros2-control \
        ros-humble-ros2-controllers
    
    # lbr_fri_ros2_stack
    RUN apt-get update && apt-get install -y \
        ros-humble-pilz-industrial-motion-planner \
        ros-humble-control-toolbox \
        ros-humble-controller-interface \
        ros-humble-kinematics-interface \
        ros-dev-tools
    
    # Set up user
    ARG UID GID USERNAME GROUPNAME PASSWORD
    RUN groupadd -g $GID $GROUPNAME && \
        useradd -m -s /bin/bash -u $UID -g $GID -G sudo $USERNAME && \
        echo $USERNAME:$PASSWORD | chpasswd && \
        echo "$USERNAME   ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers
    
    USER $USERNAME
    WORKDIR /home/$USERNAME/workspace
    
    COPY requirements.txt /home/$USERNAME/
    RUN pip install --upgrade --no-cache-dir -r /home/$USERNAME/requirements.txt
    
    RUN echo "source /opt/ros/humble/setup.bash" >> /home/$USERNAME/.bashrc
    RUN echo "source ~/workspace/install/setup.bash" >> /home/$USERNAME/.bashrc
    
    # エントリーポイントを設定
    CMD ["bash"]
    
    ```
    

次に，Docker composeについて説明します．`compose.yml`の設定ですが，X11と，`rrtstar-planner-on-moveit2` ディレクトリ，sshをbindしています．X11の設定は，コンテナ内でRvizをGUIで表示できるようにしています．

- compose.yml
    
    ```docker
    services:
      rrtstar-planner-on-moveit2:
        build:
          context: .
          args:
            UID: ${UID}
            GID: ${GID}
            USERNAME: ${USERNAME}
            GROUPNAME: ${GROUPNAME}
            PASSWORD: ${PASSWORD}
        image: rrtstar_planner_on_moveit2
        container_name: rrtstar-planner-on-moveit2
        network_mode: host
        volumes:
          - /tmp/.X11-unix:/tmp/.X11-unix
          - type: bind
            source: ../../rrtstar-planner-on-moveit2/
            target: /home/${USERNAME}/workspace
          - type: bind
            source: ~/.ssh
            target: /home/${USERNAME}/.ssh
            read_only: true
        environment:
          - DISPLAY=$DISPLAY
        tty: true
    
    ```
    

以上で説明は終わりです．長くなりましたが，さっそくコンテナを作成していきましょう．`rrtstar-planner-on-moveit2`に移動して，以下のコマンドを実行してください．

```docker
cd .devcontainer
docker compose up -d
```

コンテナが正常に作成されたら，完了です．

## lbr_fri_ros2_stack

lbr_fri_ros2_stackをインストールして，ROS2のセットアップをしていきます

コンテナ内に入って，`/home/user/workspace`ディレクトリに，移動して，次のコマンドを実行してください．

```docker
. .devcontainer/ros2_setup.sh 
```

`ros2_setup.sh`  には，ROS2のビルドと，lbr_fri_ros2_stackの依存パッケージ（`.devcontainer/repos.yaml` に記載）をインストールするコマンドをまとめています．

実行が完了したら，[**LBR-Stack**](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_demos/lbr_moveit/doc/lbr_moveit.html#moveit-via-rviz)のチュートリアルにあるデモで，さっそくシミュレーションを動かしてみましょう．

ターミナルを2つ立ち上げてください

- terminal 1

```bash
ros2 launch lbr_bringup mock.launch.py     model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
```

- terminal 2

```bash
ros2 launch lbr_bringup move_group.launch.py     mode:=mock     rviz:=true     model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
```

以下のような画面が表示されれば，成功です．

![image 1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2734306/e68fc474-0231-4af9-bde1-9a86971e4adf.png)

それっぽい感じを味わうために，実際に動かしてみましょう．

以下の動画のように，ゴール姿勢を指定するオレンジのロボットアームを動かして，適当に姿勢を指定します．その後，下のMotionPlanningのパネルで，Planning→Commands→Plan&Executeを，実行します．


![無題の動画 ‐ Clipchampで作成 (2).gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2734306/9828f8c9-88c7-44c6-9bed-2dabcc51c8f6.gif)

実際に，ロボットが生成された経路をそって，動作しました！

次に，どういうノードが起動しているのか，中身を見ていきましょう．次のコマンドを実行してみてください．

```bash
rqt_graph
```

すべてのノードを表示するように設定すると，以下のよう画面が表示されるはずです．（されない場合は，左上の更新をクリックしてください）

![image 2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2734306/ef170e11-fd83-4358-b041-b37c8ffeca19.png)

赤の「/lbr/joint_state_broadcaster」が緑の「/lbr/joint_states」にパブリッシュすることによって，ロボットアームが制御されていることがわかります．この運動計画部分を，自作した運動計画から「/lbr/joint_states」にパブリッシュできるようにしたいため，「/lbr/joint_state_broadcaster」のノードを起動しないようにし，「/lbr/joint_states」にパブリッシュできるような変更を加えます．

src/lbr_fri_ros2_stack/lbr_bringup/launch/mock.launch.pyを開き，57行目の，joint_state_broadcasterをコメントアウトします．

```bash
    controller_event_handler = RegisterEventHandler(
        OnProcessStart(
            target_action=ros2_control_node,
            on_start=[
                # joint_state_broadcaster,
                controller,
            ],
        )
    )
```

これで，「/lbr/joint_states」に任意の角度をパブリッシュして，反映させれるようになりました．

では，次に，service通信の実装を行っていきます．

## service通信の実装

service通信は，ClinetとServerの間で同期的な通信を行う形態です．プログラム内で定義した関数（Server）に引数を渡して，実行結果を受け取る（Client）ようなもので，Topic通信とは，全く別物です．余談ですが，Service通信の理解は，一発でできたのですが，Topic通信のイメージは，全く理解できず，少し苦労した記憶があります．Topic通信は，実機の特性であるスタックを知らないと，メリットをイメージできなかった．．

![Service-SingleServiceClient.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2734306/317f163d-b591-4bd7-83ba-722708dbb9f1.gif)
図 Service通信のイメージ（[ROS2 HumbleのService通信のチュートリアルより](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Services/Understanding-ROS2-Services.html)）

今回は，RobotStateServerと，RobotStateClientの2つのクラスと，サービス通信の型である，JointLimitを定義します．またRobotStateServerはC++，RobotStateClientはPythonで実装します．

それぞれは，robot_state_interface ，robot_state_server，rrtstar_path_planningというパッケージ名で作成しています．それぞれのテストしたパッケージは，.srcディレクトリまとめてあります．

### robot_state_interfaceの実装

まず，はじめに，robot_state_interfaceを作成します．srcディレクトリに移動して，以下のコマンドを実行してください．

```bash
ros2 pkg create robot_state_interface --build-type ament_cmake --dependencies rclcpp rosidl_default_generators
```

その後，関節可動域についての型を定義するsrvファイルを作成します．requestは空で，response時に，関節の名前とその，最小角度の位置と最大角度の位置を返すようにします．

- srv/JointLimit.srv

```bash
---
string[] name
float64[] min_position
float64[] max_position
```

次に，CMakeLists.txtに定義した，srvファイルを認識させ，package.xmlに必要な依存関係を追記します．

- CMakeLists.txt

```bash
~~~
set(srv_files
  "srv/JointLimit.srv"
)

rosidl_generate_interfaces(${PROJECT_NAME}
  ${srv_files}
)
~~~
```

- package.xmlに追加

```bash
  <member_of_group>rosidl_interface_packages</member_of_group>
```

最後に，パッケージをビルドしておきましょう．

workspaceにディレクトリを移動して，次のコマンドを実行してください．

```bash
colcon build --symlink-install --packages-select robot_state_interface
source install/setup.bash
```

これで，完了です．

### RobotStateServerの実装

では，RobotStateServerの実装について説明します．

srcファイルに移動して，以下のコマンドで，パッケージを作成してください．

```bash
ros2 pkg create robot_state_server --build-type ament_cmake --dependencies rclcpp std_msgs moveit_ros_planning_interface robot_state_interface

```

直接，Service通信を実装する前に，関節角度の可動域を取得するコードを出力するサンプルコードを書いてみましょう．

まず，作成したパッケージである，robot_state_serviceで，sampleディレクトリを作成し，そこに，sample_robot_state.cppファイルを作成して，以下のコードを貼り付けます．

```cpp
#include <moveit/collision_detection/collision_detector_allocator.h>
#include <moveit/collision_detection/collision_env.h>
#include <moveit/collision_detection/world_diff.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_model_loader/robot_model_loader.h>

#include <moveit_msgs/msg/planning_scene.hpp>
#include <moveit_msgs/msg/planning_scene_components.hpp>
#include <sstream>

#include "geometry_msgs/msg/pose.hpp"
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/robot_model_loader/robot_model_loader.h"
#include "rclcpp/rclcpp.hpp"

void displayJointLimits(
    const rclcpp::Logger& logger,
    const std::vector<moveit::core::JointModel*>& joint_models) {
  for (const auto* joint : joint_models) {
    if (joint->getVariableCount() > 0) {
      const std::string& joint_name = joint->getName();
      const std::vector<moveit::core::VariableBounds>& bounds =
          joint->getVariableBounds();

      if (!bounds.empty()) {
        const auto& bound = bounds[0];

        RCLCPP_INFO(logger, "Joint %s:", joint_name.c_str());
        RCLCPP_INFO(logger, "  Position limits: [%f, %f]", bound.min_position_,
                    bound.max_position_);
        RCLCPP_INFO(logger, "  Has velocity limits: %s",
                    bound.velocity_bounded_ ? "true" : "false");
        RCLCPP_INFO(logger, "  Max velocity: %f", bound.max_velocity_);
        RCLCPP_INFO(logger, "  Has acceleration limits: %s",
                    bound.acceleration_bounded_ ? "true" : "false");
        RCLCPP_INFO(logger, "  Max acceleration: %f", bound.max_acceleration_);
      } else {
        RCLCPP_WARN(logger, "No variable bounds found for joint %s",
                    joint_name.c_str());
      }
    }
  }
}

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node_ptr = rclcpp::Node::make_shared("sample_robot_state");

  const rclcpp::Logger& logger = node_ptr->get_logger();
  robot_model_loader::RobotModelLoader robot_model_loader(node_ptr);
  const moveit::core::RobotModelPtr kinematic_model =
      robot_model_loader.getModel();

  const std::vector<moveit::core::JointModel*>& joint_models =
      kinematic_model->getJointModels();
  displayJointLimits(logger, joint_models);

  rclcpp::shutdown();
  return 0;
}

```

コードととしては，`kinematic_model` という，ロボットの情報を持った変数を定義し，そこから，関節可動域を取得してるというシンプルなコードです．

では，次に，lauchファイルを作成するために，launchディレクトリを作成して，sample_robot_state.launch.pyというファイルを作成し，以下を貼り付けてください．

```python
from typing import List

from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
from launch.actions import  OpaqueFunction
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

from lbr_bringup.description import LBRDescriptionMixin
from lbr_bringup.moveit import LBRMoveGroupMixin

def setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
    ld = LaunchDescription()

    model = LaunchConfiguration("model").perform(context)
    use_sim_time = False

    moveit_configs = LBRMoveGroupMixin.moveit_configs_builder(
        robot_name=model,
        package_name=f"{model}_moveit_config",
    )

    ld.add_action(
        Node(
            package="robot_state_server",
            executable="sample_robot_state",
            parameters=[
                moveit_configs.to_dict(),
                {"use_sim_time": use_sim_time},
                LBRDescriptionMixin.param_robot_name(),
            ],
        )
    )

    return ld.entities

def generate_launch_description() -> LaunchDescription:
    ld = LaunchDescription()
    # デフォルトで，iiwa7を選択
    ld.add_action(LBRDescriptionMixin.arg_model())
    # ノードを実行
    ld.add_action(OpaqueFunction(function=setup))

    return ld
```

最後に，CMakeLists.txtに次の記述を追加してください．

```makefile
add_executable(sample_robot_state sample/sample_robot_state.cpp)
ament_target_dependencies(sample_robot_state
  moveit_ros_planning_interface
  rclcpp
)
install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)

install(TARGETS sample_robot_state
  DESTINATION lib/${PROJECT_NAME}
)
```

以上で，設定は終わりです．workspaceディレクトリに移動し，ビルド＆環境反映するコマンド実行して，

```bash
colcon build --symlink-install --packages-select robot_state_server
 . install/setup.bash
```

起動すると，

```bash
ros2 launch robot_state_server sample_robot_state.launch.py 
```

次のように，各関節角度が表示されれば成功です．

```bash
[sample_robot_state-1] [INFO] [1739900944.054665721] [sample_robot_state]: Joint lbr_A1:
[sample_robot_state-1] [INFO] [1739900944.054737409] [sample_robot_state]:   Position limits: [-2.967060, 2.967060]
[sample_robot_state-1] [INFO] [1739900944.054748227] [sample_robot_state]:   Has velocity limits: true
[sample_robot_state-1] [INFO] [1739900944.054752176] [sample_robot_state]:   Max velocity: 1.710423
[sample_robot_state-1] [INFO] [1739900944.054755932] [sample_robot_state]:   Has acceleration limits: true
[sample_robot_state-1] [INFO] [1739900944.054759137] [sample_robot_state]:   Max acceleration: 10.000000
[sample_robot_state-1] [INFO] [1739900944.054763747] [sample_robot_state]: Joint lbr_A2:
[sample_robot_state-1] [INFO] [1739900944.054766971] [sample_robot_state]:   Position limits: [-2.094395, 2.094395]
[sample_robot_state-1] [INFO] [1739900944.054770883] [sample_robot_state]:   Has velocity limits: true
[sample_robot_state-1] [INFO] [1739900944.054774079] [sample_robot_state]:   Max velocity: 1.710423
[sample_robot_state-1] [INFO] [1739900944.054777688] [sample_robot_state]:   Has acceleration limits: true
[sample_robot_state-1] [INFO] [1739900944.054780838] [sample_robot_state]:   Max acceleration: 10.000000
[sample_robot_state-1] [INFO] [1739900944.054784392] [sample_robot_state]: Joint lbr_A3:
[sample_robot_state-1] [INFO] [1739900944.054787460] [sample_robot_state]:   Position limits: [-2.967060, 2.967060]
[sample_robot_state-1] [INFO] [1739900944.054791252] [sample_robot_state]:   Has velocity limits: true
[sample_robot_state-1] [INFO] [1739900944.054794301] [sample_robot_state]:   Max velocity: 1.745329
[sample_robot_state-1] [INFO] [1739900944.054797516] [sample_robot_state]:   Has acceleration limits: true
[sample_robot_state-1] [INFO] [1739900944.054800519] [sample_robot_state]:   Max acceleration: 10.000000
[sample_robot_state-1] [INFO] [1739900944.054804054] [sample_robot_state]: Joint lbr_A4:
[sample_robot_state-1] [INFO] [1739900944.054807021] [sample_robot_state]:   Position limits: [-2.094395, 2.094395]
[sample_robot_state-1] [INFO] [1739900944.054810612] [sample_robot_state]:   Has velocity limits: true
[sample_robot_state-1] [INFO] [1739900944.054813596] [sample_robot_state]:   Max velocity: 2.268928
[sample_robot_state-1] [INFO] [1739900944.054816893] [sample_robot_state]:   Has acceleration limits: true
[sample_robot_state-1] [INFO] [1739900944.054819814] [sample_robot_state]:   Max acceleration: 10.000000
[sample_robot_state-1] [INFO] [1739900944.054823230] [sample_robot_state]: Joint lbr_A5:
[sample_robot_state-1] [INFO] [1739900944.054826205] [sample_robot_state]:   Position limits: [-2.967060, 2.967060]
[sample_robot_state-1] [INFO] [1739900944.054829906] [sample_robot_state]:   Has velocity limits: true
[sample_robot_state-1] [INFO] [1739900944.054832836] [sample_robot_state]:   Max velocity: 2.443461
[sample_robot_state-1] [INFO] [1739900944.054836133] [sample_robot_state]:   Has acceleration limits: true
[sample_robot_state-1] [INFO] [1739900944.054839200] [sample_robot_state]:   Max acceleration: 10.000000
[sample_robot_state-1] [INFO] [1739900944.054842644] [sample_robot_state]: Joint lbr_A6:
[sample_robot_state-1] [INFO] [1739900944.054845583] [sample_robot_state]:   Position limits: [-2.094395, 2.094395]
[sample_robot_state-1] [INFO] [1739900944.054849119] [sample_robot_state]:   Has velocity limits: true
[sample_robot_state-1] [INFO] [1739900944.054852113] [sample_robot_state]:   Max velocity: 3.141593
[sample_robot_state-1] [INFO] [1739900944.054855510] [sample_robot_state]:   Has acceleration limits: true
[sample_robot_state-1] [INFO] [1739900944.054858440] [sample_robot_state]:   Max acceleration: 10.000000
[sample_robot_state-1] [INFO] [1739900944.054861948] [sample_robot_state]: Joint lbr_A7:
[sample_robot_state-1] [INFO] [1739900944.054865135] [sample_robot_state]:   Position limits: [-3.054326, 3.054326]
[sample_robot_state-1] [INFO] [1739900944.054868671] [sample_robot_state]:   Has velocity limits: true
[sample_robot_state-1] [INFO] [1739900944.054871582] [sample_robot_state]:   Max velocity: 3.141593
[sample_robot_state-1] [INFO] [1739900944.054874860] [sample_robot_state]:   Has acceleration limits: true
[sample_robot_state-1] [INFO] [1739900944.054877772] [sample_robot_state]:   Max acceleration: 10.000000
[INFO] [sample_robot_state-1]: process has finished cleanly [pid 11832]
```

では実際に，Service通信のServerとなる，RobotStateServerを作成していきましょう．

robot_state_serviceのsrcディレクトリ内に，robot_state_server_node.cppを作成して，以下のコードを記述してください．

```cpp
#include <memory>
#include <rclcpp/rclcpp.hpp>

#include "moveit/robot_model_loader/robot_model_loader.h"
#include "robot_state_interface/srv/joint_limit.hpp"

class RobotStateServerNode : public rclcpp::Node {
 public:
  RobotStateServerNode() : Node("robot_state_server_node") {
    this->get_logger().set_level(rclcpp::Logger::Level::Info);

    joint_limit_server_ =
        this->create_service<robot_state_interface::srv::JointLimit>(
            "joint_limits",
            std::bind(&RobotStateServerNode::getJointLimits, this,
                      std::placeholders::_1, std::placeholders::_2));
  }

  void initialize() {
    robot_model_loader::RobotModelLoader robot_model_loader(shared_from_this());
    kinematic_model_ = robot_model_loader.getModel();
  }

 private:
  void getJointLimits(
      const std::shared_ptr<robot_state_interface::srv::JointLimit::Request>,
      std::shared_ptr<robot_state_interface::srv::JointLimit::Response>
          response) {
    RCLCPP_DEBUG(this->get_logger(), "call getJointLimits");
    const std::vector<moveit::core::JointModel*>& joint_models =
        kinematic_model_->getJointModels();
    for (const auto& joint : joint_models) {
      if (joint->getVariableCount() > 0) {
        auto bounds = joint->getVariableBounds();
        response->name.push_back(joint->getName());
        response->min_position.push_back(bounds[0].min_position_);
        response->max_position.push_back(bounds[0].max_position_);
      }
    }
  }
  rclcpp::Service<robot_state_interface::srv::JointLimit>::SharedPtr
      joint_limit_server_;

  moveit::core::RobotModelPtr kinematic_model_;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);

  auto robot_server_state_node = std::make_shared<RobotStateServerNode>();
  robot_server_state_node->initialize();

  rclcpp::spin(robot_server_state_node);

  rclcpp::shutdown();
  return 0;
}
```

コードはシンプルで，”joint_limits”というサービスの名前を定義し，リスポンスする内容を，robot_state_interfaceで定義した，JointLimit.srvに合わせて，定義しています．

サンプル時と同じように，CMakeLists.txtに，作成したコードをビルドターゲットとして定義し，それに依存するパッケージを設定してください．その後，インストール設定をにターゲットを追加して，更新してください．

```makefile
add_executable(robot_state_server src/robot_state_server_node.cpp)
ament_target_dependencies(robot_state_server
  moveit_ros_planning_interface
  rclcpp
  robot_state_interface
)

install(TARGETS sample_robot_state robot_state_server
  DESTINATION lib/${PROJECT_NAME}
)

```

次にlaunchファイルを作成します．sample_robot_state.launch.pyに少し中身を追加して，Rvizに関するノードを立ち上げるコードを追加し，robot_state_server.launch.pyを作成しています．

```python
from typing import List

from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
from launch.actions import  OpaqueFunction
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.actions import (
    IncludeLaunchDescription,
    OpaqueFunction,
)
from launch_ros.substitutions import FindPackageShare
from lbr_bringup.description import LBRDescriptionMixin
from lbr_bringup.moveit import LBRMoveGroupMixin

def setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
    ld = LaunchDescription()

    model = LaunchConfiguration("model").perform(context)
    use_sim_time = False

    moveit_configs = LBRMoveGroupMixin.moveit_configs_builder(
        robot_name=model,
        package_name=f"{model}_moveit_config",
    )

    ld.add_action(
        Node(
            package="robot_state_server",
            executable="robot_state_server",
            parameters=[
                moveit_configs.to_dict(),
                {"use_sim_time": use_sim_time},
                LBRDescriptionMixin.param_robot_name(),
            ],
        )
    )

    return ld.entities

def generate_launch_description() -> LaunchDescription:
    ld = LaunchDescription()
    # デフォルトで，iiwa7を選択
    ld.add_action(LBRDescriptionMixin.arg_model())
    ld.add_action(LBRDescriptionMixin.arg_mode())

    ld.add_action(
        IncludeLaunchDescription(
            FindPackageShare("lbr_bringup").find("lbr_bringup")
            + "/launch/mock.launch.py",
            launch_arguments={"model": LaunchConfiguration("model")}.items(),
        )
    )

    ld.add_action(
        IncludeLaunchDescription(
            FindPackageShare("lbr_bringup").find("lbr_bringup")
            + "/launch/move_group.launch.py",
            launch_arguments={
                "mode": "mock",
                "rviz": "true",
                "model": LaunchConfiguration("model"),
            }.items(),
        )
    )
    # ノードを実行
    ld.add_action(OpaqueFunction(function=setup))

    return ld
```

一旦，Serverの実装は終わりです．

### RobotStateClientの実装

次に，RobotStateClientの実装について説明します．

srcディレクトリに移動して，以下のコマンドで，パッケージを作成してください．

```bash
ros2 pkg create rrtstar_path_planning --build-type ament_python --dependencies rclpy moveit_ros_planning_interface robot_state_interface

```

rrtstar_path_planningのパッケージ内の，rrtstar_path_planningディレクトリに，robot_state_client_node.pyというファイルを作成してください．

```python
import rclpy
from rclpy.node import Node

from robot_state_interface.srv import (
    JointLimit,
)

class RobotStateClientNode(Node):
    def __init__(self):
        super().__init__("robot_state_client_node")
        self.get_logger().set_level(rclpy.logging.LoggingSeverity.INFO)

        self.joint_limit_client = self.create_client(JointLimit, f"joint_limits")

    def get_joint_limits(self):
        if not self.joint_limit_client.wait_for_service(timeout_sec=5.0):
            self.get_logger().error("Joint limit service not available.")
            return {"success": False, "data": {}}

        request = JointLimit.Request()
        future = self.joint_limit_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)

        if future.result() is not None:
            response = future.result()
            self.get_logger().debug("Joint limits successfully retrieved.")
            return {"success": True, "data": response}
        else:
            self.get_logger().error("Failed to retrieve joint limits.")
            return {"success": False, "data": {}}

```

これで，joint_limitsというサービスにリクエストを送信し，リスポンスを受け取れるコードです．

sampleディレクトリを作成して，空の**__init__.py**を作成後，sample_robot_state_client_node.pyを作成して，以下のコードを張り付けてください．

```python

from rrtstar_path_planning.robot_state_client_node import (
    RobotStateClientNode
)

import rclpy

def main(args=None):
    rclpy.init(args=args)

    logger = rclpy.logging.get_logger("rrtstar_path_planning")
    logger.info("ROS 2 initialized")

    robot_state_node = RobotStateClientNode()
    logger.info("RobotStateClientNode initialized")
    joint_limits = robot_state_node.get_joint_limits()
    logger.info(f"Joint limits received: {joint_limits}")

    logger.info("MotionPlanner initialized")

    robot_state_node.destroy_node()
    logger.info("Node destroyed")

    rclpy.shutdown()
    logger.info("ROS 2 shutdown complete")

if __name__ == "__main__":
    main()

```

レスポンスで受け取った，joint_limitをそのまま出力するコードです．

では，実行するための設定をしていきます．launchディレクトリを作成して，sample_robot_state_client_node.launch.pyを作成して，以下のコードを張り付けてください．

```python
from typing import List

from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
from launch.actions import  OpaqueFunction
from launch_ros.actions import Node

def setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
    ld = LaunchDescription()

    ld.add_action(
        Node(
            package="rrtstar_path_planning",
            executable="sample_robot_state_client_node",
        )
    )

    return ld.entities

def generate_launch_description() -> LaunchDescription:
    ld = LaunchDescription()

    ld.add_action(OpaqueFunction(function=setup))

    return ld
```

また，setup.pyも変更します．

```python
from setuptools import find_packages, setup
import os
from glob import glob
package_name = 'rrtstar_path_planning'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(),
    package_dir={"": "."},
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join("share", package_name, "launch"), glob("launch/*.py")),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='user',
    maintainer_email='shimohara7192@gmail.com',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            f"sample_robot_state_client_node = sample.sample_robot_state_client_node:main",
        ],
    },
)

```

実際に実行してみましょう．

workspaceディレクトリに移動し，ビルドして，環境を反映するために，次のコマンドを実行してください．

```bash
colcon build --symlink-install --packages-select rrtstar_path_planning
 . install/setup.bash
```

その後，ターミナルを2つ立ち上げて，次のコマンドを実行してください．

- terminal 1

```bash
 ros2 launch robot_state_server robot_state_server.launch.py 
```

- terminal 2

```bash
ros2 launch rrtstar_path_planning sample_robot_state_client_node.launch.py 
```

以下のようなログが出力されれば成功です．

```bash
[sample_robot_state_client_node-1] [INFO] [1740050710.758330172] [rrtstar_path_planning]: Joint limits received: {'success': True, 'data': robot_state_interface.srv.JointLimit_Response(name=['lbr_A1', 'lbr_A2', 'lbr_A3', 'lbr_A4', 'lbr_A5', 'lbr_A6', 'lbr_A7'], min_position=[-2.9670597283903604, -2.0943951023931953, -2.9670597283903604, -2.0943951023931953, -2.9670597283903604, -2.0943951023931953, -3.0543261909900763], max_position=[2.9670597283903604, 2.0943951023931953, 2.9670597283903604, 2.0943951023931953, 2.9670597283903604, 2.0943951023931953, 3.0543261909900763])}
```

## rrt-algorithmsの組み込み

rrt-algorithmsを実装して行きます．

今回，rrt-algorithmsは次のレポジトリのコードを，ライブラリとしてインストールして，使用します．

[https://github.com/motion-planning/rrt-algorithms.git](https://github.com/motion-planning/rrt-algorithms.git)

まず，適当なコンテナ内で，rrt-algorithmsをクローンしてください．

```bash
git clone https://github.com/motion-planning/rrt-algorithms.git
cd rrt-algorithms
```

その後，rrt_algorithms/rrtディレクトリに，空の**__init__.py**を作成してください．

その後，rrt-algorithmsディレクトリで，次のコマンドを実行して，ライブラリとしてインストールしてください．

```bash
pip install .
```

一応ライブラリのインストール確認のため，次のコマンドを実行してください．

```bash
 pip show rrt-algorithms
```

ちゃんと，rrt-algorithmsの情報が表示されていれば，大丈夫です．

次に実装する前に，関節角度をロボットにpublishするコードを作成します．rrtstar_path_planningパッケージのrrtstar_path_planningディレクトリにrobot_state_publisher_node.pyというファイルを作成して，以下のコードを張り付けてください．

```bash
import time

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState

class RobotStatePublisherNode(Node):
    def __init__(self):
        super().__init__("robot_state_publisher_node")
        self.get_logger().set_level(rclpy.logging.LoggingSeverity.INFO)

        self.joint_state_publisher = self.create_publisher(
            JointState, "/lbr/joint_states", 10
        )

    def publish_joint_states(self, positions):
        message = JointState()
        message.header.stamp = self.get_clock().now().to_msg()
        message.name = ["lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", "lbr_A5", "lbr_A6", "lbr_A7"]
        if len(positions) < 7:
            positions.extend([0.0] * (7 - len(positions)))

        message.position = positions
        message.velocity = [0.0] * len(message.name)
        message.effort = [0.0] * len(message.name)
        self.joint_state_publisher.publish(message)
        self.get_logger().info("Joint states published.")
        time.sleep(0.5)

```

"/lbr/joint_states"に，角度を送ることで，Rviz上のロボットを動かすことができます．

では，実際に実装していきます．

rrtstar_path_planningパッケージのrrtstar_path_planningディレクトリにrrtstar_path_planning.pyというファイルを作成して，以下のコードを張り付けてください．

```bash

from rrtstar_path_planning.robot_state_client_node import (
    RobotStateClientNode
)
from rrtstar_path_planning.robot_state_publisher_node import (
    RobotStatePublisherNode
)
import rclpy
import numpy as np

from rrt_algorithms.rrt.rrt_star import RRTStar
from rrt_algorithms.search_space.search_space import SearchSpace

def main(args=None):
    rclpy.init(args=args)

    logger = rclpy.logging.get_logger("lbr_iiwa7_rrtstar_path_planing")
    logger.info("ROS 2 initialized")

    robot_state_node = RobotStateClientNode()
    logger.info("RobotStateClientNode initialized")
    joint_limits = robot_state_node.get_joint_limits()
    logger.info(f"Joint limits received: {joint_limits}")

    logger.info("MotionPlanner initialized")

    if joint_limits['success']:
            # Extract joint limits data
            min_positions = joint_limits['data'].min_position
            max_positions = joint_limits['data'].max_position
            # Create X_dimensions array with joint limits
            X_dimensions = np.array([(min_pos, max_pos) for min_pos, max_pos in zip(min_positions, max_positions)])

            x_init = (
                1.8500490071139892,
                0.12217304763960307,
                -0.4014257279586958,
                -1.1693705988362009,
                0.5410520681182421,
                0.3141592653589793,
                0.0)  # starting location
            x_goal = (
                2.530727415391778,
                -1.3089969389957472,
                -1.7453292519943295,
                -1.3089969389957472,
                -1.2217304763960306,
                1.4835298641951802,
                0.0)  # goal location

            q = 0.06  # length of tree edges
            r = 1  # length of smallest edge to check for intersection with obstacles
            max_samples = 2**15  # max number of samples to take before timing out
            rewire_count = 32  # optional, number of nearby branches to rewire
            prc = 0.0  # probability of checking for a connection to goal

            # create Search Space
            X = SearchSpace(X_dimensions)

            # create rrt_search
            rrt = RRTStar(X, q, x_init, x_goal, max_samples, r, prc, rewire_count)
            path = rrt.rrt_star()
            robot_state_publisher_node =  RobotStatePublisherNode()
            for point in path:
                robot_state_publisher_node.publish_joint_states(point)
    else:
        logger.error("Failed to get joint limits, using default values")

    robot_state_node.destroy_node()
    logger.info("Node destroyed")

    rclpy.shutdown()
    logger.info("ROS 2 shutdown complete")

if __name__ == "__main__":
    main()

```

初期姿勢は，緑の姿勢，目標姿勢は，オレンジの姿勢を指定しています．また障害物は考慮していません．

|![experimental_setting_xz](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2734306/0dcd3bd0-9d83-49c1-b15d-579bd02b07ec.png)|![experimental_setting_yz](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2734306/9cd0edca-39da-439e-b8a3-a938be5aea05.png)|![experimental_setting_xy](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2734306/4b95e7ba-f484-4a91-907e-8ae5bd429ea7.png)|
|:---:|:---:|:---:|
|xz平面|yz平面|xy平面|





次にlaunchファイルを作成します．

launchディレクトリに，rrtstar_path_planing.launch.pyというファイルを作成し，以下のコードを張り付けてください．

```
from typing import List

from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
from launch.actions import  OpaqueFunction
from launch_ros.actions import Node

def setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
    ld = LaunchDescription()

    ld.add_action(
        Node(
            package="rrtstar_path_planning",
            executable="rrtstar_path_planning",
        )
    )

    return ld.entities

def generate_launch_description() -> LaunchDescription:
    ld = LaunchDescription()

    ld.add_action(OpaqueFunction(function=setup))

    return ld
```

最後に，setup.pyのentry_pointsのconsole_scriptsのvalueの配列に，次のように変更してください．

```bash
    entry_points={
        'console_scripts': [
            f"sample_robot_state_client_node = sample.sample_robot_state_client_node:main",
            f"{package_name} = {package_name}.{package_name}:main",
        ],
    },
```

これで，完了です．実際にシミュレートしましょう．ビルドして，環境を反映するために，次のコマンドを実行してください．

```bash
colcon build --symlink-install --packages-select rrtstar_path_planning
 . install/setup.bash
```

その後，ターミナルを2つ立ち上げて，次のコマンドを実行してください．

- terminal 1

```bash
 ros2 launch robot_state_server robot_state_server.launch.py 
```

- terminal 2

```bash
 ros2 launch rrtstar_path_planning rrtstar_path_planing.launch.py 
```

少し時間（1分ぐらい）たった後に，次のような経路を描いて，ロボットが動作すれば成功です．

![無題の動画 ‐ Clipchampで作成 (3).gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2734306/0260f67c-f98d-425c-88aa-ef9e180afe1a.gif)

RRT*アルゴリズムなので，カクカクした動作になっているので，CHOMPやSTOMPのような最適化ベースの運動計画アルゴリズムを追加で実装したほうがいいかもしれませんね．ランダム性が高いので，ごくたまにいい感じの軌跡を描くこともありますが，また別の機会で頑張ります．．．

# おわり

以上，MoveIt2の上で，RRT*アルゴリズムを例に，独自プランナーの構築を行ってみました．*ロボットアームがカクカク動くのは，*RRT*なら仕方ないなので、もっとスムーズな動きにしたい人は、CHOMPやSTOMPを追加で実装してみてください⁠⁠．この記事が誰かの役に立てば嬉しいです．

# 追加の実装の際に参考になりそうなサイト

- MoveIt2のチュートリアル
    - [障害物の追加の仕方](https://moveit.picknik.ai/humble/doc/tutorials/planning_around_objects/planning_around_objects.html)
    - [自己衝突の有無，や関節角度，手先位置の値の取得例](https://moveit.picknik.ai/humble/doc/examples/robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html)