Spawn turtlebot3 separately (ros-navigation#2473)

* add nav2 gazebo spawner

* fix origin pose

* add robot_description

* remove urdf arg

* fix whitespace

* spawn from file ;add initial pose

* remove spawn launch include

* param and rviz edit

* conflict resolve

* pose

* remove world tags and add tf remap

* use nav2 waffle model and spawn entity

* change nav2_spawner to  spawn_entity

* add tf remap

* delete nav2_gazebo_spawner

* sdf param

* added in readme

* changes

nav2_bringup/bringup/README.md

@@ -134,3 +134,9 @@ In RVIZ:
 * Make sure all transforms from odom are present. (odom->base_link->base_scan)
 * Localize the robot using “2D Pose Estimate” button.
 * Send the robot a goal pose using “2D Nav Goal” button.
+
+Note:
+* nav2_gazebo_spawner pkg inside nav2_bringup directory is deleted.
+* use of nav2_gazebo_spawner to spawn the robot in gazebo is not recommended any more. Instead use spawn_entity.py of gazebo_ros to spawn the robot.
+* gazebo should be started with both libgazebo_ros_init.so and libgazebo_ros_factory.so to work correctly.
+* spawn_entity node could not remap /tf and /tf_static to tf and tf_static in the launch file yet, used only for multi-robot situations. Instead it should be done as remapping argument <remapping>/tf:=tf</remapping>  <remapping>/tf_static:=tf_static</remapping> under ros2 tag in each plugin which publishs transforms in the SDF file. It is essential to differentiate the tf's of the different robot.

nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py

@@ -13,7 +13,7 @@
 # limitations under the License.
 
 """
-Example for spawing multiple robots in Gazebo.
+Example for spawning multiple robots in Gazebo.
 
 This is an example on how to create a launch file for spawning multiple robots into Gazebo
 and launch multiple instances of the navigation stack, each controlling one robot.
@@ -39,8 +39,10 @@ def generate_launch_description():
 
     # Names and poses of the robots
     robots = [
-        {'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01},
-        {'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01}]
+        {'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01,
+                           'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0},
+        {'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01,
+                           'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}]
 
     # Simulation settings
     world = LaunchConfiguration('world')
@@ -100,27 +102,12 @@ def generate_launch_description():
         default_value='True',
         description='Whether to start RVIZ')
 
-    # Start Gazebo with plugin providing the robot spawing service
+    # Start Gazebo with plugin providing the robot spawning service
     start_gazebo_cmd = ExecuteProcess(
         cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so',
                                      '-s', 'libgazebo_ros_factory.so', world],
         output='screen')
 
-    # Define commands for spawing the robots into Gazebo
-    spawn_robots_cmds = []
-    for robot in robots:
-        spawn_robots_cmds.append(
-            IncludeLaunchDescription(
-                PythonLaunchDescriptionSource(os.path.join(bringup_dir, 'launch',
-                                                           'spawn_tb3_launch.py')),
-                launch_arguments={
-                                  'x_pose': TextSubstitution(text=str(robot['x_pose'])),
-                                  'y_pose': TextSubstitution(text=str(robot['y_pose'])),
-                                  'z_pose': TextSubstitution(text=str(robot['z_pose'])),
-                                  'robot_name': robot['name'],
-                                  'turtlebot_type': TextSubstitution(text='waffle')
-                                  }.items()))
-
     # Define commands for launching the navigation instances
     nav_instances_cmds = []
     for robot in robots:
@@ -149,7 +136,14 @@ def generate_launch_description():
                                   'use_rviz': 'False',
                                   'use_simulator': 'False',
                                   'headless': 'False',
-                                  'use_robot_state_pub': use_robot_state_pub}.items()),
+                                  'use_robot_state_pub': use_robot_state_pub,
+                                  'x_pose': TextSubstitution(text=str(robot['x_pose'])),
+                                  'y_pose': TextSubstitution(text=str(robot['y_pose'])),
+                                  'z_pose': TextSubstitution(text=str(robot['z_pose'])),
+                                  'roll': TextSubstitution(text=str(robot['roll'])),
+                                  'pitch': TextSubstitution(text=str(robot['pitch'])),
+                                  'yaw': TextSubstitution(text=str(robot['yaw'])),
+                                  'robot_name':TextSubstitution(text=robot['name']), }.items()),
 
             LogInfo(
                 condition=IfCondition(log_settings),
@@ -190,9 +184,6 @@ def generate_launch_description():
     # Add the actions to start gazebo, robots and simulations
     ld.add_action(start_gazebo_cmd)
 
-    for spawn_robot_cmd in spawn_robots_cmds:
-        ld.add_action(spawn_robot_cmd)
-
     for simulation_instance_cmd in nav_instances_cmds:
         ld.add_action(simulation_instance_cmd)
 

nav2_bringup/bringup/launch/tb3_simulation_launch.py

@@ -47,6 +47,14 @@ def generate_launch_description():
     use_rviz = LaunchConfiguration('use_rviz')
     headless = LaunchConfiguration('headless')
     world = LaunchConfiguration('world')
+    pose = {'x': LaunchConfiguration('x_pose', default='-2.00'),
+            'y': LaunchConfiguration('y_pose', default='-0.50'),
+            'z': LaunchConfiguration('z_pose', default='0.01'),
+            'R': LaunchConfiguration('roll', default='0.00'),
+            'P': LaunchConfiguration('pitch', default='0.00'),
+            'Y': LaunchConfiguration('yaw', default='0.00')}
+    robot_name = LaunchConfiguration('robot_name')
+    robot_sdf = LaunchConfiguration('robot_sdf')
 
     # Map fully qualified names to relative ones so the node's namespace can be prepended.
     # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
@@ -125,9 +133,19 @@ def generate_launch_description():
         #              https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91
         # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'),
         # worlds/turtlebot3_worlds/waffle.model')
-        default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'),
+        default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'),
         description='Full path to world model file to load')
 
+    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, 'worlds', 'waffle.model'),
+        description='Full path to robot sdf file to spawn the robot in gazebo')
+
     # Specify the actions
     start_gazebo_server_cmd = ExecuteProcess(
         condition=IfCondition(use_simulator),
@@ -141,6 +159,8 @@ def generate_launch_description():
         cwd=[launch_dir], output='screen')
 
     urdf = os.path.join(bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
+    with open(urdf, 'r') as infp:
+        robot_description = infp.read()
 
     start_robot_state_publisher_cmd = Node(
         condition=IfCondition(use_robot_state_pub),
@@ -149,16 +169,27 @@ def generate_launch_description():
         name='robot_state_publisher',
         namespace=namespace,
         output='screen',
-        parameters=[{'use_sim_time': use_sim_time}],
-        remappings=remappings,
-        arguments=[urdf])
+        parameters=[{'use_sim_time': use_sim_time,
+                     'robot_description': robot_description}],
+        remappings=remappings)
+
+    start_gazebo_spawner_cmd = Node(
+        package='gazebo_ros',
+        executable='spawn_entity.py',
+        output='screen',
+        arguments=[
+            '-entity', robot_name,
+            '-file', robot_sdf,
+            '-robot_namespace', namespace,
+            '-x', pose['x'], '-y', pose['y'], '-z', pose['z'],
+            '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']])
 
     rviz_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(launch_dir, 'rviz_launch.py')),
         condition=IfCondition(use_rviz),
-        launch_arguments={'namespace': '',
-                          'use_namespace': 'False',
+        launch_arguments={'namespace': namespace,
+                          'use_namespace': use_namespace,
                           'rviz_config': rviz_config_file}.items())
 
     bringup_cmd = IncludeLaunchDescription(
@@ -190,10 +221,13 @@ def generate_launch_description():
     ld.add_action(declare_use_rviz_cmd)
     ld.add_action(declare_simulator_cmd)
     ld.add_action(declare_world_cmd)
+    ld.add_action(declare_robot_name_cmd)
+    ld.add_action(declare_robot_sdf_cmd)
 
     # Add any conditioned actions
     ld.add_action(start_gazebo_server_cmd)
     ld.add_action(start_gazebo_client_cmd)
+    ld.add_action(start_gazebo_spawner_cmd)
 
     # Add the actions to launch all of the navigation nodes
     ld.add_action(start_robot_state_publisher_cmd)

nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml

@@ -275,3 +275,13 @@ recoveries_server:
 robot_state_publisher:
   ros__parameters:
     use_sim_time: True
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"   
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200

nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml

@@ -275,3 +275,13 @@ recoveries_server:
 robot_state_publisher:
   ros__parameters:
     use_sim_time: True
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"   
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200

nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz

@@ -304,6 +304,18 @@ Visualization Manager:
           Value: true
       Enabled: true
       Name: Controller
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: <robot_namespace>/waypoints
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

nav2_bringup/bringup/worlds/waffle.model

@@ -1,57 +1,7 @@
 <?xml version="1.0"?>
 <sdf version="1.6">
-  <world name="default">
-
-    <include>
-      <uri>model://ground_plane</uri>
-    </include>
-
-    <include>
-      <uri>model://sun</uri>
-    </include>
-
-    <scene>
-      <shadows>false</shadows>
-    </scene>
-
-    <gui fullscreen='0'>
-      <camera name='user_camera'>
-        <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
-        <view_controller>orbit</view_controller>
-        <projection_type>perspective</projection_type>
-      </camera>
-    </gui>
-
-    <physics type="ode">
-      <real_time_update_rate>1000.0</real_time_update_rate>
-      <max_step_size>0.001</max_step_size>
-      <real_time_factor>1</real_time_factor>
-      <ode>
-        <solver>
-          <type>quick</type>
-          <iters>150</iters>
-          <precon_iters>0</precon_iters>
-          <sor>1.400000</sor>
-          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
-        </solver>
-        <constraints>
-          <cfm>0.00001</cfm>
-          <erp>0.2</erp>
-          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
-          <contact_surface_layer>0.01000</contact_surface_layer>
-        </constraints>
-      </ode>
-    </physics>
-
-    <model name="turtlebot3_world">
-      <static>1</static>
-      <include>
-        <uri>model://turtlebot3_world</uri>
-      </include>
-    </model>
-
     <model name="turtlebot3_waffle"> 
-      <pose>-2.0 -0.5 0.01 0.0 0.0 0.0</pose>
+      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
 
       <link name="base_footprint"/>
 
@@ -517,6 +467,9 @@
 
         <ros>
           <!-- <namespace>/tb3</namespace> -->
+          <!--since gazebo_plugins publish tf topic in global namespace /tf only and it
+            cannot be remapped like (namespace/tf) through launch arguments -->
+          <remapping>/tf:=tf</remapping>
         </ros>
 
         <update_rate>30</update_rate>
@@ -557,6 +510,4 @@
       </plugin>
 
     </model>
-
-  </world>
 </sdf>