ROSにて複数台の同一ロボット制御について

コミュニティの皆様、はじめまして!

choreonoid_ros + NavigationStackを使用させて頂きまして1台の2輪ロボット(Turtlebot3)の自律移動には成功しております。
参考にしたサイト:
https://rtc-fukushima.jp/technical/6295/
https://rtc-fukushima.jp/technical/6309/

1つのChoreonoid上に同じロボットをもう一台配置しまして、同様に制御したいと考えております。
このようなことは可能でしょうか。
もし可能であればサンプルを頂けますと幸いです。

以下、私のローカル環境です。
OS: Ubuntu20.04
ROS: Noetic
Choreonoid: commit e9a40566164d5b5e12b8446ff6098621082c1262
choreonoid_ros: commit 558cec152ffa84830fad47e6cc56c9790ad489dc

追伸:choreonoid_rosのIssueに投げるべきか迷いましたが、PVはこちらのほうが多いと思い、こちらに投げさせていただきました。

はい、ロボットの名前を変えれば、可能です。取り急ぎ……

追記:ここで言うロボットの名前というのは、URDFやBodyファイルに書くモデルの名前ではなく、cnoidファイルにのみ記載されるmodelというパラメータ(GUIでアイテムリストに表示される名前)です。

Yuki_Onishiさま

ありがとうございます!早速試してみましたので、修正を追いながら状況をご報告させていただきます。

choreonoid側のアイテムツリーからロボットをコピペ後、元のロボット名は(waffle_pi⇒waffle_pi_01),コピーしたロボット名(waffle_pi⇒waffle_pi_02)にしまして、ロボットの位置(rootPosition及びinitialRootPosition)をずらしました。

上記はややこしくなるので、一旦navigation stack関係は外したchoreonoidとRviz飲み立ち上げるlaunchファイル(noid_turtlebot3_pkgs/cnoid_turtlebot3_bringup/launch/waffle_world.launch)の実行画面です。
この状態でシミュレーションを開始すると以下のような警告が出てしまいました。
$ roslaunch cnoid_turtlebot3_bringup cnoid_turtlebot3_pkgs/cnoid_turtlebot3_bringup/launch/waffle_world.launch

[ WARN] [1666228234.993900240, 0.331000000]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame base_footprint at time 0.334000 according to authority unknown_publisher
[ WARN] [1666228235.714333391, 0.661000000]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame base_footprint at time 0.667000 according to authority unknown_publisher

$ rostopic list
/clock
/cmd_vel
/imu
/odom
/rosout
/rosout_agg
/tf
/tf_static
/waffle_pi/joint_states
/waffle_pi_01/AccelSensor
/waffle_pi_01/GyroSensor
/waffle_pi_01/LiDAR/scan
/waffle_pi_01/RealSense/image_raw
/waffle_pi_01/RealSense/image_raw/compressed
/waffle_pi_01/RealSense/image_raw/compressed/parameter_descriptions
/waffle_pi_01/RealSense/image_raw/compressed/parameter_updates
/waffle_pi_01/RealSense/image_raw/compressedDepth
/waffle_pi_01/RealSense/image_raw/compressedDepth/parameter_descriptions
/waffle_pi_01/RealSense/image_raw/compressedDepth/parameter_updates
/waffle_pi_01/RealSense/image_raw/theora
/waffle_pi_01/RealSense/image_raw/theora/parameter_descriptions
/waffle_pi_01/RealSense/image_raw/theora/parameter_updates
/waffle_pi_01/RealSense/point_cloud
/waffle_pi_01/joint_states
/waffle_pi_02/AccelSensor
/waffle_pi_02/GyroSensor
/waffle_pi_02/LiDAR/scan
/waffle_pi_02/RealSense/image_raw
/waffle_pi_02/RealSense/image_raw/compressed
/waffle_pi_02/RealSense/image_raw/compressed/parameter_descriptions
/waffle_pi_02/RealSense/image_raw/compressed/parameter_updates
/waffle_pi_02/RealSense/image_raw/compressedDepth
/waffle_pi_02/RealSense/image_raw/compressedDepth/parameter_descriptions
/waffle_pi_02/RealSense/image_raw/compressedDepth/parameter_updates
/waffle_pi_02/RealSense/image_raw/theora
/waffle_pi_02/RealSense/image_raw/theora/parameter_descriptions
/waffle_pi_02/RealSense/image_raw/theora/parameter_updates
/waffle_pi_02/RealSense/point_cloud
/waffle_pi_02/joint_states

TF名とtopicの一部(/cmd_vel、/imu、/odom)が被ってしまってるのが原因かと思い、SimpleController(cnoid_turtlebot3_pkgs/cnoid_turtlebot3_control/src/TurtleBot3RosTeleopController.cpp)を修正しました。

以下修正内容:

std::string odom_frame_id_;
std::string child_frame_id_;
std::string imu_frame_id_;

virtual bool initialize(cnoid::SimpleControllerIO* io) override
{
cnoid::Body* body = io->body();
std::string robot_name_ = body->name();
odom_frame_id_	= robot_name_ + "/" + ODOM_FRAME_ID; // odom
child_frame_id_	= robot_name_ + "/" + CHILD_FRAME_ID; // base_footprint
imu_frame_id_	= robot_name_ + "/" + IMU_FRAME_ID; // base_footprint
(中略)
// cmd_vel subscriber.
cmdvel_sub_ = nh_.subscribe(robot_name_+"/cmd_vel", 1, &TurtleBot3RosTeleopController::cmdvelCallback, this);
// Create the publisher of odometry(topic: odom, buffer_size: 10).
odom_pub_ = nh_.advertise<nav_msgs::Odometry>(robot_name_+"/odom", 10);
// Create the publisher of imu(topic: imu, buffer_size: 10).
imu_pub_ = nh_.advertise<sensor_msgs::Imu>(robot_name_+"/imu", 10);
(中略)
}
void updateOdom()
{
(中略)
	odomTrans.header.frame_id = odom_frame_id_;
	odomTrans.child_frame_id = child_frame_id_;
(中略)
	tfBroadcaster_.sendTransform(odomTrans);	// TF更新
(中略)
	odom.header.frame_id = odom_frame_id_;
	odom.child_frame_id = child_frame_id_;
(中略)
	odom_pub_.publish(odom);	// /odomトピックパブリッシュ
(中略)
}
void updateImu()
{
(中略)
	imu.header.frame_id = imu_frame_id_;
(中略)
}

また、
/waffle_pi/joint_states

robot_state_publisherのサブスクライバトピック名のため、waffle_world.launchから呼び出されるdisplay.launchのremapの修正が必要でした。
ただ、複数ファイルに分かれると修正がややこしくなるので、waffle_world.launchとdisplay.launchをベースにmulti_robot_world.launchを作成し、修正対応を行いました。
修正前:

 <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
    <remap from="joint_states" to="/$(env TURTLEBOT3_MODEL)/joint_states"/>
 </node>

修正後:

 <arg name="robot_name_1" default="waffle_pi_01"/>
 <node name="robot_state_publisher_robot_1" pkg="robot_state_publisher" type="robot_state_publisher">
    <param name="publish_frequency" type="double" value="50.0" />
    <param name="tf_prefix" value="$(arg robot_name_1)" />
    <remap from="joint_states" to="/$(arg robot_name_1)/joint_states" />
 </node>

また、base_link⇔LiDARフレーム間のstatic_transform_publisherが無かったので合わせて追記しました。

RVizのコンフィグはGlobal OptionのFixed Frameを
base_footprint⇒waffle_pi_01/base_footprint
に、RobotModelのTF Prefixにwaffle_pi_01を追記、
あとはImageのトピックを選択し直し、waffle_pi_01用waffle_pi_02用で2つ作成しました。

最終的には以下のようなlaunchファイルにて、2台の同一ロボットを動かせました。

<launch>
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle_pi, waffle_pi_open_manipulator]"/>
  <arg name="robot_name_1" default="waffle_pi_01"/>
  <arg name="robot_name_2" default="waffle_pi_02"/>
  <arg name="urdf_model" default="$(find cnoid_turtlebot3_description)/urdf/turtlebot3_$(env TURTLEBOT3_MODEL).urdf.xacro" />
  <arg name="rviz_config" default="$(find cnoid_turtlebot3_description)/launch/config/$(env TURTLEBOT3_MODEL)/display.rviz"/>

  <!-- Choreonoid ROS -->
  <node pkg="choreonoid_ros" name="choreonoid" type="choreonoid" 
    args="$(find cnoid_turtlebot3_bringup)/project/multi_robot_world_2d.cnoid " output="screen"/>
  
  <param name="use_sim_time" value="true"/>
  <param name="use_gui" value="true"/>

  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(arg urdf_model)'" />
  
  <!-- ROBOT_01 -->
  <node pkg="tf" type="static_transform_publisher" name="lidar_broadcaster_robot_1"
      args="0 0 0 0 0 0 1 $(arg robot_name_1)/base_link $(arg robot_name_1)/LiDAR 100" />

  <node name="robot_state_publisher_robot_1" pkg="robot_state_publisher" type="robot_state_publisher">
    <param name="publish_frequency" type="double" value="50.0" />
    <param name="tf_prefix" value="$(arg robot_name_1)" />
    <remap from="joint_states" to="/$(arg robot_name_1)/joint_states" />
  </node>

  <node name="rviz_robot_1" pkg="rviz" type="rviz" args="-d $(find cnoid_turtlebot3_description)/launch/config/$(env TURTLEBOT3_MODEL)/display_$(arg robot_name_1).rviz" required="true"/>

  <!-- ROBOT_02 -->
  <node pkg="tf" type="static_transform_publisher" name="lidar_broadcaster_robot_2"
      args="0 0 0 0 0 0 1 $(arg robot_name_2)/base_link $(arg robot_name_2)/LiDAR 100" />

  <node name="robot_state_publisher_robot_2" pkg="robot_state_publisher" type="robot_state_publisher">
    <param name="publish_frequency" type="double" value="50.0" />
    <param name="tf_prefix" value="$(arg robot_name_2)" />
    <remap from="joint_states" to="/$(arg robot_name_2)/joint_states" />
  </node>

  <node name="rviz_robot_2" pkg="rviz" type="rviz" args="-d $(find cnoid_turtlebot3_description)/launch/config/$(env TURTLEBOT3_MODEL)/display_$(arg robot_name_2).rviz" required="true"/>

</launch>

動作時のRViz画面です。

現状で1点だけ解決できていない問題があります。LiDARのTFフレーム名がかぶらないように ”ロボット名/LiDAR”になるように修正しましたが、Choreonoidから出力される/scanトピックのフレームIDは”LiDAR”のままで、座標変換に失敗してしまいます。

以下/scanトピックのキャプチャデータ:
header:
seq: 5762
stamp:
secs: 1152
nsecs: 600000000
frame_id: “LiDAR”
angle_min: -3.1415927410125732
angle_max: 3.1415927410125732
angle_increment: 0.01745329238474369
time_increment: 0.0
scan_time: 0.0
range_min: 0.10000000149011612
range_max: 3.5
ranges: [0.6762774586677551, 0.6694416999816895, 0.6630979776382446, ・・・省略]
intensities:

他のセンサ(例えばRealsenseのPointCloud)も同様でした。
以下RealsenseのPointCloud2データ:
header:
seq: 11971
stamp:
secs: 199
nsecs: 534000000
frame_id: “RealSense”
height: 480
width: 640
fields:
(以下省略)

こちらに付きまして、対処方法があれば教えてください。
よろしくお願い致します。

ご報告ありがとうございます.
ご指摘の通りで,現在のchoreonoid_rosは,複数の全く同じロボット(あるいは全く同じ名前のリンクを持つ複数のロボット)が存在する場合を想定して設計されておらず,センサフレームの名前の問題が生じますね.

取り急ぎはお手元で,choreonoid_rosのBodyROSのコードを書き換えていただくのが良いかと思います.

例えばカメラの場合,404行目

vision.header.frame_id = sensor->name();

vision.header.frame_id = simulationBody->name() + "/" + sensor->name();

としていただければ,名前空間を分けられるかと思います.

深度カメラをシミュレーション上で利用されている場合は以下を,

LiDAR(2D)は以下,

LiDAR(3D)は以下

を同様に修正いただくと,お手元の問題が解決できるようにお見受けしました.

お試しください.