diff --git a/nav2_bringup/CMakeLists.txt b/nav2_bringup/CMakeLists.txt
index 629034556e..1d5d6838a4 100644
--- a/nav2_bringup/CMakeLists.txt
+++ b/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})
diff --git a/nav2_bringup/config/control.yaml b/nav2_bringup/config/control.yaml
new file mode 100644
index 0000000000..f83b5a35e8
--- /dev/null
+++ b/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
diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py
index e391fa863c..1513ea280d 100644
--- a/nav2_bringup/launch/tb3_simulation_launch.py
+++ b/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)
diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml
index 7080f580ed..4c69255526 100644
--- a/nav2_bringup/package.xml
+++ b/nav2_bringup/package.xml
@@ -15,9 +15,14 @@
navigation2
launch_ros
+ diff_drive_controller
+ ign_ros2_control
+ joint_state_broadcaster
launch_ros
navigation2
nav2_common
+ ros_gz_bridge
+ ros_gz_sim
slam_toolbox
turtlebot3_gazebo
diff --git a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf
new file mode 100644
index 0000000000..d8576d7550
--- /dev/null
+++ b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf
@@ -0,0 +1,423 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ "
+ scan
+ base_scan
+ -0.064 0 0.15 0 0 0
+ 5
+
+
+
+ 180
+ 1
+ 0.000000
+ 6.280000
+
+
+ 1
+ 0.01
+ 0
+ 0
+
+
+
+ 0.08
+ 10.0
+ 0.01
+
+
+ 1
+ true
+
+
+
+
+
+ true
+ 200
+ imu
+ imu_link
+
+
+
+
+ 0.0
+ 2e-4
+
+
+
+
+ 0.0
+ 2e-4
+
+
+
+
+ 0.0
+ 2e-4
+
+
+
+
+
+
+ 0.0
+ 1.7e-2
+
+
+
+
+ 0.0
+ 1.7e-2
+
+
+
+
+ 0.0
+ 1.7e-2
+
+
+
+
+
+
+
+
+
+ ign_ros2_control/IgnitionSystem
+
+
+
+ -1
+ 1
+
+
+
+
+
+
+ -1
+ 1
+
+
+
+
+
+
+
+
+ $(find nav2_bringup)/config/control.yaml
+
+ ~/odom:=odom
+ /tf:=tf
+ /tf_static:=tf_static
+ /diagnostics:=diagnostics
+ /diffdrive_controller/cmd_vel_unstamped:=cmd_vel
+
+
+
+
+ true
+ true
+ false
+ false
+ true
+ true
+ true
+
+
+
+
diff --git a/nav2_bringup/urdf/turtlebot3_waffle.urdf b/nav2_bringup/urdf/turtlebot3_waffle.urdf
index f56bd6d479..aeb089a100 100644
--- a/nav2_bringup/urdf/turtlebot3_waffle.urdf
+++ b/nav2_bringup/urdf/turtlebot3_waffle.urdf
@@ -290,4 +290,164 @@
+
+
+ true
+ 200
+
+
+
+
+ 0.0
+ 2e-4
+
+
+
+
+ 0.0
+ 2e-4
+
+
+
+
+ 0.0
+ 2e-4
+
+
+
+
+
+
+ 0.0
+ 1.7e-2
+
+
+
+
+ 0.0
+ 1.7e-2
+
+
+
+
+ 0.0
+ 1.7e-2
+
+
+
+
+
+ false
+
+
+ ~/out:=imu
+
+
+
+
+
+
+
+ 1
+ 5
+ 0.064 -0.047 0.107 0 0 0
+
+
+
+
+
+ intel_realsense_r200_depth
+ camera_depth_frame
+ 0.07
+ 0.001
+ 5.0
+
+
+
+
+
+
+
+
+
+
+ /tf:=tf
+
+
+ 30
+
+
+ wheel_left_joint
+ wheel_right_joint
+
+
+ 0.287
+ 0.066
+
+
+ 20
+ 1.0
+
+ cmd_vel
+
+
+ true
+ true
+ false
+
+ odom
+ odom
+ base_footprint
+
+
+
+
+
+
+ ~/out:=joint_states
+
+ 30
+ wheel_left_joint
+ wheel_right_joint
+
+
+
+
+
+ true
+ true
+ -0.064 0 0.2 0 0 0
+ 5
+
+
+
+ 360
+ 1.000000
+ 0.000000
+ 6.280000
+
+
+
+ 0.08
+ 20.0
+ 0.015000
+
+
+ gaussian
+ 0.0
+ 0.01
+
+
+
+
+
+ ~/out:=scan
+
+ sensor_msgs/LaserScan
+ base_scan
+
+
+
+
diff --git a/nav2_bringup/worlds/gz_world_only.model b/nav2_bringup/worlds/gz_world_only.model
new file mode 100644
index 0000000000..bb8f90d63f
--- /dev/null
+++ b/nav2_bringup/worlds/gz_world_only.model
@@ -0,0 +1,112 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+ ogre2
+
+
+
+
+
+ 1
+ 0 0 10 0 0 0
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+
+ 1000
+ 0.9
+ 0.01
+ 0.001
+
+ -0.5 0.1 -0.9
+
+
+ 1
+
+
+
+
+ 0 0 1
+ 100 100
+
+
+
+
+
+
+ 0 0 1
+ 100 100
+
+
+
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+
+
+
+
+
+
+ false
+
+
+
+
+ 0.319654 -0.235002 9.29441 0 1.5138 0.009599
+ orbit
+ perspective
+
+
+
+
+ 1000.0
+ 0.001
+ 1
+
+
+ quick
+ 150
+ 0
+ 1.400000
+ 1
+
+
+ 0.00001
+ 0.2
+ 2000.000000
+ 0.01000
+
+
+
+
+
+ 1
+
+ model://turtlebot3_world
+
+
+
+
+