From c52b2b051b6d706cb47059df645a38b33fbaacfb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 16 Jun 2023 17:03:13 +0200 Subject: [PATCH] Initial support to the new Gazebo MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_bringup/CMakeLists.txt | 1 + nav2_bringup/config/control.yaml | 78 ++++ nav2_bringup/launch/tb3_simulation_launch.py | 141 +++++-- nav2_bringup/package.xml | 5 + nav2_bringup/urdf/gz_turtlebot3_waffle.urdf | 423 +++++++++++++++++++ nav2_bringup/urdf/turtlebot3_waffle.urdf | 160 +++++++ nav2_bringup/worlds/gz_world_only.model | 112 +++++ 7 files changed, 885 insertions(+), 35 deletions(-) create mode 100644 nav2_bringup/config/control.yaml create mode 100644 nav2_bringup/urdf/gz_turtlebot3_waffle.urdf create mode 100644 nav2_bringup/worlds/gz_world_only.model 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 + + + + +