LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@koki3141

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

Last updated at Posted at 2025-02-22

はじめに

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

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

ソフトウェア構成

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

image.png

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

実行環境

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

実装方法

実装したコードは，Githubにあげています．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のメタパッケージと主なパッケージ（参照元）

メタパッケージ 継承/含まれるパッケージ 主なパッケージ
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, OSRF ROS Docker Hub）

メタパッケージ 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

    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

    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に移動して，以下のコマンドを実行してください．

cd .devcontainer
docker compose up -d

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

lbr_fri_ros2_stack

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

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

. .devcontainer/ros2_setup.sh

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

実行が完了したら，LBR-Stackのチュートリアルにあるデモで，さっそくシミュレーションを動かしてみましょう．

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

  • terminal 1
ros2 launch lbr_bringup mock.launch.py     model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
  • terminal 2
ros2 launch lbr_bringup move_group.launch.py     mode:=mock     rviz:=true     model:=iiwa7 # [iiwa7, iiwa14, med7, med14]

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

image 1.png

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

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

無題の動画 ‐ Clipchampで作成 (2).gif

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

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

rqt_graph

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

image 2.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をコメントアウトします．

    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
図 Service通信のイメージ（ROS2 HumbleのService通信のチュートリアルより）

今回は，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ディレクトリに移動して，以下のコマンドを実行してください．

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

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

  • srv/JointLimit.srv
---
string[] name
float64[] min_position
float64[] max_position

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

  • CMakeLists.txt
~~~
set(srv_files
  "srv/JointLimit.srv"
)

rosidl_generate_interfaces(${PROJECT_NAME}
  ${srv_files}
)
~~~
  • package.xmlに追加
  <member_of_group>rosidl_interface_packages</member_of_group>

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

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

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

これで，完了です．

RobotStateServerの実装

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

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

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ファイルを作成して，以下のコードを貼り付けます．

#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というファイルを作成し，以下を貼り付けてください．

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に次の記述を追加してください．

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ディレクトリに移動し，ビルド＆環境反映するコマンド実行して，

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

起動すると，

ros2 launch robot_state_server sample_robot_state.launch.py

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

[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を作成して，以下のコードを記述してください．

#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に，作成したコードをビルドターゲットとして定義し，それに依存するパッケージを設定してください．その後，インストール設定をにターゲットを追加して，更新してください．

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を作成しています．

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ディレクトリに移動して，以下のコマンドで，パッケージを作成してください．

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というファイルを作成してください．

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を作成して，以下のコードを張り付けてください．


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を作成して，以下のコードを張り付けてください．

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も変更します．

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

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

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

  • terminal 1
 ros2 launch robot_state_server robot_state_server.launch.py
  • terminal 2
ros2 launch rrtstar_path_planning sample_robot_state_client_node.launch.py

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

[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

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

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

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

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

pip install .

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

 pip show rrt-algorithms

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

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

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というファイルを作成して，以下のコードを張り付けてください．


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 experimental_setting_yz experimental_setting_xy
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の配列に，次のように変更してください．

    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",
        ],
    },

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

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

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

  • terminal 1
 ros2 launch robot_state_server robot_state_server.launch.py
  • terminal 2
 ros2 launch rrtstar_path_planning rrtstar_path_planing.launch.py

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

無題の動画 ‐ Clipchampで作成 (3).gif

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

おわり

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

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

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?