Initial support to the new Gazebo

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

nav2_bringup/CMakeLists.txt

@@ -7,6 +7,7 @@ find_package(navigation2 REQUIRED)
 
 nav2_package()
 
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY maps DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})

nav2_bringup/config/control.yaml

@@ -0,0 +1,78 @@
+/**:
+  controller_manager:
+    ros__parameters:
+      update_rate: 1000  # Hz
+
+      joint_state_broadcaster:
+        type: joint_state_broadcaster/JointStateBroadcaster
+
+      diffdrive_controller:
+        type: diff_drive_controller/DiffDriveController
+
+  diffdrive_controller:
+    ros__parameters:
+      use_sim_time: True
+      left_wheel_names: ["wheel_left_joint"]
+      right_wheel_names: ["wheel_right_joint"]
+
+      wheel_separation: 0.287
+      wheel_radius: 0.033
+
+      # Multiplier applied to the wheel separation parameter.
+      # This is used to account for a difference between the robot model and a real robot.
+      wheel_separation_multiplier: 1.0
+
+      # Multipliers applied to the wheel radius parameter.
+      # This is used to account for a difference between the robot model and a real robot.
+      left_wheel_radius_multiplier: 1.0
+      right_wheel_radius_multiplier: 1.0
+
+      publish_rate: 62.0
+      odom_frame_id: odom
+      base_frame_id: base_footprint
+      pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+      twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+
+      open_loop: false
+      enable_odom_tf: true
+
+      cmd_vel_timeout: 0.5
+      use_stamped_vel: false
+
+      # Preserve turning radius when limiting speed/acceleration/jerk
+      preserve_turning_radius: true
+
+      # Publish limited velocity
+      publish_cmd: true
+
+      # Publish wheel data
+      publish_wheel_data: true
+
+      # Velocity and acceleration limits in cartesian
+      # These limits do not factor in per wheel limits
+      # that might be exceeded when linear and angular are combined
+      # Whenever a min_* is unspecified, default to -max_*
+      linear.x.has_velocity_limits: true
+      linear.x.has_acceleration_limits: true
+      linear.x.has_jerk_limits: false
+      # This is max if system is safety_override "full"
+      # motion_control_node will bound this to 0.306 if safety enabled (as is by default)
+      linear.x.max_velocity: 0.46
+      linear.x.min_velocity: -0.46
+      linear.x.max_acceleration: 0.9
+      # Not using jerk limits yet
+      # linear.x.max_jerk: 0.0
+      # linear.x.min_jerk: 0.0
+
+      angular.z.has_velocity_limits: true
+      angular.z.has_acceleration_limits: true
+      angular.z.has_jerk_limits: false
+      angular.z.max_velocity: 1.9
+      angular.z.min_velocity: -1.9
+      # Using 0.9 linear for each wheel, assuming one wheel accel to .9
+      # and other to -.9 with wheelbase leads to 7.725 rad/s^2
+      angular.z.max_acceleration: 7.725
+      angular.z.min_acceleration: -7.725
+      # Not using jerk limits yet
+      # angular.z.max_jerk: 0.0
+      # angular.z.min_jerk: 0.0

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>ign_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>