マニピュレーター付きロボットで疑似的に壁を塗装してみた

1. はじめに

前回の記事[1]ではNeobotix社の公開リポジトリを活用し、Gazeboシミュレーション環境下で、マニピュレーターを搭載したロボットによる自律走行と簡単な塗装作業（平面壁へのスプレーガンなどによる塗装作業）を再現することに成功しました。しかしながら、塗装作業をしたとはいえ、色が変わるような様子を再現できていませんでしたので、本記事ではその課題解決に取り組みました。
なお、目標としたのは[2]の論文の図９にあるようなマニピュレーターが動いたところが黄色い帯で示されるような様子をrviz2上で再現することです。

2. 実行環境

• CPU: CORE i7 7th Gen
• メモリ: 32GB
• GPU: GeForce RTX 2070
• OS: Ubuntu22.04(WSL2ではなくPCに直接インストール)
• ROS2: Humble
• Nvidia Driver: 535
• 対象ロボット: Neobotix社 MPO-700(マニピュレーター搭載）[1]で構築した環境

3. 構築手順

3.1 パッケージの作成

rivz2上でマニピュレーターが動いたところを帯状で表すためのプログラムは、私が探した限り公開されておりませんでした。なので、今回はROS2のパッケージを自作しました。

前回の記事[1]で作成したros2_mmo500というフォルダの中に以下のコマンドでパッケージを作成していきます

terminal

cd ros2_mmo500
cd src
source /opt/ros/humble/setup.bash
ros2 pkg create paint_path --build-type ament_python

こうして作成したpaint_pathというフォルダ内には同じ名前のpaint_pathというフォルダがありますので、その中に以下のpaint_path_node.pyというプログラムを作成・保存します。

このpaint_path_node.pyですが、以下の特徴を持たせるつもりで作ってます。しかしながら検証がまだ十分にできていないのでお使いになる時は注意してください

• 特徴１：マニピュレータにZ方向の変化があり、それが高さ５０ｃｍ以上なら塗装しているとしてマニピュレータが通ったところを黄色の帯を表示する。
• 特徴２：ロボットが動いたこともマニピュレータの通ったところとして、すべて塗装面として塗装されないように、ロボットが10cm以上動いたら新しいマーカーidを設定する。

paint_path_node.py

import rclpy
from rclpy.node import Node
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
from tf2_ros import TransformListener, Buffer
from collections import deque

class PaintPathNode(Node):
    def __init__(self):
        super().__init__('paint_path_node')
        
        # ペイントパスを表示するパブリッシャー
        self.marker_pub = self.create_publisher(Marker, 'visualization_marker', 10)
        self.timer = self.create_timer(0.1, self.publish_marker)

        # マーカーの初期設定
        self.marker_id = 0  # 各セグメントのID
        self.initialize_marker()

        # TFバッファとリスナーの初期化
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        
        self.prev_z = None  # マニピュレータの前のZ座標（base_link基準）
        self.paint_active = False  # ペイントがアクティブかどうかのフラグ
        self.move_threshold = 0.1  # 移動距離の閾値（メートル）
        self.time_window = 5.0  # 時間窓（秒）

        # 位置と時刻の履歴を保持するデック（両端キュー）
        self.position_history = deque()

    def initialize_marker(self):
        """新しいマーカーを初期化する"""
        self.marker = Marker()
        self.marker.header.frame_id = 'map'  # mapフレームに固定
        self.marker.type = Marker.LINE_STRIP
        self.marker.action = Marker.ADD
        self.marker.scale.x = 0.2  # ペイントの線の太さ
        self.marker.color.r = 1.0  # 黄色
        self.marker.color.g = 1.0
        self.marker.color.b = 0.0
        self.marker.color.a = 1.0
        self.marker.pose.orientation.w = 1.0
        self.marker.id = self.marker_id  # 新しいIDを設定

    def publish_marker(self):
        try:
            # base_linkを基準にしたマニピュレータの位置を取得
            wrist_transform = self.tf_buffer.lookup_transform('base_link', 'ur10wrist_3_link', rclpy.time.Time())
            base_transform = self.tf_buffer.lookup_transform('map', 'base_link', rclpy.time.Time())
            
            # 現在の時刻を取得
            current_time = self.get_clock().now().seconds_nanoseconds()[0]

            # 現在のbase_linkの位置を取得
            current_base_position = base_transform.transform.translation

            # 現在の位置と時刻を履歴に追加
            self.position_history.append((current_time, current_base_position))

            # 時間窓外の古いデータを削除
            while self.position_history and (current_time - self.position_history[0][0]) > self.time_window:
                self.position_history.popleft()

            # 時間窓内の移動距離を計算
            if len(self.position_history) > 1:
                total_distance = 0.0
                positions = [pos for _, pos in self.position_history]
                for i in range(1, len(positions)):
                    dx = positions[i].x - positions[i-1].x
                    dy = positions[i].y - positions[i-1].y
                    distance = (dx**2 + dy**2) ** 0.5
                    total_distance += distance

                # 閾値以上動いたら新しいペイントを開始
                if total_distance > self.move_threshold:
                    self.marker_id += 1  # マーカーID
                    self.initialize_marker()  # 新しいマーカーを作成
                    self.paint_active = False  # ペイントを待機状態にする
                    self.position_history.clear()  # 履歴をクリア

            # マニピュレータのZ座標をbase_link基準で取得
            current_z = wrist_transform.transform.translation.z

            # Z方向の動きがあった場合のみペイントをアクティブにする
            if self.prev_z is not None:
                if abs(current_z - self.prev_z) > 0.01 and current_z > 0.5:  # Z変化が0.01以上、かつ0.5m以上
                    self.paint_active = True
                else:
                    self.paint_active = False
            else:
                # 初回はprev_zを設定するだけ
                self.paint_active = False

            # ペイントがアクティブならポイントを追加
            if self.paint_active:
                # マニピュレータの位置をmapフレームで取得（表示のため）
                wrist_in_map = self.tf_buffer.lookup_transform('map', 'ur10wrist_3_link', rclpy.time.Time())
                point = Point(
                    x=wrist_in_map.transform.translation.x,
                    y=wrist_in_map.transform.translation.y,
                    z=wrist_in_map.transform.translation.z
                )
                # ポイントを追加
                self.marker.points.append(point)
                self.marker.header.stamp = self.get_clock().now().to_msg()
                self.marker_pub.publish(self.marker)

            # 現在のZ座標を更新
            self.prev_z = current_z

        except Exception as e:
            self.get_logger().warn(f"Transform not available: {e}")

def main(args=None):
    rclpy.init(args=args)
    node = PaintPathNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

3.2 rviz2のlaunchファイルの作成

上記で作成したパッケージを読み込めるように以下のlaunchファイル（rviz_launch2.py）を作成し、ros2_mmo500\src\neo_nav2_bringup\launchの中に保存しました。

rviz_launch2.py

# rviz_launch2.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler
from launch.conditions import IfCondition, UnlessCondition
from launch.event_handlers import OnProcessExit
from launch.events import Shutdown
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from nav2_common.launch import ReplaceString

def generate_launch_description():
    bringup_dir = get_package_share_directory('neo_nav2_bringup')
    namespace = LaunchConfiguration('namespace')
    use_namespace = LaunchConfiguration('use_namespace')
    rviz_config_file = LaunchConfiguration('rviz_config')

    declare_namespace_cmd = DeclareLaunchArgument(
        'namespace', default_value='',
        description=('Top-level namespace. The value will be used to replace the '
                     '<robot_namespace> keyword on the rviz config file.'))

    declare_use_namespace_cmd = DeclareLaunchArgument(
        'use_namespace', default_value='False',
        description='Whether to apply a namespace to the navigation stack')

    # single_robot.rvizを使用
    declare_rviz_config_file_cmd = DeclareLaunchArgument(
        'rviz_config', default_value=os.path.join(bringup_dir, 'rviz', 'single_robot.rviz'),
        description='Full path to the RVIZ config file to use')

    namespaced_rviz_config_file = ReplaceString(
        source_file=rviz_config_file,
        replacements={'<robot_namespace>': ('/', namespace)})

    no_namespaced_rviz_config_file = ReplaceString(
        source_file=rviz_config_file,
        replacements={'<robot_namespace>': ('', namespace)})

    start_namespaced_rviz_cmd = Node(
        package='rviz2',
        executable='rviz2',
        condition=IfCondition(use_namespace),
        namespace=namespace,
        arguments=['-d', namespaced_rviz_config_file],
        output='screen',
    )

    start_rviz_cmd = Node(
        package='rviz2',
        executable='rviz2',
        condition=IfCondition(PythonExpression(['not ', use_namespace])),
        arguments=['-d', no_namespaced_rviz_config_file],
        output='screen',
    )

    # Paint Path Node - マニピュレーター先端の軌跡を描画するノード
    paint_path_node = Node(
        package='paint_path',  # 新しく作成したパッケージ名
        executable='paint_path_node',
        name='paint_path_node',
        output='screen',
    )

    exit_event_handler_namespaced = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=start_namespaced_rviz_cmd,
            on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))))

    ld = LaunchDescription()
    ld.add_action(declare_namespace_cmd)
    ld.add_action(declare_use_namespace_cmd)
    ld.add_action(declare_rviz_config_file_cmd)
    ld.add_action(start_namespaced_rviz_cmd)
    ld.add_action(start_rviz_cmd)
    ld.add_action(paint_path_node)
    ld.add_action(exit_event_handler_namespaced)

    return ld

3.3 ビルド

以下のコマンドでビルドしました。

terminal

cd ~/ros2_mmo500
rosdep update
rosdep install --from-paths src --ignore-src -r -y
source /opt/ros/humble/setup.bash
colcon build

4. シミュレーション結果

ビルド完了後、以下のコマンドでシミュレーション環境(Gazebo)とrviz2を起動しました。

terminal(1)

source ~/ros2_mmo500/install/setup.bash
ros2 launch neo_simulation2 simulation.launch.py

terminal(2)

source ~/ros2_mmo500/install/setup.bash
ros2 launch neo_simulation2 navigation.launch.py

terminal(3)

source ~/ros2_mmo500/install/setup.bash
ros2 launch neo_nav2_bringup rviz_launch2.py

またKimらの論文[2]では、ロボットが自律走行後にマニピュレーターで塗装を行うシミュレーションを行っています。本記事では、このシミュレーションを再現するために、以下のコマンドを実行してマニピュレーターを動かしました。

terminal(4)

source ~/ros2_mmo500/install/setup.bash
ros2 topic pub --once /arm_controller/joint_trajectory trajectory_msgs/JointTrajectory '
{
  "joint_names": ["ur10shoulder_pan_joint", "ur10shoulder_lift_joint", "ur10elbow_joint", "ur10wrist_1_joint", "ur10wrist_2_joint", "ur10wrist_3_joint"],
  "points": [
    {
      "positions": [0.0, 1.5, 0.5, 0.0, 0.0, 0.0],
      "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
      "time_from_start": { "sec": 3, "nanosec": 0 }
    }
  ]
}'

そうしますと以下の図のように、マニピュレーターの軌跡を塗装されているような形で表示できました。

なお、このように表示するには、rviz2上で左のウィンドウのAdd>赤枠で囲んだtopicを選択>OK　としておかないと表示されないです。

それからデフォルトだと真上から見た視点になりますので、以下の図で赤枠で囲んだところを"XYOrbit"を選択して、斜めから見ているような視点に変えると見やすいです。

次にロボットを少し横に自律移動させ、次は以下のコマンドを実行してマニピュレーターを動かしました。

terminal(4)

source ~/ros2_mmo500/install/setup.bash
ros2 topic pub --once /arm_controller/joint_trajectory trajectory_msgs/JointTrajectory '
{
  "joint_names": ["ur10shoulder_pan_joint", "ur10shoulder_lift_joint", "ur10elbow_joint", "ur10wrist_1_joint", "ur10wrist_2_joint", "ur10wrist_3_joint"],
  "points": [
    {
      "positions": [0.0, -0.25, 0.5, 0.0, 0.0, 0.0],
      "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
      "time_from_start": { "sec": 3, "nanosec": 0 }
    }
  ]
}'

そうしますと以下の図のように、2本目の塗装面を表示できました。

以下が上記の手順で、「マニピュレーターで塗装を行う」という様子をシミュレーションで再現した動画です。

5. まとめ

今回は自分でROS2パッケージを作成し、マニピュレーターを搭載したロボットによる塗装面をrviz2上で疑似的に表現することに成功しました。ただ塗装面が湾曲していることから、マニピュレーターの動作制御が不十分と考えられます。今後はこの課題に取り組むとともにKimらの論文[2]を参考に、BIM(Building Information Modeling)データをROS2のシミュレーションに取り込めるようにするツールの再現にも取り組みたいと考えています。

参考記事やサイト

[1]マニピュレーター付きロボットをシミュレーション環境（Gazebo）で自律走行させてみた
[2]Development of BIM-integrated construction robot task planning and simulation system