🐬

ROS2: MoveIt2パッケージの作成方法

2022/10/04に公開

海洋ロボコンをやってた人です。
今回は、ROS2用のMoveIt2パッケージの作成方法について私の知見をまとめました。
これからMoveIt2を自分の研究機体に実装したい方のお力添えになれば幸いです。


※2023/02/26追記: 本記事の「ROS2」表記、正しくは「ROS 2」です。

1: ROS2 Moveit2パッケージの作成方法

この記事を読むことで、自身のオリジナルロボットをMoveit2で動かせるようになります。

また使用するモデルは、アイコンでも表示されているマンタ型ロボットで紹介していきます。
なお、ロボットのハード設計からソフト設計まで全て私が担当しており、所属研究室の教授からも承諾を得ています。

1.1: 本記事のプログラム

下記のGithubにて公開しているので、こちらを参考にしてください。
スター頂けると、大変嬉しいです・ω・

https://github.com/tasada038/manta_v2

1.2: moveit setup assistantでパッケージを準備

まずは、moveit setup assistant でmoveitに必要なconfigフォルダ群を準備していきます。

2022/10月現在では、まだsetup assistantがMoveit2に対応していないみたいなので、ROS1のsetup assistantを使用して設定ファイル群を作成していきます。

ROS1はご使用のUbuntuバージョンに合わせてインストールしてください。
また、setup assistantを使う際に、ロボットのURDF/xacroモデルが必要になりますので、まだ作成していない方は下記よりパッケージを作成してください。


私はROS1 Noeticで動作させており

terminal
sudo apt install ros-$ROS_DISTRO-moveit

でmoveitパッケージをインストールしておいてください。

まずはROS1用のワークスペースにxxxxx_descriptionフォルダを入れて、buildしておきます。

その後、setup assistantをローンチしましょう。

terminal
cd ~/catkin_manta_ws
source devel/setup.bash
roslaunch moveit_setup_assistant setup_assistant.launch

基本的には、setup assistant の使い方を紹介されている記事通りに進めてもらえれば問題なく作成できます。
以下は私がとても勉強になった記事です。非常に勉強になりました。ありがとうございました。


Moveit! Setup Assistant を使って簡単に Moveit! する

ROS講座104 moveitでオリジナルのアームを使う

ROS入門 (46) - mycobot風ロボットのURDFを作成して、MoveIt構成ファイルを作成して、MoveItで制御


こちらでも、備忘録として、私なりの手順をまとめておきます。

  1. Create New Moveit Configuration Packageボタンから
  • Browse > packages_name_description/urdf/packages_name.xacro > Load Files
    によりxxxxx_descriptionフォルダ内のmanta_v2.xacroを読み込む

  1. Self-Collisions
    Generate Collision Matrixより、Collisionの設定をしてください。

  2. Planning Groups
    Add GroupよりPlanning Groupを設定します。
    産業用ロボット/人協働ロボットなどはアーム部とエンドエフェクタ部を

  • arm_controller
  • gripper_controller
    と分けることがあります。

今回は、マンタ型ロボットの各翼に対してcontrollerを適用させたいので、6つのPlanning Groupを作成しました。

  1. Robot Poses > Add Pose
    こちらはPlanning/Query/Start State:, Goal State:で予め定義できるロボット位置になります。
    ロボット起動時の初期位置や、良く使う位置を定義しておくと良いと思います。

エンドエフェクタを搭載している場合は、ハンドのOpen/Closeを定義しておくと良いです。

こちらは、パッケージ作成後にxxx.srdfファイルより再編集できるので、適当でも問題ありません。

  1. End Effectors
    作成したグループをエンドエフェクタへ指定する設定です。
    今回は使用しないため割愛させていただきます。

  2. Controllers
    これはros_controlを使って制御するための設定です。
    ros_controlを用いてロボットを動かす場合に、ここで設定したControllerをController Managerよりスポーンする必要があります。

今回はROS2対応ということで、ros2_controlを使用するためのyamlファイルを新たに作るので、設定しなくてもよいですが、ROS1でros_controlも使うという方は以下のどちらかで設定をしてください。

  • Auto Add FollowJointsTrajectory Controllers For Each Planning Group
    からPlanning Groupより自動でControllerを生成

  • Add Controller
    より、手動でControllerを追加


またros_controlについて、とても勉強になった記事も記載させていただきます。
ros_controls/ros_controllers の制御の仕組み (position/effort/velocity_controllers の基礎)

  1. Simulation > Generate URDF
    これはGazeboシミュレーターで使用するためのURDFを生成する項目です。
    ROS2では.xacroを呼び出すのは(実行例はありますが)かなり困難なので、URDFを生成しておくのが良いかと思います。

  2. Author Information
    著者、メアドを適宜入力してください。
    メアドを入力しないと、次の項目には進めない点も注意が必要です。

  3. Configuration Files > Generate Package
    パッケージを生成できたら、ROS1で確認してみるのも良いかと思います。

1.3: moveit2 パッケージを作成する

上記で、moveitに必要なパッケージを作成できたら、moveit用のROS2パッケージを作成します。
moveitのパッケージ名は"xxxxx_moveit_config"とつけることが多いです。

cd ~/xxxxx_ws
. install/setup.bash
ros2 pkg create --build-type ament_cmake xxxxx_moveit_config

パッケージを作成したら

  1. SetupAssistantで作成したmoveit_config/configをROS2下へコピー

config下へ
2. config下へyamlを作成

  • packages_name_controllers.yaml
  • packages_name_ros_controllers.yaml
    します。

ロボット名_controllers.yamlファイルは、moveitの動かすために必要なコントローラー情報になっています。
コントローラーの記述方法ですが、moveit setup assistantで生成される"simple_moveit_controllers.yaml"を参考に記述してもらうと良いと思います。

下記では、"simple_moveit_controllers.yaml"内の-nameをcontroller_namesとして先にまとめていますが、プログラムの記述内容は"simple_moveit_controllers.yaml"と同じになっています。

言い換えれば、別に新しく作る必要なないという事ですね。(-ω-)/

manta_v2_controllers.yaml
controller_names:
  - rtop_controller
  - rmid_controller
  - rbtm_controller

rtop_controller:
  action_ns: follow_joint_trajectory
  type: FollowJointTrajectory
  default: true
  joints:
    - rev_link_Rtop_roll1
    - rev_link_Rtop_pitch
    - rev_link_Rtop_roll2

rmid_controller:
  action_ns: follow_joint_trajectory
  type: FollowJointTrajectory
  default: true
  joints:
    - rev_link_Rmid_roll1
    - rev_link_Rmid_pitch
    - rev_link_Rmid_roll2

rbtm_controller:
  action_ns: follow_joint_trajectory
  type: FollowJointTrajectory
  default: true
  joints:
    - rev_link_Rbtm_roll1
    - rev_link_Rbtm_pitch
    - rev_link_Rbtm_roll2

続いて、ロボット名_ros_controllers.yamlはmoveitをros2_controlで動かすためのファイルです。
こちらは、moveit setup assistantで生成される"ros_controllers.yaml"のcontrollerタイプを一部変更して記述しています。

ros2_controllersの公式ドキュメントのAvailable Controllersでは6種類のcontrollerが説明されていますので、ご自身のロボットに応じてタイプを変更してください。
また、ros-controls-GitHubのページに飛ぶと、より多くのコントローラーがあることも確認できます。

私のロボットでは各関節が動けば良いと思ったので、joint_trajectory_controllerを使用します。
この辺は私自身勉強中ですので、訂正等あればご教授お願い致します。

manta_v2_ros_controllers.yaml
controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    rtop_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    rmid_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    rbtm_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

rtop_controller:
  ros__parameters:
    joints:
      - rev_link_Rtop_roll1
      - rev_link_Rtop_pitch
      - rev_link_Rtop_roll2
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity

rmid_controller:
  ros__parameters:
    joints:
      - rev_link_Rmid_roll1
      - rev_link_Rmid_pitch
      - rev_link_Rmid_roll2
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity

rbtm_controller:
  ros__parameters:
    joints:
      - rev_link_Rbtm_roll1
      - rev_link_Rbtm_pitch
      - rev_link_Rbtm_roll2
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity

コントローラーの作成ができたら、ros2_controllerでmoveitのコントローラーを動かすためにURDF/xacroのロボットモデルに紐づけしていき、手順としては以下になります。

  • xxxxx_description/rviz/run_move_group.rvizを作成
  • xxxxx_description/ros2_control/xxxxx_ros2_control.xacroを作成
  • xxxxx.xacro にxxxxx_ros2_control.xacroをインクルードするよう記述
  • xxxxx_description/CMakeLisit.txtにros2_controlのincludeを追加

rvizファイルの作成やros2_control.xacroのディレクトリは任意です。
私はros2_control_demoのディレクトリ構成に沿ってxxxxx_description下にros2_controlのファイルを置いています。

また、rvizファイルはmoveitのデモで使用される

  • /opt/ros/foxy/share/moveit_resources_panda_moveit_config/launch/moveit.rviz
    を参考にしています。
    こちらはrvizのDisplayプラグインからmoveit_ros_visualizationを追加すれば良いので任意で良いでしょう。

ros2_controlの記述としては以下のようになります。

manta_v2_ros2_control.xacro
<?xml version="1.0" ?>
<robot name="manta_v2" xmlns:xacro="http://www.ros.org/wiki/xacro" >
  <xacro:macro name="manta_v2_ros2_control" params="name">

    <ros2_control name="${name}" type="system">
      <hardware>
        <plugin>fake_components/GenericSystem</plugin>
      </hardware>

      <joint name="con_link_ballast">
        <command_interface name="position">
          <param name="min">-0.5</param>
          <param name="max">0.5</param>
        </command_interface>
        <state_interface name="position">
          <param name="initial_value">0.0</param>
        </state_interface>
        <state_interface name="velocity"/>
        <state_interface name="effort"/>
      </joint>
      <!-- 以下略 -->
    </ros2_control>
  </xacro:macro>

</robot>

xacroをros2_control下に配置した場合は、CMakeListsにインクルード関係を記述しビルドを通しておきましょう。

  • moveit launchファイルの作成
    ここまでできたら、xxxxx_moveit_config/launch下に"moveit_demo.launch.pyを作成します。
    また、以下のフォルダ名,load_controllersなどは任意に書き換えてください。
    description_folder = "manta_v2_description"
    moveit_config_folder = "manta_v2_moveit_config"
    xacro_file = "urdf/manta_v2.xacro"
    srdf_file = "config/manta_v2.srdf"
    rviz_file = "rviz/run_move_group.rviz"

    # Load controllers
    load_controllers = []
    for controller in [
        "rtop_controller",
        "rmid_controller",
        "rbtm_controller",
        "joint_state_broadcaster",
    ]:

以下、launch全文

moveit_demo.launch.py
import os
import yaml
from launch import LaunchDescription
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory
import xacro


def load_file(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return file.read()
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None

def load_yaml(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return yaml.safe_load(file)
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None

def generate_launch_description():

    description_folder = "manta_v2_description"
    moveit_config_folder = "manta_v2_moveit_config"
    xacro_file = "urdf/manta_v2.xacro"
    srdf_file = "config/manta_v2.srdf"
    rviz_file = "rviz/run_move_group.rviz"

    # planning_context
    robot_description_config = xacro.process_file(
        os.path.join(
            get_package_share_directory(description_folder),
            xacro_file,
        )
    )
    robot_description = {"robot_description": robot_description_config.toxml()}

    robot_description_semantic_config = load_file(
        moveit_config_folder, srdf_file
    )
    robot_description_semantic = {
        "robot_description_semantic": robot_description_semantic_config
    }

    kinematics_yaml = load_yaml(
        moveit_config_folder, "config/kinematics.yaml"
    )
    robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}

    # Planning Functionality
    ompl_planning_pipeline_config = {
        "move_group": {
            "planning_plugin": "ompl_interface/OMPLPlanner",
            "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
            "start_state_max_bounds_error": 0.1,
        }
    }
    ompl_planning_yaml = load_yaml(
        moveit_config_folder, "config/ompl_planning.yaml"
    )
    ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)

    # Trajectory Execution Functionality
    moveit_simple_controllers_yaml = load_yaml(
        moveit_config_folder, "config/manta_v2_controllers.yaml"
    )
    moveit_controllers = {
        "moveit_simple_controller_manager": moveit_simple_controllers_yaml,
        "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
    }

    trajectory_execution = {
        "moveit_manage_controllers": True,
        "trajectory_execution.allowed_execution_duration_scaling": 1.2,
        "trajectory_execution.allowed_goal_duration_margin": 0.5,
        "trajectory_execution.allowed_start_tolerance": 0.01,
    }

    planning_scene_monitor_parameters = {
        "publish_planning_scene": True,
        "publish_geometry_updates": True,
        "publish_state_updates": True,
        "publish_transforms_updates": True,
    }

    # Start the actual move_group node/action server
    run_move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[
            robot_description,
            robot_description_semantic,
            kinematics_yaml,
            ompl_planning_pipeline_config,
            trajectory_execution,
            moveit_controllers,
            planning_scene_monitor_parameters,
        ],
    )

    # RViz
    rviz_config_file = PathJoinSubstitution(
        [FindPackageShare(description_folder), rviz_file]
    )

    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config_file],
        parameters=[
            robot_description,
            robot_description_semantic,
            ompl_planning_pipeline_config,
            kinematics_yaml,
        ],
    )

    # Static TF
    static_tf = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="log",
        arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"],
    )

    # Publish TF
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="both",
        parameters=[robot_description],
    )

    # ros2_control using FakeSystem as hardware
    ros2_controllers_path = os.path.join(
        get_package_share_directory(moveit_config_folder),
        "config",
        "manta_v2_ros_controllers.yaml",
    )
    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[robot_description, ros2_controllers_path],
        output="screen",
    )

    # Load controllers
    load_controllers = []
    for controller in [
        "rtop_controller",
        "rmid_controller",
        "rbtm_controller",
        "ltop_controller",
        "lmid_controller",
        "lbtm_controller",
        "joint_state_broadcaster",
    ]:
        load_controllers += [
            ExecuteProcess(
                cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
                shell=True,
                output="screen",
            )
        ]

    # Warehouse mongodb server
    mongodb_server_node = Node(
        package="warehouse_ros_mongo",
        executable="mongo_wrapper_ros.py",
        parameters=[
            {"warehouse_port": 33829},
            {"warehouse_host": "localhost"},
            {"warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection"},
        ],
        output="screen",
    )

    return LaunchDescription(
        [
            rviz_node,
            static_tf,
            robot_state_publisher,
            run_move_group_node,
            ros2_control_node,
            # mongodb_server_node,
        ]
        + load_controllers
    )

moveit_demo.launch.py
+ cmd=["ros2 run controller_manager spawner {}".format(controller)],
- cmd=["ros2 run controller_manager spawner.py {}".format(controller)],

foxyでcontroller_mangagerをspawn(スポーン)する場合はspawner.pyという記述で良いみたいですが、Humbleで使用する際はspawner(スポナー)という記述でないとErrorが生じることを確認しています。

そのため、バージョンによって記述を変更する必要がありそうです。

1.4: moveit2 パッケージを実行する

パッケージの作成およびビルドができたら

cd ~/xxxxx_ws
. install/setup.bash
ros2 launch manta_v2_moveit_config moveit_demo.launch.py

で実行してみましょう

マーカーが表示されるので、適当に動かして、Plan & Executeを押すと

動きましたね!

軌道の可視化には、jsk_visualizationをROS2対応してくださった、Kottakuさんのパッケージ群を使用しています。

jsk_visualization for ROS2

私自身、待ち望んでおり、非常に使いやすいのでjskの皆様、Kottaku様には感謝しきれませんね。

オプション:moveit2 group_state

moveitには"Start State:", "Goal State:"という項目があり、srdfファイル内にgroup_stateタグを記述することで、デフォルトポーズを呼び出すことができます。

manta_v2.srdf
+    <group_state group="Ltop" name="Init">
+        <joint name="rev_link_Ltop_roll1" value="0"/>
+        <joint name="rev_link_Ltop_pitch" value="0"/>
+        <joint name="rev_link_Ltop_roll2" value="0"/>
+    </group_state>

1.5: よくあるエラー

Controller Manager よりros2のcontrollerを読み込めないと、エラーになり、ロボットモデルが正常に動作しません。

エラーが発生しているんだなと分かる点を、正常動作時と比較して記載しておきます。

1. You can start planning now!が表示されるタイミング

launchファイルが正常に起動すると、You can start planning now!の緑色の文字が表示されるタイミングとほぼ同時にRvizのスプラッシュ画面が表示されます。

Rvizのスプラッシュ画面が表示されているのに、青文字、緑文字の表記がでていなかったら、controllerが上手く読み込めていない可能性が高いです。

  1. controller managerのスポーン

ros2_controlを使用する場合、設定したcontrollerを正常にスポーンできると
Configured and started xxx_controller
と色付き文字で表示されます。

逆に、この表記が見られなかったら、呼び出しに失敗しているのでプログラムを見直す必要があります。

疑問点、改善点等あればお気軽にご連絡ください。
また、この記事が良かったと感じたら、"いいね"ボタンよろしくお願いします。

以上。

Discussion