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>