マニピュレータ付きロボットによる塗装作業の進捗表示と指示出しを兼ねた帳票システムをROS2-Web-Bridge使って作ってみた

Posted at 2024-12-22

1. はじめに

前回の記事[1]では簡単なものではありますが、BIM(Building Information Modeling)データの中でも形状を表すデータであるIFCファイルをROS2のシミュレーションに取り込み、そして作業工程やスケジュールを記載したcsvをxml形式に変換し、それらを活用してロボットによる塗装作業シミュレーションに成功しました。そしてその記事において、「Kimらの論文[2]の特徴の１つでもある、「⑤ レポート生成機能」の再現に取り組んでみたい」と述べましたが、それに取り組んだのがこの記事の内容になります。

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

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

Kimらの論文[2]の特徴は以下だと、私は考えております。（[1]の記事でも同じことを書いてますが、ここでも再掲します）

① 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の統合フレームワークの開発

これについては前回の記事[1]で取り組みました。

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

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

本件の課題としては、マニピュレータの軌跡に沿って塗装するので、塗装面が湾曲してしまうという課題がありました。この点についての取り組みは前回の記事[1]の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. ROS2-Web-Bridgeのセットアップ

まず以下のコマンドで、依存パッケージのインストールをします。

terminal

sudo apt update
sudo apt install -y nodejs npm

ただ、私の環境では他のパッケージとの競合があり上記のコマンドだけではエラーが出まして、libnode-devパッケージを削除したあとNode.js 16をインストールしました。

次に以下のコマンドを実行して、ROS2-Web-Bridgeをインストールしました。

terminal

git clone https://github.com/RobotWebTools/ros2-web-bridge.git
cd ros2-web-bridge
source /opt/ros/humble/setup.bash
npm install

4. HTMLページの構築

homeディレクトリにおいて、~/ros2_paint_web_serverというフォルダを作成し、以下のindex.htmlを保存しました。このindex.htmlの機能としては以下です

工程管理機能
作業の進捗を管理するための機能
- 人が作業を完了した場合、緑色の「Complete」ボタンをクリックすると、ボタンは灰色になり非活性化されます。
- ロボットの作業進捗も監視し、完了時にはそのステータスが「Completed」と表示されます。
ロボット作業トリガー機能
作業の進行に応じてロボットに指示を出す機能
- 作業が「Complete」になった情報をロボットに通知します。
- ロボットは、前工程の作業完了通知を受けると、自身の作業を開始します。
ロボットの現在地表示機能
ロボットの位置情報をリアルタイムで表示するための機能
- 走行用地図を画面上に表示します。
- ロボットから取得した現在地情報を地図上に反映し、視覚的に確認できるようにします。

index.html

<!DOCTYPE html>
<html lang="en">
<head>
<meta charset="UTF-8">
<title>Construction Tasks</title>
<style>
body {
   font-family: Arial, sans-serif;
   margin: 20px;
}
.container {
   display: flex;
}
.left-panel {
   flex: 1;
   margin-right: 20px;
   border: 1px solid #ccc;
   padding: 10px;
}
.right-panel {
   flex: 1;
   display: flex;
   flex-direction: column;
   border: 1px solid #ccc;
   padding: 10px;
}
.top-right {
   flex: 1;
   border: 1px solid #ccc;
   margin-bottom: 10px;
   position: relative;
   display: flex;
   flex-direction: column;
}
.map-container {
   position: relative;
   width: 100%;
   flex: 1; /* 余白いっぱいに画像を広げる */
   background: #eee;
   overflow: hidden;
}
.map-container img {
   width: 100%; /* コンテナに合わせて拡大 */
   height: auto;
   display: block;
}
#overlay-canvas {
   position: absolute;
   top:0;
   left:0;
   pointer-events: none; /* マウス操作を透過 */
}
.bottom-right {
   flex: 1;
   border: 1px solid #ccc;
   overflow-y: auto;
   padding: 10px;
}
table {
   border-collapse: collapse;
   width: 100%;
   margin-bottom: 20px;
}
th, td {
   border: 1px solid #ccc;
   padding: 8px;
   text-align: left;
}
.button-complete {
   background-color: #28a745;
   color: #fff;
   border: none;
   padding: 5px 8px;
   cursor: pointer;
   border-radius: 3px;
}
.button-complete:disabled {
   background-color: #aaa;
   cursor: not-allowed;
}
</style>
<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script>
<script>
let ros;
let robotPositions = []; // ロボット位置履歴
let canvas, ctx;

window.onload = () => {
   ros = new ROSLIB.Ros({ url: 'ws://localhost:9090' });
   ros.on('connection', function() {
       console.log('Connected to ros2-web-bridge');
       loadCSVData('schedule.csv', displayScheduleTasks);
       loadCSVData('paint_routes.csv', displayPaintRoutes);
       subscribeRobotTaskStatus();
       subscribeRobotPath();
   });

   ros.on('error', function(error) {
       console.error('Error connecting to ROS: ', error);
   });

   ros.on('close', function() {
       console.log('Connection to ros2-web-bridge closed.');
   });

   // canvas初期化
   canvas = document.getElementById('overlay-canvas');
   ctx = canvas.getContext('2d');

   // ウィンドウサイズ変更時にcanvasサイズ合わせ
   window.addEventListener('resize', updateCanvasSize);
   updateCanvasSize();
};

function updateCanvasSize() {
   const mapContainer = document.querySelector('.map-container');
   if (!mapContainer) return;
   canvas.width = mapContainer.clientWidth;
   canvas.height = mapContainer.clientHeight;
   drawRobotPath();
}

function loadCSVData(csvFile, callback) {
   fetch(csvFile)
       .then(response => response.text())
       .then(data => {
           const tasks = parseCSV(data);
           callback(tasks);
       })
       .catch(err => console.error(`Failed to load ${csvFile}:`, err));
}

function parseCSV(csvText) {
   const lines = csvText.trim().split('\n');
   const headers = lines[0].split(',');
   const tasks = [];
   for (let i = 1; i < lines.length; i++) {
       const row = lines[i].split(',');
       const task = {};
       for (let j = 0; j < headers.length; j++) {
           task[headers[j].trim()] = row[j] ? row[j].trim() : '';
       }
       tasks.push(task);
   }
   return tasks;
}

function displayScheduleTasks(tasks) {
   const table = document.getElementById('task-table-body');
   table.innerHTML = '';

   tasks.forEach(task => {
       const tr = document.createElement('tr');

       const tdTaskId = document.createElement('td');
       tdTaskId.textContent = task['Task_ID'];
       tr.appendChild(tdTaskId);

       const tdType = document.createElement('td');
       tdType.textContent = task['Task_Type'];
       tr.appendChild(tdType);

       const tdDesc = document.createElement('td');
       tdDesc.textContent = task['Task_Description'];
       tr.appendChild(tdDesc);

       const tdArea = document.createElement('td');
       tdArea.textContent = task['Area'];
       tr.appendChild(tdArea);

       const tdResources = document.createElement('td');
       tdResources.textContent = task['Resources'];
       tr.appendChild(tdResources);

       const tdAction = document.createElement('td');
       if (task['Resources'] !== 'Neobotix robot painter') {
           const btn = document.createElement('button');
           btn.className = 'button-complete';
           btn.textContent = 'Complete';
           btn.onclick = () => {
               publishCompletion(task['Task_ID']);
               btn.disabled = true; // ボタン非活性化
           };
           tdAction.appendChild(btn);
       } else {
           tdAction.textContent = '-';
       }
       tr.appendChild(tdAction);

       table.appendChild(tr);
   });
}

function displayPaintRoutes(tasks) {
   const table = document.getElementById('paint-table-body');
   table.innerHTML = '';

   tasks.forEach(task => {
       const tr = document.createElement('tr');

       const tdTaskId = document.createElement('td');
       tdTaskId.textContent = task['Task_ID'];
       tr.appendChild(tdTaskId);

       const tdArea = document.createElement('td');
       tdArea.textContent = task['Area'];
       tr.appendChild(tdArea);

       const tdWallID = document.createElement('td');
       tdWallID.textContent = task['Wall_ID'];
       tr.appendChild(tdWallID);

       const tdInitialPos = document.createElement('td');
       tdInitialPos.textContent = `X:${task['Initial_Position_X']}, Y:${task['Initial_Position_Y']}, Yaw:${task['Orientation_Yaw']}`;
       tr.appendChild(tdInitialPos);

       const tdStations = document.createElement('td');
       tdStations.textContent = task['Number_of_Stations'];
       tr.appendChild(tdStations);

       const tdWidthOfStroke = document.createElement('td');
       tdWidthOfStroke.textContent = task['Width_of_Stroke'];
       tr.appendChild(tdWidthOfStroke);

       const tdMoveDirection = document.createElement('td');
       tdMoveDirection.textContent = task['Move_Direction'];
       tr.appendChild(tdMoveDirection);

       table.appendChild(tr);
   });
}

function publishCompletion(taskId) {
   const topic = new ROSLIB.Topic({
       ros: ros,
       name: '/human_task_status',
       messageType: 'std_msgs/String'
   });

   const messageData = {
       task_id: taskId,
       status: "completed"
   };

   const msg = new ROSLIB.Message({
       data: JSON.stringify(messageData)
   });

   topic.publish(msg);
   console.log(`Published completion for task_id=${taskId}`);
}

function subscribeRobotTaskStatus() {
   const robotStatusTopic = new ROSLIB.Topic({
       ros: ros,
       name: '/robot_task_status',
       messageType: 'std_msgs/String'
   });

   robotStatusTopic.subscribe((message) => {
       const data = JSON.parse(message.data);
       const tid = data.task_id;
       const status = data.status;
       console.log(`Robot task completed: ${tid}`);
       updateRobotTaskCompletion(tid);
   });
}

function updateRobotTaskCompletion(taskId) {
   // ロボットタスク完了時に対応する行をCompleted表示
   const table = document.getElementById('task-table-body');
   const rows = table.querySelectorAll('tr');
   rows.forEach(row => {
       const tds = row.querySelectorAll('td');
       if (tds.length > 0 && tds[0].textContent === taskId) {
           tds[tds.length - 1].textContent = 'Completed';  
       }
   });
}

function subscribeRobotPath() {
   const robotPathTopic = new ROSLIB.Topic({
       ros: ros,
       name: '/robot_path',
       messageType: 'std_msgs/String'
   });

   robotPathTopic.subscribe((message) => {
       const data = JSON.parse(message.data);
       robotPositions.push(data);
       drawRobotPath();
   });
}

function drawRobotPath() {
   if (!ctx) return;
   ctx.clearRect(0,0,canvas.width,canvas.height);

   // 原点を左下端にする
   // 必要に応じてスケールを変更 今回は80を選択
   const scale = 80;

   // 左下を原点とするため、originX=0, originY=canvas.heightとし、
   // pos.xが増えると右へ、pos.yが増えると上へとなるようにyを反転
   const originX = 0;
   const originY = canvas.height;

   ctx.strokeStyle = 'red';
   ctx.lineWidth = 2;

   if (robotPositions.length > 1) {
       ctx.beginPath();
       for (let i=0; i<robotPositions.length; i++) {
           const pos = robotPositions[i];
           const px = originX + pos.x * scale;
           const py = originY - pos.y * scale; 
           if (i===0) {
               ctx.moveTo(px, py);
           } else {
               ctx.lineTo(px, py);
           }
       }
       ctx.stroke();
   }
}
</script>
</head>
<body>
<h1>Construction Tasks</h1>
<div class="container">
   <div class="left-panel">
       <h2>Schedule (From schedule.csv)</h2>
       <table>
           <thead>
               <tr>
                   <th>Task ID</th>
                   <th>Type</th>
                   <th>Description</th>
                   <th>Area</th>
                   <th>Resources</th>
                   <th>Action</th>
               </tr>
           </thead>
           <tbody id="task-table-body">
               <!-- JSで挿入 -->
           </tbody>
       </table>
   </div>

   <div class="right-panel">
       <div class="top-right">
           <h3>Map & Robot Path</h3>
           <div class="map-container">
               <!-- pngに変換した走行用地図ファイルを同じディレクトに保存する -->
               <img src="ifcunit.png" alt="Map">
               <canvas id="overlay-canvas"></canvas>
           </div>
       </div>
       <div class="bottom-right">
           <h3>Paint Routes (From paint_routes.csv)</h3>
           <table>
               <thead>
                   <tr>
                       <th>Task ID</th>
                       <th>Area</th>
                       <th>Wall_ID</th>
                       <th>Initial Position</th>
                       <th>Stations</th>
                       <th>Width_of_Stroke</th>
                       <th>Move_Direction</th>
                   </tr>
               </thead>
               <tbody id="paint-table-body">
                   <!-- JSで挿入 -->
               </tbody>
           </table>
       </div>
   </div>
</div>
</body>
</html>

またindex.htmlを保存したのと同じディレクトリにロボット走行用地図をpngに変換したもの（この記事ではifcunit.png）、作業スケジュールを示したcsv(6章に示すschedule.csv)、塗装箇所を示したcsv(7章に示すpaint_routes.csv)を保存します。

5. ロボット塗装作業用コードの修正（paint_path_node_R1.py）

[1]で作成したロボット塗装作業用プログラム（paint_path_node_R1.py）を本記事でも使いますが、4章で述べた工程管理機能とロボットの現在地表示機能を実現するために以下のようにコードを修正しました。

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 geometry_msgs.msg import PoseWithCovarianceStamped  # 追加
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')

        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
        )

        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)

        self.robot_task_status_pub = self.create_publisher(String, '/robot_task_status', 10)

        # ロボットパス用パブリッシャー追加
        self.robot_path_pub = self.create_publisher(String, '/robot_path', 10)

        # amcl_poseから位置取得
        self.amcl_sub = self.create_subscription(
            PoseWithCovarianceStamped,
            '/amcl_pose',
            self.amcl_pose_callback,
            10
        )

        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
        self.current_task_id = None

    def amcl_pose_callback(self, msg):
        # amcl_poseからロボット位置取得
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y
        data = {"x": x, "y": y}
        msg_str = String()
        msg_str.data = json.dumps(data)
        self.robot_path_pub.publish(msg_str)
        self.get_logger().debug(f"Published robot path: x={x}, y={y}")

    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:
            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]
            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
                    self.initialize_marker()
                    self.paint_active = False
                    self.position_history.clear()

            current_z = wrist_transform.transform.translation.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:
                self.paint_active = False

            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)

                offset_distance = -0.7
                point_x = base_transform.transform.translation.x + offset_distance*math.cos(yaw)
                point_y = base_transform.transform.translation.y + offset_distance*math.sin(yaw)

                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.current_task_id = task_id
        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("すべてのステーションでの作業が完了しました。")
            self.publish_robot_task_completion()
            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
        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
            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
            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 publish_robot_task_completion(self):
        if self.current_task_id is not None:
            msg = String()
            data = {"task_id": str(self.current_task_id), "status": "robot_completed"}
            msg.data = json.dumps(data)
            self.robot_task_status_pub.publish(msg)
            self.get_logger().info("Published robot task completion status.")
            self.current_task_id = None

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

if __name__ == '__main__':
    main()

6. スケジュールの設定

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

各項目の内容

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

7. 塗装箇所の設定

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

各項目の内容

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度回転した図です。

8. ビルド

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

terminal

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

9. マニピュレータ付きロボットによる塗装作業の進捗表示と指示出しを兼ねた帳票システムについてのシミュレーション結果

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

terminal(1)

cd ros2-web-bridge
source /opt/ros/humble/setup.bash
node bin/rosbridge.js

terminal(2)

cd ~/ros2_paint_web_server
source /opt/ros/humble/setup.bash
python3 -m http.server 8000

ここでウェブブラウザでhttp://localhost:8000/index.html にアクセスします。そして続けて以下のコマンドを実行します。

terminal(3)

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

terminal(4)

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

terminal(5)

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

そして以下が作業を完了した際のhttp://localhost:8000/index.html の画面です

そして以下が画面の様子をrviz2の様子とともに示した動画です。

10. まとめ

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

ここで一旦Kimらの論文[2]の再現は終わりにしようと思いますが、今後は以下のことに取り組んでいけたらと考えています。

より実際の建物の形に近いIFCファイルを対象とした塗装作業シミュレーション
スケジュールなどの形状情報以外の情報を含んだBIMデータを用いた塗装作業シミュレーション
複数台のロボットで協調して塗装作業をするシミュレーション
塗装作業だけでなく窓枠取付作業などのものを把持する作業が必要なシミュレーション

参考記事やサイト

[1]IFCファイルをSDFファイルに変換して構築したシミュレーション環境（Gazebo）において、マニピュレーター付きロボットで塗装作業をしてみた
[2]Development of BIM-integrated construction robot task planning and simulation system

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up