From 2604bf7a5d9c8b9e91c3d6428ee3098629f6770e Mon Sep 17 00:00:00 2001 From: Frederik Pasch Date: Tue, 17 Dec 2024 15:49:50 +0100 Subject: [PATCH] Fix nav2 --- .../gazebo/tb4_sim_scenario/CMakeLists.txt | 2 +- .../launch/nav2/bringup_launch.py | 218 --------- .../launch/nav2/navigation_launch.py | 341 -------------- .../launch/sim_nav_scenario_launch.py | 16 +- .../gazebo/tb4_sim_scenario/package.xml | 3 +- .../tb4_sim_scenario/params/nav2_params.yaml | 440 ------------------ .../gazebo/tb4_sim_scenario/worlds/maze.sdf | 14 +- 7 files changed, 12 insertions(+), 1022 deletions(-) delete mode 100644 simulation/gazebo/tb4_sim_scenario/launch/nav2/bringup_launch.py delete mode 100644 simulation/gazebo/tb4_sim_scenario/launch/nav2/navigation_launch.py delete mode 100644 simulation/gazebo/tb4_sim_scenario/params/nav2_params.yaml diff --git a/simulation/gazebo/tb4_sim_scenario/CMakeLists.txt b/simulation/gazebo/tb4_sim_scenario/CMakeLists.txt index bf83ac0a..a59808f7 100644 --- a/simulation/gazebo/tb4_sim_scenario/CMakeLists.txt +++ b/simulation/gazebo/tb4_sim_scenario/CMakeLists.txt @@ -9,7 +9,7 @@ endif() find_package(ament_cmake REQUIRED) install( - DIRECTORY config launch params worlds urdf maps + DIRECTORY config launch worlds urdf maps DESTINATION share/${PROJECT_NAME} ) diff --git a/simulation/gazebo/tb4_sim_scenario/launch/nav2/bringup_launch.py b/simulation/gazebo/tb4_sim_scenario/launch/nav2/bringup_launch.py deleted file mode 100644 index 74a91e13..00000000 --- a/simulation/gazebo/tb4_sim_scenario/launch/nav2/bringup_launch.py +++ /dev/null @@ -1,218 +0,0 @@ -# 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. - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - GroupAction, - IncludeLaunchDescription, - SetEnvironmentVariable, -) -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import Node -from launch_ros.actions import PushROSNamespace -from launch_ros.descriptions import ParameterFile -from nav2_common.launch import ReplaceString, RewrittenYaml - - -def generate_launch_description(): - # Get the launch directory - tb4_sim_scenario_dir = get_package_share_directory('tb4_sim_scenario') - bringup_dir = get_package_share_directory('nav2_bringup') - launch_dir = os.path.join(bringup_dir, 'launch') - - # Create the launch configuration variables - namespace = LaunchConfiguration('namespace') - use_namespace = LaunchConfiguration('use_namespace') - slam = LaunchConfiguration('slam') - 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') - log_level = LaunchConfiguration('log_level') - - # 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')] - - # Only it applys when `use_namespace` is True. - # '' keyword shall be replaced by 'namespace' launch argument - # in config file 'nav2_multirobot_params.yaml' as a default & example. - # User defined config file should contain '' keyword for the replacements. - params_file = ReplaceString( - source_file=params_file, - replacements={'': ('/', namespace)}, - condition=IfCondition(use_namespace), - ) - - configured_params = ParameterFile( - RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites={}, - convert_types=True, - ), - allow_substs=True, - ) - - stdout_linebuf_envvar = SetEnvironmentVariable( - 'RCUTILS_LOGGING_BUFFERED_STREAM', '1' - ) - - 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='', description='Full path to map yaml file to load' - ) - - declare_use_sim_time_cmd = DeclareLaunchArgument( - 'use_sim_time', - default_value='false', - 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_log_level_cmd = DeclareLaunchArgument( - 'log_level', default_value='info', description='log level' - ) - - # Specify the actions - bringup_cmd_group = GroupAction( - [ - PushROSNamespace(condition=IfCondition(use_namespace), namespace=namespace), - Node( - condition=IfCondition(use_composition), - name='nav2_container', - package='rclcpp_components', - executable='component_container_isolated', - parameters=[configured_params, {'autostart': autostart}], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - output='screen', - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'slam_launch.py') - ), - condition=IfCondition(slam), - launch_arguments={ - 'namespace': namespace, - 'use_sim_time': use_sim_time, - 'autostart': autostart, - 'use_respawn': use_respawn, - 'params_file': params_file, - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'localization_launch.py') - ), - condition=IfCondition(PythonExpression(['not ', slam])), - launch_arguments={ - 'namespace': namespace, - 'map': map_yaml_file, - 'use_sim_time': use_sim_time, - 'autostart': autostart, - 'params_file': params_file, - 'use_composition': use_composition, - 'use_respawn': use_respawn, - 'container_name': 'nav2_container', - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(tb4_sim_scenario_dir, 'launch', 'nav2', 'navigation_launch.py') - ), - launch_arguments={ - 'namespace': namespace, - 'use_sim_time': use_sim_time, - 'autostart': autostart, - 'params_file': params_file, - 'use_composition': use_composition, - 'use_respawn': use_respawn, - 'container_name': 'nav2_container', - }.items(), - ), - ] - ) - - # Create the launch description and populate - ld = LaunchDescription() - - # Set environment variables - ld.add_action(stdout_linebuf_envvar) - - # 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_use_respawn_cmd) - ld.add_action(declare_log_level_cmd) - - # Add the actions to launch all of the navigation nodes - ld.add_action(bringup_cmd_group) - - return ld diff --git a/simulation/gazebo/tb4_sim_scenario/launch/nav2/navigation_launch.py b/simulation/gazebo/tb4_sim_scenario/launch/nav2/navigation_launch.py deleted file mode 100644 index 2bb75203..00000000 --- a/simulation/gazebo/tb4_sim_scenario/launch/nav2/navigation_launch.py +++ /dev/null @@ -1,341 +0,0 @@ -# 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. - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import LoadComposableNodes, SetParameter -from launch_ros.actions import Node -from launch_ros.descriptions import ComposableNode, ParameterFile -from nav2_common.launch import RewrittenYaml - - -def generate_launch_description(): - # Get the launch directory - bringup_dir = get_package_share_directory('nav2_bringup') - - namespace = LaunchConfiguration('namespace') - use_sim_time = LaunchConfiguration('use_sim_time') - autostart = LaunchConfiguration('autostart') - params_file = LaunchConfiguration('params_file') - use_composition = LaunchConfiguration('use_composition') - container_name = LaunchConfiguration('container_name') - container_name_full = (namespace, '/', container_name) - use_respawn = LaunchConfiguration('use_respawn') - log_level = LaunchConfiguration('log_level') - - lifecycle_nodes = [ - 'controller_server', - 'smoother_server', - 'planner_server', - 'behavior_server', - 'velocity_smoother', - 'collision_monitor', - 'bt_navigator', - 'waypoint_follower', - 'docking_server', - ] - - # 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')] - - # Create our own temporary YAML files that include substitutions - param_substitutions = {'autostart': autostart} - - configured_params = ParameterFile( - RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True, - ), - allow_substs=True, - ) - - stdout_linebuf_envvar = SetEnvironmentVariable( - 'RCUTILS_LOGGING_BUFFERED_STREAM', '1' - ) - - declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', default_value='', description='Top-level namespace' - ) - - declare_use_sim_time_cmd = DeclareLaunchArgument( - 'use_sim_time', - default_value='false', - 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='False', - description='Use composed bringup if True', - ) - - declare_container_name_cmd = DeclareLaunchArgument( - 'container_name', - default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition', - ) - - declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', - default_value='False', - description='Whether to respawn if a node crashes. Applied when composition is disabled.', - ) - - declare_log_level_cmd = DeclareLaunchArgument( - 'log_level', default_value='info', description='log level' - ) - - load_nodes = GroupAction( - condition=IfCondition(PythonExpression(['not ', use_composition])), - actions=[ - SetParameter('use_sim_time', use_sim_time), - Node( - package='nav2_controller', - executable='controller_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], - ), - Node( - package='nav2_smoother', - executable='smoother_server', - name='smoother_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - ), - Node( - package='nav2_planner', - executable='planner_server', - name='planner_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - ), - Node( - package='nav2_behaviors', - executable='behavior_server', - name='behavior_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], - ), - Node( - package='nav2_bt_navigator', - executable='bt_navigator', - name='bt_navigator', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - ), - Node( - package='nav2_waypoint_follower', - executable='waypoint_follower', - name='waypoint_follower', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - ), - Node( - package='nav2_velocity_smoother', - executable='velocity_smoother', - name='velocity_smoother', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings - + [('cmd_vel', 'cmd_vel_nav')], - ), - Node( - package='nav2_collision_monitor', - executable='collision_monitor', - name='collision_monitor', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - ), - Node( - package='opennav_docking', - executable='opennav_docking', - name='docking_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - ), - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_navigation', - output='screen', - arguments=['--ros-args', '--log-level', log_level], - parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}, {'bond_timeout': 60000.}], - ), - ], - ) - - load_composable_nodes = GroupAction( - condition=IfCondition(use_composition), - actions=[ - SetParameter('use_sim_time', use_sim_time), - LoadComposableNodes( - target_container=container_name_full, - composable_node_descriptions=[ - ComposableNode( - package='nav2_controller', - plugin='nav2_controller::ControllerServer', - name='controller_server', - parameters=[configured_params], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], - ), - ComposableNode( - package='nav2_smoother', - plugin='nav2_smoother::SmootherServer', - name='smoother_server', - parameters=[configured_params], - remappings=remappings, - ), - ComposableNode( - package='nav2_planner', - plugin='nav2_planner::PlannerServer', - name='planner_server', - parameters=[configured_params], - remappings=remappings, - ), - ComposableNode( - package='nav2_behaviors', - plugin='behavior_server::BehaviorServer', - name='behavior_server', - parameters=[configured_params], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], - ), - ComposableNode( - package='nav2_bt_navigator', - plugin='nav2_bt_navigator::BtNavigator', - name='bt_navigator', - parameters=[configured_params], - remappings=remappings, - ), - ComposableNode( - package='nav2_waypoint_follower', - plugin='nav2_waypoint_follower::WaypointFollower', - name='waypoint_follower', - parameters=[configured_params], - remappings=remappings, - ), - ComposableNode( - package='nav2_velocity_smoother', - plugin='nav2_velocity_smoother::VelocitySmoother', - name='velocity_smoother', - parameters=[configured_params], - remappings=remappings - + [('cmd_vel', 'cmd_vel_nav')], - ), - ComposableNode( - package='nav2_collision_monitor', - plugin='nav2_collision_monitor::CollisionMonitor', - name='collision_monitor', - parameters=[configured_params], - remappings=remappings, - ), - ComposableNode( - package='opennav_docking', - plugin='opennav_docking::DockingServer', - name='docking_server', - parameters=[configured_params], - remappings=remappings, - ), - ComposableNode( - package='nav2_lifecycle_manager', - plugin='nav2_lifecycle_manager::LifecycleManager', - name='lifecycle_manager_navigation', - parameters=[ - {'autostart': autostart, 'node_names': lifecycle_nodes, 'bond_timeout': 60000.} - ], - ), - ], - ), - ], - ) - - # Create the launch description and populate - ld = LaunchDescription() - - # Set environment variables - ld.add_action(stdout_linebuf_envvar) - - # Declare the launch options - ld.add_action(declare_namespace_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_container_name_cmd) - ld.add_action(declare_use_respawn_cmd) - ld.add_action(declare_log_level_cmd) - # Add the actions to launch all of the navigation nodes - ld.add_action(load_nodes) - ld.add_action(load_composable_nodes) - - return ld diff --git a/simulation/gazebo/tb4_sim_scenario/launch/sim_nav_scenario_launch.py b/simulation/gazebo/tb4_sim_scenario/launch/sim_nav_scenario_launch.py index 686ad7af..c2db351b 100644 --- a/simulation/gazebo/tb4_sim_scenario/launch/sim_nav_scenario_launch.py +++ b/simulation/gazebo/tb4_sim_scenario/launch/sim_nav_scenario_launch.py @@ -48,14 +48,11 @@ def generate_launch_description(): map_conf = LaunchConfiguration('map') arg_map = DeclareLaunchArgument('map', default_value=os.path.join(tb4_sim_scenario_dir, 'maps', 'maze.yaml'), description='Full path to map yaml file to load') - params_file = LaunchConfiguration('params_file') - arg_params_file = DeclareLaunchArgument('params_file', default_value=PathJoinSubstitution([nav2_bringup_dir, 'params', 'nav2_params.yaml']), - description='nav2 parameter file') x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z') yaw = LaunchConfiguration('yaw') - simulation = IncludeLaunchDescription( - PythonLaunchDescriptionSource([PathJoinSubstitution([nav2_minimal_tb4_sim_dir, 'launch', 'simulation.launch.py'])]), + nav2_bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([nav2_bringup_dir, 'launch', 'tb4_simulation_launch.py'])]), launch_arguments={ 'world': [PathJoinSubstitution([tb4_sim_scenario_dir, 'worlds', 'maze.sdf'])], 'x_pose': x, @@ -63,14 +60,7 @@ def generate_launch_description(): 'z_pose': z, 'yaw': yaw, 'rviz_config_file': [PathJoinSubstitution([tb4_sim_scenario_dir, 'config', 'tb4_sim_scenario.rviz'])], - }.items() - ) - - nav2_bringup = IncludeLaunchDescription( - PythonLaunchDescriptionSource([PathJoinSubstitution([tb4_sim_scenario_dir, 'launch', 'nav2', 'bringup_launch.py'])]), - launch_arguments={ 'use_sim_time': 'True', - 'params_file': params_file, 'map': map_conf, }.items() ) @@ -98,13 +88,11 @@ def generate_launch_description(): arg_scenario, arg_scenario_execution, arg_world_name, - arg_params_file, arg_map ]) for pose_element in ['x', 'y', 'z', 'yaw']: ld.add_action(DeclareLaunchArgument(pose_element, default_value='0.0', description=f'{pose_element} component of the robot pose.')) - ld.add_action(simulation) ld.add_action(nav2_bringup) ld.add_action(groundtruth_publisher) ld.add_action(scenario_exec) diff --git a/simulation/gazebo/tb4_sim_scenario/package.xml b/simulation/gazebo/tb4_sim_scenario/package.xml index 922c2d8d..c5ca87bc 100644 --- a/simulation/gazebo/tb4_sim_scenario/package.xml +++ b/simulation/gazebo/tb4_sim_scenario/package.xml @@ -12,7 +12,8 @@ gazebo_tf_publisher - scenario_execution + scenario_execution_ros + scenario_execution_nav2 message_modification tf_to_pose_publisher nav2_minimal_tb4_sim diff --git a/simulation/gazebo/tb4_sim_scenario/params/nav2_params.yaml b/simulation/gazebo/tb4_sim_scenario/params/nav2_params.yaml deleted file mode 100644 index 2f997af4..00000000 --- a/simulation/gazebo/tb4_sim_scenario/params/nav2_params.yaml +++ /dev/null @@ -1,440 +0,0 @@ ---- -amcl: - ros__parameters: - use_sim_time: True - alpha1: 0.2 - alpha2: 0.2 - alpha3: 0.2 - alpha4: 0.2 - alpha5: 0.2 - base_frame_id: "base_link" - beam_skip_distance: 0.5 - beam_skip_error_threshold: 0.9 - beam_skip_threshold: 0.3 - do_beamskip: false - global_frame_id: "map" - lambda_short: 0.1 - laser_likelihood_max_dist: 2.0 - laser_max_range: 100.0 - laser_min_range: -1.0 - laser_model_type: "likelihood_field" - max_beams: 60 - max_particles: 2000 - min_particles: 500 - odom_frame_id: "odom" - pf_err: 0.05 - pf_z: 0.99 - recovery_alpha_fast: 0.0 - recovery_alpha_slow: 0.0 - resample_interval: 1 - robot_model_type: "nav2_amcl::DifferentialMotionModel" - save_pose_rate: 0.5 - sigma_hit: 0.2 - tf_broadcast: true - transform_tolerance: 1.0 - update_min_a: 0.2 - update_min_d: 0.25 - z_hit: 0.5 - z_max: 0.05 - z_rand: 0.5 - z_short: 0.05 - scan_topic: scan - -bt_navigator: - ros__parameters: - use_sim_time: True - global_frame: map - robot_base_frame: base_link - odom_topic: /odom - bt_loop_duration: 10 - default_server_timeout: 20 - # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' - # are use defaults: - # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml - # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - # They can be set here or via a RewrittenYaml remap from a parent launch - # ile to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node - -bt_navigator_navigate_through_poses_rclcpp_node: - ros__parameters: - use_sim_time: True - -bt_navigator_navigate_to_pose_rclcpp_node: - ros__parameters: - use_sim_time: True - -controller_server: - ros__parameters: - use_sim_time: True - controller_frequency: 20.0 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - min_theta_velocity_threshold: 0.001 - failure_tolerance: 0.3 - progress_checker_plugin: "progress_checker" - goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" - controller_plugins: ["FollowPath"] - - # Progress checker parameters - progress_checker: - plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.5 - movement_time_allowance: 10.0 - # Goal checker parameters - #precise_goal_checker: - # plugin: "nav2_controller::SimpleGoalChecker" - # xy_goal_tolerance: 0.25 - # yaw_goal_tolerance: 0.25 - # stateful: True - general_goal_checker: - stateful: True - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 - # DWB parameters - FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.26 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.26 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 2.5 - acc_lim_y: 0.0 - acc_lim_theta: 3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - decel_lim_theta: -3.2 - vx_samples: 20 - vy_samples: 5 - vtheta_samples: 20 - sim_time: 1.7 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.25 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True - stateful: True - critics: - - "RotateToGoal" - - "Oscillation" - - "BaseObstacle" - - "GoalAlign" - - "PathAlign" - - "PathDist" - - "GoalDist" - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 - -local_costmap: - local_costmap: - ros__parameters: - update_frequency: 5.0 - publish_frequency: 2.0 - global_frame: odom - robot_base_frame: base_link - use_sim_time: True - rolling_window: true - width: 3 - height: 3 - resolution: 0.05 - robot_radius: 0.22 - plugins: ["voxel_layer", "inflation_layer"] - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - always_send_full_costmap: True - -global_costmap: - global_costmap: - ros__parameters: - update_frequency: 1.0 - publish_frequency: 1.0 - global_frame: map - robot_base_frame: base_link - use_sim_time: True - robot_radius: 0.22 - resolution: 0.05 - track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 - always_send_full_costmap: True - -map_server: - ros__parameters: - use_sim_time: True - # Overridden in launch by the "map" launch configuration or provided - # default value. To use in yaml, remove the default "map" value in the - # tb3_simulation_launch.py file & provide full path to map below. - yaml_filename: "" - -map_saver: - ros__parameters: - use_sim_time: True - save_map_timeout: 5.0 - free_thresh_default: 0.25 - occupied_thresh_default: 0.65 - map_subscribe_transient_local: True - -planner_server: - ros__parameters: - expected_planner_frequency: 20.0 - use_sim_time: True - planner_plugins: ["GridBased"] - GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true - -smoother_server: - ros__parameters: - use_sim_time: True - smoother_plugins: ["simple_smoother"] - simple_smoother: - plugin: "nav2_smoother::SimpleSmoother" - tolerance: 1.0e-10 - max_its: 1000 - do_refinement: True - -behavior_server: - ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint - cycle_frequency: 10.0 - behavior_plugins: - - "spin" - - "backup" - - "drive_on_heading" - - "assisted_teleop" - - "wait" - spin: - plugin: "nav2_behaviors/Spin" - backup: - plugin: "nav2_behaviors/BackUp" - drive_on_heading: - plugin: "nav2_behaviors/DriveOnHeading" - wait: - plugin: "nav2_behaviors/Wait" - assisted_teleop: - plugin: "nav2_behaviors/AssistedTeleop" - global_frame: odom - robot_base_frame: base_link - transform_tolerance: 0.1 - use_sim_time: true - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - -robot_state_publisher: - ros__parameters: - use_sim_time: True - -waypoint_follower: - ros__parameters: - use_sim_time: True - loop_rate: 20 - stop_on_failure: false - waypoint_task_executor_plugin: "wait_at_waypoint" - wait_at_waypoint: - plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: True - waypoint_pause_duration: 200 - -velocity_smoother: - ros__parameters: - use_sim_time: True - smoothing_frequency: 20.0 - scale_velocities: False - feedback: "OPEN_LOOP" - max_velocity: [0.26, 0.0, 1.0] - min_velocity: [-0.26, 0.0, -1.0] - max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] - odom_topic: "odom" - odom_duration: 0.1 - deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 - -slam_toolbox: - ros__parameters: - - # Plugin params - solver_plugin: solver_plugins::CeresSolver - ceres_linear_solver: SPARSE_NORMAL_CHOLESKY - ceres_preconditioner: SCHUR_JACOBI - ceres_trust_strategy: LEVENBERG_MARQUARDT - ceres_dogleg_type: TRADITIONAL_DOGLEG - ceres_loss_function: None - - # ROS Parameters - odom_frame: odom - map_frame: map - base_frame: base_link - scan_topic: /scan - mode: mapping #localization - - # if you'd like to immediately start continuing a map at a given pose - # or at the dock, but they are mutually exclusive, if pose is given - # will use pose - #map_file_name: test_steve - #map_start_pose: [0.0, 0.0, 0.0] - #map_start_at_dock: true - - debug_logging: false - throttle_scans: 1 - transform_publish_period: 0.02 #if 0 never publishes odometry - map_update_interval: 5.0 - resolution: 0.05 - max_laser_range: 20.0 #for rastering images - minimum_time_interval: 0.5 - transform_timeout: 0.7 - tf_buffer_duration: 30. - stack_size_to_use: 40000000 # program needs a larger stack size - # to serialize large maps - enable_interactive_mode: true - - # General Parameters - use_scan_matching: true - use_scan_barycenter: true - minimum_travel_distance: 0.5 - minimum_travel_heading: 0.5 - scan_buffer_size: 10 - scan_buffer_maximum_scan_distance: 10.0 - link_match_minimum_response_fine: 0.1 - link_scan_maximum_distance: 1.5 - loop_search_maximum_distance: 3.0 - do_loop_closing: true - loop_match_minimum_chain_size: 10 - loop_match_maximum_variance_coarse: 3.0 - loop_match_minimum_response_coarse: 0.35 - loop_match_minimum_response_fine: 0.45 - - # Correlation Parameters - Correlation Parameters - correlation_search_space_dimension: 0.5 - correlation_search_space_resolution: 0.01 - correlation_search_space_smear_deviation: 0.1 - - # Correlation Parameters - Loop Closure Parameters - loop_search_space_dimension: 8.0 - loop_search_space_resolution: 0.05 - loop_search_space_smear_deviation: 0.03 - - # Scan Matcher Parameters - distance_variance_penalty: 0.5 - angle_variance_penalty: 1.0 - - fine_search_angle_offset: 0.00349 - coarse_search_angle_offset: 0.349 - coarse_angle_resolution: 0.0349 - minimum_angle_penalty: 0.9 - minimum_distance_penalty: 0.5 - use_response_expansion: true diff --git a/simulation/gazebo/tb4_sim_scenario/worlds/maze.sdf b/simulation/gazebo/tb4_sim_scenario/worlds/maze.sdf index cde97194..e6ce6c8e 100644 --- a/simulation/gazebo/tb4_sim_scenario/worlds/maze.sdf +++ b/simulation/gazebo/tb4_sim_scenario/worlds/maze.sdf @@ -23,12 +23,12 @@ SPDX-License-Identifier: Apache-2.0 --> 1 1000 - - - - - - ogre + + + + + + ogre2 @@ -1063,4 +1063,4 @@ SPDX-License-Identifier: Apache-2.0 --> - \ No newline at end of file +