1. はじめに

前回の記事[1]ではBIM(Building Information Modeling)データの中でも形状を表すデータであるIFCファイルをROS2のシミュレーションに取り込み、そしてその環境でロボット走行用地図を作成してロボットを自律走行させることに成功しました。
そして、[1]の記事にも書きましたが、Kimらの論文[2]を参考に作業工程をロボットでの塗装作業シミュレーションに取り込むことの再現をしたいと述べました。それに取り組んでみたのが、この記事の内容になります。

最初にどのような結果になったかを動画でお見せします。

1.1 Kimらの論文[2]の特徴

Kimらの論文[2]の特徴は以下だと、私は考えております。

① BIMデータのロボット計画への活用：

IFC形式のBIMデータ（3Dデータの部分）をSDF形式に変換するツールを開発し、ROS対応のGazebo上でシミュレーション可能な環境を構築。これにより、建設現場の物理的な情報をロボットシミュレーションに反映できるようにしている。

② BIMとROSの統合フレームワークの開発

BIMデータから作業スケジュールを抽出し、それをロボット作業に必要なXMLファイルへ変換するシステムを構築。このシステムには、「ロボットと人間が同じエリアで作業しない」というルールを含むプロジェクト特有の条件に対応する機能を備えている。

③ 室内壁塗装ロボットのシミュレーション

壁塗装タスクを想定し、ロボットの自律移動およびマニピュレータ動作を再現するロボットモデルを構築。Gazeboを使用し、塗装動作のシミュレーションを実施した。

④ 動的障害物回避の検証

建設作業現場を想定し、シミュレーション環境内に障害物を配置。ロボットが障害物を検知して動的に経路を調整する機能をシミュレーションにより評価した。障害物の種類や配置を変化させることで、多様な状況に対応する計画機能も検証している。

⑤ レポート生成機能

シミュレーション結果をHTML形式で出力し、ロボットの作業進捗や作業結果を人が視覚的に確認できるレポート生成機能を実装した。

⑥ 汎用性のある設計

市販のROS対応ロボット（Neobotix社 MMO-500）を活用することで、他の作業タスクや他のロボットに応用可能な柔軟性を持つシステムを構築し、壁塗装以外の建設作業にも適用可能性を示唆した。

1.2 Kimらの論文[2]を再現しようと私が取り組んだこと

Kimらの論文[2]を再現しようと今まで取り組んだこと、まだ実現できていないことを以下に列挙します。そして本記事では実現できていること及び本記事で取り組んだことを統合して「マニピュレーター付きロボットでの塗装作業のシミュレーション」をしたいと思います。

① BIMデータのロボット計画への活用：

以下の記事で、IFC形式のBIMデータ（3Dデータの部分）をSDF形式に変換するツールをgithubで見つけて自分の環境で構築し、ROS2対応のGazebo上でシミュレーション可能な環境を構築しました。

本件の課題としてはサンプルファイルの変換しかできいないということであり、より複雑なIFCデータの変換にも挑戦したいと思っていますが、それは別記事で取り組みたいと思います。

② BIMとROSの統合フレームワークの開発

これについては本記事で取り組みましたので、３章で記載しております。

③ 室内壁塗装ロボットのシミュレーション

以下の記事で、非常に簡易的な壁塗装タスクを想定し、ロボットの自律移動およびマニピュレータ動作を再現するロボットモデルを構築。Gazeboを使用し、塗装動作のシミュレーションを実施しました。

本件の課題としては、マニピュレータの軌跡に沿って塗装するので、塗装面が湾曲してしまうという課題がありました。この点については本記事で解決に向けた取り組みを行い、3.3節で記載しております。

④ 動的障害物回避の検証

この機能については、ロボット自律走行用地図を作成し、その地図を基に自律走行できるようにすることで実現できると考え、以下の記事で取り組みました。

⑤ レポート生成機能

この機能については、まだ取り組んでいませんので別記事で取り組みたいと思います。

⑥ 汎用性のある設計

これについては、Neobotix社 MPO-700(マニピュレーター搭載）という機種を選択しましたので、自分の構築したシミュレーション環境においても汎用性の確保はできていると考えています。

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. ② BIMとROSの統合フレームワークの開発（スケジュールとペイントルートをxmlファイルに変換してロボットにタスクとして与える）

Kimらの論文[2]では以下の特徴があると先ほど述べました：
「BIMデータから作業スケジュールを抽出し、それをロボット作業に必要なXMLファイルへ変換するシステムを構築。このシステムには、「ロボットと人間が同じエリアで作業しない」というルールを含むプロジェクト特有の条件に対応する機能を備えている。」
これについて簡単な形で再現を試みたのが、この章の内容になります。

3.1 スケジュールの設定

3.1.1 スケジュールをcsvファイルとして作成

本来ならばBIMデータに入っているスケジュールデータを抽出のうえXMLファイルに変換する形にしたいのですが、今回はスケジュールデータが入っているBIMデータを用意できなかったため、Kimらの論文[2]の内容を参考にスケジュールを以下のschedule.csvとして作成しました。

各項目の内容

Task_ID: タスクID
Task_Type: 作業内容を表し、本記事では塗装（Paint）作業をロボットで行うスケジュールを立てています
Task_Description: どのエリアで作業をするかを記載しています
Area: 作業エリア
Start_Time: 作業開始時間
Duration: 作業時間
Predecesso: 前工程のタスクID
Resources: 誰がもしくはどれが作業をするのか

Task_ID	Task_Type	Task_Description	Area	Start_Time	Duration	Predecessor	Resources
1	Drywall	Drywall installation in Area 1	Area1	2024-11-22T09:00:00	1day		Drywall crew
2	Paint	Wall painting in Area 1	Area1	2024-11-23T09:00:00	4hours	1	Neobotix robot painter
3	Flooring	Floor installation in Area 1	Area1	2024-11-24T09:00:00	2days	2	Flooring crew
4	Drywall	Drywall installation in Area 2	Area2	2024-11-22T09:00:00	2days		Drywall crew
5	Paint	Wall painting in Area 2	Area2	2024-11-24T09:00:00	4hours	4	Neobotix robot painter
6	Flooring	Floor installation in Area 2	Area2	2024-11-25T09:00:00	2days	5	Flooring crew

3.1.2 スケジュールについてのcsvファイルをXMLに変換するコード

csvファイルをXMLに変換するコードとして、以下のコードをcsv_to_robot_schedule.pyという名前で作成しました。

csv_to_robot_schedule.py

import csv
import xml.etree.ElementTree as ET
from xml.dom.minidom import parseString

def csv_to_robot_schedule_xml(csv_file, xml_file):
    # XMLルート作成
    root = ET.Element("Robot_Schedule")

    with open(csv_file, mode="r") as file:
        reader = csv.DictReader(file)
        
        for row in reader:
            # リソースが「Neobotix robot painter」の場合のみ処理
            if "Neobotix robot painter" in row.get("Resources", ""):
                task = ET.SubElement(root, "Task")
                ET.SubElement(task, "Task_ID").text = row["Task_ID"]
                ET.SubElement(task, "Task_Type").text = row["Task_Type"]
                ET.SubElement(task, "Task_Description").text = row["Task_Description"]
                ET.SubElement(task, "Area").text = row["Area"]
                ET.SubElement(task, "Start_Time").text = row["Start_Time"]
                ET.SubElement(task, "Duration").text = row["Duration"]
                ET.SubElement(task, "Predecessor").text = row["Predecessor"]

    # XMLのフォーマット整形
    rough_string = ET.tostring(root, 'utf-8')
    reparsed = parseString(rough_string)
    pretty_xml = reparsed.toprettyxml(indent="  ")

    # フォーマット済みXMLを書き出し
    with open(xml_file, mode="w", encoding="utf-8") as file:
        file.write(pretty_xml)

### CSVをXMLに変換
csv_to_robot_schedule_xml("schedule.csv", "robot_tasks_schedule.xml")

csv_to_robot_schedule.pyとschedule.csvを同じディレクトリに保存し、以下のコマンドで実行しました。

terminal

python3 csv_to_robot_schedule.py

そして、生成されたxmlファイルは以下のrobot_tasks_schedule.xmlです。

robot_tasks_schedule.xml

<?xml version="1.0" ?>
<Robot_Schedule>
  <Task>
    <Task_ID>2</Task_ID>
    <Task_Type>Paint</Task_Type>
    <Task_Description>Wall painting in Area 1</Task_Description>
    <Area>Area1</Area>
    <Start_Time>2024-11-23T09:00:00</Start_Time>
    <Duration>4hours</Duration>
    <Predecessor>1</Predecessor>
  </Task>
  <Task>
    <Task_ID>5</Task_ID>
    <Task_Type>Paint</Task_Type>
    <Task_Description>Wall painting in Area 2</Task_Description>
    <Area>Area2</Area>
    <Start_Time>2024-11-24T09:00:00</Start_Time>
    <Duration>4hours</Duration>
    <Predecessor>4</Predecessor>
  </Task>
</Robot_Schedule>

3.2. 塗装箇所の設定

3.2.1 塗装箇所をcsvファイルとして作成

本来ならばBIMデータに入っている塗装箇所を抽出のうえXMLファイルに変換する形にしたいのですが、今回は塗装箇所が入っているBIMデータを用意できなかったため、Kimらの論文[2]の内容を参考に塗装が必要な箇所をペイントルートとして以下のpaint_routes.csvを作成しました。

各項目の内容

Task_ID: タスクID　作業内容や作業エリアによりスケジュールと紐づける方がスマートですが、この記事では簡単のためにタスクIDを記載しました。この点は今後改善したいと思います。
Task_Type: 作業内容を表し、本記事では塗装（Paint）作業をロボットで行うスケジュールを立てています
Area: 作業エリア
Wall_ID: 塗装する壁のID
Initial_Position_X: 塗装を開始する位置のx座標
Initial_Position_Y: 塗装を開始する位置のy座標
Orientation_Yaw: 塗装を開始する位置の方向
Number_of_Stations: 塗装作業回数　塗装作業は塗装開始位置での作業が終わったら「Width_of_Stroke」分だけ横に動き、移動完了したたらまたその位置で同じように塗装するという形にしました。ここではその塗装回数を記載しています（初期位置での塗装作業も含む回数としています）。
Width_of_Stroke: 前述した塗装作業の際に横に移動する距離(m)
Move_Direction: 前述した塗装作業の際に横に移動する方向

Task_ID	Area	Wall_ID	Initial_Position_X	Initial_Position_Y	Orientation_Yaw	Number_of_Stations	Width_of_Stroke	Move_Direction
2	Area1	Wall1	7.71	2.42	3.1415	3	0.2	Right
5	Area2	Wall3	7.95	5.52	3.1415	3	0.2	Right

また塗装箇所対象箇所をrviz2で表すと、以下の図の赤枠の２か所を塗装箇所として設定しました。

以下は視点を180度回転した図です。

なお、Gazeboで表すと以下の図の赤枠で示した柱の内側の１側面になります。

3.2.2 塗装箇所についてのcsvファイルをXMLに変換するコード

csvファイルをXMLに変換するコードとして、以下のコードをcsv_to_robot_schedule.pyという名前で作成しました。

csv_to_paint_routes.py

import csv
import xml.etree.ElementTree as ET
from xml.dom.minidom import parseString

def csv_to_paint_routes_xml(csv_file, xml_file):
    # XMLルート作成
    root = ET.Element("Paint_Plan")
    
    # ロボットパラメータの設定
    robot_params = ET.SubElement(root, "Robot_Parameters")
    ET.SubElement(robot_params, "Arm_Type").text = "Neobotix mmo-500"
    ET.SubElement(robot_params, "Stroke_Width").text = "1.0"
    
    # エリア情報の設定
    room_routes = ET.SubElement(root, "Room_Routes")

    with open(csv_file, mode="r") as file:
        reader = csv.DictReader(file)
        current_area = None
        area_element = None

        for row in reader:
            # エリアが変わるたびに新しいエリア要素を作成
            area = row["Area"]
            if area != current_area:
                current_area = area
                area_element = ET.SubElement(room_routes, "Area")
                ET.SubElement(area_element, "Area_ID").text = current_area

            # 壁情報を追加
            wall = ET.SubElement(area_element, "Wall")
            ET.SubElement(wall, "Task_ID").text = row["Task_ID"]  # Task_IDを追加
            ET.SubElement(wall, "Wall_ID").text = row["Wall_ID"]
            initial_position = ET.SubElement(wall, "Initial_Position")
            ET.SubElement(initial_position, "X_Coord").text = row["Initial_Position_X"]
            ET.SubElement(initial_position, "Y_Coord").text = row["Initial_Position_Y"]
            ET.SubElement(initial_position, "Yaw").text = row["Orientation_Yaw"]  
            ET.SubElement(wall, "Number_of_Stations").text = row["Number_of_Stations"]
            ET.SubElement(wall, "Width_of_Stroke").text = row["Width_of_Stroke"]
            ET.SubElement(wall, "Move_Direction").text = row["Move_Direction"]

    # XMLのフォーマット整形
    tree = ET.ElementTree(root)
    rough_string = ET.tostring(root, 'utf-8')
    reparsed = parseString(rough_string)
    pretty_xml = reparsed.toprettyxml(indent="  ")

    # フォーマット済みXMLを書き出し
    with open(xml_file, mode="w", encoding="utf-8") as file:
        file.write(pretty_xml)

# CSVをXMLに変換
csv_to_paint_routes_xml("paint_routes.csv", "paint_routes.xml")

csv_to_paint_routes.pyとpaint_routes.csvを同じディレクトリに保存し、以下のコマンドで実行しました。

terminal

python3 csv_to_paint_routes.py

そして、生成されたxmlファイルは以下のpaint_routes.xmlです。

paint_routes.xml

<?xml version="1.0" ?>
<Paint_Plan>
  <Robot_Parameters>
    <Arm_Type>Neobotix mmo-500</Arm_Type>
    <Stroke_Width>1.0</Stroke_Width>
  </Robot_Parameters>
  <Room_Routes>
    <Area>
      <Area_ID>Area1</Area_ID>
      <Wall>
        <Task_ID>2</Task_ID>
        <Wall_ID>Wall1</Wall_ID>
        <Initial_Position>
          <X_Coord>7.71</X_Coord>
          <Y_Coord>2.42</Y_Coord>
          <Yaw>3.1415</Yaw>
        </Initial_Position>
        <Number_of_Stations>3</Number_of_Stations>
        <Width_of_Stroke>0.2</Width_of_Stroke>
        <Move_Direction>Right</Move_Direction>
      </Wall>
    </Area>
    <Area>
      <Area_ID>Area2</Area_ID>
      <Wall>
        <Task_ID>5</Task_ID>
        <Wall_ID>Wall3</Wall_ID>
        <Initial_Position>
          <X_Coord>7.95</X_Coord>
          <Y_Coord>5.52</Y_Coord>
          <Yaw>3.1415</Yaw>
        </Initial_Position>
        <Number_of_Stations>3</Number_of_Stations>
        <Width_of_Stroke>0.2</Width_of_Stroke>
        <Move_Direction>Right</Move_Direction>
      </Wall>
    </Area>
  </Room_Routes>
</Paint_Plan>

3.3. ROS2ノードの準備

この記事では3つのROS2ノードを準備するため、[3]の記事で作成したpaint_pathパッケージ内（ros2_mmo500\src\paint_path）のpaint_pathフォルダの中に保存します。

human_task_notifier.py: 人間タスクの完了を通知するノード
task_trigger_node.py: 通知を受け取り、ロボットタスクをトリガーするノード
paint_path_node_R1.py: ロボットの塗装作業を実行するノード

またこのフォルダ内にはutilityというフォルダを作成し、前節で作成したpaint_routes.xmlとrobot_tasks_schedule.xmlを保存しておきます。

human_task_notifier.py

このプログラムは人間が行ったタスク（例: Drywall Installation）が完了したことを通知します。完了したことは、/human_task_statusトピックに以下のようにros2 topicにメッセージを投げて通知します。ここについてはターミナルから打つのではなく、何かUIを作って投げるようにしたいと思っていますが、それは今後の課題として別記事で取り組みたいと思います。

terminalで打つタスク完了通知のコマンド

source ~/ros2_mmo500/install/setup.bash
ros2 topic pub /human_task_status std_msgs/String '{"data": "{\"task_id\": \"1\", \"status\": \"completed\"}"}'

human_task_notifier.py

import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import json
from datetime import datetime

class HumanTaskNotifier(Node):
    def __init__(self):
        super().__init__('human_task_notifier')

        # タスク完了を通知するパブリッシャー
        self.publisher = self.create_publisher(String, 'human_task_status', 10)

        # 外部トリガー（例: 手動入力など）を受信するサブスクライバー
        self.subscription = self.create_subscription(
            String,
            'external_task_trigger',
            self.trigger_callback,
            10
        )

        self.get_logger().info("HumanTaskNotifier is ready to receive external triggers.")

    def trigger_callback(self, msg):
        """外部トリガーを受信してタスク完了を通知"""
        task_id = msg.data  # 外部トリガーからタスクIDを取得
        message = {
            "task_id": task_id,
            "status": "completed",
            "timestamp": datetime.now().isoformat()
        }
        notification_msg = String()
        notification_msg.data = json.dumps(message)
        self.publisher.publish(notification_msg)
        self.get_logger().info(f"Notified completion of task: {task_id}")

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

if __name__ == '__main__':
    main()

task_trigger_node.py

このプログラムはhuman_task_notifier.pyから送信される/human_task_statusを監視します。
完了通知を受け取り、次のロボットタスクをトリガーします。

task_trigger_node.py

import os
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import xml.etree.ElementTree as ET
import json
from ament_index_python.packages import get_package_share_directory

class TaskTriggerNode(Node):
    def __init__(self):
        super().__init__('task_trigger_node')

        # XMLスケジュールファイルのパス
        package_share_directory = get_package_share_directory('paint_path')
        self.schedule_path = os.path.join(package_share_directory, 'utility', 'robot_tasks_schedule.xml')

        # 実行済みタスクリスト
        self.completed_tasks = []  # 完了済みの全タスク（人間+ロボット）

        # サブスクライバーとパブリッシャー
        self.subscription = self.create_subscription(
            String,
            'human_task_status',
            self.task_status_callback,
            10
        )
        self.publisher = self.create_publisher(String, 'task_command', 10)
        self.get_logger().info("TaskTriggerNode is ready to monitor human task status.")

    def task_status_callback(self, msg):
        """人間タスク完了通知を受け取る"""
        task_status = json.loads(msg.data)
        task_id = task_status.get("task_id", "Unknown")
        status = task_status.get("status", "Unknown")

        if status == "completed":
            self.get_logger().info(f"Received completion of human task: {task_id}")
            self.completed_tasks.append(task_id)  # 完了済みに追加
            self.check_next_task()

    def check_next_task(self):
        """次に実行可能なロボットタスクをチェックしてトリガー"""
        if not os.path.exists(self.schedule_path):
            self.get_logger().error(f"Schedule file not found: {self.schedule_path}")
            return

        tree = ET.parse(self.schedule_path)
        root = tree.getroot()

        for task in root.findall('Task'):
            task_id = task.find('Task_ID').text
            predecessor = task.find('Predecessor').text

            # ロボットタスクが未完了で、先行タスクが完了している場合のみトリガー
            if task_id not in self.completed_tasks and predecessor in self.completed_tasks:
                self.trigger_task(task_id)
                break  # 1つのタスクを実行したらループを終了

    def trigger_task(self, task_id):
        """ロボットタスクをトリガー"""
        command_message = {"task_id": task_id}
        msg = String()
        msg.data = json.dumps(command_message)
        self.publisher.publish(msg)
        self.get_logger().info(f"Triggered robot task: {task_id}")
        self.completed_tasks.append(task_id)  # ロボットタスクも完了済みに追加

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

if __name__ == '__main__':
    main()

paint_path_node_R1.py

このプログラムはトリガーされたタスクIDに基づいて、ペイント動作を実行します。XMLファイル（paint_routes.xml）を読み込み、ペイントの設定を取得します。

なお、「③室内壁塗装ロボットのシミュレーション」のところで課題として挙げた、「マニピュレータの軌跡に沿って塗装するので、塗装面が湾曲してしまう」という課題については、塗装する箇所のx,y方向はロボット前方70cmのところに固定し、z方向だけマニピュレータの軌跡を再現するというプログラムにして対処しました。

paint_path_node_R1.py

import os
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from nav2_msgs.action import NavigateToPose
from std_msgs.msg import String
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
from tf2_ros import TransformListener, Buffer
from ament_index_python.packages import get_package_share_directory
import xml.etree.ElementTree as ET
import math
import json
from collections import deque

class PaintPathNodeR1(Node):
    def __init__(self):
        super().__init__('paint_path_node_R1')

        # XMLファイルパスの指定
        package_share_directory = get_package_share_directory('paint_path')
        self.paint_routes_path = os.path.join(package_share_directory, 'utility', 'paint_routes.xml')

        # タスクコマンドを受信するサブスクライバー
        self.task_sub = self.create_subscription(
            String,
            'task_command',
            self.task_command_callback,
            10
        )

        # Nav2のNavigateToPoseアクションクライアント
        self.nav_action_client = ActionClient(self, NavigateToPose, '/navigate_to_pose')

        # マニピュレーターの動作を送信するパブリッシャー
        self.arm_pub = self.create_publisher(JointTrajectory, '/arm_controller/joint_trajectory', 10)

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

        # マーカーとTFの初期化
        self.marker_id = 0
        self.initialize_marker()
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.prev_z = None
        self.paint_active = False
        self.move_threshold = 0.1
        self.time_window = 5.0
        self.position_history = deque()

        self.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG)
        self.get_logger().info("PaintPathNodeR1 initialized and ready to receive tasks.")

        # タイマー管理用の変数
        self.timer_paint = None

    def initialize_marker(self):
        """新しいマーカーを初期化する"""
        self.marker = Marker()
        self.marker.header.frame_id = '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

    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.005 and current_z > 0.2:
                    self.paint_active = True
                else:
                    self.paint_active = False
            else:
                # 初回はprev_zを設定するだけ
                self.paint_active = False

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

            # ペイントがアクティブならポイントを追加
            if self.paint_active:
                # ロボットの向きを取得（クォータニオンからヨー角に変換）
                q = base_transform.transform.rotation
                siny_cosp = 2.0 * (q.w * q.z + q.x * q.y)
                cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
                yaw = math.atan2(siny_cosp, cosy_cosp)

                # ロボットの前方70cmの位置を計算
                offset_distance = -0.7  # 70cm
                point_x = base_transform.transform.translation.x + offset_distance * math.cos(yaw)
                point_y = base_transform.transform.translation.y + offset_distance * math.sin(yaw)

                # マニピュレータのZ座標をmapフレームで取得
                wrist_in_map = self.tf_buffer.lookup_transform('map', 'ur10wrist_3_link', rclpy.time.Time())
                point_z = wrist_in_map.transform.translation.z

                # ペイントポイントを作成
                point = Point(
                    x=point_x,
                    y=point_y,
                    z=point_z
                )
                # ポイントを追加
                self.marker.points.append(point)
                self.marker.header.stamp = self.get_clock().now().to_msg()
                self.marker_pub.publish(self.marker)

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


    def task_command_callback(self, msg):
        task_id = json.loads(msg.data).get("task_id", "Unknown")
        self.get_logger().info(f"Received task command: {task_id}")
        self.execute_task(task_id)

    def execute_task(self, task_id):
        if not os.path.exists(self.paint_routes_path):
            self.get_logger().error(f"Paint routes file not found: {self.paint_routes_path}")
            return

        tree = ET.parse(self.paint_routes_path)
        root = tree.getroot()

        for area in root.findall('./Room_Routes/Area'):
            for wall in area.findall('./Wall'):
                wall_task_id = wall.find('Task_ID').text
                if wall_task_id == task_id:
                    x = float(wall.find('./Initial_Position/X_Coord').text)
                    y = float(wall.find('./Initial_Position/Y_Coord').text)
                    yaw = float(wall.find('./Initial_Position/Yaw').text)
                    stations = int(wall.find('./Number_of_Stations').text)
                    width_of_stroke = float(wall.find('./Width_of_Stroke').text)
                    move_direction = wall.find('./Move_Direction').text
                    self.navigate_to_stations(x, y, yaw, stations, width_of_stroke, move_direction)
                    return
        self.get_logger().warn(f"No matching task found for Task ID: {task_id}")

    def navigate_to_stations(self, x, y, yaw, stations, width_of_stroke, move_direction):
        # 初期位置を保存
        self.initial_x = x
        self.initial_y = y
        self.initial_yaw = yaw
        self.stations = stations
        self.width_of_stroke = width_of_stroke
        self.move_direction = move_direction
        self.current_station = 0

        # ステーションごとにナビゲーションとペインティングを実行
        self.navigate_and_paint_next_station()

    def navigate_and_paint_next_station(self):
        if self.current_station >= self.stations:
            self.get_logger().info("すべてのステーションでの作業が完了しました。")
            return

        # 現在のステーションの位置を計算
        offset_x = 0.0
        offset_y = 0.0

        if self.move_direction.lower() == "left":
            offset_x = -self.width_of_stroke * self.current_station * math.sin(self.initial_yaw)
            offset_y = self.width_of_stroke * self.current_station * math.cos(self.initial_yaw)
        elif self.move_direction.lower() == "right":
            offset_x = self.width_of_stroke * self.current_station * math.sin(self.initial_yaw)
            offset_y = -self.width_of_stroke * self.current_station * math.cos(self.initial_yaw)
        else:
            self.get_logger().warn(f"無効な移動方向: {self.move_direction}")
            return

        target_x = self.initial_x + offset_x
        target_y = self.initial_y + offset_y
        target_yaw = self.initial_yaw

        self.get_logger().info(f"ステーション {self.current_station + 1}/{self.stations} へ移動します。")
        self.navigate_to_pose(target_x, target_y, target_yaw)

    def navigate_to_pose(self, x, y, yaw):
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose.header.frame_id = 'map'
        goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
        goal_msg.pose.pose.position.x = x
        goal_msg.pose.pose.position.y = y
        goal_msg.pose.pose.orientation.z = math.sin(yaw / 2.0)
        goal_msg.pose.pose.orientation.w = math.cos(yaw / 2.0)

        self.nav_action_client.wait_for_server(timeout_sec=10.0)
        send_goal_future = self.nav_action_client.send_goal_async(goal_msg)
        send_goal_future.add_done_callback(
            lambda future: self.goal_response_callback(future)
        )

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().warn("Navigation goal was rejected")
            return

        self.get_logger().info("Navigation goal accepted")
        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(
            lambda future: self.navigation_result_callback(future)
        )

    def navigation_result_callback(self, future):
        result = future.result()
        if result.status == 4:  # SUCCEEDED
            self.get_logger().info("Navigation succeeded. Now starting painting.")
            self.simulate_painting()
        else:
            self.get_logger().warn(f"Navigation failed with status: {result.status}")

    def simulate_painting(self):
        self.painting_step = 0  # 0: 上昇, 1: 下降
        self.get_logger().info(f"ステーション {self.current_station + 1}/{self.stations} のペインティングを開始します。")
        self.perform_painting_step()

    def perform_painting_step(self):
        # 既存のタイマーがあればキャンセル
        if hasattr(self, 'timer_paint') and self.timer_paint is not None:
            self.timer_paint.cancel()
            self.timer_paint = None

        if self.painting_step == 0:
            self.get_logger().info("アームを上昇させます。")
            self.publish_arm_movement([0.0, 1.5, 0.5, 0.0, 0.0, 0.0], duration=5.0)
            self.painting_step = 1
            # 次のステップを5秒後に実行
            self.timer_paint = self.create_timer(5.0, self.perform_painting_step)
        elif self.painting_step == 1:
            self.get_logger().info("アームを下降させます。")
            self.publish_arm_movement([0.0, -0.25, 0.5, 0.0, 0.0, 0.0], duration=5.0)
            self.painting_step = 2
            # 次のステップを5秒後に実行
            self.timer_paint = self.create_timer(5.0, self.perform_painting_step)
        elif self.painting_step == 2:
            self.get_logger().info("ペインティングが完了しました。次のステーションに移動します。")
            self.painting_step = 0
            self.current_station += 1
            # 次のステーションへ移動
            self.navigate_and_paint_next_station()

    def publish_arm_movement(self, positions, duration=5.0):
        msg = JointTrajectory()
        msg.joint_names = [
            "ur10shoulder_pan_joint", "ur10shoulder_lift_joint",
            "ur10elbow_joint", "ur10wrist_1_joint",
            "ur10wrist_2_joint", "ur10wrist_3_joint"
        ]
        point = JointTrajectoryPoint()
        point.positions = positions
        point.time_from_start = rclpy.duration.Duration(seconds=duration).to_msg()
        msg.points.append(point)

        # 現在時刻をヘッダーに追加
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = "base_link"  # フレームを明示

        # ログ出力
        self.get_logger().info(f"Publishing arm movement: {msg}")
        self.arm_pub.publish(msg)


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


if __name__ == '__main__':
    main()

3.4 setup.pyの設定

上記のプログラムやutilityに保存したxmlファイルがビルドされるように、ros2_mmo500\src\paint_pathの中にあるsetup.pyを以下のように修正します

setup.py

from setuptools import find_packages, setup
from glob import glob
import os

package_name = 'paint_path'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        # utilityディレクトリ内のすべてのXMLファイルをインストール
        (os.path.join('share', package_name, 'utility'), glob('paint_path/utility/*.xml')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='test',
    maintainer_email='test@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'paint_path_node = paint_path.paint_path_node:main',
            'paint_path_node_R1 = paint_path.paint_path_node_R1:main',
            'human_task_notifier = paint_path.human_task_notifier:main',
            'task_trigger_node = paint_path.task_trigger_node:main',
        ],
    },
)

3.5 rvizのlaunchファイルの設定

上記のプログラムで実行された結果がrviz2上でに表示されるように、以下のrviz_launch3.pyをros2_mmo500\src\neo_nav2_bringup\launchの中に保存します。

rviz_launch3.py

# rviz_launch3.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設定ファイルのパスを指定（ここは各環境で変更）
    rviz_config_file_path = os.path.expanduser('~/paint_path_config.rviz')
    rviz_config_file = LaunchConfiguration('rviz_config', default=rviz_config_file_path)

    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')

    declare_rviz_config_file_cmd = DeclareLaunchArgument(
        'rviz_config', default_value=rviz_config_file_path,
        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)})

    # RVizノード
    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 R1
    paint_path_node_r1 = Node(
        package='paint_path', 
        executable='paint_path_node_R1',
        name='paint_path_node_R1',
        output='screen',
        parameters=[{'use_sim_time': True}], 
    )

    # Human Task Notifier Node
    human_task_notifier_node = Node(
        package='paint_path',
        executable='human_task_notifier',
        name='human_task_notifier',
        output='screen',
    )

    # Task Trigger Node
    task_trigger_node = Node(
        package='paint_path',
        executable='task_trigger_node',
        name='task_trigger_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_r1)
    ld.add_action(human_task_notifier_node)
    ld.add_action(task_trigger_node)
    ld.add_action(exit_event_handler_namespaced)

    return ld

3.6 ビルド

これらのプログラムが準備できたところで、以下のコマンドでビルドしました。

terminal

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

4. マニピュレーター付きロボットで塗装作業をxmlファイルに従い実施するシミュレーション結果

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

terminal(1)

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

terminal(2)

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

terminal(3)

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

そして人間が行った作業が完了したことを以下のコマンドで通知します

terminal(4)

source ~/ros2_mmo500/install/setup.bash
ros2 topic pub /human_task_status std_msgs/String '{"data": "{\"task_id\": \"1\", \"status\": \"completed\"}"}'

そうしますとロボットが指定された位置へ移動し塗装を開始します。

そしてその作業が完了したら以下のコマンドで、人間が行った次の作業が完了したことを通知します。

terminal(4)

source ~/ros2_mmo500/install/setup.bash
ros2 topic pub /human_task_status std_msgs/String '{"data": "{\"task_id\": \"4\", \"status\": \"completed\"}"}'

そうしますとロボットは再度指定された位置へ移動し塗装を開始します。

以下がロボットによるすべての塗装作業が完了した際の画像です。

そして以下がその作業の動画です。

5. まとめ

今回は簡単なものではありますが、BIM(Building Information Modeling)データの中でも形状を表すデータであるIFCファイルをROS2のシミュレーションに取り込み、そして作業工程やスケジュールを記載したcsvをxml形式に変換し、それらを活用してロボットによる塗装作業シミュレーションに成功しました。

今後取り組む課題としては、1.2節で述べた、Kimらの論文[2]の特徴の１つでもある、「⑤ レポート生成機能」の再現に取り組んでみたいとおもいます。

参考記事やサイト

[1]IFCファイルをSDFファイルに変換してシミュレーション環境（Gazebo）に読み込んで、ロボ走行用地図を作り自律走行させてみた
[2]Development of BIM-integrated construction robot task planning and simulation system
[3]マニピュレーター付きロボットで疑似的に壁を塗装してみた