Multirobot abilities for TB3 restored (#11)

* Attempts for multirobot namespacing in GZ

* expanding gazebo names

* Use xacro file to plumb namespace into gz topics (#10)

* Use xacro file to plumb namespace into gz topics

Signed-off-by: Addisu Z. Taddese <[email protected]>

* Fix linter

Signed-off-by: Addisu Z. Taddese <[email protected]>

---------

Signed-off-by: Addisu Z. Taddese <[email protected]>

---------

Signed-off-by: Addisu Z. Taddese <[email protected]>
Co-authored-by: Addisu Z. Taddese <[email protected]>

Author: SteveMacenski

nav2_minimal_tb3_sim/configs/turtlebot3_waffle_bridge.yaml

@@ -1,51 +1,44 @@
 # replace clock_bridge
-- ros_topic_name: "clock"
-  gz_topic_name: "/clock"
+- topic_name: "/clock"
   ros_type_name: "rosgraph_msgs/msg/Clock"
   gz_type_name: "gz.msgs.Clock"
   direction: GZ_TO_ROS
 
 # no equivalent in TB3 - add
-- ros_topic_name: "joint_states"
-  gz_topic_name: "joint_states"
+- topic_name: "joint_states"
   ros_type_name: "sensor_msgs/msg/JointState"
   gz_type_name: "gz.msgs.Model"
   direction: GZ_TO_ROS
 
 # replace odom_bridge - check gz topic name
 # gz topic published by DiffDrive plugin
-- ros_topic_name: "odom"
-  gz_topic_name: "/odom"
+- topic_name: "odom"
   ros_type_name: "nav_msgs/msg/Odometry"
   gz_type_name: "gz.msgs.Odometry"
   direction: GZ_TO_ROS
 
 # replace odom_tf_bridge - check gz and ros topic names
 # gz topic published by DiffDrive plugin
 # prefix ros2 topic with gz
-- ros_topic_name: "tf"
-  gz_topic_name: "/tf"
+- topic_name: "tf"
   ros_type_name: "tf2_msgs/msg/TFMessage"
   gz_type_name: "gz.msgs.Pose_V"
   direction: GZ_TO_ROS
 
 # replace imu_bridge - check gz topic name
-- ros_topic_name: "imu"
-  gz_topic_name: "/imu"
+- topic_name: "imu"
   ros_type_name: "sensor_msgs/msg/Imu"
   gz_type_name: "gz.msgs.IMU"
   direction: GZ_TO_ROS
 
 # replace lidar_bridge
-- ros_topic_name: "scan"
-  gz_topic_name: "/scan"
+- topic_name: "scan"
   ros_type_name: "sensor_msgs/msg/LaserScan"
   gz_type_name: "gz.msgs.LaserScan"
   direction: GZ_TO_ROS
 
 # replace cmd_vel_bridge
-- ros_topic_name: "cmd_vel"
-  gz_topic_name: "/cmd_vel"
+- topic_name: "cmd_vel"
   ros_type_name: "geometry_msgs/msg/Twist"
   gz_type_name: "gz.msgs.Twist"
   direction: ROS_TO_GZ

nav2_minimal_tb3_sim/launch/spawn_tb3.launch.py

@@ -16,13 +16,15 @@
 import os
 from pathlib import Path
 
+
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
 from launch.actions import AppendEnvironmentVariable
 from launch.actions import DeclareLaunchArgument
-from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration
+from launch.substitutions.command import Command
+from launch.substitutions.find_executable import FindExecutable
 
 from launch_ros.actions import Node
 
@@ -31,7 +33,6 @@ def generate_launch_description():
     bringup_dir = get_package_share_directory('nav2_minimal_tb3_sim')
 
     namespace = LaunchConfiguration('namespace')
-    use_simulator = LaunchConfiguration('use_simulator')
     robot_name = LaunchConfiguration('robot_name')
     robot_sdf = LaunchConfiguration('robot_sdf')
     pose = {'x': LaunchConfiguration('x_pose', default='-2.00'),
@@ -47,43 +48,42 @@ def generate_launch_description():
         default_value='',
         description='Top-level namespace')
 
-    declare_use_simulator_cmd = DeclareLaunchArgument(
-        'use_simulator',
-        default_value='True',
-        description='Whether to start the simulator')
-
     declare_robot_name_cmd = DeclareLaunchArgument(
         'robot_name',
         default_value='turtlebot3_waffle',
         description='name of the robot')
 
     declare_robot_sdf_cmd = DeclareLaunchArgument(
         'robot_sdf',
-        default_value=os.path.join(bringup_dir, 'urdf', 'gz_waffle.sdf'),
+        default_value=os.path.join(bringup_dir, 'urdf', 'gz_waffle.sdf.xacro'),
         description='Full path to robot sdf file to spawn the robot in gazebo')
 
     bridge = Node(
         package='ros_gz_bridge',
         executable='parameter_bridge',
+        namespace=namespace,
         parameters=[
             {
                 'config_file': os.path.join(
                     bringup_dir, 'configs', 'turtlebot3_waffle_bridge.yaml'
                 ),
+                'expand_gz_topic_names': True,
+                'use_sim_time': True,
             }
         ],
         output='screen',
     )
 
     spawn_model = Node(
-        condition=IfCondition(use_simulator),
         package='ros_gz_sim',
         executable='create',
         output='screen',
+        namespace=namespace,
         arguments=[
-            '-entity', robot_name,
-            '-file', robot_sdf,
-            '-robot_namespace', namespace,
+            '-name', robot_name,
+            '-string', Command([
+                FindExecutable(name='xacro'), ' ', 'namespace:=',
+                LaunchConfiguration('namespace'), ' ', robot_sdf]),
             '-x', pose['x'], '-y', pose['y'], '-z', pose['z'],
             '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']]
     )
@@ -99,7 +99,6 @@ def generate_launch_description():
     ld.add_action(declare_namespace_cmd)
     ld.add_action(declare_robot_name_cmd)
     ld.add_action(declare_robot_sdf_cmd)
-    ld.add_action(declare_use_simulator_cmd)
 
     ld.add_action(set_env_vars_resources)
     ld.add_action(set_env_vars_resources2)

nav2_minimal_tb3_sim/urdf/gz_waffle.sdf renamed to nav2_minimal_tb3_sim/urdf/gz_waffle.sdf.xacro

@@ -1,5 +1,7 @@
 <?xml version="1.0"?>
-<sdf version="1.6">
+<sdf version="1.6" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:arg name="namespace" default=""/>
+
     <model name="turtlebot3_waffle"> 
       <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
 
@@ -47,7 +49,7 @@
         <sensor name="tb3_imu" type="imu">
           <always_on>true</always_on>
           <update_rate>200</update_rate>
-          <topic>imu</topic>
+          <topic>$(arg namespace)/imu</topic>
           <gz_frame_id>imu_link</gz_frame_id>
           <imu>
             <angular_velocity>
@@ -136,7 +138,7 @@
           <visualize>true</visualize>
           <pose>-0.064 0 0.15 0 0 0</pose>
           <update_rate>5</update_rate>
-          <topic>scan</topic>
+          <topic>$(arg namespace)/scan</topic>
           <gz_frame_id>base_scan</gz_frame_id>
           <ray>
             <scan>
@@ -469,9 +471,9 @@
         <min_linear_velocity>-0.46</min_linear_velocity>
         <max_angular_velocity>1.9</max_angular_velocity>
         <min_angular_velocity>-1.9</min_angular_velocity>
-        <topic>/cmd_vel</topic>
-        <odom_topic>/odom</odom_topic>
-        <tf_topic>tf</tf_topic>
+        <topic>$(arg namespace)/cmd_vel</topic>
+        <odom_topic>$(arg namespace)/odom</odom_topic>
+        <tf_topic>$(arg namespace)/tf</tf_topic>
         <frame_id>odom</frame_id>
         <child_frame_id>base_footprint</child_frame_id>
         <odom_publish_frequency>30</odom_publish_frequency>
@@ -482,7 +484,7 @@
         name="gz::sim::systems::JointStatePublisher">
         <joint_name>wheel_left_joint</joint_name>
         <joint_name>wheel_right_joint</joint_name>
-        <topic>joint_states</topic>
+        <topic>$(arg namespace)/joint_states</topic>
         <update_rate>30</update_rate>
       </plugin>
 

nav2_minimal_tb3_sim/worlds/tb3_sandbox.sdf.xacro

@@ -72,10 +72,9 @@
       <shadows>0</shadows>
     </scene>
 
-    <physics name='1ms' type='ode'>
-        <max_step_size>0.001</max_step_size>
+    <physics name='3ms' type='ode'>
+        <max_step_size>0.003</max_step_size>
         <real_time_factor>1</real_time_factor>
-        <real_time_update_rate>1000.0</real_time_update_rate>
     </physics>
 
     <model name="turtlebot3_world">