Initial support to the new Gazebo

Signed-off-by: Alejandro Hernández Cordero <[email protected]>

nav2_bringup/launch/tb3_simulation_launch.py

@@ -19,11 +19,14 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, GroupAction
+from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable
 from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import PythonExpression
 from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description():
@@ -43,6 +46,7 @@ def generate_launch_description():
     autostart = LaunchConfiguration('autostart')
     use_composition = LaunchConfiguration('use_composition')
     use_respawn = LaunchConfiguration('use_respawn')
+    use_gz = LaunchConfiguration('use_gz')
 
     # Launch configuration variables specific to simulation
     rviz_config_file = LaunchConfiguration('rviz_config_file')
@@ -134,6 +138,11 @@ def generate_launch_description():
         default_value='True',
         description='Whether to start RVIZ')
 
+    declare_use_gz_cmd = DeclareLaunchArgument(
+        'use_gz',
+        default_value='False',
+        description='Use Gazebo (gz) or Gazebo classic')
+
     declare_simulator_cmd = DeclareLaunchArgument(
         'headless',
         default_value='True',
@@ -155,26 +164,9 @@ def generate_launch_description():
 
     declare_robot_sdf_cmd = DeclareLaunchArgument(
         'robot_sdf',
-        default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'),
+        default_value=os.path.join(bringup_dir, 'urdf', 'turtlebot3_waffle.urdf'),
         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),
-        cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
-             '-s', 'libgazebo_ros_factory.so', world],
-        cwd=[launch_dir], output='screen')
-
-    start_gazebo_client_cmd = ExecuteProcess(
-        condition=IfCondition(PythonExpression(
-            [use_simulator, ' and not ', headless])),
-        cmd=['gzclient'],
-        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),
         package='robot_state_publisher',
@@ -183,20 +175,9 @@ def generate_launch_description():
         namespace=namespace,
         output='screen',
         parameters=[{'use_sim_time': use_sim_time,
-                     'robot_description': robot_description}],
+                     'robot_description': Command(['xacro', ' ', robot_sdf])}],
         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')),
@@ -218,6 +199,96 @@ def generate_launch_description():
                           'use_composition': use_composition,
                           'use_respawn': use_respawn}.items())
 
+    env_vars = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'models') + ':' + \
+        os.path.join(get_package_share_directory(
+            'aws_robomaker_small_warehouse_world'), 'models') + ':' + \
+        os.path.join(get_package_share_directory(
+            'turtlebot3_gazebo'), '..')
+
+    load_gazeboclassic = GroupAction(
+        condition=IfCondition(PythonExpression(['not ', use_gz])),
+        actions=[
+            SetEnvironmentVariable('GAZEBO_MODEL_PATH', env_vars),
+            # Specify the actions
+            ExecuteProcess(
+                condition=IfCondition(use_simulator),
+                cmd=['gzserver', '--verbose', '-s', 'libgazebo_ros_init.so',
+                     '-s', 'libgazebo_ros_factory.so', world],
+                cwd=[launch_dir], output='screen'),
+            ExecuteProcess(
+                condition=IfCondition(PythonExpression(
+                    [use_simulator, ' and not ', headless])),
+                cmd=['gzclient'],
+                cwd=[launch_dir], output='screen'),
+            Node(
+                package='gazebo_ros',
+                executable='spawn_entity.py',
+                output='screen',
+                arguments=[
+                    '-entity', robot_name,
+                    '-topic', 'robot_description',
+                    '-robot_namespace', namespace,
+                    '-x', pose['x'], '-y', pose['y'], '-z', pose['z'],
+                    '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']])
+        ]
+    )
+
+    load_gz = GroupAction(
+        condition=IfCondition(use_gz),
+        actions=[
+            SetEnvironmentVariable('IGN_GAZEBO_RESOURCE_PATH', env_vars),
+            Node(
+                package='ros_gz_bridge', executable='parameter_bridge',
+                name='clock_bridge',
+                output='screen',
+                parameters=[{
+                    'use_sim_time': True
+                }],
+                arguments=['/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock']
+            ),
+            Node(
+                package='ros_gz_bridge',
+                executable='parameter_bridge',
+                name='lidar_bridge',
+                output='screen',
+                parameters=[{
+                    'use_sim_time': True
+                }],
+                arguments=[['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan']],
+            ),
+            ExecuteProcess(
+                cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+                     'joint_state_broadcaster'],
+                output='screen'
+            ),
+            ExecuteProcess(
+                cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+                     'diffdrive_controller'],
+                output='screen'
+            ),
+            Node(
+                package='ros_gz_sim',
+                executable='create',
+                output='screen',
+                arguments=[
+                    '-entity', robot_name,
+                    '-string', Command(['xacro', ' ', robot_sdf]),
+                    '-robot_namespace', namespace,
+                    '-x', pose['x'], '-y', pose['y'], '-z', pose['z'],
+                    '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']]),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    PathJoinSubstitution([
+                        FindPackageShare('ros_gz_sim'),
+                        'launch',
+                        'gz_sim.launch.py'
+                    ])
+                ),
+                launch_arguments={'gz_args': ['-r -v 4 ', world]}.items(),
+            )
+        ]
+    )
+
     # Create the launch description and populate
     ld = LaunchDescription()
 
@@ -240,11 +311,11 @@ def generate_launch_description():
     ld.add_action(declare_robot_name_cmd)
     ld.add_action(declare_robot_sdf_cmd)
     ld.add_action(declare_use_respawn_cmd)
+    ld.add_action(declare_use_gz_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)
+    ld.add_action(load_gazeboclassic)
+    ld.add_action(load_gz)
 
     # Add the actions to launch all of the navigation nodes
     ld.add_action(start_robot_state_publisher_cmd)

nav2_bringup/package.xml

@@ -15,9 +15,14 @@
   <build_depend>navigation2</build_depend>
   <build_depend>launch_ros</build_depend>
 
+  <exec_depend>diff_drive_controller</exec_depend>
+  <exec_depend>gz_ros2_control</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>navigation2</exec_depend>
   <exec_depend>nav2_common</exec_depend>
+  <exec_depend>ros_gz_bridge</exec_depend>
+  <exec_depend>ros_gz_sim</exec_depend>
   <exec_depend>slam_toolbox</exec_depend>
   <exec_depend>turtlebot3_gazebo</exec_depend>