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 01/26] 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 + + + + + From fd50e02a992142da52db271a66e5e67f5840cd8b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Sat, 17 Jun 2023 00:39:31 +0200 Subject: [PATCH 02/26] Added feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../launch/tb3_gz_simulation_launch.py | 297 ++++++++++++++++++ nav2_bringup/launch/tb3_simulation_launch.py | 141 +++------ nav2_bringup/{config => params}/control.yaml | 0 nav2_bringup/urdf/gz_turtlebot3_waffle.urdf | 9 +- 4 files changed, 333 insertions(+), 114 deletions(-) create mode 100644 nav2_bringup/launch/tb3_gz_simulation_launch.py rename nav2_bringup/{config => params}/control.yaml (100%) diff --git a/nav2_bringup/launch/tb3_gz_simulation_launch.py b/nav2_bringup/launch/tb3_gz_simulation_launch.py new file mode 100644 index 0000000000..1aac161d6f --- /dev/null +++ b/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 diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index 1513ea280d..e391fa863c 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -19,14 +19,11 @@ 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.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription 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.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare def generate_launch_description(): @@ -46,7 +43,6 @@ 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') @@ -138,11 +134,6 @@ 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', @@ -164,9 +155,26 @@ def generate_launch_description(): declare_robot_sdf_cmd = DeclareLaunchArgument( 'robot_sdf', - default_value=os.path.join(bringup_dir, 'urdf', 'turtlebot3_waffle.urdf'), + default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'), 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', @@ -175,9 +183,20 @@ def generate_launch_description(): namespace=namespace, output='screen', parameters=[{'use_sim_time': use_sim_time, - 'robot_description': Command(['xacro', ' ', robot_sdf])}], + 'robot_description': robot_description}], 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')), @@ -199,96 +218,6 @@ 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() @@ -311,11 +240,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(load_gazeboclassic) - ld.add_action(load_gz) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(start_robot_state_publisher_cmd) diff --git a/nav2_bringup/config/control.yaml b/nav2_bringup/params/control.yaml similarity index 100% rename from nav2_bringup/config/control.yaml rename to nav2_bringup/params/control.yaml diff --git a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf index d8576d7550..e35f055ad2 100644 --- a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf +++ b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf @@ -1,12 +1,5 @@ - - @@ -397,7 +390,7 @@ - $(find nav2_bringup)/config/control.yaml + $(find nav2_bringup)/params/control.yaml ~/odom:=odom /tf:=tf From 2500c8cabdce95d546a21ba6e760099b4e9e6c28 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 19 Jun 2023 10:14:46 +0200 Subject: [PATCH 03/26] Fixed build 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 - 1 file changed, 1 deletion(-) diff --git a/nav2_bringup/CMakeLists.txt b/nav2_bringup/CMakeLists.txt index 1d5d6838a4..629034556e 100644 --- a/nav2_bringup/CMakeLists.txt +++ b/nav2_bringup/CMakeLists.txt @@ -7,7 +7,6 @@ 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}) From 52157681e1aa892c2f6aa7438a811d9df44fd310 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 20 Jun 2023 12:24:06 +0200 Subject: [PATCH 04/26] Added feeback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../launch/tb3_gz_simulation_launch.py | 59 +++++-- nav2_bringup/urdf/gz_turtlebot3_waffle.urdf | 13 +- nav2_bringup/urdf/turtlebot3_waffle.urdf | 160 ------------------ 3 files changed, 52 insertions(+), 180 deletions(-) diff --git a/nav2_bringup/launch/tb3_gz_simulation_launch.py b/nav2_bringup/launch/tb3_gz_simulation_launch.py index 1aac161d6f..7dbac17e77 100644 --- a/nav2_bringup/launch/tb3_gz_simulation_launch.py +++ b/nav2_bringup/launch/tb3_gz_simulation_launch.py @@ -1,11 +1,11 @@ -# Copyright (c) 2018 Intel Corporation -# -# Licensed under the Apache License, Version 2.0 (the "License"); +# Copyright (C) 2023 Open Source Robotics Foundation + +# 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 -# + +# 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. @@ -19,7 +19,7 @@ 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 DeclareLaunchArgument, ExecuteProcess from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -137,11 +137,6 @@ 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', @@ -206,6 +201,7 @@ def generate_launch_description(): set_env_vars_resources = SetEnvironmentVariable('IGN_GAZEBO_RESOURCE_PATH', env_vars) clock_bridge = Node( + condition=IfCondition(use_simulator), package='ros_gz_bridge', executable='parameter_bridge', name='clock_bridge', output='screen', @@ -214,7 +210,9 @@ def generate_launch_description(): }], arguments=['/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] ) + lidar_bridge = Node( + condition=IfCondition(use_simulator), package='ros_gz_bridge', executable='parameter_bridge', name='lidar_bridge', @@ -224,17 +222,31 @@ def generate_launch_description(): }], arguments=[['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan']], ) + imu_bridge = Node( + condition=IfCondition(use_simulator), + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) load_joint_state_broadcaster = ExecuteProcess( + condition=IfCondition(use_simulator), cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'], output='screen' ) load_diffdrive_controller = ExecuteProcess( + condition=IfCondition(use_simulator), cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'diffdrive_controller'], output='screen' ) spawn_model = Node( + condition=IfCondition(use_simulator), package='ros_gz_sim', executable='create', output='screen', @@ -245,7 +257,19 @@ def generate_launch_description(): '-x', pose['x'], '-y', pose['y'], '-z', pose['z'], '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']] ) - gazebo = IncludeLaunchDescription( + gazebo_server = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ', world]}.items(), + condition=IfCondition(use_simulator) + ) + + gazebo_client = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution([ FindPackageShare('ros_gz_sim'), @@ -253,7 +277,9 @@ def generate_launch_description(): 'gz_sim.launch.py' ]) ), - launch_arguments={'gz_args': ['-r -v 4 ', world]}.items(), + condition=IfCondition(PythonExpression( + [use_simulator, ' and not ', headless])), + launch_arguments={'gz_args': ['-g ']}.items(), ) # Create the launch description and populate @@ -278,16 +304,17 @@ 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(set_env_vars_resources) ld.add_action(clock_bridge) ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) ld.add_action(load_joint_state_broadcaster) ld.add_action(load_diffdrive_controller) ld.add_action(spawn_model) - ld.add_action(gazebo) + ld.add_action(gazebo_server) + ld.add_action(gazebo_client) # Add the actions to launch all of the navigation nodes ld.add_action(start_robot_state_publisher_cmd) diff --git a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf index e35f055ad2..4d8e38a771 100644 --- a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf +++ b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf @@ -292,8 +292,8 @@ - 180 - 1 + 360 + 1.000000 0.000000 6.280000 @@ -306,9 +306,14 @@ 0.08 - 10.0 - 0.01 + 20.0 + 0.015000 + + gaussian + 0.0 + 0.01 + 1 true diff --git a/nav2_bringup/urdf/turtlebot3_waffle.urdf b/nav2_bringup/urdf/turtlebot3_waffle.urdf index aeb089a100..f56bd6d479 100644 --- a/nav2_bringup/urdf/turtlebot3_waffle.urdf +++ b/nav2_bringup/urdf/turtlebot3_waffle.urdf @@ -290,164 +290,4 @@ - - - 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 - - - - From d3c421afc23dc98dddcd67326c0ef634e46cbb3d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 20 Jun 2023 17:12:23 +0200 Subject: [PATCH 05/26] Fixed physics tag MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_bringup/worlds/gz_world_only.model | 34 +++++++++++-------------- 1 file changed, 15 insertions(+), 19 deletions(-) diff --git a/nav2_bringup/worlds/gz_world_only.model b/nav2_bringup/worlds/gz_world_only.model index bb8f90d63f..184991bbe6 100644 --- a/nav2_bringup/worlds/gz_world_only.model +++ b/nav2_bringup/worlds/gz_world_only.model @@ -80,25 +80,21 @@ - - 1000.0 - 0.001 - 1 - - - quick - 150 - 0 - 1.400000 - 1 - - - 0.00001 - 0.2 - 2000.000000 - 0.01000 - - + + 0.001 + 1 + 1000.0 + + + + quick + 150 + 0 + 1.400000 + 1 + + + From 975bc7833bc3b074100afbbce4d582279c52e647 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 20 Jun 2023 17:19:01 +0200 Subject: [PATCH 06/26] Fixed physics tag MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_bringup/worlds/gz_world_only.model | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_bringup/worlds/gz_world_only.model b/nav2_bringup/worlds/gz_world_only.model index 184991bbe6..e6875e910a 100644 --- a/nav2_bringup/worlds/gz_world_only.model +++ b/nav2_bringup/worlds/gz_world_only.model @@ -93,7 +93,6 @@ 1.400000 1 - From 469c2a27e3e4f0012036d93b989e01c55f30c7b8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 20 Jun 2023 23:14:54 +0200 Subject: [PATCH 07/26] Fix copyright and urdf MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_bringup/launch/tb3_gz_simulation_launch.py | 10 +++++----- nav2_bringup/urdf/gz_turtlebot3_waffle.urdf | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/nav2_bringup/launch/tb3_gz_simulation_launch.py b/nav2_bringup/launch/tb3_gz_simulation_launch.py index 7dbac17e77..8554f81f47 100644 --- a/nav2_bringup/launch/tb3_gz_simulation_launch.py +++ b/nav2_bringup/launch/tb3_gz_simulation_launch.py @@ -1,11 +1,11 @@ # Copyright (C) 2023 Open Source Robotics Foundation - -# Licensed under the Apache License, Version 2.0 (the "License") +# +# 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 - +# +# 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. diff --git a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf index 4d8e38a771..627ee8b158 100644 --- a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf +++ b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf @@ -305,7 +305,7 @@ - 0.08 + 0.0 20.0 0.015000 From 34f466e1b1d9fda0b2882e050286cb3355db4bae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 21 Jun 2023 13:19:19 +0200 Subject: [PATCH 08/26] min range cannot be zero MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_bringup/urdf/gz_turtlebot3_waffle.urdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf index 627ee8b158..e9957d36aa 100644 --- a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf +++ b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf @@ -305,7 +305,7 @@ - 0.0 + 0.00001 20.0 0.015000 From 01babc9bdedb53bfb01006fcc3f6c627f22fc9cb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 22 Jun 2023 00:43:06 +0200 Subject: [PATCH 09/26] Simplify files MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../launch/tb3_gz_simulation_launch.py | 12 +++-- nav2_bringup/params/control.yaml | 4 +- nav2_bringup/urdf/gz_turtlebot3_waffle.urdf | 51 +------------------ 3 files changed, 11 insertions(+), 56 deletions(-) diff --git a/nav2_bringup/launch/tb3_gz_simulation_launch.py b/nav2_bringup/launch/tb3_gz_simulation_launch.py index 8554f81f47..11f03ccb3f 100644 --- a/nav2_bringup/launch/tb3_gz_simulation_launch.py +++ b/nav2_bringup/launch/tb3_gz_simulation_launch.py @@ -305,19 +305,21 @@ def generate_launch_description(): ld.add_action(declare_robot_sdf_cmd) ld.add_action(declare_use_respawn_cmd) - # Add any conditioned actions + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(set_env_vars_resources) + ld.add_action(gazebo_server) + ld.add_action(gazebo_client) + + # Add any conditioned actions ld.add_action(clock_bridge) ld.add_action(lidar_bridge) ld.add_action(imu_bridge) + ld.add_action(spawn_model) ld.add_action(load_joint_state_broadcaster) ld.add_action(load_diffdrive_controller) - ld.add_action(spawn_model) - ld.add_action(gazebo_server) - ld.add_action(gazebo_client) # 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) diff --git a/nav2_bringup/params/control.yaml b/nav2_bringup/params/control.yaml index f83b5a35e8..667342f5b2 100644 --- a/nav2_bringup/params/control.yaml +++ b/nav2_bringup/params/control.yaml @@ -30,8 +30,8 @@ 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] + pose_covariance_diagonal : [1.0e-05, 1.0e-05, 1000000000000.0, 1000000000000.0, 1000000000000.0, 0.001] + twist_covariance_diagonal: [1.0e-05, 1.0e-05, 1000000000000.0, 1000000000000.0, 1000000000000.0, 0.001] open_loop: false enable_odom_tf: true diff --git a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf index e9957d36aa..6d9ceae481 100644 --- a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf +++ b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf @@ -1,46 +1,5 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -376,18 +335,12 @@ ign_ros2_control/IgnitionSystem - - -1 - 1 - + - - -1 - 1 - + From 792569a9518eee7a4fcf94f58e4b4493bd49a0e4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 23 Jun 2023 23:56:05 +0200 Subject: [PATCH 10/26] Added feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_bringup/launch/gz_robot_launch.py | 142 ++++++++++++++++++ .../launch/tb3_gz_simulation_launch.py | 82 +++------- nav2_bringup/urdf/gz_turtlebot3_waffle.urdf | 42 +++++- nav2_bringup/worlds/gz_world_only.model | 10 -- 4 files changed, 197 insertions(+), 79 deletions(-) create mode 100644 nav2_bringup/launch/gz_robot_launch.py diff --git a/nav2_bringup/launch/gz_robot_launch.py b/nav2_bringup/launch/gz_robot_launch.py new file mode 100644 index 0000000000..e1935c5fc9 --- /dev/null +++ b/nav2_bringup/launch/gz_robot_launch.py @@ -0,0 +1,142 @@ +# Copyright (C) 2023 Open Source Robotics Foundation +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.conditions import IfCondition +from launch.substitutions import Command, LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + bringup_dir = get_package_share_directory('nav2_bringup') + + namespace = LaunchConfiguration('namespace') + use_simulator = LaunchConfiguration('use_simulator') + use_sim_time = LaunchConfiguration('use_sim_time') + robot_name = LaunchConfiguration('robot_name') + robot_sdf = LaunchConfiguration('robot_sdf') + 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')} + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_simulator_cmd = DeclareLaunchArgument( + 'use_simulator', + default_value='True', + description='Whether to start the simulator') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation (Gazebo) clock if true') + + 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') + + clock_bridge = Node( + condition=IfCondition(use_simulator), + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time + }], + arguments=['/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + lidar_bridge = Node( + condition=IfCondition(use_simulator), + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time + }], + arguments=[['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan']], + ) + imu_bridge = Node( + condition=IfCondition(use_simulator), + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + load_joint_state_broadcaster = ExecuteProcess( + condition=IfCondition(use_simulator), + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + + load_diffdrive_controller = ExecuteProcess( + condition=IfCondition(use_simulator), + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + spawn_model = Node( + condition=IfCondition(use_simulator), + 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']] + ) + + # Create the launch description and populate + ld = LaunchDescription() + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_robot_name_cmd) + ld.add_action(declare_robot_sdf_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(spawn_model) + ld.add_action(load_joint_state_broadcaster) + ld.add_action(load_diffdrive_controller) + return ld diff --git a/nav2_bringup/launch/tb3_gz_simulation_launch.py b/nav2_bringup/launch/tb3_gz_simulation_launch.py index 11f03ccb3f..d0240ed21e 100644 --- a/nav2_bringup/launch/tb3_gz_simulation_launch.py +++ b/nav2_bringup/launch/tb3_gz_simulation_launch.py @@ -19,7 +19,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.actions import DeclareLaunchArgument from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -200,63 +200,7 @@ def generate_launch_description(): 'turtlebot3_gazebo'), '..') set_env_vars_resources = SetEnvironmentVariable('IGN_GAZEBO_RESOURCE_PATH', env_vars) - clock_bridge = Node( - condition=IfCondition(use_simulator), - 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( - condition=IfCondition(use_simulator), - 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']], - ) - imu_bridge = Node( - condition=IfCondition(use_simulator), - package='ros_gz_bridge', - executable='parameter_bridge', - name='imu_bridge', - output='screen', - parameters=[{ - 'use_sim_time': True - }], - arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], - ) - load_joint_state_broadcaster = ExecuteProcess( - condition=IfCondition(use_simulator), - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_state_broadcaster'], - output='screen' - ) - load_diffdrive_controller = ExecuteProcess( - condition=IfCondition(use_simulator), - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'diffdrive_controller'], - output='screen' - ) - spawn_model = Node( - condition=IfCondition(use_simulator), - 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_server = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution([ @@ -282,6 +226,21 @@ def generate_launch_description(): launch_arguments={'gz_args': ['-g ']}.items(), ) + gz_robot = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'gz_robot_launch.py')), + launch_arguments={'namespace': namespace, + 'use_simulator': use_simulator, + 'use_sim_time': use_sim_time, + 'robot_name': robot_name, + 'robot_sdf': robot_sdf, + 'x_pose': pose['x'], + 'y_pose': pose['y'], + 'z_pose': pose['z'], + 'roll': pose['R'], + 'pitch': pose['P'], + 'yaw': pose['Y']}.items()) + # Create the launch description and populate ld = LaunchDescription() @@ -310,14 +269,7 @@ def generate_launch_description(): ld.add_action(set_env_vars_resources) ld.add_action(gazebo_server) ld.add_action(gazebo_client) - - # Add any conditioned actions - ld.add_action(clock_bridge) - ld.add_action(lidar_bridge) - ld.add_action(imu_bridge) - ld.add_action(spawn_model) - ld.add_action(load_joint_state_broadcaster) - ld.add_action(load_diffdrive_controller) + ld.add_action(gz_robot) # Add the actions to launch all of the navigation nodes ld.add_action(rviz_cmd) diff --git a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf index 6d9ceae481..fd63334ad4 100644 --- a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf +++ b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf @@ -8,13 +8,22 @@ + + + + 0.4 0.4 0.4 1 + 0.4 0.4 0.4 1 + 0.0 0.0 1.0 1 + + + + - @@ -40,13 +49,21 @@ + + + + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + 0.0 0.0 1.0 1 + + + - @@ -72,13 +89,21 @@ + + + + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + 0.0 0.0 1.0 1 + + + - @@ -157,13 +182,22 @@ + + + + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + 0.0 0.0 1.0 1 + + + + - diff --git a/nav2_bringup/worlds/gz_world_only.model b/nav2_bringup/worlds/gz_world_only.model index e6875e910a..0b9a19d209 100644 --- a/nav2_bringup/worlds/gz_world_only.model +++ b/nav2_bringup/worlds/gz_world_only.model @@ -1,7 +1,6 @@ - @@ -72,19 +71,10 @@ false - - - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 - orbit - perspective - - - 0.001 1 1000.0 - quick From 960fdd764c93588349aff374576da1411d16c37f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 26 Jun 2023 11:20:49 +0200 Subject: [PATCH 11/26] Added feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- ...{gz_robot_launch.py => tb3_gz_robot_launch.py} | 14 ++++++++++++++ nav2_bringup/launch/tb3_gz_simulation_launch.py | 15 +++------------ 2 files changed, 17 insertions(+), 12 deletions(-) rename nav2_bringup/launch/{gz_robot_launch.py => tb3_gz_robot_launch.py} (91%) diff --git a/nav2_bringup/launch/gz_robot_launch.py b/nav2_bringup/launch/tb3_gz_robot_launch.py similarity index 91% rename from nav2_bringup/launch/gz_robot_launch.py rename to nav2_bringup/launch/tb3_gz_robot_launch.py index e1935c5fc9..e916e46ead 100644 --- a/nav2_bringup/launch/gz_robot_launch.py +++ b/nav2_bringup/launch/tb3_gz_robot_launch.py @@ -18,6 +18,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.actions import SetEnvironmentVariable from launch.conditions import IfCondition from launch.substitutions import Command, LaunchConfiguration from launch_ros.actions import Node @@ -86,6 +87,7 @@ def generate_launch_description(): }], arguments=[['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan']], ) + imu_bridge = Node( condition=IfCondition(use_simulator), package='ros_gz_bridge', @@ -125,6 +127,16 @@ def generate_launch_description(): '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']] ) + env_vars = os.getenv('IGN_GAZEBO_RESOURCE_PATH') + env_vars += ':' + \ + os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'models') \ + + ':' + \ + os.path.join(get_package_share_directory( + 'turtlebot3_gazebo'), '..') + print('tb3_gz_robot_launch', env_vars) + + set_env_vars_resources = SetEnvironmentVariable('IGN_GAZEBO_RESOURCE_PATH', env_vars) + # Create the launch description and populate ld = LaunchDescription() ld.add_action(declare_namespace_cmd) @@ -133,6 +145,8 @@ def generate_launch_description(): ld.add_action(declare_use_simulator_cmd) ld.add_action(declare_use_sim_time_cmd) + ld.add_action(set_env_vars_resources) + ld.add_action(clock_bridge) ld.add_action(lidar_bridge) ld.add_action(imu_bridge) diff --git a/nav2_bringup/launch/tb3_gz_simulation_launch.py b/nav2_bringup/launch/tb3_gz_simulation_launch.py index d0240ed21e..4208f12877 100644 --- a/nav2_bringup/launch/tb3_gz_simulation_launch.py +++ b/nav2_bringup/launch/tb3_gz_simulation_launch.py @@ -20,7 +20,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution @@ -193,14 +193,6 @@ 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'), '..') - - set_env_vars_resources = SetEnvironmentVariable('IGN_GAZEBO_RESOURCE_PATH', env_vars) - gazebo_server = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution([ @@ -228,7 +220,7 @@ def generate_launch_description(): gz_robot = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'gz_robot_launch.py')), + os.path.join(launch_dir, 'tb3_gz_robot_launch.py')), launch_arguments={'namespace': namespace, 'use_simulator': use_simulator, 'use_sim_time': use_sim_time, @@ -266,10 +258,9 @@ def generate_launch_description(): ld.add_action(start_robot_state_publisher_cmd) - ld.add_action(set_env_vars_resources) + ld.add_action(gz_robot) ld.add_action(gazebo_server) ld.add_action(gazebo_client) - ld.add_action(gz_robot) # Add the actions to launch all of the navigation nodes ld.add_action(rviz_cmd) From 5a3e06c06aa39dcbbc34d7c4518a8e046c9b6b8d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 28 Jun 2023 13:37:01 +0200 Subject: [PATCH 12/26] Fix MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_bringup/launch/tb3_gz_robot_launch.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_bringup/launch/tb3_gz_robot_launch.py b/nav2_bringup/launch/tb3_gz_robot_launch.py index e916e46ead..4d571b2e72 100644 --- a/nav2_bringup/launch/tb3_gz_robot_launch.py +++ b/nav2_bringup/launch/tb3_gz_robot_launch.py @@ -127,13 +127,12 @@ def generate_launch_description(): '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']] ) - env_vars = os.getenv('IGN_GAZEBO_RESOURCE_PATH') + env_vars = os.getenv('IGN_GAZEBO_RESOURCE_PATH', default="") env_vars += ':' + \ os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'models') \ + ':' + \ os.path.join(get_package_share_directory( 'turtlebot3_gazebo'), '..') - print('tb3_gz_robot_launch', env_vars) set_env_vars_resources = SetEnvironmentVariable('IGN_GAZEBO_RESOURCE_PATH', env_vars) From b17def886cdaeee21faa50c967a55ed8cd4291ed Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 3 Jul 2023 13:11:57 +0200 Subject: [PATCH 13/26] Try to reduce load MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_bringup/params/control.yaml | 2 +- nav2_bringup/urdf/gz_turtlebot3_waffle.urdf | 6 +- nav2_bringup/worlds/gz_world_only.model | 148 ++++++++++++++++++-- 3 files changed, 143 insertions(+), 13 deletions(-) diff --git a/nav2_bringup/params/control.yaml b/nav2_bringup/params/control.yaml index 667342f5b2..a41ec798b7 100644 --- a/nav2_bringup/params/control.yaml +++ b/nav2_bringup/params/control.yaml @@ -1,7 +1,7 @@ /**: controller_manager: ros__parameters: - update_rate: 1000 # Hz + update_rate: 100 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster diff --git a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf index fd63334ad4..3cc08a69ee 100644 --- a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf +++ b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf @@ -191,7 +191,7 @@ - + @@ -248,7 +248,6 @@ - @@ -263,7 +262,6 @@ - @@ -402,6 +400,8 @@ true true true + 50 + 50 diff --git a/nav2_bringup/worlds/gz_world_only.model b/nav2_bringup/worlds/gz_world_only.model index 0b9a19d209..e41e65cbf2 100644 --- a/nav2_bringup/worlds/gz_world_only.model +++ b/nav2_bringup/worlds/gz_world_only.model @@ -27,6 +27,145 @@ name="ignition::gazebo::systems::Imu"> + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + 0.319654 -0.235002 9.29441 0 1.5138 0.009599 + + 0.25 + 25000 + + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + 1 0 0 10 0 0 0 @@ -75,15 +214,6 @@ 0.001 1 1000.0 - - - quick - 150 - 0 - 1.400000 - 1 - - From b41399419db34f22041c170831134b8c98da8148 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 17 Jul 2023 15:14:13 +0200 Subject: [PATCH 14/26] Removed cast shadows MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_bringup/urdf/gz_turtlebot3_waffle.urdf | 2 -- nav2_bringup/worlds/gz_world_only.model | 12 +++++------- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf index 3cc08a69ee..3dc8b245e4 100644 --- a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf +++ b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf @@ -306,8 +306,6 @@ 0.01 - 1 - true diff --git a/nav2_bringup/worlds/gz_world_only.model b/nav2_bringup/worlds/gz_world_only.model index e41e65cbf2..5c31e27da8 100644 --- a/nav2_bringup/worlds/gz_world_only.model +++ b/nav2_bringup/worlds/gz_world_only.model @@ -13,10 +13,6 @@ filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster"> - - @@ -43,7 +39,7 @@ 0.319654 -0.235002 9.29441 0 1.5138 0.009599 0.25 - 25000 + 50 @@ -167,7 +163,7 @@ - 1 + 0 0 0 10 0 0 0 0.8 0.8 0.8 1 0.8 0.8 0.8 1 @@ -207,7 +203,9 @@ - false + 0 + false + false From ed65b71a33a9aa3156a1e13adbf5ee8040fe57e5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 24 Jul 2023 13:46:56 +0200 Subject: [PATCH 15/26] Use Gazebo plugins instead of gz_ros2_control MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_bringup/launch/tb3_gz_robot_launch.py | 59 +++++++++++++++++---- nav2_bringup/urdf/gz_turtlebot3_waffle.urdf | 31 +++++++++-- nav2_bringup/worlds/gz_world_only.model | 1 + 3 files changed, 77 insertions(+), 14 deletions(-) diff --git a/nav2_bringup/launch/tb3_gz_robot_launch.py b/nav2_bringup/launch/tb3_gz_robot_launch.py index 4d571b2e72..e36b830ec1 100644 --- a/nav2_bringup/launch/tb3_gz_robot_launch.py +++ b/nav2_bringup/launch/tb3_gz_robot_launch.py @@ -100,20 +100,56 @@ def generate_launch_description(): arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], ) - load_joint_state_broadcaster = ExecuteProcess( + odom_bridge = Node( condition=IfCondition(use_simulator), - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_state_broadcaster'], - output='screen' + package='ros_gz_bridge', + executable='parameter_bridge', + name='odom_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time + }], + arguments=[['/odom' + '@nav_msgs/msg/Odometry[ignition.msgs.Odometry']], + ) + + odom_tf_bridge = Node( + condition=IfCondition(use_simulator), + package='ros_gz_bridge', + executable='parameter_bridge', + name='odom_tf_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time + }], + arguments=[['/tf' + '@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V']], ) - load_diffdrive_controller = ExecuteProcess( + cm_vel_bridge = Node( condition=IfCondition(use_simulator), - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'diffdrive_controller'], - output='screen' + package='ros_gz_bridge', + executable='parameter_bridge', + name='cm_vel_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time + }], + arguments=[['/cmd_vel' + '@geometry_msgs/msg/Twist]ignition.msgs.Twist']], ) + # load_joint_state_broadcaster = ExecuteProcess( + # condition=IfCondition(use_simulator), + # cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + # 'joint_state_broadcaster'], + # output='screen' + # ) + # + # load_diffdrive_controller = ExecuteProcess( + # condition=IfCondition(use_simulator), + # cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + # 'diffdrive_controller'], + # output='screen' + # ) + spawn_model = Node( condition=IfCondition(use_simulator), package='ros_gz_sim', @@ -150,6 +186,9 @@ def generate_launch_description(): ld.add_action(lidar_bridge) ld.add_action(imu_bridge) ld.add_action(spawn_model) - ld.add_action(load_joint_state_broadcaster) - ld.add_action(load_diffdrive_controller) + ld.add_action(odom_bridge) + ld.add_action(odom_tf_bridge) + ld.add_action(cm_vel_bridge) + # ld.add_action(load_joint_state_broadcaster) + # ld.add_action(load_diffdrive_controller) return ld diff --git a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf index 3dc8b245e4..81d71217b6 100644 --- a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf +++ b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf @@ -360,9 +360,9 @@ - + - + + + + wheel_left_joint + wheel_right_joint + 0.287 + 0.033 + 1 + -1 + 2 + -2 + 0.46 + -0.46 + 1.9 + -1.9 + /cmd_vel + /odom + tf + odom + base_footprint + 25 100 100 + 10 From 41d7f4e6df102fe01b2f953d0269e0a93ccf35fc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 24 Jul 2023 14:01:55 +0200 Subject: [PATCH 16/26] update dependency MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_bringup/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 4c69255526..1821d1260b 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -16,7 +16,7 @@ launch_ros diff_drive_controller - ign_ros2_control + gz_ros2_control joint_state_broadcaster launch_ros navigation2 From 9a36ecd131722460636a65d1fc8a1fffef7eac94 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 24 Jul 2023 17:41:25 +0200 Subject: [PATCH 17/26] Removed dependency MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_bringup/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 1821d1260b..8bad693f51 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -16,7 +16,6 @@ launch_ros diff_drive_controller - gz_ros2_control joint_state_broadcaster launch_ros navigation2 From 564cf39e8e811a15d69218e4f074eaa8e16b0f9a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 5 Oct 2023 23:12:12 +0200 Subject: [PATCH 18/26] Removed ros2_control and use ogre MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_bringup/launch/tb3_gz_robot_launch.py | 28 ++------ nav2_bringup/params/control.yaml | 78 --------------------- nav2_bringup/urdf/gz_turtlebot3_waffle.urdf | 31 +------- nav2_bringup/worlds/gz_world_only.model | 4 +- 4 files changed, 10 insertions(+), 131 deletions(-) delete mode 100644 nav2_bringup/params/control.yaml diff --git a/nav2_bringup/launch/tb3_gz_robot_launch.py b/nav2_bringup/launch/tb3_gz_robot_launch.py index e36b830ec1..de83526ecb 100644 --- a/nav2_bringup/launch/tb3_gz_robot_launch.py +++ b/nav2_bringup/launch/tb3_gz_robot_launch.py @@ -73,7 +73,7 @@ def generate_launch_description(): parameters=[{ 'use_sim_time': use_sim_time }], - arguments=['/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + arguments=['/clock' + '@rosgraph_msgs/msg/Clock' + '[gz.msgs.Clock'] ) lidar_bridge = Node( @@ -85,7 +85,7 @@ def generate_launch_description(): parameters=[{ 'use_sim_time': use_sim_time }], - arguments=[['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan']], + arguments=[['/scan' + '@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan']], ) imu_bridge = Node( @@ -97,7 +97,7 @@ def generate_launch_description(): parameters=[{ 'use_sim_time': use_sim_time }], - arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[gz.msgs.IMU']], ) odom_bridge = Node( @@ -109,7 +109,7 @@ def generate_launch_description(): parameters=[{ 'use_sim_time': use_sim_time }], - arguments=[['/odom' + '@nav_msgs/msg/Odometry[ignition.msgs.Odometry']], + arguments=[['/odom' + '@nav_msgs/msg/Odometry[gz.msgs.Odometry']], ) odom_tf_bridge = Node( @@ -121,7 +121,7 @@ def generate_launch_description(): parameters=[{ 'use_sim_time': use_sim_time }], - arguments=[['/tf' + '@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V']], + arguments=[['/tf' + '@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V']], ) cm_vel_bridge = Node( @@ -133,23 +133,9 @@ def generate_launch_description(): parameters=[{ 'use_sim_time': use_sim_time }], - arguments=[['/cmd_vel' + '@geometry_msgs/msg/Twist]ignition.msgs.Twist']], + arguments=[['/cmd_vel' + '@geometry_msgs/msg/Twist]gz.msgs.Twist']], ) - # load_joint_state_broadcaster = ExecuteProcess( - # condition=IfCondition(use_simulator), - # cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - # 'joint_state_broadcaster'], - # output='screen' - # ) - # - # load_diffdrive_controller = ExecuteProcess( - # condition=IfCondition(use_simulator), - # cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - # 'diffdrive_controller'], - # output='screen' - # ) - spawn_model = Node( condition=IfCondition(use_simulator), package='ros_gz_sim', @@ -189,6 +175,4 @@ def generate_launch_description(): ld.add_action(odom_bridge) ld.add_action(odom_tf_bridge) ld.add_action(cm_vel_bridge) - # ld.add_action(load_joint_state_broadcaster) - # ld.add_action(load_diffdrive_controller) return ld diff --git a/nav2_bringup/params/control.yaml b/nav2_bringup/params/control.yaml deleted file mode 100644 index a41ec798b7..0000000000 --- a/nav2_bringup/params/control.yaml +++ /dev/null @@ -1,78 +0,0 @@ -/**: - controller_manager: - ros__parameters: - update_rate: 100 # 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 : [1.0e-05, 1.0e-05, 1000000000000.0, 1000000000000.0, 1000000000000.0, 0.001] - twist_covariance_diagonal: [1.0e-05, 1.0e-05, 1000000000000.0, 1000000000000.0, 1000000000000.0, 0.001] - - 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/urdf/gz_turtlebot3_waffle.urdf b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf index 81d71217b6..525a6df333 100644 --- a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf +++ b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf @@ -360,34 +360,7 @@ - - - - @@ -421,8 +394,8 @@ true true true - 50 - 50 + 25 + 25 diff --git a/nav2_bringup/worlds/gz_world_only.model b/nav2_bringup/worlds/gz_world_only.model index f2919e8d9c..1b13396c62 100644 --- a/nav2_bringup/worlds/gz_world_only.model +++ b/nav2_bringup/worlds/gz_world_only.model @@ -16,7 +16,7 @@ - ogre2 + ogre docked - ogre2 + ogre scene 0.4 0.4 0.4 0.8 0.8 0.8 From ddb760dac89bdbc0e5ab147dfead3cb9a358abcc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 14 Nov 2023 12:47:21 +0100 Subject: [PATCH 19/26] Use param file to configure bridge MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_bringup/launch/tb3_gz_robot_launch.py | 96 +++---------------- .../launch/tb3_gz_simulation_launch.py | 4 +- .../params/turtlebot3_waffle_bridge.yaml | 60 ++++++++++++ nav2_bringup/worlds/gz_world_only.model | 4 +- 4 files changed, 78 insertions(+), 86 deletions(-) create mode 100644 nav2_bringup/params/turtlebot3_waffle_bridge.yaml diff --git a/nav2_bringup/launch/tb3_gz_robot_launch.py b/nav2_bringup/launch/tb3_gz_robot_launch.py index de83526ecb..a5537f66bf 100644 --- a/nav2_bringup/launch/tb3_gz_robot_launch.py +++ b/nav2_bringup/launch/tb3_gz_robot_launch.py @@ -29,7 +29,6 @@ def generate_launch_description(): namespace = LaunchConfiguration('namespace') use_simulator = LaunchConfiguration('use_simulator') - use_sim_time = LaunchConfiguration('use_sim_time') robot_name = LaunchConfiguration('robot_name') robot_sdf = LaunchConfiguration('robot_sdf') pose = {'x': LaunchConfiguration('x_pose', default='-2.00'), @@ -50,11 +49,6 @@ def generate_launch_description(): default_value='True', description='Whether to start the simulator') - declare_use_sim_time_cmd = DeclareLaunchArgument( - 'use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - declare_robot_name_cmd = DeclareLaunchArgument( 'robot_name', default_value='turtlebot3_waffle', @@ -65,75 +59,19 @@ def generate_launch_description(): 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') - clock_bridge = Node( - condition=IfCondition(use_simulator), - package='ros_gz_bridge', executable='parameter_bridge', - name='clock_bridge', - output='screen', - parameters=[{ - 'use_sim_time': use_sim_time - }], - arguments=['/clock' + '@rosgraph_msgs/msg/Clock' + '[gz.msgs.Clock'] - ) - - lidar_bridge = Node( - condition=IfCondition(use_simulator), - package='ros_gz_bridge', - executable='parameter_bridge', - name='lidar_bridge', - output='screen', - parameters=[{ - 'use_sim_time': use_sim_time - }], - arguments=[['/scan' + '@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan']], - ) - - imu_bridge = Node( - condition=IfCondition(use_simulator), - package='ros_gz_bridge', - executable='parameter_bridge', - name='imu_bridge', - output='screen', - parameters=[{ - 'use_sim_time': use_sim_time - }], - arguments=[['/imu' + '@sensor_msgs/msg/Imu[gz.msgs.IMU']], - ) - - odom_bridge = Node( - condition=IfCondition(use_simulator), - package='ros_gz_bridge', - executable='parameter_bridge', - name='odom_bridge', - output='screen', - parameters=[{ - 'use_sim_time': use_sim_time - }], - arguments=[['/odom' + '@nav_msgs/msg/Odometry[gz.msgs.Odometry']], - ) - - odom_tf_bridge = Node( - condition=IfCondition(use_simulator), - package='ros_gz_bridge', - executable='parameter_bridge', - name='odom_tf_bridge', - output='screen', - parameters=[{ - 'use_sim_time': use_sim_time - }], - arguments=[['/tf' + '@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V']], - ) - - cm_vel_bridge = Node( - condition=IfCondition(use_simulator), - package='ros_gz_bridge', - executable='parameter_bridge', - name='cm_vel_bridge', - output='screen', - parameters=[{ - 'use_sim_time': use_sim_time - }], - arguments=[['/cmd_vel' + '@geometry_msgs/msg/Twist]gz.msgs.Twist']], + pkg_nav2_bringup = get_package_share_directory("nav2_bringup") + bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + parameters=[ + { + "config_file": os.path.join( + pkg_nav2_bringup, "params", "turtlebot3_waffle_bridge.yaml" + ), + "qos_overrides./tf_static.publisher.durability": "transient_local", + } + ], + output="screen", ) spawn_model = Node( @@ -164,15 +102,9 @@ def generate_launch_description(): ld.add_action(declare_robot_name_cmd) ld.add_action(declare_robot_sdf_cmd) ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) ld.add_action(set_env_vars_resources) - ld.add_action(clock_bridge) - ld.add_action(lidar_bridge) - ld.add_action(imu_bridge) + ld.add_action(bridge) ld.add_action(spawn_model) - ld.add_action(odom_bridge) - ld.add_action(odom_tf_bridge) - ld.add_action(cm_vel_bridge) return ld diff --git a/nav2_bringup/launch/tb3_gz_simulation_launch.py b/nav2_bringup/launch/tb3_gz_simulation_launch.py index 4208f12877..644b6cbb5a 100644 --- a/nav2_bringup/launch/tb3_gz_simulation_launch.py +++ b/nav2_bringup/launch/tb3_gz_simulation_launch.py @@ -96,7 +96,7 @@ def generate_launch_description(): declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', - default_value='true', + default_value='True', description='Use simulation (Gazebo) clock if true') declare_params_file_cmd = DeclareLaunchArgument( @@ -105,7 +105,7 @@ def generate_launch_description(): description='Full path to the ROS2 parameters file to use for all launched nodes') declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='true', + 'autostart', default_value='True', description='Automatically startup the nav2 stack') declare_use_composition_cmd = DeclareLaunchArgument( diff --git a/nav2_bringup/params/turtlebot3_waffle_bridge.yaml b/nav2_bringup/params/turtlebot3_waffle_bridge.yaml new file mode 100644 index 0000000000..9dbbcff33c --- /dev/null +++ b/nav2_bringup/params/turtlebot3_waffle_bridge.yaml @@ -0,0 +1,60 @@ +# replace clock_bridge +- ros_topic_name: "clock" + gz_topic_name: "/clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "gz.msgs.Clock" + direction: GZ_TO_ROS + +# no equivalent in TB3 - add +- ros_topic_name: "joint_states" + gz_topic_name: "/world/default/model/turtlebot3_waffle/joint_state" + ros_type_name: "sensor_msgs/msg/JointState" + gz_type_name: "gz.msgs.Model" + direction: GZ_TO_ROS + +# replace odom_bridge - check gz topic name +# gz topic published by DiffDrive plugin +- ros_topic_name: "odom" + gz_topic_name: "/odom" + ros_type_name: "nav_msgs/msg/Odometry" + gz_type_name: "gz.msgs.Odometry" + direction: GZ_TO_ROS + +# replace odom_tf_bridge - check gz and ros topic names +# gz topic published by DiffDrive plugin +# prefix ros2 topic with gz +- ros_topic_name: "tf" + gz_topic_name: "/tf" + ros_type_name: "tf2_msgs/msg/TFMessage" + gz_type_name: "gz.msgs.Pose_V" + direction: GZ_TO_ROS + +# no equivalent in TB3 - add +# gz topic published by PosePublisher +# prefix ros2 topic with gz +- ros_topic_name: "tf_static" + gz_topic_name: "/model/turtlebot3_waffle/pose_static" + ros_type_name: "tf2_msgs/msg/TFMessage" + gz_type_name: "gz.msgs.Pose_V" + direction: GZ_TO_ROS + +# replace imu_bridge - check gz topic name +- ros_topic_name: "imu" + gz_topic_name: "/imu" + ros_type_name: "sensor_msgs/msg/Imu" + gz_type_name: "gz.msgs.IMU" + direction: GZ_TO_ROS + +# replace lidar_bridge +- ros_topic_name: "scan" + gz_topic_name: "/scan" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "gz.msgs.LaserScan" + direction: GZ_TO_ROS + +# replace cmd_vel_bridge +- ros_topic_name: "cmd_vel" + gz_topic_name: "/cmd_vel" + ros_type_name: "geometry_msgs/msg/Twist" + gz_type_name: "gz.msgs.Twist" + direction: ROS_TO_GZ diff --git a/nav2_bringup/worlds/gz_world_only.model b/nav2_bringup/worlds/gz_world_only.model index 1b13396c62..f2919e8d9c 100644 --- a/nav2_bringup/worlds/gz_world_only.model +++ b/nav2_bringup/worlds/gz_world_only.model @@ -16,7 +16,7 @@ - ogre + ogre2 docked - ogre + ogre2 scene 0.4 0.4 0.4 0.8 0.8 0.8 From 076f1f02b23a993ae55c2e8b43b219db72317c7e Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 15 Dec 2023 15:11:59 -0600 Subject: [PATCH 20/26] Use sdf model and change to ogre instead of ogre2 for sensors Signed-off-by: Addisu Z. Taddese --- .../launch/tb3_gz_simulation_launch.py | 11 +- nav2_bringup/launch/tb3_simulation_launch.py | 3 +- nav2_bringup/worlds/gz_waffle.model | 479 ++++++++++++++++++ nav2_bringup/worlds/gz_world_only.model | 145 +----- 4 files changed, 490 insertions(+), 148 deletions(-) create mode 100644 nav2_bringup/worlds/gz_waffle.model diff --git a/nav2_bringup/launch/tb3_gz_simulation_launch.py b/nav2_bringup/launch/tb3_gz_simulation_launch.py index 644b6cbb5a..1bdc448e73 100644 --- a/nav2_bringup/launch/tb3_gz_simulation_launch.py +++ b/nav2_bringup/launch/tb3_gz_simulation_launch.py @@ -158,9 +158,13 @@ def generate_launch_description(): declare_robot_sdf_cmd = DeclareLaunchArgument( 'robot_sdf', - default_value=os.path.join(bringup_dir, 'urdf', 'gz_turtlebot3_waffle.urdf'), + default_value=os.path.join(bringup_dir, 'worlds', 'gz_waffle.model'), description='Full path to robot sdf file to spawn the robot in gazebo') + 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', @@ -168,8 +172,9 @@ def generate_launch_description(): name='robot_state_publisher', namespace=namespace, output='screen', - parameters=[{'use_sim_time': use_sim_time, - 'robot_description': Command(['xacro', ' ', robot_sdf])}], + parameters=[ + {'use_sim_time': use_sim_time, 'robot_description': robot_description} + ], remappings=remappings) rviz_cmd = IncludeLaunchDescription( diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index 6598ac94ba..d428d32cd8 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -176,6 +176,7 @@ def generate_launch_description(): condition=IfCondition(use_simulator), cmd=[ 'gzserver', + '--verbose', '-s', 'libgazebo_ros_init.so', '-s', @@ -292,6 +293,6 @@ def generate_launch_description(): # 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) + # ld.add_action(bringup_cmd) return ld diff --git a/nav2_bringup/worlds/gz_waffle.model b/nav2_bringup/worlds/gz_waffle.model new file mode 100644 index 0000000000..baa5442f51 --- /dev/null +++ b/nav2_bringup/worlds/gz_waffle.model @@ -0,0 +1,479 @@ + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + + + + + -0.064 0 0.048 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 1.0 + + + + -0.064 0 0.048 0 0 0 + + + 0.265 0.265 0.089 + + + + + + -0.064 0 0 0 0 0 + + + model://turtlebot3_common/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + + + + + 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 + + + + + + + + + + -0.064 0 0.121 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.125 + + + + -0.052 0 0.111 0 0 0 + + + 0.0508 + 0.055 + + + + + + -0.064 0 0.121 0 0 0 + + + model://turtlebot3_common/meshes/lds.dae + 0.001 0.001 0.001 + + + + + + true + true + -0.064 0 0.15 0 0 0 + 5 + scan + base_scan + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + 0.00001 + 20.0 + 0.015000 + + + gaussian + 0.0 + 0.01 + + + + + + + + + 0.0 0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 0.144 0.023 0 0 0 + + + model://turtlebot3_common/meshes/tire.dae + 0.001 0.001 0.001 + + + + + + + + + 0.0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 -0.144 0.023 0 0 0 + + + model://turtlebot3_common/meshes/tire.dae + 0.001 0.001 0.001 + + + + + + + -0.177 -0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + -0.177 0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + + 0.069 -0.047 0.107 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.035 + + + 0 0.047 -0.005 0 0 0 + + + 0.008 0.130 0.022 + + + + + 0.069 -0.047 0.107 0 0 0 + + + 1 + 5 + 0.064 -0.047 0.107 0 0 0 + camera_depth_frame + + 1.047 + + 320 + 240 + + + + + + 19.4 + + + + + 0.001 + 5.0 + + + + + + + + base_footprint + base_link + 0.0 0.0 0.010 0 0 0 + + + + base_link + wheel_left_link + 0.0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + wheel_right_link + 0.0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + caster_back_right_link + + + + base_link + caster_back_left_link + + + + base_link + base_scan + -0.064 0 0.121 0 0 0 + + 0 0 1 + + + + + base_link + camera_link + 0.064 -0.065 0.094 0 0 0 + + 0 0 1 + + + + + + wheel_left_joint + wheel_right_joint + 0.287 + 0.033 + 1 + -1 + 2 + -2 + 0.46 + -0.46 + 1.9 + -1.9 + /cmd_vel + /odom + tf + odom + base_footprint + 30 + + + + wheel_left_joint + wheel_right_joint + joint_states + + + + diff --git a/nav2_bringup/worlds/gz_world_only.model b/nav2_bringup/worlds/gz_world_only.model index f2919e8d9c..fc5b0d9889 100644 --- a/nav2_bringup/worlds/gz_world_only.model +++ b/nav2_bringup/worlds/gz_world_only.model @@ -9,159 +9,16 @@ filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands"> - - - ogre2 + ogre - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 - - 0.25 - 50 - - - - - - - floating - 5 - 5 - false - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - - - - - - - - false - 5 - 5 - floating - false - - - - - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - true - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - - 0 0 0 10 0 0 0 From b2deaee4e39e7d7e25ad39371d910b7a3f2c1343 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 20 Dec 2023 16:43:07 -0600 Subject: [PATCH 21/26] Support Garden and later, performance improvements Signed-off-by: Addisu Z. Taddese --- nav2_bringup/launch/tb3_gz_robot_launch.py | 17 +- .../launch/tb3_gz_simulation_launch.py | 177 ++++++++++-------- nav2_bringup/launch/tb3_simulation_launch.py | 4 +- .../params/turtlebot3_waffle_bridge.yaml | 2 +- nav2_bringup/worlds/gz_sim_server_config.xml | 10 + ...gz_sim_server_scene_broadcaster_config.xml | 11 ++ .../worlds/{gz_waffle.model => gz_waffle.sdf} | 30 +-- nav2_bringup/worlds/gz_world_only.model | 20 -- 8 files changed, 147 insertions(+), 124 deletions(-) create mode 100644 nav2_bringup/worlds/gz_sim_server_config.xml create mode 100644 nav2_bringup/worlds/gz_sim_server_scene_broadcaster_config.xml rename nav2_bringup/worlds/{gz_waffle.model => gz_waffle.sdf} (95%) diff --git a/nav2_bringup/launch/tb3_gz_robot_launch.py b/nav2_bringup/launch/tb3_gz_robot_launch.py index a5537f66bf..8428252fcc 100644 --- a/nav2_bringup/launch/tb3_gz_robot_launch.py +++ b/nav2_bringup/launch/tb3_gz_robot_launch.py @@ -18,7 +18,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess -from launch.actions import SetEnvironmentVariable +from launch.actions import AppendEnvironmentVariable from launch.conditions import IfCondition from launch.substitutions import Command, LaunchConfiguration from launch_ros.actions import Node @@ -56,7 +56,7 @@ def generate_launch_description(): declare_robot_sdf_cmd = DeclareLaunchArgument( 'robot_sdf', - default_value=os.path.join(bringup_dir, 'urdf', 'gz_turtlebot3_waffle.urdf'), + default_value=os.path.join(bringup_dir, 'worlds', 'gz_waffle.sdf'), description='Full path to robot sdf file to spawn the robot in gazebo') pkg_nav2_bringup = get_package_share_directory("nav2_bringup") @@ -81,20 +81,15 @@ def generate_launch_description(): output='screen', arguments=[ '-entity', robot_name, - '-string', Command(['xacro', ' ', robot_sdf]), + '-file', robot_sdf, '-robot_namespace', namespace, '-x', pose['x'], '-y', pose['y'], '-z', pose['z'], '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']] ) - env_vars = os.getenv('IGN_GAZEBO_RESOURCE_PATH', default="") - env_vars += ':' + \ - os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'models') \ - + ':' + \ - os.path.join(get_package_share_directory( - 'turtlebot3_gazebo'), '..') - - set_env_vars_resources = SetEnvironmentVariable('IGN_GAZEBO_RESOURCE_PATH', env_vars) + set_env_vars_resources = AppendEnvironmentVariable('GZ_SIM_RESOURCE_PATH', + os.path.join(get_package_share_directory('turtlebot3_gazebo'), + 'models')) # Create the launch description and populate ld = LaunchDescription() diff --git a/nav2_bringup/launch/tb3_gz_simulation_launch.py b/nav2_bringup/launch/tb3_gz_simulation_launch.py index 1bdc448e73..638dc93473 100644 --- a/nav2_bringup/launch/tb3_gz_simulation_launch.py +++ b/nav2_bringup/launch/tb3_gz_simulation_launch.py @@ -19,14 +19,17 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import IncludeLaunchDescription -from launch.conditions import IfCondition +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) +from launch.conditions import IfCondition, UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution -from launch.substitutions import PythonExpression +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import Command from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare def generate_launch_description(): @@ -54,12 +57,14 @@ def generate_launch_description(): 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')} + 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') @@ -69,78 +74,84 @@ def generate_launch_description(): # 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')] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + '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') + description='Whether to apply a namespace to the navigation stack', + ) declare_slam_cmd = DeclareLaunchArgument( - 'slam', - default_value='False', - description='Whether run a SLAM') + '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') + 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') + 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') + 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') + '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') + '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.') + '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') + 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') + 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') + description='Whether to start the robot state publisher', + ) declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='True', - description='Whether to execute gzclient)') + 'headless', default_value='True', description='Whether to execute gzclient)' + ) declare_world_cmd = DeclareLaunchArgument( 'world', @@ -149,17 +160,18 @@ def generate_launch_description(): # 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') + 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') + '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, 'worlds', 'gz_waffle.model'), - description='Full path to robot sdf file to spawn the robot in gazebo') + default_value=os.path.join(bringup_dir, 'worlds', 'gz_waffle.sdf'), + description='Full path to robot sdf file to spawn the robot in gazebo', + ) urdf = os.path.join(bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') with open(urdf, 'r') as infp: @@ -175,36 +187,47 @@ def generate_launch_description(): parameters=[ {'use_sim_time': use_sim_time, 'robot_description': robot_description} ], - remappings=remappings) + remappings=remappings, + ) rviz_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'rviz_launch.py')), + 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()) + 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()) + 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(), + ) + + gz_sim_server_config = SetEnvironmentVariable('GZ_SIM_SERVER_CONFIG_PATH', + os.path.join(bringup_dir, 'worlds', 'gz_sim_server_config.xml'), + condition=IfCondition(headless)) + + gz_start_scene_broadcaster = SetEnvironmentVariable('GZ_SIM_SERVER_CONFIG_PATH', + os.path.join(bringup_dir, 'worlds', 'gz_sim_server_scene_broadcaster_config.xml'), + condition=UnlessCondition(headless)) gazebo_server = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare('ros_gz_sim'), + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', - 'gz_sim.launch.py' - ]) + 'gz_sim.launch.py') ), launch_arguments={'gz_args': ['-r -s ', world]}.items(), condition=IfCondition(use_simulator) @@ -212,15 +235,13 @@ def generate_launch_description(): gazebo_client = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare('ros_gz_sim'), + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', - 'gz_sim.launch.py' - ]) + 'gz_sim.launch.py') ), condition=IfCondition(PythonExpression( [use_simulator, ' and not ', headless])), - launch_arguments={'gz_args': ['-g ']}.items(), + launch_arguments={'gz_args': ['-v4 -g ']}.items(), ) gz_robot = IncludeLaunchDescription( @@ -261,13 +282,15 @@ def generate_launch_description(): ld.add_action(declare_robot_sdf_cmd) ld.add_action(declare_use_respawn_cmd) - ld.add_action(start_robot_state_publisher_cmd) ld.add_action(gz_robot) + ld.add_action(gz_sim_server_config) + ld.add_action(gz_start_scene_broadcaster) ld.add_action(gazebo_server) ld.add_action(gazebo_client) # 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) diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index d428d32cd8..cf6ba2c11c 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -181,6 +181,8 @@ def generate_launch_description(): 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', + '-e', + 'dart', world, ], cwd=[launch_dir], @@ -293,6 +295,6 @@ def generate_launch_description(): # 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) + ld.add_action(bringup_cmd) return ld diff --git a/nav2_bringup/params/turtlebot3_waffle_bridge.yaml b/nav2_bringup/params/turtlebot3_waffle_bridge.yaml index 9dbbcff33c..4c5b5018ec 100644 --- a/nav2_bringup/params/turtlebot3_waffle_bridge.yaml +++ b/nav2_bringup/params/turtlebot3_waffle_bridge.yaml @@ -7,7 +7,7 @@ # no equivalent in TB3 - add - ros_topic_name: "joint_states" - gz_topic_name: "/world/default/model/turtlebot3_waffle/joint_state" + gz_topic_name: "joint_states" ros_type_name: "sensor_msgs/msg/JointState" gz_type_name: "gz.msgs.Model" direction: GZ_TO_ROS diff --git a/nav2_bringup/worlds/gz_sim_server_config.xml b/nav2_bringup/worlds/gz_sim_server_config.xml new file mode 100644 index 0000000000..55447f7720 --- /dev/null +++ b/nav2_bringup/worlds/gz_sim_server_config.xml @@ -0,0 +1,10 @@ + + + + + + ogre + + + + diff --git a/nav2_bringup/worlds/gz_sim_server_scene_broadcaster_config.xml b/nav2_bringup/worlds/gz_sim_server_scene_broadcaster_config.xml new file mode 100644 index 0000000000..4d9b75cae7 --- /dev/null +++ b/nav2_bringup/worlds/gz_sim_server_scene_broadcaster_config.xml @@ -0,0 +1,11 @@ + + + + + + + ogre + + + + diff --git a/nav2_bringup/worlds/gz_waffle.model b/nav2_bringup/worlds/gz_waffle.sdf similarity index 95% rename from nav2_bringup/worlds/gz_waffle.model rename to nav2_bringup/worlds/gz_waffle.sdf index baa5442f51..32f202cacb 100644 --- a/nav2_bringup/worlds/gz_waffle.model +++ b/nav2_bringup/worlds/gz_waffle.sdf @@ -46,6 +46,7 @@ 200 imu imu_link + imu_link @@ -132,6 +133,7 @@ 5 scan base_scan + base_scan @@ -179,14 +181,13 @@ - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 + 1 + 1 + 0.035 + 0 + 0 0 1 @@ -237,14 +238,13 @@ - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 + 1 + 1 + 0.035 + 0 + 0 0 1 @@ -368,6 +368,7 @@ 5 0.064 -0.047 0.107 0 0 0 camera_depth_frame + camera_depth_frame 1.047 @@ -445,7 +446,7 @@ wheel_left_joint wheel_right_joint @@ -468,11 +469,12 @@ wheel_left_joint wheel_right_joint joint_states + 30 diff --git a/nav2_bringup/worlds/gz_world_only.model b/nav2_bringup/worlds/gz_world_only.model index fc5b0d9889..b6c5b1acb0 100644 --- a/nav2_bringup/worlds/gz_world_only.model +++ b/nav2_bringup/worlds/gz_world_only.model @@ -1,24 +1,6 @@ - - - - - - ogre - - - - 0 0 0 10 0 0 0 @@ -62,8 +44,6 @@ 0 - false - false From 1cc14742c1ea4ea23dfefe3e81af71a222ab314b Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 15 Feb 2024 15:18:46 -0600 Subject: [PATCH 22/26] Use xacro for world file (#2) Signed-off-by: Addisu Z. Taddese --- nav2_bringup/launch/tb3_gz_robot_launch.py | 26 +++++---- .../launch/tb3_gz_simulation_launch.py | 55 ++++++++++--------- nav2_bringup/launch/tb3_simulation_launch.py | 3 - nav2_bringup/urdf/gz_turtlebot3_waffle.urdf | 10 ++-- nav2_bringup/worlds/gz_sim_server_config.xml | 10 ---- ...gz_sim_server_scene_broadcaster_config.xml | 11 ---- nav2_bringup/worlds/gz_waffle.sdf | 15 ++++- ...rld_only.model => gz_world_only.sdf.xacro} | 28 +++++++++- 8 files changed, 88 insertions(+), 70 deletions(-) delete mode 100644 nav2_bringup/worlds/gz_sim_server_config.xml delete mode 100644 nav2_bringup/worlds/gz_sim_server_scene_broadcaster_config.xml rename nav2_bringup/worlds/{gz_world_only.model => gz_world_only.sdf.xacro} (70%) diff --git a/nav2_bringup/launch/tb3_gz_robot_launch.py b/nav2_bringup/launch/tb3_gz_robot_launch.py index 8428252fcc..24fa9e1d6a 100644 --- a/nav2_bringup/launch/tb3_gz_robot_launch.py +++ b/nav2_bringup/launch/tb3_gz_robot_launch.py @@ -17,10 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess from launch.actions import AppendEnvironmentVariable +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.substitutions import Command, LaunchConfiguration +from launch.substitutions import LaunchConfiguration + from launch_ros.actions import Node @@ -59,19 +60,19 @@ def generate_launch_description(): default_value=os.path.join(bringup_dir, 'worlds', 'gz_waffle.sdf'), description='Full path to robot sdf file to spawn the robot in gazebo') - pkg_nav2_bringup = get_package_share_directory("nav2_bringup") + pkg_nav2_bringup = get_package_share_directory('nav2_bringup') bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", + package='ros_gz_bridge', + executable='parameter_bridge', parameters=[ { - "config_file": os.path.join( - pkg_nav2_bringup, "params", "turtlebot3_waffle_bridge.yaml" + 'config_file': os.path.join( + pkg_nav2_bringup, 'params', 'turtlebot3_waffle_bridge.yaml' ), - "qos_overrides./tf_static.publisher.durability": "transient_local", + 'qos_overrides./tf_static.publisher.durability': 'transient_local', } ], - output="screen", + output='screen', ) spawn_model = Node( @@ -87,9 +88,10 @@ def generate_launch_description(): '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']] ) - set_env_vars_resources = AppendEnvironmentVariable('GZ_SIM_RESOURCE_PATH', - os.path.join(get_package_share_directory('turtlebot3_gazebo'), - 'models')) + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(get_package_share_directory('turtlebot3_gazebo'), + 'models')) # Create the launch description and populate ld = LaunchDescription() diff --git a/nav2_bringup/launch/tb3_gz_simulation_launch.py b/nav2_bringup/launch/tb3_gz_simulation_launch.py index 638dc93473..9aca1a5351 100644 --- a/nav2_bringup/launch/tb3_gz_simulation_launch.py +++ b/nav2_bringup/launch/tb3_gz_simulation_launch.py @@ -15,20 +15,24 @@ """This is all-in-one launch script intended for use by nav2 developers.""" import os +import tempfile from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( + AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, - SetEnvironmentVariable, + OpaqueFunction, + RegisterEventHandler, ) -from launch.conditions import IfCondition, UnlessCondition +from launch.conditions import IfCondition +from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression -from launch.substitutions import Command + from launch_ros.actions import Node @@ -159,7 +163,7 @@ def generate_launch_description(): # 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'), + default_value=os.path.join(bringup_dir, 'worlds', 'gz_world_only.sdf.xacro'), description='Full path to world model file to load', ) @@ -215,29 +219,30 @@ def generate_launch_description(): }.items(), ) - gz_sim_server_config = SetEnvironmentVariable('GZ_SIM_SERVER_CONFIG_PATH', - os.path.join(bringup_dir, 'worlds', 'gz_sim_server_config.xml'), - condition=IfCondition(headless)) - - gz_start_scene_broadcaster = SetEnvironmentVariable('GZ_SIM_SERVER_CONFIG_PATH', - os.path.join(bringup_dir, 'worlds', 'gz_sim_server_scene_broadcaster_config.xml'), - condition=UnlessCondition(headless)) - + world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') + world_sdf_xacro = ExecuteProcess( + cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world]) gazebo_server = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('ros_gz_sim'), - 'launch', - 'gz_sim.launch.py') - ), - launch_arguments={'gz_args': ['-r -s ', world]}.items(), - condition=IfCondition(use_simulator) - ) - + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', + 'gz_sim.launch.py')), + launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items(), + condition=IfCondition(use_simulator)) + + remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( + on_shutdown=[ + OpaqueFunction(function=lambda context: os.remove(world_sdf)) + ])) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(get_package_share_directory('turtlebot3_gazebo'), + 'models')) gazebo_client = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(get_package_share_directory('ros_gz_sim'), - 'launch', - 'gz_sim.launch.py') + 'launch', + 'gz_sim.launch.py') ), condition=IfCondition(PythonExpression( [use_simulator, ' and not ', headless])), @@ -282,10 +287,10 @@ def generate_launch_description(): ld.add_action(declare_robot_sdf_cmd) ld.add_action(declare_use_respawn_cmd) - + ld.add_action(set_env_vars_resources) + ld.add_action(world_sdf_xacro) + ld.add_action(remove_temp_sdf_file) ld.add_action(gz_robot) - ld.add_action(gz_sim_server_config) - ld.add_action(gz_start_scene_broadcaster) ld.add_action(gazebo_server) ld.add_action(gazebo_client) diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index cf6ba2c11c..6598ac94ba 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -176,13 +176,10 @@ def generate_launch_description(): condition=IfCondition(use_simulator), cmd=[ 'gzserver', - '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', - '-e', - 'dart', world, ], cwd=[launch_dir], diff --git a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf index 525a6df333..aaafe2579c 100644 --- a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf +++ b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf @@ -277,7 +277,7 @@ " scan - base_scan + base_scan -0.064 0 0.15 0 0 0 5 @@ -314,7 +314,7 @@ true 200 imu - imu_link + imu_link @@ -362,7 +362,7 @@ wheel_left_joint wheel_right_joint @@ -385,8 +385,8 @@ + filename="gz-sim-pose-publisher-system" + name="gz::sim::systems::PosePublisher"> true true false diff --git a/nav2_bringup/worlds/gz_sim_server_config.xml b/nav2_bringup/worlds/gz_sim_server_config.xml deleted file mode 100644 index 55447f7720..0000000000 --- a/nav2_bringup/worlds/gz_sim_server_config.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - ogre - - - - diff --git a/nav2_bringup/worlds/gz_sim_server_scene_broadcaster_config.xml b/nav2_bringup/worlds/gz_sim_server_scene_broadcaster_config.xml deleted file mode 100644 index 4d9b75cae7..0000000000 --- a/nav2_bringup/worlds/gz_sim_server_scene_broadcaster_config.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - ogre - - - - diff --git a/nav2_bringup/worlds/gz_waffle.sdf b/nav2_bringup/worlds/gz_waffle.sdf index 32f202cacb..283dc4e4a7 100644 --- a/nav2_bringup/worlds/gz_waffle.sdf +++ b/nav2_bringup/worlds/gz_waffle.sdf @@ -37,6 +37,9 @@ 0.001 0.001 0.001 + + 1 1 1 + @@ -45,7 +48,6 @@ true 200 imu - imu_link imu_link @@ -124,6 +126,9 @@ 0.001 0.001 0.001 + + 1 1 1 + @@ -132,7 +137,6 @@ -0.064 0 0.15 0 0 0 5 scan - base_scan base_scan @@ -211,6 +215,9 @@ 0.001 0.001 0.001 + + 1 1 1 + @@ -268,6 +275,9 @@ 0.001 0.001 0.001 + + 1 1 1 + @@ -367,7 +377,6 @@ 1 5 0.064 -0.047 0.107 0 0 0 - camera_depth_frame camera_depth_frame 1.047 diff --git a/nav2_bringup/worlds/gz_world_only.model b/nav2_bringup/worlds/gz_world_only.sdf.xacro similarity index 70% rename from nav2_bringup/worlds/gz_world_only.model rename to nav2_bringup/worlds/gz_world_only.sdf.xacro index b6c5b1acb0..11026d1c67 100644 --- a/nav2_bringup/worlds/gz_world_only.model +++ b/nav2_bringup/worlds/gz_world_only.sdf.xacro @@ -1,6 +1,32 @@ - + + + + + + + + + + + + + ogre + + + + 0 0 0 10 0 0 0 From 6a462beb7b5d51992a66490739c10eb2fc3f2134 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Sat, 17 Feb 2024 10:25:53 -0600 Subject: [PATCH 23/26] Reviewer feedback Signed-off-by: Addisu Z. Taddese --- nav2_bringup/launch/tb3_gz_robot_launch.py | 1 - nav2_bringup/package.xml | 1 + .../params/turtlebot3_waffle_bridge.yaml | 9 - nav2_bringup/urdf/gz_turtlebot3_waffle.urdf | 402 ------------------ 4 files changed, 1 insertion(+), 412 deletions(-) delete mode 100644 nav2_bringup/urdf/gz_turtlebot3_waffle.urdf diff --git a/nav2_bringup/launch/tb3_gz_robot_launch.py b/nav2_bringup/launch/tb3_gz_robot_launch.py index 24fa9e1d6a..dbc4f19ddc 100644 --- a/nav2_bringup/launch/tb3_gz_robot_launch.py +++ b/nav2_bringup/launch/tb3_gz_robot_launch.py @@ -69,7 +69,6 @@ def generate_launch_description(): 'config_file': os.path.join( pkg_nav2_bringup, 'params', 'turtlebot3_waffle_bridge.yaml' ), - 'qos_overrides./tf_static.publisher.durability': 'transient_local', } ], output='screen', diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 8bad693f51..7862e08cd3 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -24,6 +24,7 @@ ros_gz_sim slam_toolbox turtlebot3_gazebo + xacro ament_lint_common ament_lint_auto diff --git a/nav2_bringup/params/turtlebot3_waffle_bridge.yaml b/nav2_bringup/params/turtlebot3_waffle_bridge.yaml index 4c5b5018ec..843699f8eb 100644 --- a/nav2_bringup/params/turtlebot3_waffle_bridge.yaml +++ b/nav2_bringup/params/turtlebot3_waffle_bridge.yaml @@ -29,15 +29,6 @@ gz_type_name: "gz.msgs.Pose_V" direction: GZ_TO_ROS -# no equivalent in TB3 - add -# gz topic published by PosePublisher -# prefix ros2 topic with gz -- ros_topic_name: "tf_static" - gz_topic_name: "/model/turtlebot3_waffle/pose_static" - ros_type_name: "tf2_msgs/msg/TFMessage" - gz_type_name: "gz.msgs.Pose_V" - direction: GZ_TO_ROS - # replace imu_bridge - check gz topic name - ros_topic_name: "imu" gz_topic_name: "/imu" diff --git a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf b/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf deleted file mode 100644 index aaafe2579c..0000000000 --- a/nav2_bringup/urdf/gz_turtlebot3_waffle.urdf +++ /dev/null @@ -1,402 +0,0 @@ - - - - - - - - - - - - - - 0.4 0.4 0.4 1 - 0.4 0.4 0.4 1 - 0.0 0.0 1.0 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.3 0.3 0.3 1 - 0.3 0.3 0.3 1 - 0.0 0.0 1.0 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.3 0.3 0.3 1 - 0.3 0.3 0.3 1 - 0.0 0.0 1.0 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.3 0.3 0.3 1 - 0.3 0.3 0.3 1 - 0.0 0.0 1.0 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - " - scan - base_scan - -0.064 0 0.15 0 0 0 - 5 - - - - 360 - 1.000000 - 0.000000 - 6.280000 - - - 1 - 0.01 - 0 - 0 - - - - 0.00001 - 20.0 - 0.015000 - - - gaussian - 0.0 - 0.01 - - - - - - - - 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 - - - - - - - - - - wheel_left_joint - wheel_right_joint - 0.287 - 0.033 - 1 - -1 - 2 - -2 - 0.46 - -0.46 - 1.9 - -1.9 - /cmd_vel - /odom - tf - odom - base_footprint - 25 - - - - true - true - false - false - true - true - true - 25 - 25 - - - - From 225f1f3154624d447d80fd7e053aba519a49bb39 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Sat, 17 Feb 2024 10:46:43 -0600 Subject: [PATCH 24/26] Add comment explaining temp file Signed-off-by: Addisu Z. Taddese --- nav2_bringup/launch/tb3_gz_simulation_launch.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/nav2_bringup/launch/tb3_gz_simulation_launch.py b/nav2_bringup/launch/tb3_gz_simulation_launch.py index 9aca1a5351..64c49aded2 100644 --- a/nav2_bringup/launch/tb3_gz_simulation_launch.py +++ b/nav2_bringup/launch/tb3_gz_simulation_launch.py @@ -218,8 +218,12 @@ def generate_launch_description(): 'use_respawn': use_respawn, }.items(), ) - - world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') + # The SDF file for the world is a xacro file because we wanted to + # conditionally load the SceneBroadcaster plugin based on wheter we're + # running in headless mode. But currently, the Gazebo command line doesn't + # take SDF strings for worlds, so the output of xacro needs to be saved into + # a temporary file and passed to Gazebo. + world_sdf = tempfile.mktemp(prefix="nav2_", suffix=".sdf") world_sdf_xacro = ExecuteProcess( cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world]) gazebo_server = IncludeLaunchDescription( @@ -231,7 +235,7 @@ def generate_launch_description(): remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( on_shutdown=[ - OpaqueFunction(function=lambda context: os.remove(world_sdf)) + OpaqueFunction(function=lambda _: os.remove(world_sdf)) ])) set_env_vars_resources = AppendEnvironmentVariable( From 1d8634d96bcb05145bad5cb403f0ea8ca416dce6 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Sat, 17 Feb 2024 11:26:26 -0600 Subject: [PATCH 25/26] Fix linter Signed-off-by: Addisu Z. Taddese --- nav2_bringup/launch/tb3_gz_simulation_launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/launch/tb3_gz_simulation_launch.py b/nav2_bringup/launch/tb3_gz_simulation_launch.py index 64c49aded2..a6147518d9 100644 --- a/nav2_bringup/launch/tb3_gz_simulation_launch.py +++ b/nav2_bringup/launch/tb3_gz_simulation_launch.py @@ -223,7 +223,7 @@ def generate_launch_description(): # running in headless mode. But currently, the Gazebo command line doesn't # take SDF strings for worlds, so the output of xacro needs to be saved into # a temporary file and passed to Gazebo. - world_sdf = tempfile.mktemp(prefix="nav2_", suffix=".sdf") + world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') world_sdf_xacro = ExecuteProcess( cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world]) gazebo_server = IncludeLaunchDescription( From cc5bde0a02acc0265c177349e3ec45150798c962 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 28 May 2024 15:59:10 -0700 Subject: [PATCH 26/26] Update nav2_bringup/worlds/gz_world_only.sdf.xacro Co-authored-by: Addisu Z. Taddese Signed-off-by: Steve Macenski --- nav2_bringup/worlds/gz_world_only.sdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/worlds/gz_world_only.sdf.xacro b/nav2_bringup/worlds/gz_world_only.sdf.xacro index 11026d1c67..d93c7facf0 100644 --- a/nav2_bringup/worlds/gz_world_only.sdf.xacro +++ b/nav2_bringup/worlds/gz_world_only.sdf.xacro @@ -20,7 +20,7 @@ - ogre + ogre2