WSL2環境下で構築したOpen-RMFで複数ロボットとエレベータの連携をシミュレーションできる環境で遊んでみる　軌跡プロット編

Last updated at 2024-03-25Posted at 2024-03-23

1. はじめに

WSL2の環境下において、Traffic-Editorを使って複数ロボットとエレベータの連携をシミュレーションできる環境を構築してみたの記事では、WSL2の環境下において、Open-RMFとtraffic-editorを使ってエレベータ付きの2階建ての建物環境を構築して複数台のロボットを動かすことができました。
ただ、それだけだとロボットが単に動いているだけで、なんの情報も取得できず面白くありません。
そこで今回はロボットの現在地を時系列データとして取得して、3次元プロットを行うことでロボットの軌跡を可視化することに取り組みたいと思います。

2. 実行環境

CPU: CORE i7 7th Gen
メモリ: 32GB
GPU: GeForce RTX 2070
OSと構築した環境: Window11でWSL2にUbuntu22.04をインストールした環境
ROS2: Humble
Open-RMFの構築環境：WSL2の環境下において、Traffic-Editorを使って複数ロボットとエレベータの連携をシミュレーションできる環境を構築してみたの記事で構築した環境

3. 手順

この章で紹介する手順は、ある程度ROSやROS2の知識がないと何やってるか分からないかもしれません。
ただ、ROSやROS2とはなんぞやということについては数多の記事がありますので、本記事では割愛します。ここでは私がどんな手順でこのことに取り組んだかを記載していきたいと思います。

3.1 シミュレーション環境の起動

まずターミナルを開き、WSL2の環境下において、Traffic-Editorを使って複数ロボットとエレベータの連携をシミュレーションできる環境を構築してみたの記事で構築した環境を以下のコマンドで起動しました。

terminal(1)

cd ~/rmf_ws
source ~/rmf_ws/install/setup.bash
ros2 launch rmf_demos_gz_classic evtest.launch.xml

3.2 ノードの確認

どんなノードがあるかを把握することが、情報取得するための第１歩なので、👆の環境を起動した状態のままでターミナルを開き以下のコマンドを実行しました。

terminal(2)

cd ~/rmf_ws
source ~/rmf_ws/install/setup.bash
ros2 node list

そしてそのコマンドを実行した結果が以下です

terminal(2)

/CabinDoor_lift_1_evdoor1_node
/L2_indoor1_node
/L2_indoor2_node
/ShaftDoor_lift_1_L1_evdoor1_node
/ShaftDoor_lift_1_L2_evdoor1_node
/building_map_server
/building_systems_visualizer
/coredoor1_node
/coredoor2_node
/door_panel_requester_node
/door_supervisor
/fleet_states_visualizer
/floorplan_visualizer
/gazebo
/indoor1_node
/indoor2_node
/indoor3_node
/indoor4_node
/lift_1_node
/lift_panel_session_node
/navgraph_visualizer
/null_node
/outdoor2_node
/rmf_dispatcher_node
/rmf_lift_supervisor
/rmf_obstacle_visualizer
/rmf_schedule_panel
/rmf_traffic_blockade_node
/rmf_traffic_schedule_primary
/rviz
/schedule_data_node
/schedule_visualizer_node
/simple_api_server
/tinyRobot_1_node
/tinyRobot_2_node
/tinyRobot_command_handle
/tinyRobot_fleet_adapter
/tinyRobot_fleet_manager
/toggle_floors

これを見まして、「お、/tinyRobot_1_nodeとか/tinyRobot_2_nodeなら現在地の情報を持ってそうだな。。。」と思いました。

3.3 ノード/tinyRobot_1_nodeの中身の確認

次に以下のコマンドを実行しました。

terminal(2)

ros2 node info /tinyRobot_1_node

そしてそのコマンドを実行した結果が以下です

terminal(2)

/tinyRobot_1_node
  Subscribers:
    /clock: rosgraph_msgs/msg/Clock
    /map: rmf_building_map_msgs/msg/BuildingMap
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /robot_mode_requests: rmf_fleet_msgs/msg/ModeRequest
    /robot_path_requests: rmf_fleet_msgs/msg/PathRequest
    /robot_pause_requests: rmf_fleet_msgs/msg/PauseRequest
  Publishers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /robot_state: rmf_fleet_msgs/msg/RobotState
    /rosout: rcl_interfaces/msg/Log
    /tf: tf2_msgs/msg/TFMessage
  Service Servers:
    /tinyRobot_1_node/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /tinyRobot_1_node/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /tinyRobot_1_node/get_parameters: rcl_interfaces/srv/GetParameters
    /tinyRobot_1_node/list_parameters: rcl_interfaces/srv/ListParameters
    /tinyRobot_1_node/set_parameters: rcl_interfaces/srv/SetParameters
    /tinyRobot_1_node/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
  Service Clients:

  Action Servers:

  Action Clients:

そしてこの結果を見て「お、/robot_stateというトピックから現在地の情報が発信されてってそうだな。。。」と思いました。

3.4 トピック/robot_stateの中身の確認

次に以下のコマンドを実行しました。

terminal(2)

ros2 topic echo /robot_state

そしてそのコマンドを実行した結果が以下です

terminal(2)

name: tinyRobot_1
model: ''
task_id: ''
seq: 86
mode:
  mode: 1
  mode_request_id: 0
battery_percent: 100.0
location:
  t:
    sec: 43
    nanosec: 82000000
  x: 4.843132019042969
  y: -14.3148775100708
  yaw: -0.00061315658967942
  obey_approach_speed_limit: false
  approach_speed_limit: 0.0
  level_name: L1
  index: 0
path: []
---
name: tinyRobot_2
model: ''
task_id: ''
seq: 86
mode:
  mode: 1
  mode_request_id: 0
battery_percent: 100.0
location:
  t:
    sec: 43
    nanosec: 82000000
  x: 26.166173934936523
  y: -2.2016260623931885
  yaw: -0.00061315658967942
  obey_approach_speed_limit: false
  approach_speed_limit: 0.0
  level_name: L1
  index: 0
path: []

そしてこの結果を見て「お、これだロボット名と現在地の情報がばっちり入っている」と考えて、シミュレーション環境を一旦終了させ、ここからロボットの現在地を抽出して保存していくプログラムを作成していきました。

3.5 ロボットの現在地を抽出するプログラム

/robot_stateのトピックから発信されている情報を以下のpythoのプログラムでcsvに保存しました。抽出するデータは時刻、ロボットの名前、X座標、Y座標、階数です。ファイル名はプログラムを実行したときの日時を含める形にしています。

get_tinyRobot_position.py

import rclpy
from rclpy.node import Node
from rmf_fleet_msgs.msg import RobotState
import csv
import time
from datetime import datetime

class RobotStateLogger(Node):
    def __init__(self):
        super().__init__('robot_state_logger')
        # ファイル名の指定
        timestamp = time.strftime('%Y-%m-%d_%H-%M-%S')
        self.csv_file_path = f'robot_positions_{timestamp}.csv'
        self.subscription = self.create_subscription(
            RobotState,
            '/robot_state',
            self.listener_callback,
            10)

        # CSVファイルのヘッダー　時刻、ロボット名、X座標、Y座標、階数
        with open(self.csv_file_path, mode='w', newline='') as file:
            writer = csv.writer(file)
            writer.writerow(['Time', 'Robot Name', 'X Coordinate', 'Y Coordinate', 'Level Name'])

    def listener_callback(self, msg):
        now = self.get_clock().now().to_msg()
        formatted_time = f'{now.sec}.{now.nanosec // 1000000}'
        with open(self.csv_file_path, mode='a', newline='') as file:
            writer = csv.writer(file)
            writer.writerow([
                formatted_time,
                msg.name,
                msg.location.x,
                msg.location.y,
                msg.location.level_name
            ])

def main(args=None):
    rclpy.init(args=args)
    robot_state_logger = RobotStateLogger()
    rclpy.spin(robot_state_logger)
    robot_state_logger.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

3.6 データの取得

以下のコマンドでgazeboとrvizを起動します（シミュレーション環境の起動）。

terminal(1)

cd ~/rmf_ws
source ~/rmf_ws/install/setup.bash
ros2 launch rmf_demos_gz_classic evtest.launch.xml

次に別ターミナルを開き、先ほど作成したget_tinyRobot_position.pyを保存してあるディレクトリに移動して以下のコマンドで実行します。

terminal(2)

python get_tinyRobot_position.py

そしてまた別ターミナルを開き、以下のようにして2つのタスクを投げました。

terminal(3)

cd ~/rmf_ws
source ~/rmf_ws/install/setup.bash
ros2 run rmf_demos_tasks dispatch_patrol -p tinyRobot_1_charger point_0_L2 -n 3 --use_sim_time
ros2 run rmf_demos_tasks dispatch_patrol -p tinyRobot_2_charger point_1_L2 -n 3 --use_sim_time

これでシミュレーションをし、現在地を時系列データとしてcsvに保存していきました。

3.7 取得したデータからの3次元プロット

csvで保存したデータのうち「階数」をZ方向の高さの値として使用したいため、"L1"とか"L2"とかになっているのを数字だけに加工して、再度保存します。そして[1]の記事を参考に作成した以下のプログラムでそれぞれのロボットの軌跡をプロットして画像に保存します。

3d_plot_position_tinyRobot.py

from matplotlib import pyplot as plt
import pandas as pd
from matplotlib.ticker import MultipleLocator

df = pd.read_csv(r"ファイル名.csv", encoding="utf-8")

colorlist = ["#FFA500"] #色の指定　オレンジ色の場合を表示

fig = plt.figure()
ax =fig.add_subplot(111, projection = "3d")
ax.scatter(df["X Coordinate"], df["Y Coordinate"],df["Level"], color=colorlist)
ax.set_xlabel('X', fontdict={'fontsize': 10, 'fontweight': 'medium'})
ax.set_ylabel('y', fontdict={'fontsize': 10, 'fontweight': 'medium'})
ax.set_zlabel('z', fontdict={'fontsize': 10, 'fontweight': 'medium'})
ax.zaxis.set_major_locator(MultipleLocator(0.2))
ax.set_zticks([1, 2])
ax.xaxis.set_major_locator(MultipleLocator(5))
ax.set_xticks([-5, 0, 5, 10, 15, 20, 25, 30, 35])
ax.yaxis.set_major_locator(MultipleLocator(2))
ax.set_yticks([0, -2, -4, -6, -8, -10, -12])

plt.savefig("保存する画像ファイル名.png")
plt.show()

そしてそれぞれのロボットの軌跡をプロットした結果が以下です。

tinyRobot_1の軌跡
tinyRobot_2の軌跡

次にそれぞれのロボットの軌跡を以下のプログラムで重ねてみます。

3d_plot_position_tinyRobot.py

from matplotlib import pyplot as plt
import pandas as pd
from matplotlib.ticker import MultipleLocator

df = pd.read_csv(r"ファイル名.csv", encoding="utf-8")
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")

# ロボットごとに異なる色でプロット
colors = {"tinyRobot_1": "#FFA500", "tinyRobot_2": "#0000FF"}  # オレンジ色と青色を割り当て
for robot_name, color in colors.items():
    # データフレームから特定のロボットのデータをフィルタリング
    df_robot = df[df["Robot Name"] == robot_name]
    ax.scatter(df_robot["X Coordinate"], df_robot["Y Coordinate"], df_robot["Level"], color=color, label=robot_name)

ax.set_xlabel('X', fontdict={'fontsize': 10, 'fontweight': 'medium'})
ax.set_ylabel('Y', fontdict={'fontsize': 10, 'fontweight': 'medium'})
ax.set_zlabel('Z', fontdict={'fontsize': 10, 'fontweight': 'medium'})
ax.zaxis.set_major_locator(MultipleLocator(0.2))
ax.set_zticks([1, 2])
ax.xaxis.set_major_locator(MultipleLocator(5))
ax.set_xticks([-5, 0, 5, 10, 15, 20, 25, 30, 35])
ax.yaxis.set_major_locator(MultipleLocator(2))
ax.set_yticks([0, -2, -4, -6, -8, -10, -12])
ax.legend()

plt.savefig("ファイル名.png")
plt.show()

そしてそれぞれのロボットの軌跡を重ねてプロットした結果が以下です。

このプロットから設定した経路に沿ってロボットが動いていることが読み取れますが、経路に逃げ道があまりなくて交通整理が難しそうと感じます。

4. まとめ

上記の手順にて、WSL2の環境下において、Open-RMFとtraffic-editorを使ってエレベータ付きの2階建ての建物環境を構築して複数台のロボットを動かすしている環境で、ロボットの現在地を時系列データとして取得して、その結果を3次元プロットしてロボットの軌跡を可視化することができました。
今後は、もうちょっと複雑なタスクの指示とその分析方法のノウハウの蓄積、ブラウザからの指示、配送シミュレーション、他のソフトウェアとの連携などを試していきたいと考えています。

参考記事やサイト

[1]Pythonで3次元プロットをして散布図を作ろう

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

WSL2環境下で構築したOpen-RMFで複数ロボットとエレベータの連携をシミュレーションできる環境で遊んでみる 軌跡プロット編

1. はじめに

2. 実行環境

3. 手順

3.1 シミュレーション環境の起動

3.2 ノードの確認

3.3 ノード/tinyRobot_1_nodeの中身の確認

3.4 トピック/robot_stateの中身の確認

3.5 ロボットの現在地を抽出するプログラム

3.6 データの取得

3.7 取得したデータからの3次元プロット

4. まとめ

参考記事やサイト

WSL2環境下で構築したOpen-RMFで複数ロボットとエレベータの連携をシミュレーションできる環境で遊んでみる　軌跡プロット編