Initial support to the new Gazebo

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/launch/tb3_gz_simulation_launch.py

@@ -0,0 +1,297 @@
+# Copyright (c) 2018 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""This is all-in-one launch script intended for use by nav2 developers."""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+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 Command, LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import PythonExpression
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    launch_dir = os.path.join(bringup_dir, 'launch')
+    # This checks that tb3 exists needed for the URDF. If not using TB3, its safe to remove.
+    _ = get_package_share_directory('turtlebot3_gazebo')
+
+    # Create the launch configuration variables
+    slam = LaunchConfiguration('slam')
+    namespace = LaunchConfiguration('namespace')
+    use_namespace = LaunchConfiguration('use_namespace')
+    map_yaml_file = LaunchConfiguration('map')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    params_file = LaunchConfiguration('params_file')
+    autostart = LaunchConfiguration('autostart')
+    use_composition = LaunchConfiguration('use_composition')
+    use_respawn = LaunchConfiguration('use_respawn')
+
+    # Launch configuration variables specific to simulation
+    rviz_config_file = LaunchConfiguration('rviz_config_file')
+    use_simulator = LaunchConfiguration('use_simulator')
+    use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
+    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
+    # https://github.com/ros/geometry2/issues/32
+    # https://github.com/ros/robot_state_publisher/pull/30
+    # TODO(orduno) Substitute with `PushNodeRemapping`
+    #              https://github.com/ros2/launch_ros/issues/56
+    remappings = [('/tf', 'tf'),
+                  ('/tf_static', 'tf_static')]
+
+    # Declare the launch arguments
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+        description='Top-level namespace')
+
+    declare_use_namespace_cmd = DeclareLaunchArgument(
+        'use_namespace',
+        default_value='false',
+        description='Whether to apply a namespace to the navigation stack')
+
+    declare_slam_cmd = DeclareLaunchArgument(
+        'slam',
+        default_value='False',
+        description='Whether run a SLAM')
+
+    declare_map_yaml_cmd = DeclareLaunchArgument(
+        'map',
+        default_value=os.path.join(
+            bringup_dir, 'maps', 'turtlebot3_world.yaml'),
+        description='Full path to map file to load')
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='true',
+        description='Use simulation (Gazebo) clock if true')
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
+        description='Full path to the ROS2 parameters file to use for all launched nodes')
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        'autostart', default_value='true',
+        description='Automatically startup the nav2 stack')
+
+    declare_use_composition_cmd = DeclareLaunchArgument(
+        'use_composition', default_value='True',
+        description='Whether to use composed bringup')
+
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        'use_respawn', default_value='False',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
+
+    declare_rviz_config_file_cmd = DeclareLaunchArgument(
+        'rviz_config_file',
+        default_value=os.path.join(
+            bringup_dir, 'rviz', 'nav2_default_view.rviz'),
+        description='Full path to the RVIZ config file to use')
+
+    declare_use_simulator_cmd = DeclareLaunchArgument(
+        'use_simulator',
+        default_value='True',
+        description='Whether to start the simulator')
+
+    declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
+        'use_robot_state_pub',
+        default_value='True',
+        description='Whether to start the robot state publisher')
+
+    declare_use_rviz_cmd = DeclareLaunchArgument(
+        'use_rviz',
+        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',
+        description='Whether to execute gzclient)')
+
+    declare_world_cmd = DeclareLaunchArgument(
+        'world',
+        # TODO(orduno) Switch back once ROS argument passing has been fixed upstream
+        #              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', 'gz_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, 'urdf', 'gz_turtlebot3_waffle.urdf'),
+        description='Full path to robot sdf file to spawn the robot in gazebo')
+
+    start_robot_state_publisher_cmd = Node(
+        condition=IfCondition(use_robot_state_pub),
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        namespace=namespace,
+        output='screen',
+        parameters=[{'use_sim_time': use_sim_time,
+                     'robot_description': Command(['xacro', ' ', robot_sdf])}],
+        remappings=remappings)
+
+    rviz_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_dir, 'rviz_launch.py')),
+        condition=IfCondition(use_rviz),
+        launch_arguments={'namespace': namespace,
+                          'use_namespace': use_namespace,
+                          'rviz_config': rviz_config_file}.items())
+
+    bringup_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_dir, 'bringup_launch.py')),
+        launch_arguments={'namespace': namespace,
+                          'use_namespace': use_namespace,
+                          'slam': slam,
+                          'map': map_yaml_file,
+                          'use_sim_time': use_sim_time,
+                          'params_file': params_file,
+                          'autostart': autostart,
+                          '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'), '..')
+
+    set_env_vars_resources = SetEnvironmentVariable('IGN_GAZEBO_RESOURCE_PATH', env_vars)
+    clock_bridge = 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']
+    )
+    lidar_bridge = 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']],
+    )
+    load_joint_state_broadcaster = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster'],
+        output='screen'
+    )
+    load_diffdrive_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'diffdrive_controller'],
+        output='screen'
+    )
+    spawn_model = 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']]
+    )
+    gazebo = 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()
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_use_namespace_cmd)
+    ld.add_action(declare_slam_cmd)
+    ld.add_action(declare_map_yaml_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_autostart_cmd)
+    ld.add_action(declare_use_composition_cmd)
+
+    ld.add_action(declare_rviz_config_file_cmd)
+    ld.add_action(declare_use_simulator_cmd)
+    ld.add_action(declare_use_robot_state_pub_cmd)
+    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)
+    ld.add_action(declare_use_respawn_cmd)
+    ld.add_action(declare_use_gz_cmd)
+
+    # Add any conditioned actions
+    ld.add_action(set_env_vars_resources)
+    ld.add_action(clock_bridge)
+    ld.add_action(lidar_bridge)
+    ld.add_action(load_joint_state_broadcaster)
+    ld.add_action(load_diffdrive_controller)
+    ld.add_action(spawn_model)
+    ld.add_action(gazebo)
+
+    # Add the actions to launch all of the navigation nodes
+    ld.add_action(start_robot_state_publisher_cmd)
+    ld.add_action(rviz_cmd)
+    ld.add_action(bringup_cmd)
+
+    return ld

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>
 

nav2_bringup/params/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