From 60d2e612b3968234d6b80435b48a1c28b42c2ea9 Mon Sep 17 00:00:00 2001 From: Steve Macenski <stevenmacenski@gmail.com> Date: Tue, 11 Jun 2024 19:01:56 -0700 Subject: [PATCH 01/18] remove multirobot (#4421) --- .../cloned_multi_tb3_simulation_launch.py | 233 ----------- .../unique_multi_tb3_simulation_launch.py | 256 ------------ .../params/nav2_multirobot_params_1.yaml | 269 ------------- .../params/nav2_multirobot_params_2.yaml | 268 ------------- .../params/nav2_multirobot_params_all.yaml | 330 --------------- nav2_bringup/rviz/nav2_namespaced_view.rviz | 378 ------------------ 6 files changed, 1734 deletions(-) delete mode 100644 nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py delete mode 100644 nav2_bringup/launch/unique_multi_tb3_simulation_launch.py delete mode 100644 nav2_bringup/params/nav2_multirobot_params_1.yaml delete mode 100644 nav2_bringup/params/nav2_multirobot_params_2.yaml delete mode 100644 nav2_bringup/params/nav2_multirobot_params_all.yaml delete mode 100644 nav2_bringup/rviz/nav2_namespaced_view.rviz diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py deleted file mode 100644 index 54f9d60f80..0000000000 --- a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ /dev/null @@ -1,233 +0,0 @@ -# Copyright (c) 2023 LG Electronics. -# Copyright (c) 2024 Open Navigation LLC -# -# 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 pathlib import Path -import tempfile - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - GroupAction, - IncludeLaunchDescription, - LogInfo, - OpaqueFunction, - RegisterEventHandler, -) -from launch.conditions import IfCondition -from launch.event_handlers import OnShutdown -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, TextSubstitution -from nav2_common.launch import ParseMultiRobotPose - - -def generate_launch_description(): - """ - Bring up the multi-robots with given launch arguments. - - Launch arguments consist of robot name(which is namespace) and pose for initialization. - Keep general yaml format for pose information. - ex) robots:='robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}' - ex) robots:='robot3={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}; - robot4={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}' - """ - # Get the launch directory - bringup_dir = get_package_share_directory('nav2_bringup') - launch_dir = os.path.join(bringup_dir, 'launch') - sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') - - # Simulation settings - world = LaunchConfiguration('world') - - # On this example all robots are launched with the same settings - map_yaml_file = LaunchConfiguration('map') - params_file = LaunchConfiguration('params_file') - autostart = LaunchConfiguration('autostart') - rviz_config_file = LaunchConfiguration('rviz_config') - use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') - use_rviz = LaunchConfiguration('use_rviz') - log_settings = LaunchConfiguration('log_settings', default='true') - - # Declare the launch arguments - declare_world_cmd = DeclareLaunchArgument( - 'world', - default_value=os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro'), - description='Full path to world file to load', - ) - - declare_map_yaml_cmd = DeclareLaunchArgument( - 'map', - default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'), - description='Full path to map file to load', - ) - - declare_params_file_cmd = DeclareLaunchArgument( - 'params_file', - default_value=os.path.join( - bringup_dir, 'params', 'nav2_multirobot_params_all.yaml' - ), - description='Full path to the ROS2 parameters file to use for all launched nodes', - ) - - declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', - default_value='false', - description='Automatically startup the stacks', - ) - - declare_rviz_config_file_cmd = DeclareLaunchArgument( - 'rviz_config', - default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), - description='Full path to the RVIZ config file to use.', - ) - - 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' - ) - - # Start Gazebo with plugin providing the robot spawning service - world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') - world_sdf_xacro = ExecuteProcess( - cmd=['xacro', '-o', world_sdf, ['headless:=', 'False'], world]) - start_gazebo_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', - 'gz_sim.launch.py')), - launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items()) - - remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( - on_shutdown=[ - OpaqueFunction(function=lambda _: os.remove(world_sdf)) - ])) - - robots_list = ParseMultiRobotPose('robots').value() - - # Define commands for launching the navigation instances - bringup_cmd_group = [] - for robot_name in robots_list: - init_pose = robots_list[robot_name] - group = GroupAction( - [ - LogInfo( - msg=[ - 'Launching namespace=', - robot_name, - ' init_pose=', - str(init_pose), - ] - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'rviz_launch.py') - ), - condition=IfCondition(use_rviz), - launch_arguments={ - 'namespace': TextSubstitution(text=robot_name), - 'use_namespace': 'True', - 'rviz_config': rviz_config_file, - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py') - ), - launch_arguments={ - 'namespace': robot_name, - 'use_namespace': 'True', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': params_file, - 'autostart': autostart, - 'use_rviz': 'False', - 'use_simulator': 'False', - 'headless': 'False', - 'use_robot_state_pub': use_robot_state_pub, - 'x_pose': TextSubstitution(text=str(init_pose['x'])), - 'y_pose': TextSubstitution(text=str(init_pose['y'])), - 'z_pose': TextSubstitution(text=str(init_pose['z'])), - 'roll': TextSubstitution(text=str(init_pose['roll'])), - 'pitch': TextSubstitution(text=str(init_pose['pitch'])), - 'yaw': TextSubstitution(text=str(init_pose['yaw'])), - 'robot_name': TextSubstitution(text=robot_name), - }.items(), - ), - ] - ) - - bringup_cmd_group.append(group) - - set_env_vars_resources = AppendEnvironmentVariable( - 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')) - set_env_vars_resources2 = AppendEnvironmentVariable( - 'GZ_SIM_RESOURCE_PATH', - str(Path(os.path.join(sim_dir)).parent.resolve())) - - # Create the launch description and populate - ld = LaunchDescription() - ld.add_action(set_env_vars_resources) - ld.add_action(set_env_vars_resources2) - - # Declare the launch options - ld.add_action(declare_world_cmd) - ld.add_action(declare_map_yaml_cmd) - ld.add_action(declare_params_file_cmd) - ld.add_action(declare_use_rviz_cmd) - ld.add_action(declare_autostart_cmd) - ld.add_action(declare_rviz_config_file_cmd) - ld.add_action(declare_use_robot_state_pub_cmd) - - # Add the actions to start gazebo, robots and simulations - ld.add_action(world_sdf_xacro) - ld.add_action(start_gazebo_cmd) - ld.add_action(remove_temp_sdf_file) - - ld.add_action(LogInfo(msg=['number_of_robots=', str(len(robots_list))])) - - ld.add_action( - LogInfo(condition=IfCondition(log_settings), msg=['map yaml: ', map_yaml_file]) - ) - ld.add_action( - LogInfo(condition=IfCondition(log_settings), msg=['params yaml: ', params_file]) - ) - ld.add_action( - LogInfo( - condition=IfCondition(log_settings), - msg=['rviz config file: ', rviz_config_file], - ) - ) - ld.add_action( - LogInfo( - condition=IfCondition(log_settings), - msg=['using robot state pub: ', use_robot_state_pub], - ) - ) - ld.add_action( - LogInfo(condition=IfCondition(log_settings), msg=['autostart: ', autostart]) - ) - - for cmd in bringup_cmd_group: - ld.add_action(cmd) - - return ld diff --git a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py deleted file mode 100644 index 7eaaf1a97d..0000000000 --- a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py +++ /dev/null @@ -1,256 +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. - -""" -Example for spawning multiple robots in Gazebo. - -This is an example on how to create a launch file for spawning multiple robots into Gazebo -and launch multiple instances of the navigation stack, each controlling one robot. -The robots co-exist on a shared environment and are controlled by independent nav stacks. -""" - -import os -from pathlib import Path -import tempfile - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - GroupAction, - IncludeLaunchDescription, - LogInfo, - OpaqueFunction, - RegisterEventHandler, -) -from launch.conditions import IfCondition -from launch.event_handlers import OnShutdown -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, TextSubstitution - - -def generate_launch_description(): - # Get the launch directory - bringup_dir = get_package_share_directory('nav2_bringup') - launch_dir = os.path.join(bringup_dir, 'launch') - sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') - - # Names and poses of the robots - robots = [ - { - 'name': 'robot1', - 'x_pose': 0.0, - 'y_pose': 0.5, - 'z_pose': 0.01, - 'roll': 0.0, - 'pitch': 0.0, - 'yaw': 0.0, - }, - { - 'name': 'robot2', - 'x_pose': 0.0, - 'y_pose': -0.5, - 'z_pose': 0.01, - 'roll': 0.0, - 'pitch': 0.0, - 'yaw': 0.0, - }, - ] - - # Simulation settings - world = LaunchConfiguration('world') - - # On this example all robots are launched with the same settings - map_yaml_file = LaunchConfiguration('map') - - autostart = LaunchConfiguration('autostart') - rviz_config_file = LaunchConfiguration('rviz_config') - use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') - use_rviz = LaunchConfiguration('use_rviz') - log_settings = LaunchConfiguration('log_settings', default='true') - - # Declare the launch arguments - declare_world_cmd = DeclareLaunchArgument( - 'world', - default_value=os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro'), - description='Full path to world file to load', - ) - - declare_map_yaml_cmd = DeclareLaunchArgument( - 'map', - default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'), - description='Full path to map file to load', - ) - - declare_robot1_params_file_cmd = DeclareLaunchArgument( - 'robot1_params_file', - default_value=os.path.join( - bringup_dir, 'params', 'nav2_multirobot_params_1.yaml' - ), - description='Full path to the ROS2 parameters file to use for robot1 launched nodes', - ) - - declare_robot2_params_file_cmd = DeclareLaunchArgument( - 'robot2_params_file', - default_value=os.path.join( - bringup_dir, 'params', 'nav2_multirobot_params_2.yaml' - ), - description='Full path to the ROS2 parameters file to use for robot2 launched nodes', - ) - - declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', - default_value='false', - description='Automatically startup the stacks', - ) - - declare_rviz_config_file_cmd = DeclareLaunchArgument( - 'rviz_config', - default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), - description='Full path to the RVIZ config file to use.', - ) - - 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' - ) - - # Start Gazebo with plugin providing the robot spawning service - world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') - world_sdf_xacro = ExecuteProcess( - cmd=['xacro', '-o', world_sdf, ['headless:=', 'False'], world]) - start_gazebo_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', - 'gz_sim.launch.py')), - launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items()) - - remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( - on_shutdown=[ - OpaqueFunction(function=lambda _: os.remove(world_sdf)) - ])) - - # Define commands for launching the navigation instances - nav_instances_cmds = [] - for robot in robots: - params_file = LaunchConfiguration(f"{robot['name']}_params_file") - - group = GroupAction( - [ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'rviz_launch.py') - ), - condition=IfCondition(use_rviz), - launch_arguments={ - 'namespace': TextSubstitution(text=robot['name']), - 'use_namespace': 'True', - 'rviz_config': rviz_config_file, - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py') - ), - launch_arguments={ - 'namespace': robot['name'], - 'use_namespace': 'True', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': params_file, - 'autostart': autostart, - 'use_rviz': 'False', - 'use_simulator': 'False', - 'headless': 'False', - 'use_robot_state_pub': use_robot_state_pub, - 'x_pose': TextSubstitution(text=str(robot['x_pose'])), - 'y_pose': TextSubstitution(text=str(robot['y_pose'])), - 'z_pose': TextSubstitution(text=str(robot['z_pose'])), - 'roll': TextSubstitution(text=str(robot['roll'])), - 'pitch': TextSubstitution(text=str(robot['pitch'])), - 'yaw': TextSubstitution(text=str(robot['yaw'])), - 'robot_name': TextSubstitution(text=robot['name']), - }.items(), - ), - LogInfo( - condition=IfCondition(log_settings), - msg=['Launching ', robot['name']], - ), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' map yaml: ', map_yaml_file], - ), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' params yaml: ', params_file], - ), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' rviz config file: ', rviz_config_file], - ), - LogInfo( - condition=IfCondition(log_settings), - msg=[ - robot['name'], - ' using robot state pub: ', - use_robot_state_pub, - ], - ), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' autostart: ', autostart], - ), - ] - ) - - nav_instances_cmds.append(group) - - set_env_vars_resources = AppendEnvironmentVariable( - 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')) - set_env_vars_resources2 = AppendEnvironmentVariable( - 'GZ_SIM_RESOURCE_PATH', - str(Path(os.path.join(sim_dir)).parent.resolve())) - - # Create the launch description and populate - ld = LaunchDescription() - ld.add_action(set_env_vars_resources) - ld.add_action(set_env_vars_resources2) - - # Declare the launch options - ld.add_action(declare_world_cmd) - ld.add_action(declare_map_yaml_cmd) - ld.add_action(declare_robot1_params_file_cmd) - ld.add_action(declare_robot2_params_file_cmd) - ld.add_action(declare_use_rviz_cmd) - ld.add_action(declare_autostart_cmd) - ld.add_action(declare_rviz_config_file_cmd) - ld.add_action(declare_use_robot_state_pub_cmd) - - # Add the actions to start gazebo, robots and simulations - ld.add_action(world_sdf_xacro) - ld.add_action(start_gazebo_cmd) - ld.add_action(remove_temp_sdf_file) - - for simulation_instance_cmd in nav_instances_cmds: - ld.add_action(simulation_instance_cmd) - - return ld diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml deleted file mode 100644 index a62d1221c2..0000000000 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ /dev/null @@ -1,269 +0,0 @@ -amcl: - ros__parameters: - alpha1: 0.2 - alpha2: 0.2 - alpha3: 0.2 - alpha4: 0.2 - alpha5: 0.2 - base_frame_id: "base_footprint" - 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: - global_frame: map - robot_base_frame: base_link - odom_topic: /odom - bt_loop_duration: 10 - default_server_timeout: 20 - wait_for_service_timeout: 1000 - action_server_result_timeout: 900.0 - navigators: ["navigate_to_pose", "navigate_through_poses"] - navigate_to_pose: - plugin: "nav2_bt_navigator::NavigateToPoseNavigator" - navigate_through_poses: - plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" - # '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 file to Nav2. - - # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). - # Built-in plugins are added automatically - # plugin_lib_names: [] - - error_code_names: - - compute_path_error_code - - follow_path_error_code - -controller_server: - ros__parameters: - controller_frequency: 20.0 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - min_theta_velocity_threshold: 0.001 - progress_checker_plugins: ["progress_checker"] - goal_checker_plugins: ["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 - goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 - stateful: True - # 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 - 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 - 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: 0.0 - GoalAlign.scale: 0.0 - 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: - global_frame: odom - 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: False - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /robot1/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: - map_subscribe_transient_local: True - always_send_full_costmap: True - -global_costmap: - global_costmap: - ros__parameters: - robot_radius: 0.22 - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /robot1/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: - yaml_filename: "tb3_sandbox.yaml" - save_map_timeout: 5.0 - -planner_server: - ros__parameters: - planner_plugins: ["GridBased"] - GridBased: - plugin: "nav2_navfn_planner::NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: 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", "wait"] - spin: - plugin: "nav2_behaviors::Spin" - backup: - plugin: "nav2_behaviors::BackUp" - drive_on_heading: - plugin: "nav2_behaviors::DriveOnHeading" - wait: - plugin: "nav2_behaviors::Wait" - global_frame: odom - robot_base_frame: base_link - transform_tolerance: 0.1 - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - -waypoint_follower: - ros__parameters: - loop_rate: 20 - stop_on_failure: false - action_server_result_timeout: 900.0 - waypoint_task_executor_plugin: "wait_at_waypoint" - wait_at_waypoint: - plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: True - waypoint_pause_duration: 200 - -collision_monitor: - ros__parameters: - base_frame_id: "base_footprint" - odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_smoothed" - cmd_vel_out_topic: "cmd_vel" - state_topic: "collision_monitor_state" - transform_tolerance: 0.2 - source_timeout: 1.0 - base_shift_correction: True - stop_pub_timeout: 2.0 - # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, - # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: - type: "polygon" - action_type: "approach" - footprint_topic: "local_costmap/published_footprint" - time_before_collision: 2.0 - simulation_time_step: 0.1 - min_points: 6 - visualize: False - enabled: True - observation_sources: ["scan"] - scan: - type: "scan" - topic: "/robot1/scan" - min_height: 0.15 - max_height: 2.0 - enabled: True diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml deleted file mode 100644 index d2befc70f5..0000000000 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ /dev/null @@ -1,268 +0,0 @@ -amcl: - ros__parameters: - alpha1: 0.2 - alpha2: 0.2 - alpha3: 0.2 - alpha4: 0.2 - alpha5: 0.2 - base_frame_id: "base_footprint" - 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: - global_frame: map - robot_base_frame: base_link - odom_topic: /odom - bt_loop_duration: 10 - default_server_timeout: 20 - wait_for_service_timeout: 1000 - action_server_result_timeout: 900.0 - navigators: ["navigate_to_pose", "navigate_through_poses"] - navigate_to_pose: - plugin: "nav2_bt_navigator::NavigateToPoseNavigator" - navigate_through_poses: - plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" - # '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 file to Nav2. - - # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). - # Built-in plugins are added automatically - # plugin_lib_names: [] - - error_code_names: - - compute_path_error_code - - follow_path_error_code - -controller_server: - ros__parameters: - controller_frequency: 20.0 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - min_theta_velocity_threshold: 0.001 - progress_checker_plugins: ["progress_checker"] - goal_checker_plugins: ["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 - goal_checker: - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 - stateful: True - # 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 - 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 - 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: 0.0 - GoalAlign.scale: 0.0 - 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: - global_frame: odom - 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: False - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /robot2/scan - max_obstacle_height: 2.0 - clearing: True - marking: True - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - map_subscribe_transient_local: True - always_send_full_costmap: True - -global_costmap: - global_costmap: - ros__parameters: - robot_radius: 0.22 - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /robot2/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: - yaml_filename: "tb3_sandbox.yaml" - save_map_timeout: 5.0 - -planner_server: - ros__parameters: - planner_plugins: ["GridBased"] - GridBased: - plugin: "nav2_navfn_planner::NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: 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", "wait"] - spin: - plugin: "nav2_behaviors::Spin" - backup: - plugin: "nav2_behaviors::BackUp" - drive_on_heading: - plugin: "nav2_behaviors::DriveOnHeading" - wait: - plugin: "nav2_behaviors::Wait" - global_frame: odom - robot_base_frame: base_link - transform_tolerance: 0.1 - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - -waypoint_follower: - ros__parameters: - loop_rate: 20 - stop_on_failure: false - action_server_result_timeout: 900.0 - waypoint_task_executor_plugin: "wait_at_waypoint" - wait_at_waypoint: - plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: True - waypoint_pause_duration: 200 - -collision_monitor: - ros__parameters: - base_frame_id: "base_footprint" - odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_smoothed" - cmd_vel_out_topic: "cmd_vel" - state_topic: "collision_monitor_state" - transform_tolerance: 0.2 - source_timeout: 1.0 - base_shift_correction: True - stop_pub_timeout: 2.0 - # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, - # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: - type: "polygon" - action_type: "approach" - footprint_topic: "local_costmap/published_footprint" - time_before_collision: 2.0 - simulation_time_step: 0.1 - min_points: 6 - visualize: False - enabled: True - observation_sources: ["scan"] - scan: - type: "scan" - topic: "/robot2/scan" - min_height: 0.15 - max_height: 2.0 - enabled: True diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml deleted file mode 100644 index b5960cbbd4..0000000000 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ /dev/null @@ -1,330 +0,0 @@ -amcl: - ros__parameters: - alpha1: 0.2 - alpha2: 0.2 - alpha3: 0.2 - alpha4: 0.2 - alpha5: 0.2 - base_frame_id: "base_footprint" - 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: - global_frame: map - robot_base_frame: base_link - odom_topic: /odom - bt_loop_duration: 10 - default_server_timeout: 20 - wait_for_service_timeout: 1000 - action_server_result_timeout: 900.0 - navigators: ["navigate_to_pose", "navigate_through_poses"] - navigate_to_pose: - plugin: "nav2_bt_navigator::NavigateToPoseNavigator" - navigate_through_poses: - plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" - # '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 file to Nav2. - - # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). - # Built-in plugins are added automatically - # plugin_lib_names: [] - - error_code_names: - - compute_path_error_code - - follow_path_error_code - -controller_server: - ros__parameters: - 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_plugins: ["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 - 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 - 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: - # '<robot_namespace>' keyword shall be replaced with 'namespace' where user defined. - # It doesn't need to start with '/' - topic: <robot_namespace>/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 - 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: - # '<robot_namespace>' keyword shall be replaced with 'namespace' where user defined. - # It doesn't need to start with '/' - topic: <robot_namespace>/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 - -# The yaml_filename does not need to be specified since it going to be set by defaults in launch. -# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py -# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. -# map_server: -# ros__parameters: -# yaml_filename: "" - -map_saver: - ros__parameters: - 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 - planner_plugins: ["GridBased"] - GridBased: - plugin: "nav2_navfn_planner::NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true - -smoother_server: - ros__parameters: - smoother_plugins: ["simple_smoother"] - simple_smoother: - plugin: "nav2_smoother::SimpleSmoother" - tolerance: 1.0e-10 - max_its: 1000 - do_refinement: True - -behavior_server: - ros__parameters: - local_costmap_topic: local_costmap/costmap_raw - global_costmap_topic: global_costmap/costmap_raw - local_footprint_topic: local_costmap/published_footprint - global_footprint_topic: global_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" - local_frame: odom - global_frame: map - robot_base_frame: base_link - transform_tolerance: 0.1 - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - -waypoint_follower: - ros__parameters: - loop_rate: 20 - stop_on_failure: false - action_server_result_timeout: 900.0 - 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: - 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 - -collision_monitor: - ros__parameters: - base_frame_id: "base_footprint" - odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_smoothed" - cmd_vel_out_topic: "cmd_vel" - state_topic: "collision_monitor_state" - transform_tolerance: 0.2 - source_timeout: 1.0 - base_shift_correction: True - stop_pub_timeout: 2.0 - # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, - # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: - type: "polygon" - action_type: "approach" - footprint_topic: "/local_costmap/published_footprint" - time_before_collision: 2.0 - simulation_time_step: 0.1 - min_points: 6 - visualize: False - enabled: True - observation_sources: ["scan"] - scan: - type: "scan" - topic: "<robot_namespace>/scan" - min_height: 0.15 - max_height: 2.0 - enabled: True diff --git a/nav2_bringup/rviz/nav2_namespaced_view.rviz b/nav2_bringup/rviz/nav2_namespaced_view.rviz deleted file mode 100644 index 9f70030775..0000000000 --- a/nav2_bringup/rviz/nav2_namespaced_view.rviz +++ /dev/null @@ -1,378 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 195 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /RobotModel1/Status1 - - /TF1/Frames1 - - /TF1/Tree1 - - /Global Planner1/Global Costmap1 - Splitter Ratio: 0.5833333134651184 - Tree Height: 464 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: nav2_rviz_plugins/Navigation 2 - Name: Navigation 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: <Fixed Frame> - Value: true - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: <robot_namespace>/robot_description - Enabled: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - Name: RobotModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - {} - Update Interval: 0 - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/LaserScan - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: LaserScan - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: <robot_namespace>/scan - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Bumper Hit - Position Transformer: "" - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: <robot_namespace>/mobile_base/sensors/bumper_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: true - Enabled: true - Name: Map - Topic: - Depth: 1 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: <robot_namespace>/map - Use Timestamp: false - Value: true - - Alpha: 1 - Class: nav2_rviz_plugins/ParticleCloud - Color: 0; 180; 0 - Enabled: true - Max Arrow Length: 0.3 - Min Arrow Length: 0.02 - Name: Amcl Particle Swarm - Shape: Arrow (Flat) - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: <robot_namespace>/particle_cloud - Value: true - - Class: rviz_common/Group - Displays: - - Alpha: 0.30000001192092896 - Class: rviz_default_plugins/Map - Color Scheme: costmap - Draw Behind: false - Enabled: true - Name: Global Costmap - Topic: - Depth: 1 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: <robot_namespace>/global_costmap/costmap - Use Timestamp: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 255; 0; 0 - Enabled: true - Head Diameter: 0.019999999552965164 - Head Length: 0.019999999552965164 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: Arrows - Radius: 0.029999999329447746 - Shaft Diameter: 0.004999999888241291 - Shaft Length: 0.019999999552965164 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: <robot_namespace>/plan - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: Polygon - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: <robot_namespace>/global_costmap/published_footprint - Value: false - Enabled: true - Name: Global Planner - - Class: rviz_common/Group - Displays: - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: costmap - Draw Behind: false - Enabled: true - Name: Local Costmap - Topic: - Depth: 1 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: <robot_namespace>/local_costmap/costmap - Use Timestamp: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 0; 12; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Local Plan - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: <robot_namespace>/local_plan - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Trajectories - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: <robot_namespace>/marker - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: true - Name: Polygon - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: <robot_namespace>/local_costmap/published_footprint - Value: true - Enabled: true - Name: Controller - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MarkerArray - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: <robot_namespace>/waypoints - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: <robot_namespace>/initialpose - - Class: nav2_rviz_plugins/GoalTool - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Angle: -0.0008007669821381569 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 54 - Target Frame: <Fixed Frame> - Value: TopDownOrtho (rviz_default_plugins) - X: -5.41 - Y: 0 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 914 - Hide Left Dock: false - Hide Right Dock: true - Navigation 2: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000338fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000003070000006e0000006200ffffff000000010000010f00000338fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000338000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000033800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1545 - X: 186 - Y: 117 From eb560c4769d746707e916490655eb9f24cb846e3 Mon Sep 17 00:00:00 2001 From: Davide Faconti <davide.faconti@gmail.com> Date: Wed, 12 Jun 2024 19:43:17 +0200 Subject: [PATCH 02/18] [nav2_costmap_2s]: free correctly the memory in the InflationLayer (#4424) * [nav2_costmap_2s]: free correctly the memory in the InflationLayer Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * Remove 4 redundant bytes from CellData in Inflationlayer Note: performance was measured and this is slightly faster, even if we need to recompute the index. Probably a benefit at the level of the CPU cache Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * formatting Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --- .../nav2_costmap_2d/inflation_layer.hpp | 6 ++-- nav2_costmap_2d/plugins/inflation_layer.cpp | 33 +++++++++---------- .../test/integration/inflation_tests.cpp | 19 +++++------ 3 files changed, 25 insertions(+), 33 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index b83010c62d..0ae010e94b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -60,18 +60,16 @@ class CellData public: /** * @brief Constructor for a CellData objects - * @param i The index of the cell in the cost map * @param x The x coordinate of the cell in the cost map * @param y The y coordinate of the cell in the cost map * @param sx The x coordinate of the closest obstacle cell in the costmap * @param sy The y coordinate of the closest obstacle cell in the costmap * @return */ - CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) - : index_(static_cast<unsigned int>(i)), x_(x), y_(y), src_x_(sx), src_y_(sy) + CellData(unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) + : x_(x), y_(y), src_x_(sx), src_y_(sy) { } - unsigned int index_; unsigned int x_, y_; unsigned int src_x_, src_y_; }; diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 067877e548..e2542aa9cf 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -230,12 +230,13 @@ InflationLayer::updateCosts( // Start with lethal obstacles: by definition distance is 0.0 auto & obs_bin = inflation_cells_[0]; + obs_bin.reserve(200); for (int j = min_j; j < max_j; j++) { for (int i = min_i; i < max_i; i++) { int index = static_cast<int>(master_grid.getIndex(i, j)); unsigned char cost = master_array[index]; if (cost == LETHAL_OBSTACLE || (inflate_around_unknown_ && cost == NO_INFORMATION)) { - obs_bin.emplace_back(index, i, j, i, j); + obs_bin.emplace_back(i, j, i, j); } } } @@ -243,12 +244,18 @@ InflationLayer::updateCosts( // Process cells by increasing distance; new cells are appended to the // corresponding distance bin, so they // can overtake previously inserted but farther away cells - for (const auto & dist_bin : inflation_cells_) { + for (auto & dist_bin : inflation_cells_) { + dist_bin.reserve(200); for (std::size_t i = 0; i < dist_bin.size(); ++i) { // Do not use iterator or for-range based loops to // iterate though dist_bin, since it's size might // change when a new cell is enqueued, invalidating all iterators - unsigned int index = dist_bin[i].index_; + const CellData & cell = dist_bin[i]; + unsigned int mx = cell.x_; + unsigned int my = cell.y_; + unsigned int sx = cell.src_x_; + unsigned int sy = cell.src_y_; + unsigned int index = master_grid.getIndex(mx, my); // ignore if already visited if (seen_[index]) { @@ -257,11 +264,6 @@ InflationLayer::updateCosts( seen_[index] = true; - unsigned int mx = dist_bin[i].x_; - unsigned int my = dist_bin[i].y_; - unsigned int sx = dist_bin[i].src_x_; - unsigned int sy = dist_bin[i].src_y_; - // assign the cost associated with the distance from an obstacle to the cell unsigned char cost = costLookup(mx, my, sx, sy); unsigned char old_cost = master_array[index]; @@ -296,11 +298,9 @@ InflationLayer::updateCosts( enqueue(index + size_x, mx, my + 1, sx, sy); } } - } - - for (auto & dist : inflation_cells_) { - dist.clear(); - dist.reserve(200); + // This level of inflation_cells_ is not needed anymore. We can free the memory + // Note that dist_bin.clear() is not enough, because it won't free the memory + dist_bin = std::vector<CellData>(); } current_ = true; @@ -334,8 +334,8 @@ InflationLayer::enqueue( const unsigned int r = cell_inflation_radius_ + 2; // push the cell data onto the inflation list and mark - inflation_cells_[distance_matrix_[mx - src_x + r][my - src_y + r]].emplace_back( - index, mx, my, src_x, src_y); + const auto dist = distance_matrix_[mx - src_x + r][my - src_y + r]; + inflation_cells_[dist].emplace_back(mx, my, src_x, src_y); } } @@ -372,9 +372,6 @@ InflationLayer::computeCaches() int max_dist = generateIntegerDistances(); inflation_cells_.clear(); inflation_cells_.resize(max_dist + 1); - for (auto & dist : inflation_cells_) { - dist.reserve(200); - } } int diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index ad867128b1..b88c373688 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -125,15 +125,16 @@ void TestNode::validatePointInflation( bool * seen = new bool[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()]; memset(seen, false, costmap->getSizeInCellsX() * costmap->getSizeInCellsY() * sizeof(bool)); std::map<double, std::vector<CellData>> m; - CellData initial(costmap->getIndex(mx, my), mx, my, mx, my); + CellData initial(mx, my, mx, my); m[0].push_back(initial); for (std::map<double, std::vector<CellData>>::iterator bin = m.begin(); bin != m.end(); ++bin) { for (unsigned int i = 0; i < bin->second.size(); ++i) { const CellData cell = bin->second[i]; - if (!seen[cell.index_]) { - seen[cell.index_] = true; + const auto index = costmap->getIndex(cell.x_, cell.y_); + if (!seen[index]) { + seen[index] = true; unsigned int dx = (cell.x_ > cell.src_x_) ? cell.x_ - cell.src_x_ : cell.src_x_ - cell.x_; unsigned int dy = (cell.y_ > cell.src_y_) ? cell.y_ - cell.src_y_ : cell.src_y_ - cell.y_; double dist = std::hypot(dx, dy); @@ -152,23 +153,19 @@ void TestNode::validatePointInflation( } if (cell.x_ > 0) { - CellData data(costmap->getIndex(cell.x_ - 1, cell.y_), - cell.x_ - 1, cell.y_, cell.src_x_, cell.src_y_); + CellData data(cell.x_ - 1, cell.y_, cell.src_x_, cell.src_y_); m[dist].push_back(data); } if (cell.y_ > 0) { - CellData data(costmap->getIndex(cell.x_, cell.y_ - 1), - cell.x_, cell.y_ - 1, cell.src_x_, cell.src_y_); + CellData data(cell.x_, cell.y_ - 1, cell.src_x_, cell.src_y_); m[dist].push_back(data); } if (cell.x_ < costmap->getSizeInCellsX() - 1) { - CellData data(costmap->getIndex(cell.x_ + 1, cell.y_), - cell.x_ + 1, cell.y_, cell.src_x_, cell.src_y_); + CellData data(cell.x_ + 1, cell.y_, cell.src_x_, cell.src_y_); m[dist].push_back(data); } if (cell.y_ < costmap->getSizeInCellsY() - 1) { - CellData data(costmap->getIndex(cell.x_, cell.y_ + 1), - cell.x_, cell.y_ + 1, cell.src_x_, cell.src_y_); + CellData data(cell.x_, cell.y_ + 1, cell.src_x_, cell.src_y_); m[dist].push_back(data); } } From 23ddd865a558aeeee9388d1bdab15dc2e29cea73 Mon Sep 17 00:00:00 2001 From: Davide Faconti <davide.faconti@gmail.com> Date: Wed, 12 Jun 2024 22:10:35 +0200 Subject: [PATCH 03/18] [AMCL] reduce the amount of memory used (#4426) * iprove memory used by AMCL and add parameter "freespace_downsampling" Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix message and linting Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add class member Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 4 +++- nav2_amcl/include/nav2_amcl/map/map.hpp | 9 +++++---- nav2_amcl/src/amcl_node.cpp | 22 +++++++++++++++------- 3 files changed, 23 insertions(+), 12 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 030a85f38c..514bba4c55 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -152,7 +152,8 @@ class AmclNode : public nav2_util::LifecycleNode std::recursive_mutex mutex_; rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_; #if NEW_UNIFORM_SAMPLING - static std::vector<std::pair<int, int>> free_space_indices; + struct Point2D { int32_t x; int32_t y; }; + static std::vector<Point2D> free_space_indices; #endif // Transforms @@ -391,6 +392,7 @@ class AmclNode : public nav2_util::LifecycleNode double z_rand_; std::string scan_topic_{"scan"}; std::string map_topic_{"map"}; + bool freespace_downsampling_ = false; }; } // namespace nav2_amcl diff --git a/nav2_amcl/include/nav2_amcl/map/map.hpp b/nav2_amcl/include/nav2_amcl/map/map.hpp index c9557efa0b..3cf6f0d597 100644 --- a/nav2_amcl/include/nav2_amcl/map/map.hpp +++ b/nav2_amcl/include/nav2_amcl/map/map.hpp @@ -41,20 +41,21 @@ struct _rtk_fig_t; // Limits #define MAP_WIFI_MAX_LEVELS 8 - +// make sure that the sizeof(map_cell_t) == 5 +#pragma pack(push, 1) // Description for a single map cell. typedef struct { // Occupancy state (-1 = free, 0 = unknown, +1 = occ) - int occ_state; + int8_t occ_state; // Distance to the nearest occupied cell - double occ_dist; + float occ_dist; // Wifi levels // int wifi_levels[MAP_WIFI_MAX_LEVELS]; } map_cell_t; - +#pragma pack(pop) // Description for a map typedef struct diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index cdd4c5adbd..1a10c7aa41 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -227,6 +227,11 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) add_parameter( "first_map_only", rclcpp::ParameterValue(false), "Set this to true, when you want to load a new map published from the map_server"); + + add_parameter( + "freespace_downsampling", rclcpp::ParameterValue( + false), + "Downsample the free space used by the Pose Generator. Use it with large maps to save memory"); } AmclNode::~AmclNode() @@ -404,7 +409,7 @@ AmclNode::checkElapsedTime(std::chrono::seconds check_interval, rclcpp::Time las } #if NEW_UNIFORM_SAMPLING -std::vector<std::pair<int, int>> AmclNode::free_space_indices; +std::vector<AmclNode::Point2D> AmclNode::free_space_indices; #endif bool @@ -446,10 +451,10 @@ AmclNode::uniformPoseGenerator(void * arg) #if NEW_UNIFORM_SAMPLING unsigned int rand_index = drand48() * free_space_indices.size(); - std::pair<int, int> free_point = free_space_indices[rand_index]; + AmclNode::Point2D free_point = free_space_indices[rand_index]; pf_vector_t p; - p.v[0] = MAP_WXGX(map, free_point.first); - p.v[1] = MAP_WYGY(map, free_point.second); + p.v[0] = MAP_WXGX(map, free_point.x); + p.v[1] = MAP_WYGY(map, free_point.y); p.v[2] = drand48() * 2 * M_PI - M_PI; #else double min_x, max_x, min_y, max_y; @@ -1106,6 +1111,7 @@ AmclNode::initParameters() get_parameter("always_reset_initial_pose", always_reset_initial_pose_); get_parameter("scan_topic", scan_topic_); get_parameter("map_topic", map_topic_); + get_parameter("freespace_downsampling", freespace_downsampling_); save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate); transform_tolerance_ = tf2::durationFromSec(tmp_tol); @@ -1435,12 +1441,14 @@ AmclNode::handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg) void AmclNode::createFreeSpaceVector() { + int delta = freespace_downsampling_ ? 2 : 1; // Index of free space free_space_indices.resize(0); - for (int i = 0; i < map_->size_x; i++) { - for (int j = 0; j < map_->size_y; j++) { + for (int i = 0; i < map_->size_x; i += delta) { + for (int j = 0; j < map_->size_y; j += delta) { if (map_->cells[MAP_INDEX(map_, i, j)].occ_state == -1) { - free_space_indices.push_back(std::make_pair(i, j)); + AmclNode::Point2D point = {i, j}; + free_space_indices.push_back(point); } } } From 29ba4b1520d58c47daca5032d400177fff85aa17 Mon Sep 17 00:00:00 2001 From: Alberto Tudela <ajtudela@gmail.com> Date: Fri, 14 Jun 2024 03:13:00 +0200 Subject: [PATCH 04/18] Added dynamic reconfigure and export controller in docking (#4429) * Added dynamic reconfigure and export controller in docking Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix vector Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update controller.cpp Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --------- Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --- nav2_docking/opennav_docking/CMakeLists.txt | 17 +++-- .../include/opennav_docking/controller.hpp | 15 +++++ .../opennav_docking/src/controller.cpp | 66 +++++++++++++++---- .../opennav_docking/test/test_controller.cpp | 34 ++++++++++ 4 files changed, 117 insertions(+), 15 deletions(-) diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt index 38e9b79516..e50590d3e4 100644 --- a/nav2_docking/opennav_docking/CMakeLists.txt +++ b/nav2_docking/opennav_docking/CMakeLists.txt @@ -56,8 +56,15 @@ set(dependencies opennav_docking_core ) -add_library(${library_name} SHARED +add_library(controller SHARED src/controller.cpp +) + +ament_target_dependencies(controller + ${dependencies} +) + +add_library(${library_name} SHARED src/docking_server.cpp src/dock_database.cpp src/navigator.cpp @@ -68,7 +75,9 @@ ament_target_dependencies(${library_name} ) target_link_libraries(${library_name} - yaml-cpp::yaml-cpp) + yaml-cpp::yaml-cpp + controller +) add_library(pose_filter SHARED src/pose_filter.cpp @@ -100,7 +109,7 @@ target_link_libraries(simple_charging_dock pose_filter) pluginlib_export_plugin_description_file(opennav_docking_core plugins.xml) -install(TARGETS ${library_name} pose_filter simple_charging_dock +install(TARGETS ${library_name} controller pose_filter simple_charging_dock ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -127,6 +136,6 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(${library_name} pose_filter) +ament_export_libraries(${library_name} controller pose_filter) ament_export_dependencies(${dependencies} yaml-cpp) ament_package() diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp index 54472e720f..afae5965d2 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp @@ -16,6 +16,7 @@ #define OPENNAV_DOCKING__CONTROLLER_HPP_ #include <memory> +#include <vector> #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -47,8 +48,22 @@ class Controller const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool backward = false); + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters); + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::mutex dynamic_params_lock_; + protected: std::unique_ptr<nav2_graceful_controller::SmoothControlLaw> control_law_; + + double k_phi_, k_delta_, beta_, lambda_; + double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_; }; } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index 3060aaf8c8..8613f2d27f 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -40,25 +40,69 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) nav2_util::declare_parameter_if_not_declared( node, "controller.slowdown_radius", rclcpp::ParameterValue(0.25)); - double k_phi, k_delta, beta, lambda; - double slowdown_radius, v_linear_min, v_linear_max, v_angular_max; - node->get_parameter("controller.k_phi", k_phi); - node->get_parameter("controller.k_delta", k_delta); - node->get_parameter("controller.beta", beta); - node->get_parameter("controller.lambda", lambda); - node->get_parameter("controller.v_linear_min", v_linear_min); - node->get_parameter("controller.v_linear_max", v_linear_max); - node->get_parameter("controller.v_angular_max", v_angular_max); - node->get_parameter("controller.slowdown_radius", slowdown_radius); + node->get_parameter("controller.k_phi", k_phi_); + node->get_parameter("controller.k_delta", k_delta_); + node->get_parameter("controller.beta", beta_); + node->get_parameter("controller.lambda", lambda_); + node->get_parameter("controller.v_linear_min", v_linear_min_); + node->get_parameter("controller.v_linear_max", v_linear_max_); + node->get_parameter("controller.v_angular_max", v_angular_max_); + node->get_parameter("controller.slowdown_radius", slowdown_radius_); control_law_ = std::make_unique<nav2_graceful_controller::SmoothControlLaw>( - k_phi, k_delta, beta, lambda, slowdown_radius, v_linear_min, v_linear_max, v_angular_max); + k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, v_linear_min_, v_linear_max_, + v_angular_max_); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&Controller::dynamicParametersCallback, this, std::placeholders::_1)); } bool Controller::computeVelocityCommand( const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool backward) { + std::lock_guard<std::mutex> lock(dynamic_params_lock_); cmd = control_law_->calculateRegularVelocity(pose, backward); return true; } +rcl_interfaces::msg::SetParametersResult +Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters) +{ + std::lock_guard<std::mutex> lock(dynamic_params_lock_); + + rcl_interfaces::msg::SetParametersResult result; + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) { + if (name == "controller.k_phi") { + k_phi_ = parameter.as_double(); + } else if (name == "controller.k_delta") { + k_delta_ = parameter.as_double(); + } else if (name == "controller.beta") { + beta_ = parameter.as_double(); + } else if (name == "controller.lambda") { + lambda_ = parameter.as_double(); + } else if (name == "controller.v_linear_min") { + v_linear_min_ = parameter.as_double(); + } else if (name == "controller.v_linear_max") { + v_linear_max_ = parameter.as_double(); + } else if (name == "controller.v_angular_max") { + v_angular_max_ = parameter.as_double(); + } else if (name == "controller.slowdown_radius") { + slowdown_radius_ = parameter.as_double(); + } + + // Update the smooth control law with the new params + control_law_->setCurvatureConstants(k_phi_, k_delta_, beta_, lambda_); + control_law_->setSlowdownRadius(slowdown_radius_); + control_law_->setSpeedLimit(v_linear_min_, v_linear_max_, v_angular_max_); + } + } + + result.successful = true; + return result; +} + } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp index da8885d89d..c2f1e25f1c 100644 --- a/nav2_docking/opennav_docking/test/test_controller.cpp +++ b/nav2_docking/opennav_docking/test/test_controller.cpp @@ -45,4 +45,38 @@ TEST(ControllerTests, ObjectLifecycle) controller.reset(); } +TEST(ControllerTests, DynamicParameters) { + auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test"); + auto controller = std::make_shared<opennav_docking::Controller>(node); + + auto params = std::make_shared<rclcpp::AsyncParametersClient>( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + // Set parameters + auto results = params->set_parameters_atomically( + {rclcpp::Parameter("controller.k_phi", 1.0), + rclcpp::Parameter("controller.k_delta", 2.0), + rclcpp::Parameter("controller.beta", 3.0), + rclcpp::Parameter("controller.lambda", 4.0), + rclcpp::Parameter("controller.v_linear_min", 5.0), + rclcpp::Parameter("controller.v_linear_max", 6.0), + rclcpp::Parameter("controller.v_angular_max", 7.0), + rclcpp::Parameter("controller.slowdown_radius", 8.0)}); + + // Spin + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); + + // Check parameters + EXPECT_EQ(node->get_parameter("controller.k_phi").as_double(), 1.0); + EXPECT_EQ(node->get_parameter("controller.k_delta").as_double(), 2.0); + EXPECT_EQ(node->get_parameter("controller.beta").as_double(), 3.0); + EXPECT_EQ(node->get_parameter("controller.lambda").as_double(), 4.0); + EXPECT_EQ(node->get_parameter("controller.v_linear_min").as_double(), 5.0); + EXPECT_EQ(node->get_parameter("controller.v_linear_max").as_double(), 6.0); + EXPECT_EQ(node->get_parameter("controller.v_angular_max").as_double(), 7.0); + EXPECT_EQ(node->get_parameter("controller.slowdown_radius").as_double(), 8.0); +} + } // namespace opennav_docking From 635880d3d6ad1f23eb75d793409189c653797f58 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy <doisyg@users.noreply.github.com> Date: Fri, 14 Jun 2024 03:15:39 +0200 Subject: [PATCH 05/18] Throttle and switch to DEBUG bt loop rate warning (#4430) Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> --- .../include/nav2_behavior_tree/behavior_tree_engine.hpp | 6 +++++- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 2 +- nav2_behavior_tree/src/behavior_tree_engine.cpp | 8 ++++++-- 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 9ee903fd78..a0d86649c0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -47,7 +47,8 @@ class BehaviorTreeEngine * @param plugin_libraries vector of BT plugin library names to load */ explicit BehaviorTreeEngine( - const std::vector<std::string> & plugin_libraries); + const std::vector<std::string> & plugin_libraries, + rclcpp::Node::SharedPtr node); virtual ~BehaviorTreeEngine() {} /** @@ -93,6 +94,9 @@ class BehaviorTreeEngine protected: // The factory that will be used to dynamically construct the behavior tree BT::BehaviorTreeFactory factory_; + + // Clock + rclcpp::Clock::SharedPtr clock_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 75dfff5597..d2b120f44c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -173,7 +173,7 @@ bool BtActionServer<ActionT>::on_configure() error_code_names_ = node->get_parameter("error_code_names").as_string_array(); // Create the class that registers our custom nodes and executes the BT - bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_); + bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_, client_node_); // Create the blackboard that will be shared by all of the nodes in the tree blackboard_ = BT::Blackboard::create(); diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 8ed1cd4e71..70a192d719 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -26,13 +26,16 @@ namespace nav2_behavior_tree { BehaviorTreeEngine::BehaviorTreeEngine( - const std::vector<std::string> & plugin_libraries) + const std::vector<std::string> & plugin_libraries, rclcpp::Node::SharedPtr node) { BT::SharedLibrary loader; for (const auto & p : plugin_libraries) { factory_.registerFromPlugin(loader.getOSName(p)); } + // clock for throttled debug log + clock_ = node->get_clock(); + // FIXME: the next two line are needed for back-compatibility with BT.CPP 3.8.x // Note that the can be removed, once we migrate from BT.CPP 4.5.x to 4.6+ BT::ReactiveSequence::EnableException(false); @@ -62,8 +65,9 @@ BehaviorTreeEngine::run( onLoop(); if (!loopRate.sleep()) { - RCLCPP_WARN( + RCLCPP_DEBUG_THROTTLE( rclcpp::get_logger("BehaviorTreeEngine"), + *clock_, 1000, "Behavior Tree tick rate %0.2f was exceeded!", 1.0 / (loopRate.period().count() * 1.0e-9)); } From d5128204d98a735b3edba81cfcb48d764c7a1774 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 18 Jun 2024 08:47:36 -0700 Subject: [PATCH 06/18] =?UTF-8?q?=F0=9F=9B=A0=EF=B8=8F=20Bump=20docker/bui?= =?UTF-8?q?ld-push-action=20from=205=20to=206=20(#4444)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Bumps [docker/build-push-action](https://github.com/docker/build-push-action) from 5 to 6. - [Release notes](https://github.com/docker/build-push-action/releases) - [Commits](https://github.com/docker/build-push-action/compare/v5...v6) --- updated-dependencies: - dependency-name: docker/build-push-action dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] <support@github.com> Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/update_ci_image.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml index ce987eb118..16d172aff8 100644 --- a/.github/workflows/update_ci_image.yaml +++ b/.github/workflows/update_ci_image.yaml @@ -99,7 +99,7 @@ jobs: - name: Build and push ${{ github.ref_name }} if: steps.config.outputs.trigger == 'true' id: docker_build - uses: docker/build-push-action@v5 + uses: docker/build-push-action@v6 with: context: . pull: true From 91b4f8ee3e892469992695eef61f6086128598db Mon Sep 17 00:00:00 2001 From: Tony Najjar <tony.najjar.1997@gmail.com> Date: Tue, 18 Jun 2024 17:48:24 +0200 Subject: [PATCH 07/18] Warn if inflation_radius_ < inscribed_radius_ (#4423) * Warn if inflation_radius_ < inscribed_radius_ Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * convert to error Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --- nav2_costmap_2d/plugins/inflation_layer.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index e2542aa9cf..01b788a7cc 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -170,6 +170,16 @@ InflationLayer::onFootprintChanged() computeCaches(); need_reinflation_ = true; + if (inflation_radius_ < inscribed_radius_) { + RCLCPP_ERROR( + logger_, + "The configured inflation radius (%.3f) is smaller than " + "the computed inscribed radius (%.3f) of your footprint, " + "it is highly recommended to set inflation radius to be at " + "least as big as the inscribed radius to avoid collisions", + inflation_radius_, inscribed_radius_); + } + RCLCPP_DEBUG( logger_, "InflationLayer::onFootprintChanged(): num footprint points: %zu," " inscribed_radius_ = %.3f, inflation_radius_ = %.3f", From ef40391157874eb3586e4512663d74dd6de5bdc6 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn <reinzor@gmail.com> Date: Tue, 18 Jun 2024 17:48:48 +0200 Subject: [PATCH 08/18] chore: cleanup ros1 leftovers (#4446) Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl> --- nav2_costmap_2d/cfg/Costmap2D.cfg | 23 ---------- nav2_costmap_2d/cfg/GenericPlugin.cfg | 7 --- nav2_costmap_2d/cfg/InflationPlugin.cfg | 12 ------ nav2_costmap_2d/cfg/ObstaclePlugin.cfg | 22 ---------- nav2_costmap_2d/cfg/VoxelPlugin.cfg | 22 ---------- nav2_costmap_2d/launch/example.launch | 21 --------- nav2_costmap_2d/launch/example_params.yaml | 43 ------------------- .../dwb_plugins/cfg/KinematicParams.cfg | 33 -------------- 8 files changed, 183 deletions(-) delete mode 100755 nav2_costmap_2d/cfg/Costmap2D.cfg delete mode 100755 nav2_costmap_2d/cfg/GenericPlugin.cfg delete mode 100755 nav2_costmap_2d/cfg/InflationPlugin.cfg delete mode 100755 nav2_costmap_2d/cfg/ObstaclePlugin.cfg delete mode 100755 nav2_costmap_2d/cfg/VoxelPlugin.cfg delete mode 100644 nav2_costmap_2d/launch/example.launch delete mode 100644 nav2_costmap_2d/launch/example_params.yaml delete mode 100644 nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg diff --git a/nav2_costmap_2d/cfg/Costmap2D.cfg b/nav2_costmap_2d/cfg/Costmap2D.cfg deleted file mode 100755 index 07c4a1628b..0000000000 --- a/nav2_costmap_2d/cfg/Costmap2D.cfg +++ /dev/null @@ -1,23 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, str_t - -gen = ParameterGenerator() - -gen.add("transform_tolerance", double_t, 0, "Specifies the delay in transform (tf) data that is tolerable in seconds.", 0.3, 0, 10) -gen.add("update_frequency", double_t, 0, "The frequency in Hz for the map to be updated.", 5, 0, 100) -gen.add("publish_frequency", double_t, 0, "The frequency in Hz for the map to be publish display information.", 0, 0, 100) - -#map params -gen.add("width", int_t, 0, "The width of the map in meters.", 10, 0) -gen.add("height", int_t, 0, "The height of the map in meters.", 10, 0) -gen.add("resolution", double_t, 0, "The resolution of the map in meters/cell.", 0.05, 0, 50) -gen.add("origin_x", double_t, 0, "The x origin of the map in the global frame in meters.", 0) -gen.add("origin_y", double_t, 0, "The y origin of the map in the global frame in meters.", 0) - -# robot footprint shape -gen.add("footprint", str_t, 0, "The footprint of the robot specified in the robot_base_frame coordinate frame as a list in the format: [ [x1, y1], [x2, y2], ...., [xn, yn] ].", "[]") -gen.add("robot_radius", double_t, 0, 'The radius of the robot in meters, this parameter should only be set for circular robots, all others should use the footprint parameter described above.', 0.46, 0, 10) -gen.add("footprint_padding", double_t, 0, "How much to pad (increase the size of) the footprint, in meters.", 0.01) - -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "Costmap2D")) diff --git a/nav2_costmap_2d/cfg/GenericPlugin.cfg b/nav2_costmap_2d/cfg/GenericPlugin.cfg deleted file mode 100755 index 555e2b5415..0000000000 --- a/nav2_costmap_2d/cfg/GenericPlugin.cfg +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t - -gen = ParameterGenerator() -gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "GenericPlugin")) diff --git a/nav2_costmap_2d/cfg/InflationPlugin.cfg b/nav2_costmap_2d/cfg/InflationPlugin.cfg deleted file mode 100755 index 5c11eaf791..0000000000 --- a/nav2_costmap_2d/cfg/InflationPlugin.cfg +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t - -gen = ParameterGenerator() - -gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -gen.add("cost_scaling_factor", double_t, 0, "A scaling factor to apply to cost values during inflation.", 10, 0, 100) -gen.add("inflation_radius", double_t, 0, "The radius in meters to which the map inflates obstacle cost values.", 0.55, 0, 50) -gen.add("inflate_unknown", bool_t, 0, "Whether to inflate unknown cells.", False) - -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "InflationPlugin")) diff --git a/nav2_costmap_2d/cfg/ObstaclePlugin.cfg b/nav2_costmap_2d/cfg/ObstaclePlugin.cfg deleted file mode 100755 index a94e98fbbe..0000000000 --- a/nav2_costmap_2d/cfg/ObstaclePlugin.cfg +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t - -gen = ParameterGenerator() - -gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) -gen.add("max_obstacle_height", double_t, 0, "The maximum height of any obstacle to be inserted into the costmap in meters.", 2, 0, 50) - -combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), - gen.const("Maximum", int_t, 1, "Take the maximum of the values"), - gen.const("Nothing", int_t, 99, "Do nothing")], - "Method for combining layers enum") -gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, edit_method=combo_enum) - - -# gen.add("obstacle_max_range", double_t, 0, "The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 2.5, 0, 50) -# gen.add("obstacle_min_range", double_t, 0, "The default minimum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 0.0, 0, 50) -# gen.add("raytrace_max_range", double_t, 0, "The default maximum range in meters at which to raytrace out obstacles from the map using sensor data.", 3, 0, 50) -# gen.add("raytrace_min_range", double_t, 0, "The default minimum range in meters from which to raytrace out obstacles from the map using sensor data.", 0.0, 0, 50) -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "ObstaclePlugin")) diff --git a/nav2_costmap_2d/cfg/VoxelPlugin.cfg b/nav2_costmap_2d/cfg/VoxelPlugin.cfg deleted file mode 100755 index 977fcb99d4..0000000000 --- a/nav2_costmap_2d/cfg/VoxelPlugin.cfg +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t - -gen = ParameterGenerator() - -gen.add("enabled", bool_t, 0, "Whether to use this plugin or not", True) -gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) -gen.add("max_obstacle_height", double_t, 0, "Max Obstacle Height", 2.0, 0, 50) -gen.add("origin_z", double_t, 0, "The z origin of the map in meters.", 0, 0) -gen.add("z_resolution", double_t, 0, "The z resolution of the map in meters/cell.", 0.2, 0, 50) -gen.add("z_voxels", int_t, 0, "The number of voxels to in each vertical column.", 10, 0, 16) -gen.add("unknown_threshold", int_t, 0, 'The number of unknown cells allowed in a column considered to be known', 15, 0, 16) -gen.add("mark_threshold", int_t, 0, 'The maximum number of marked cells allowed in a column considered to be free', 0, 0, 16) - -combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), - gen.const("Maximum", int_t, 1, "Take the maximum of the values"), - gen.const("Nothing", int_t, 99, "Do nothing")], - "Method for combining layers enum") -gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, 0, 2, edit_method=combo_enum) - -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "VoxelPlugin")) diff --git a/nav2_costmap_2d/launch/example.launch b/nav2_costmap_2d/launch/example.launch deleted file mode 100644 index ac089abfba..0000000000 --- a/nav2_costmap_2d/launch/example.launch +++ /dev/null @@ -1,21 +0,0 @@ -<launch> - <!-- - NOTE: You'll need to bring up something that publishes sensor data (see - rosstage), something that publishes a map (see map_server), and something to - visualize a costmap (see nav_view), to see things work. - - Also, on a real robot, you'd want to set the "use_sim_time" parameter to false, or just not set it. - --> - <param name="/use_sim_time" value="true"/> - - <!-- Publishes the voxel grid to rviz for display --> - <node pkg="nav2_costmap_2d" type="nav2_costmap_2d_markers" name="voxel_visualizer"> - <remap from="voxel_grid" to="costmap/voxel_grid"/> - </node> - - <!-- Run the costmap node --> - <node name="costmap_node" pkg="nav2_costmap_2d" type="nav2_costmap_2d_node" > - <rosparam file="$(find nav2_costmap_2d)/launch/example_params.yaml" command="load" ns="costmap" /> - </node> - -</launch> diff --git a/nav2_costmap_2d/launch/example_params.yaml b/nav2_costmap_2d/launch/example_params.yaml deleted file mode 100644 index 72bf695fad..0000000000 --- a/nav2_costmap_2d/launch/example_params.yaml +++ /dev/null @@ -1,43 +0,0 @@ -global_frame: map -robot_base_frame: base_link -update_frequency: 5.0 -publish_frequency: 1.0 - -#set if you want the voxel map published -publish_voxel_map: true - -#set to true if you want to initialize the costmap from a static map -static_map: false - -#begin - COMMENT these lines if you set static_map to true -rolling_window: true -width: 6.0 -height: 6.0 -resolution: 0.025 -#end - COMMENT these lines if you set static_map to true - -#START VOXEL STUFF -map_type: voxel -origin_z: 0.0 -z_resolution: 0.2 -z_voxels: 10 -unknown_threshold: 10 -mark_threshold: 0 -#END VOXEL STUFF - -transform_tolerance: 0.3 -obstacle_max_range: 2.5 -obstacle_min_range: 0.0 -max_obstacle_height: 2.0 -raytrace_max_range: 3.0 -raytrace_min_range: 0.0 - -footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] -#robot_radius: 0.46 -footprint_padding: 0.01 -inflation_radius: 0.55 -cost_scaling_factor: 10.0 -lethal_cost_threshold: 100 -observation_sources: base_scan -base_scan: {data_type: LaserScan, expected_update_rate: 0.4, - observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} diff --git a/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg b/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg deleted file mode 100644 index 51dab28aa4..0000000000 --- a/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/env python -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t - -gen = ParameterGenerator() - -# velocities -gen.add('min_vel_x', double_t, 0, 'The minimum x velocity for the robot in m/s', 0.0) -gen.add('max_vel_x', double_t, 0, 'The maximum x velocity for the robot in m/s', 0.55) -gen.add('min_vel_y', double_t, 0, 'The minimum y velocity for the robot in m/s', -0.1) -gen.add('max_vel_y', double_t, 0, 'The maximum y velocity for the robot in m/s', 0.1) -gen.add('max_vel_theta', double_t, 0, 'The absolute value of the maximum rotational velocity for the robot in rad/s. ' - 'The minimum rotational velocity is assumed to be -max_vel_theta', 1.0) - -# acceleration -gen.add('acc_lim_x', double_t, 0, 'The acceleration limit of the robot in the x direction in m/s^2', 2.5) -gen.add('acc_lim_y', double_t, 0, 'The acceleration limit of the robot in the y direction in m/s^2', 2.5) -gen.add('acc_lim_theta', double_t, 0, 'The acceleration limit of the robot in the theta direction in rad/s^2', 3.2) - -gen.add('decel_lim_x', double_t, 0, 'The deceleration limit of the robot in the x direction in m/s^2', -2.5) -gen.add('decel_lim_y', double_t, 0, 'The deceleration limit of the robot in the y direction in m/s^2', -2.5) -gen.add('decel_lim_theta', double_t, 0, 'The deceleration limit of the robot in the theta direction in rad/s^2', -3.2) - -gen.add('min_speed_xy', double_t, 0, 'The absolute value of the minimum translational/xy velocity in m/s. ' - 'If the value is negative, then the min speed will be arbitrarily close to 0.0. ' - 'Previously called min_trans_vel', 0.1) -gen.add('max_speed_xy', double_t, 0, 'The absolute value of the maximum translational/xy velocity in m/s. ' - 'If the value is negative, then the max speed is hypot(max_vel_x, max_vel_y). ' - 'Previously called max_trans_vel', 0.55) -gen.add('min_speed_theta', double_t, 0, 'The absolute value of the minimum rotational velocity in rad/s. ' - 'If the value is negative, then the min speed will be arbitrarily close to 0.0.' - ' Previously called min_rot_vel', 0.4) - -exit(gen.generate('dwb_plugins', 'dwb_plugins', 'KinematicParams')) From 6a52f463f54bc289df663aba208e2b363f0a755c Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Tue, 18 Jun 2024 19:55:58 +0200 Subject: [PATCH 09/18] Port Localization nav2_system_tests to new gazebo (#4431) * include localization Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * uncomment error code in cmakelist Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * formmatting Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * formmatting Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * formmatting python Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * formmatting python Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * removed unused path Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> --------- Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> --- nav2_system_tests/CMakeLists.txt | 2 +- .../src/localization/CMakeLists.txt | 3 -- .../localization/test_localization_launch.py | 51 +++++++++++++++---- 3 files changed, 43 insertions(+), 13 deletions(-) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 45a8097b35..d94298da77 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -114,7 +114,7 @@ if(BUILD_TESTING) add_subdirectory(src/behavior_tree) add_subdirectory(src/planning) # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634 - # add_subdirectory(src/localization) + add_subdirectory(src/localization) # add_subdirectory(src/system) # add_subdirectory(src/system_failure) # add_subdirectory(src/updown) diff --git a/nav2_system_tests/src/localization/CMakeLists.txt b/nav2_system_tests/src/localization/CMakeLists.txt index 740d38f53e..7fa35172c3 100644 --- a/nav2_system_tests/src/localization/CMakeLists.txt +++ b/nav2_system_tests/src/localization/CMakeLists.txt @@ -11,8 +11,5 @@ ament_add_test(test_localization COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_localization_launch.py" TIMEOUT 180 ENV - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml TEST_EXECUTABLE=$<TARGET_FILE:test_localization_node> - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models ) diff --git a/nav2_system_tests/src/localization/test_localization_launch.py b/nav2_system_tests/src/localization/test_localization_launch.py index d638e02c4e..aedca4fcf7 100755 --- a/nav2_system_tests/src/localization/test_localization_launch.py +++ b/nav2_system_tests/src/localization/test_localization_launch.py @@ -17,23 +17,54 @@ import os import sys +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch import LaunchService -import launch.actions -from launch.actions import ExecuteProcess +from launch.actions import AppendEnvironmentVariable, ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource import launch_ros.actions from launch_testing.legacy import LaunchTestService def main(argv=sys.argv[1:]): - mapFile = os.getenv('TEST_MAP') testExecutable = os.getenv('TEST_EXECUTABLE') - world = os.getenv('TEST_WORLD') + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + ros_gz_sim_dir = get_package_share_directory('ros_gz_sim') - launch_gazebo = launch.actions.ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', '--minimal_comms', world], - output='screen', + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') + + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') + ) + + start_gazebo_server = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(), + ) + + spawn_robot = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-2.0', + 'y_pose': '-0.5', + 'z_pose': '0.01', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), ) + link_footprint = launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', @@ -51,7 +82,7 @@ def main(argv=sys.argv[1:]): executable='map_server', name='map_server', output='screen', - parameters=[{'yaml_filename': mapFile}], + parameters=[{'yaml_filename': map_yaml_file}], ) run_amcl = launch_ros.actions.Node( package='nav2_amcl', executable='amcl', output='screen' @@ -65,7 +96,9 @@ def main(argv=sys.argv[1:]): ) ld = LaunchDescription( [ - launch_gazebo, + set_env_vars_resources, + start_gazebo_server, + spawn_robot, link_footprint, footprint_scan, run_map_server, From 8f795a2bc5802b71da982d9ab48910c7d3dfc6da Mon Sep 17 00:00:00 2001 From: Jatin Patil <89979346+JatinPatil2003@users.noreply.github.com> Date: Tue, 18 Jun 2024 23:56:32 +0530 Subject: [PATCH 10/18] Added RPP use_cancel_deceleration parameter (#4441) * Added RPP use_cancel_deceleration parameter Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com> * Fixed Linting code style Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com> --------- Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com> --- .../parameter_handler.hpp | 1 + .../src/parameter_handler.cpp | 5 +++++ .../src/regulated_pure_pursuit_controller.cpp | 4 ++++ 3 files changed, 10 insertions(+) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index d2258ceb80..b033289622 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -53,6 +53,7 @@ struct Parameters double curvature_lookahead_dist; bool use_rotate_to_heading; double max_angular_accel; + bool use_cancel_deceleration; double cancel_deceleration; double rotate_to_heading_min_angle; bool allow_reversing; diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index a47af13ef0..42876c248e 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -85,6 +85,8 @@ ParameterHandler::ParameterHandler( node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); declare_parameter_if_not_declared( node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_cancel_deceleration", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( node, plugin_name_ + ".cancel_deceleration", rclcpp::ParameterValue(3.2)); declare_parameter_if_not_declared( @@ -153,6 +155,7 @@ ParameterHandler::ParameterHandler( node->get_parameter( plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle); node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel); + node->get_parameter(plugin_name_ + ".use_cancel_deceleration", params_.use_cancel_deceleration); node->get_parameter(plugin_name_ + ".cancel_deceleration", params_.cancel_deceleration); node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing); node->get_parameter( @@ -258,6 +261,8 @@ ParameterHandler::dynamicParametersCallback( params_.use_collision_detection = parameter.as_bool(); } else if (name == plugin_name_ + ".use_rotate_to_heading") { params_.use_rotate_to_heading = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_cancel_deceleration") { + params_.use_cancel_deceleration = parameter.as_bool(); } else if (name == plugin_name_ + ".allow_reversing") { if (params_.use_rotate_to_heading && parameter.as_bool()) { RCLCPP_WARN( diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 423d0d1a04..edcd6db558 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -290,6 +290,10 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity bool RegulatedPurePursuitController::cancel() { + // if false then publish zero velocity + if (!params_->use_cancel_deceleration) { + return true; + } cancelling_ = true; return finished_cancelling_; } From 23a48154bef1be3b063c7566698a2fdf4f497f66 Mon Sep 17 00:00:00 2001 From: Alberto Tudela <ajtudela@gmail.com> Date: Tue, 18 Jun 2024 20:48:30 +0200 Subject: [PATCH 11/18] Fix unexpected behavior of docking when already in charger (#4448) * Check isDocked before dock/undock Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Linting Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Add testing Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --------- Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --- nav2_docking/opennav_docking/src/docking_server.cpp | 12 ++++++++++++ .../test/test_docking_server_unit.cpp | 8 +++++--- nav2_docking/opennav_docking/test/testing_dock.cpp | 4 +++- 3 files changed, 20 insertions(+), 4 deletions(-) diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index c70e4b29aa..2d533c0871 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -235,6 +235,12 @@ void DockingServer::dockRobot() dock = generateGoalDock(goal); } + // Check if the robot is docked or charging before proceeding + if (dock->plugin->isDocked() || dock->plugin->isCharging()) { + RCLCPP_INFO(get_logger(), "Robot is already charging, no need to dock"); + return; + } + // Send robot to its staging pose publishDockingFeedback(DockRobot::Feedback::NAV_TO_STAGING_POSE); const auto initial_staging_pose = dock->getStagingPose(); @@ -569,6 +575,12 @@ void DockingServer::undockRobot() get_logger(), "Attempting to undock robot from charger of type %s.", dock->getName().c_str()); + // Check if the robot is docked before proceeding + if (!dock->isDocked()) { + RCLCPP_INFO(get_logger(), "Robot is not in the charger, no need to undock"); + return; + } + // Get "dock pose" by finding the robot pose geometry_msgs::msg::PoseStamped dock_pose = getRobotPoseInFrame(fixed_frame_); diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp index 74da62205f..144a612f3b 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp +++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp @@ -86,9 +86,8 @@ TEST(DockingServerTests, testErrorExceptions) node->on_configure(rclcpp_lifecycle::State()); node->on_activate(rclcpp_lifecycle::State()); - node->declare_parameter( - "exception_to_throw", - rclcpp::ParameterValue("")); + node->declare_parameter("exception_to_throw", rclcpp::ParameterValue("")); + node->declare_parameter("dock_action_called", rclcpp::ParameterValue(false)); // Error codes docking std::vector<std::string> error_ids{ @@ -142,6 +141,9 @@ TEST(DockingServerTests, testErrorExceptions) "DockingException", "exception"}; std::vector<int> error_codes_undocking{999, 902, 905, 999, 999}; + // Set dock_action_called to true to simulate robot being docked in dock plugin + node->set_parameter(rclcpp::Parameter("dock_action_called", true)); + // Call action, check error code for (unsigned int i = 0; i != error_ids_undocking.size(); i++) { node->set_parameter( diff --git a/nav2_docking/opennav_docking/test/testing_dock.cpp b/nav2_docking/opennav_docking/test/testing_dock.cpp index a51924533e..4c17eb000c 100644 --- a/nav2_docking/opennav_docking/test/testing_dock.cpp +++ b/nav2_docking/opennav_docking/test/testing_dock.cpp @@ -80,7 +80,9 @@ class TestFailureDock : public opennav_docking_core::ChargingDock virtual bool isDocked() { - return false; + bool dock_action_called; + node_->get_parameter("dock_action_called", dock_action_called); + return dock_action_called; } virtual bool isCharging() From 6026754da6d1fb2986b4c6be260ea8f500768a0e Mon Sep 17 00:00:00 2001 From: Chris Lalancette <clalancette@gmail.com> Date: Wed, 19 Jun 2024 11:28:16 -0400 Subject: [PATCH 12/18] Revamp nav2_util CMakeLists.txt to use modern idioms. (#4393) This commit does a number of things: 1. Switches to using target_link_libraries everywhere. This gives us finer-grained control over what dependencies are exported to downstream as public, or private. In the particular case of nav2_util, this actually doesn't matter *too* much, but it will help for other packages. 2. Moves the include directory down one level to include/${PROJECT_NAME}, which is best practice in ROS 2 since Humble. 3. Makes sure to export nav2_util as a CMake target, so downstream users of it can use that target. 4. Moves the base_footprint_publisher.hpp header file into the src directory, as it isn't functionality that an external project could use. Signed-off-by: Chris Lalancette <clalancette@gmail.com> --- nav2_util/CMakeLists.txt | 59 +++++++++---------- .../include/nav2_util/lifecycle_node.hpp | 1 + nav2_util/package.xml | 30 ++++------ nav2_util/src/CMakeLists.txt | 39 +++++++----- nav2_util/src/base_footprint_publisher.cpp | 2 +- .../base_footprint_publisher.hpp | 6 +- nav2_util/src/lifecycle_utils.cpp | 19 +++--- nav2_util/src/string_utils.cpp | 10 ++-- nav2_util/test/CMakeLists.txt | 39 +++++------- .../test/test_base_footprint_publisher.cpp | 2 +- 10 files changed, 96 insertions(+), 111 deletions(-) rename nav2_util/{include/nav2_util => src}/base_footprint_publisher.hpp (96%) diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 1c8b9b1d64..6a82f01511 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -2,54 +2,36 @@ cmake_minimum_required(VERSION 3.5) project(nav2_util) find_package(ament_cmake REQUIRED) +find_package(bond REQUIRED) +find_package(bondcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(lifecycle_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_msgs REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(rcl_interfaces REQUIRED) find_package(rclcpp REQUIRED) -find_package(lifecycle_msgs REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -find_package(bondcpp REQUIRED) -find_package(bond REQUIRED) -find_package(action_msgs REQUIRED) - -set(dependencies - nav2_msgs - tf2_ros - tf2 - tf2_geometry_msgs - geometry_msgs - nav_msgs - rclcpp - lifecycle_msgs - rclcpp_action - rclcpp_lifecycle - bondcpp - bond - action_msgs - rcl_interfaces -) +find_package(tf2_msgs REQUIRED) +find_package(tf2_ros REQUIRED) nav2_package() -include_directories(include) - set(library_name ${PROJECT_NAME}_core) + add_subdirectory(src) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_pytest REQUIRED) - find_package(launch_testing_ament_cmake REQUIRED) # skip copyright linting set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() @@ -58,8 +40,23 @@ if(BUILD_TESTING) add_subdirectory(test) endif() -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(${library_name}) -ament_export_dependencies(${dependencies}) +ament_export_dependencies( + bondcpp + geometry_msgs + lifecycle_msgs + nav2_msgs + nav_msgs + rcl_interfaces + rclcpp + rclcpp_action + rclcpp_lifecycle + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros +) +ament_export_targets(${library_name}) ament_package() diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index 7f1ef07157..d652982aef 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -19,6 +19,7 @@ #include <string> #include <thread> +#include "rcl_interfaces/msg/parameter_descriptor.hpp" #include "nav2_util/node_thread.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 9de53f5fd5..26274e10d6 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -10,37 +10,31 @@ <license>BSD-3-Clause</license> <buildtool_depend>ament_cmake</buildtool_depend> - <build_depend>libboost-program-options-dev</build_depend> + <depend>nav2_common</depend> + <depend>bond</depend> + <depend>bondcpp</depend> + <depend>builtin_interfaces</depend> <depend>geometry_msgs</depend> - <depend>rclcpp</depend> + <depend>lifecycle_msgs</depend> <depend>nav2_msgs</depend> <depend>nav_msgs</depend> - <depend>tf2</depend> - <depend>tf2_ros</depend> - <depend>tf2_geometry_msgs</depend> - <depend>lifecycle_msgs</depend> - <depend>bondcpp</depend> - <depend>bond</depend> + <depend>rcl_interfaces</depend> + <depend>rclcpp</depend> <depend>rclcpp_action</depend> <depend>rclcpp_lifecycle</depend> - <depend>launch</depend> - <depend>launch_testing_ament_cmake</depend> - <depend>action_msgs</depend> - <depend>rcl_interfaces</depend> - - <exec_depend>libboost-program-options</exec_depend> + <depend>std_msgs</depend> + <depend>tf2</depend> + <depend>tf2_geometry_msgs</depend> + <depend>tf2_msgs</depend> + <depend>tf2_ros</depend> <test_depend>ament_lint_common</test_depend> <test_depend>ament_lint_auto</test_depend> <test_depend>ament_cmake_gtest</test_depend> - <test_depend>launch</test_depend> - <test_depend>launch_testing_ament_cmake</test_depend> <test_depend>std_srvs</test_depend> <test_depend>test_msgs</test_depend> - <test_depend>action_msgs</test_depend> - <test_depend>launch_testing_ros</test_depend> <test_depend>ament_cmake_pytest</test_depend> <export> diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index 6ddcdc6d32..96f69b5458 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -10,34 +10,41 @@ add_library(${library_name} SHARED odometry_utils.cpp array_parser.cpp ) - -ament_target_dependencies(${library_name} - rclcpp - nav2_msgs - tf2 - tf2_ros - nav_msgs - geometry_msgs - lifecycle_msgs - rclcpp_lifecycle - tf2_geometry_msgs - bondcpp +target_include_directories(${library_name} + PUBLIC + "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>" + "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>") +target_link_libraries(${library_name} PUBLIC + bondcpp::bondcpp + ${geometry_msgs_TARGETS} + ${lifecycle_msgs_TARGETS} + ${nav2_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${rcl_interfaces_TARGETS} + rclcpp::rclcpp + rclcpp_action::rclcpp_action + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros + tf2::tf2 + ${tf2_geometry_msgs_TARGETS} +) +target_link_libraries(${library_name} PRIVATE + ${bond_TARGETS} ) add_executable(lifecycle_bringup lifecycle_bringup_commandline.cpp ) -target_link_libraries(lifecycle_bringup ${library_name}) +target_link_libraries(lifecycle_bringup PRIVATE ${library_name} rclcpp::rclcpp) add_executable(base_footprint_publisher base_footprint_publisher.cpp ) -target_link_libraries(base_footprint_publisher ${library_name}) - -find_package(Boost REQUIRED COMPONENTS program_options) +target_link_libraries(base_footprint_publisher PRIVATE ${library_name} rclcpp::rclcpp ${tf2_msgs_TARGETS}) install(TARGETS ${library_name} + EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/nav2_util/src/base_footprint_publisher.cpp b/nav2_util/src/base_footprint_publisher.cpp index f3b6791db4..144ba14ab6 100644 --- a/nav2_util/src/base_footprint_publisher.cpp +++ b/nav2_util/src/base_footprint_publisher.cpp @@ -14,7 +14,7 @@ #include <memory> -#include "nav2_util/base_footprint_publisher.hpp" +#include "base_footprint_publisher.hpp" int main(int argc, char ** argv) { diff --git a/nav2_util/include/nav2_util/base_footprint_publisher.hpp b/nav2_util/src/base_footprint_publisher.hpp similarity index 96% rename from nav2_util/include/nav2_util/base_footprint_publisher.hpp rename to nav2_util/src/base_footprint_publisher.hpp index 52bcdb53eb..25d58d504b 100644 --- a/nav2_util/include/nav2_util/base_footprint_publisher.hpp +++ b/nav2_util/src/base_footprint_publisher.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ -#define NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ +#ifndef BASE_FOOTPRINT_PUBLISHER_HPP_ +#define BASE_FOOTPRINT_PUBLISHER_HPP_ #include <string> #include <memory> @@ -126,4 +126,4 @@ class BaseFootprintPublisher : public rclcpp::Node } // end namespace nav2_util -#endif // NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ +#endif // BASE_FOOTPRINT_PUBLISHER_HPP_ diff --git a/nav2_util/src/lifecycle_utils.cpp b/nav2_util/src/lifecycle_utils.cpp index c778e2abdf..e84bbbc55b 100644 --- a/nav2_util/src/lifecycle_utils.cpp +++ b/nav2_util/src/lifecycle_utils.cpp @@ -13,16 +13,13 @@ // limitations under the License. #include <chrono> +#include <stdexcept> #include <string> -#include <thread> #include <vector> -#include "lifecycle_msgs/srv/change_state.hpp" -#include "lifecycle_msgs/srv/get_state.hpp" -#include "nav2_util/lifecycle_service_client.hpp" +#include "lifecycle_msgs/msg/transition.hpp" -using std::string; -using lifecycle_msgs::msg::Transition; +#include "nav2_util/lifecycle_service_client.hpp" namespace nav2_util { @@ -34,7 +31,7 @@ namespace nav2_util try { \ fn; \ break; \ - } catch (std::runtime_error & e) { \ + } catch (const std::runtime_error & e) { \ ++count; \ if (count > (retries)) { \ throw e;} \ @@ -53,10 +50,10 @@ static void startupLifecycleNode( // service calls still frequently hang. To get reliable startup it's necessary // to timeout the service call and retry it when that happens. RETRY( - sc.change_state(Transition::TRANSITION_CONFIGURE, service_call_timeout), + sc.change_state(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, service_call_timeout), retries); RETRY( - sc.change_state(Transition::TRANSITION_ACTIVATE, service_call_timeout), + sc.change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, service_call_timeout), retries); } @@ -81,10 +78,10 @@ static void resetLifecycleNode( // service calls still frequently hang. To get reliable reset it's necessary // to timeout the service call and retry it when that happens. RETRY( - sc.change_state(Transition::TRANSITION_DEACTIVATE, service_call_timeout), + sc.change_state(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, service_call_timeout), retries); RETRY( - sc.change_state(Transition::TRANSITION_CLEANUP, service_call_timeout), + sc.change_state(lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, service_call_timeout), retries); } diff --git a/nav2_util/src/string_utils.cpp b/nav2_util/src/string_utils.cpp index 47d86aaf57..df447b5492 100644 --- a/nav2_util/src/string_utils.cpp +++ b/nav2_util/src/string_utils.cpp @@ -15,14 +15,12 @@ #include "nav2_util/string_utils.hpp" #include <string> -using std::string; - namespace nav2_util { -std::string strip_leading_slash(const string & in) +std::string strip_leading_slash(const std::string & in) { - string out = in; + std::string out = in; if ((!in.empty()) && (in[0] == '/')) { out.erase(0, 1); @@ -31,13 +29,13 @@ std::string strip_leading_slash(const string & in) return out; } -Tokens split(const string & tokenstring, char delimiter) +Tokens split(const std::string & tokenstring, char delimiter) { Tokens tokens; size_t current_pos = 0; size_t pos = 0; - while ((pos = tokenstring.find(delimiter, current_pos)) != string::npos) { + while ((pos = tokenstring.find(delimiter, current_pos)) != std::string::npos) { tokens.push_back(tokenstring.substr(current_pos, pos - current_pos)); current_pos = pos + 1; } diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 4c0f274721..14d774c298 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -1,4 +1,5 @@ ament_add_gtest(test_execution_timer test_execution_timer.cpp) +target_link_libraries(test_execution_timer ${library_name}) ament_add_gtest(test_node_utils test_node_utils.cpp) target_link_libraries(test_node_utils ${library_name}) @@ -7,56 +8,46 @@ find_package(std_srvs REQUIRED) find_package(test_msgs REQUIRED) ament_add_gtest(test_service_client test_service_client.cpp) -ament_target_dependencies(test_service_client std_srvs) -target_link_libraries(test_service_client ${library_name}) +target_link_libraries(test_service_client ${library_name} ${std_srvs_TARGETS}) ament_add_gtest(test_string_utils test_string_utils.cpp) target_link_libraries(test_string_utils ${library_name}) find_package(rclcpp_lifecycle REQUIRED) ament_add_gtest(test_lifecycle_utils test_lifecycle_utils.cpp) -ament_target_dependencies(test_lifecycle_utils rclcpp_lifecycle) -target_link_libraries(test_lifecycle_utils ${library_name}) +target_link_libraries(test_lifecycle_utils ${library_name} rclcpp_lifecycle::rclcpp_lifecycle) ament_add_gtest(test_actions test_actions.cpp) -ament_target_dependencies(test_actions rclcpp_action test_msgs) -target_link_libraries(test_actions ${library_name}) +target_link_libraries(test_actions ${library_name} rclcpp_action::rclcpp_action ${test_msgs_TARGETS}) ament_add_gtest(test_lifecycle_node test_lifecycle_node.cpp) -ament_target_dependencies(test_lifecycle_node rclcpp_lifecycle) -target_link_libraries(test_lifecycle_node ${library_name}) +target_link_libraries(test_lifecycle_node ${library_name} rclcpp_lifecycle::rclcpp_lifecycle) ament_add_gtest(test_lifecycle_cli_node test_lifecycle_cli_node.cpp) -ament_target_dependencies(test_lifecycle_cli_node rclcpp_lifecycle) -target_link_libraries(test_lifecycle_cli_node ${library_name}) +target_link_libraries(test_lifecycle_cli_node ${library_name} rclcpp_lifecycle::rclcpp_lifecycle) ament_add_gtest(test_geometry_utils test_geometry_utils.cpp) -ament_target_dependencies(test_geometry_utils geometry_msgs) -target_link_libraries(test_geometry_utils ${library_name}) +target_link_libraries(test_geometry_utils ${library_name} ${geometry_msgs_TARGETS}) ament_add_gtest(test_odometry_utils test_odometry_utils.cpp) -ament_target_dependencies(test_odometry_utils nav_msgs geometry_msgs) -target_link_libraries(test_odometry_utils ${library_name}) +target_link_libraries(test_odometry_utils ${library_name} ${nav_msgs_TARGETS} ${geometry_msgs_TARGETS}) ament_add_gtest(test_robot_utils test_robot_utils.cpp) -ament_target_dependencies(test_robot_utils geometry_msgs) -target_link_libraries(test_robot_utils ${library_name}) +target_link_libraries(test_robot_utils ${library_name} ${geometry_msgs_TARGETS}) ament_add_gtest(test_base_footprint_publisher test_base_footprint_publisher.cpp) -ament_target_dependencies(test_base_footprint_publisher geometry_msgs) -target_link_libraries(test_base_footprint_publisher ${library_name}) +target_include_directories(test_base_footprint_publisher PRIVATE "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/src>") + +target_link_libraries(test_base_footprint_publisher ${library_name} tf2_ros::tf2_ros rclcpp::rclcpp ${geometry_msgs_TARGETS}) ament_add_gtest(test_array_parser test_array_parser.cpp) target_link_libraries(test_array_parser ${library_name}) ament_add_gtest(test_twist_publisher test_twist_publisher.cpp) -ament_target_dependencies(test_twist_publisher rclcpp_lifecycle) -target_link_libraries(test_twist_publisher ${library_name}) +target_link_libraries(test_twist_publisher ${library_name} rclcpp::rclcpp ${geometry_msgs_TARGETS}) ament_add_gtest(test_twist_subscriber test_twist_subscriber.cpp) -ament_target_dependencies(test_twist_subscriber rclcpp_lifecycle) -target_link_libraries(test_twist_subscriber ${library_name}) +target_link_libraries(test_twist_subscriber ${library_name} rclcpp::rclcpp ${geometry_msgs_TARGETS}) ament_add_gtest(test_validation_messages test_validation_messages.cpp) -ament_target_dependencies(test_validation_messages rclcpp_lifecycle) -target_link_libraries(test_validation_messages ${library_name}) \ No newline at end of file +target_link_libraries(test_validation_messages ${library_name} ${builtin_interfaces_TARGETS} ${std_msgs_TARGETS} ${geometry_msgs_TARGETS}) diff --git a/nav2_util/test/test_base_footprint_publisher.cpp b/nav2_util/test/test_base_footprint_publisher.cpp index 47dc83c7f3..cd78883132 100644 --- a/nav2_util/test/test_base_footprint_publisher.cpp +++ b/nav2_util/test/test_base_footprint_publisher.cpp @@ -15,7 +15,7 @@ #include <string> #include <memory> -#include "nav2_util/base_footprint_publisher.hpp" +#include "base_footprint_publisher.hpp" #include "gtest/gtest.h" #include "tf2/exceptions.h" From 1788615b9b45ceec344c09fe882542e96de5e7b9 Mon Sep 17 00:00:00 2001 From: Vincent <46542431+VincidaB@users.noreply.github.com> Date: Wed, 19 Jun 2024 10:37:41 -0500 Subject: [PATCH 13/18] precomputeDistanceHeuristic is now computed once (#4451) Signed-off-by: Vincent Belpois <vincent.belpois@gmail.com> Co-authored-by: SiddharthaUpase <s1dupase34@gmail.com> --- nav2_smac_planner/include/nav2_smac_planner/a_star.hpp | 1 + nav2_smac_planner/src/a_star.cpp | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 64c905d01c..c95d8c82cf 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -251,6 +251,7 @@ class AStarAlgorithm const NodePtr & node, std::vector<std::tuple<float, float, float>> * expansions_log); bool _traverse_unknown; + bool _is_initialized; int _max_iterations; int _max_on_approach_iterations; int _terminal_checking_interval; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index a3cfbb4b4f..3c22ce450f 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -36,6 +36,7 @@ AStarAlgorithm<NodeT>::AStarAlgorithm( const MotionModel & motion_model, const SearchInfo & search_info) : _traverse_unknown(true), + _is_initialized(false), _max_iterations(0), _terminal_checking_interval(5000), _max_planning_time(0), @@ -70,7 +71,10 @@ void AStarAlgorithm<NodeT>::initialize( _max_on_approach_iterations = max_on_approach_iterations; _terminal_checking_interval = terminal_checking_interval; _max_planning_time = max_planning_time; - NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); + if(!_is_initialized) { + NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); + } + _is_initialized = true; _dim3_size = dim_3_size; _expander = std::make_unique<AnalyticExpansion<NodeT>>( _motion_model, _search_info, _traverse_unknown, _dim3_size); From fb5f29d34f687fa02858951c86e59e4dd5619389 Mon Sep 17 00:00:00 2001 From: Steve Macenski <stevenmacenski@gmail.com> Date: Wed, 19 Jun 2024 11:48:16 -0700 Subject: [PATCH 14/18] Fix CI with BT test from btv4.X API update (#4452) * fix CI with BT test from btv4.X API update * Update test_bt_utils.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --- nav2_behavior_tree/test/test_bt_utils.cpp | 55 +++-------------------- 1 file changed, 6 insertions(+), 49 deletions(-) diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp index 1c008d6478..d495587955 100644 --- a/nav2_behavior_tree/test/test_bt_utils.cpp +++ b/nav2_behavior_tree/test/test_bt_utils.cpp @@ -59,13 +59,7 @@ TEST(PointPortTest, test_wrong_syntax) BT::BehaviorTreeFactory factory; factory.registerNodeType<TestNode<geometry_msgs::msg::Point>>("PointPort"); - auto tree = factory.createTreeFromText(xml_txt); - - geometry_msgs::msg::Point value; - tree.rootNode()->getInput("test", value); - EXPECT_EQ(value.x, 0.0); - EXPECT_EQ(value.y, 0.0); - EXPECT_EQ(value.z, 0.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); xml_txt = R"( @@ -75,11 +69,7 @@ TEST(PointPortTest, test_wrong_syntax) </BehaviorTree> </root>)"; - tree = factory.createTreeFromText(xml_txt); - tree.rootNode()->getInput("test", value); - EXPECT_EQ(value.x, 0.0); - EXPECT_EQ(value.y, 0.0); - EXPECT_EQ(value.z, 0.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); } TEST(PointPortTest, test_correct_syntax) @@ -115,14 +105,8 @@ TEST(QuaternionPortTest, test_wrong_syntax) BT::BehaviorTreeFactory factory; factory.registerNodeType<TestNode<geometry_msgs::msg::Quaternion>>("QuaternionPort"); - auto tree = factory.createTreeFromText(xml_txt); - geometry_msgs::msg::Quaternion value; - tree.rootNode()->getInput("test", value); - EXPECT_EQ(value.x, 0.0); - EXPECT_EQ(value.y, 0.0); - EXPECT_EQ(value.z, 0.0); - EXPECT_EQ(value.w, 1.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); xml_txt = R"( @@ -132,12 +116,7 @@ TEST(QuaternionPortTest, test_wrong_syntax) </BehaviorTree> </root>)"; - tree = factory.createTreeFromText(xml_txt); - tree.rootNode()->getInput("test", value); - EXPECT_EQ(value.x, 0.0); - EXPECT_EQ(value.y, 0.0); - EXPECT_EQ(value.z, 0.0); - EXPECT_EQ(value.w, 1.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); } TEST(QuaternionPortTest, test_correct_syntax) @@ -174,19 +153,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax) BT::BehaviorTreeFactory factory; factory.registerNodeType<TestNode<geometry_msgs::msg::PoseStamped>>("PoseStampedPort"); - auto tree = factory.createTreeFromText(xml_txt); - - geometry_msgs::msg::PoseStamped value; - tree.rootNode()->getInput("test", value); - EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); - EXPECT_EQ(value.header.frame_id, ""); - EXPECT_EQ(value.pose.position.x, 0.0); - EXPECT_EQ(value.pose.position.y, 0.0); - EXPECT_EQ(value.pose.position.z, 0.0); - EXPECT_EQ(value.pose.orientation.x, 0.0); - EXPECT_EQ(value.pose.orientation.y, 0.0); - EXPECT_EQ(value.pose.orientation.z, 0.0); - EXPECT_EQ(value.pose.orientation.w, 1.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); xml_txt = R"( @@ -196,17 +163,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax) </BehaviorTree> </root>)"; - tree = factory.createTreeFromText(xml_txt); - tree.rootNode()->getInput("test", value); - EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); - EXPECT_EQ(value.header.frame_id, ""); - EXPECT_EQ(value.pose.position.x, 0.0); - EXPECT_EQ(value.pose.position.y, 0.0); - EXPECT_EQ(value.pose.position.z, 0.0); - EXPECT_EQ(value.pose.orientation.x, 0.0); - EXPECT_EQ(value.pose.orientation.y, 0.0); - EXPECT_EQ(value.pose.orientation.z, 0.0); - EXPECT_EQ(value.pose.orientation.w, 1.0); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); } TEST(PoseStampedPortTest, test_correct_syntax) From 43b238866655f749af5c6dcf6e1f2dff637fbce4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Carlos=20Garc=C3=ADa?= <40342885+jcarlosgm30@users.noreply.github.com> Date: Thu, 20 Jun 2024 17:54:59 +0200 Subject: [PATCH 15/18] [Docking] Feature: support for not needing to set dock instances (#4442) * feat: support for not needing to set dock_instances Signed-off-by: josegarcia <josegarcia@pal-robotics.com> * feat: improve logs for dock_database Signed-off-by: josegarcia <josegarcia@pal-robotics.com> * chore: comment the dock instances out from nav2_params Signed-off-by: josegarcia <josegarcia@pal-robotics.com> * chore: dock database cleanup Signed-off-by: josegarcia <josegarcia@pal-robotics.com> * fix: update reloadDBService from DatabaseTests Signed-off-by: josegarcia <josegarcia@pal-robotics.com> --------- Signed-off-by: josegarcia <josegarcia@pal-robotics.com> Co-authored-by: josegarcia <josegarcia@pal-robotics.com> --- nav2_bringup/params/nav2_params.yaml | 11 +++--- .../opennav_docking/src/dock_database.cpp | 37 ++++++++++++++----- .../test/test_dock_database.cpp | 5 ++- 3 files changed, 37 insertions(+), 16 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index b43b70318c..307eb2f37a 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -407,11 +407,12 @@ docking_server: filter_coef: 0.1 # Dock instances - docks: ['home_dock'] # Input your docks here - home_dock: - type: 'simple_charging_dock' - frame: map - pose: [0.0, 0.0, 0.0] + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] controller: k_phi: 3.0 diff --git a/nav2_docking/opennav_docking/src/dock_database.cpp b/nav2_docking/opennav_docking/src/dock_database.cpp index 6774e93620..83ef42cd67 100644 --- a/nav2_docking/opennav_docking/src/dock_database.cpp +++ b/nav2_docking/opennav_docking/src/dock_database.cpp @@ -34,21 +34,32 @@ bool DockDatabase::initialize( node_ = parent; auto node = node_.lock(); - if (getDockPlugins(node, tf) && getDockInstances(node)) { - RCLCPP_INFO( + if (!getDockPlugins(node, tf)) { + RCLCPP_ERROR( node->get_logger(), - "Docking Server has %u dock types and %u dock instances available.", - this->plugin_size(), this->instance_size()); - return true; + "An error occurred while getting the dock plugins!"); + return false; + } + + if (!getDockInstances(node)) { + RCLCPP_ERROR( + node->get_logger(), + "An error occurred while getting the dock instances!"); + return false; } + RCLCPP_INFO( + node->get_logger(), + "Docking Server has %u dock types and %u dock instances available.", + this->plugin_size(), this->instance_size()); + reload_db_service_ = node->create_service<nav2_msgs::srv::ReloadDockDatabase>( "~/reload_database", std::bind( &DockDatabase::reloadDbCb, this, std::placeholders::_1, std::placeholders::_2)); - return false; + return true; } void DockDatabase::activate() @@ -71,10 +82,14 @@ void DockDatabase::reloadDbCb( const std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Request> request, std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Response> response) { + auto node = node_.lock(); DockMap dock_instances; - if (utils::parseDockFile(request->filepath, node_.lock(), dock_instances)) { + if (utils::parseDockFile(request->filepath, node, dock_instances)) { dock_instances_ = dock_instances; response->success = true; + RCLCPP_INFO( + node->get_logger(), + "Dock database reloaded from file %s.", request->filepath.c_str()); return; } response->success = false; @@ -194,10 +209,12 @@ bool DockDatabase::getDockInstances(const rclcpp_lifecycle::LifecycleNode::Share return utils::parseDockParams(docks_param, node, dock_instances_); } - RCLCPP_ERROR( + RCLCPP_WARN( node->get_logger(), - "Dock database filepath nor dock parameters set. Unable to perform docking actions."); - return false; + "Dock database filepath nor dock parameters set. " + "Docking actions can only be executed specifying the dock pose via the action request. " + "Or update the dock database via the reload_database service."); + return true; } unsigned int DockDatabase::plugin_size() const diff --git a/nav2_docking/opennav_docking/test/test_dock_database.cpp b/nav2_docking/opennav_docking/test/test_dock_database.cpp index 2753f34f1b..0285e30f4a 100644 --- a/nav2_docking/opennav_docking/test/test_dock_database.cpp +++ b/nav2_docking/opennav_docking/test/test_dock_database.cpp @@ -102,8 +102,11 @@ TEST(DatabaseTests, findTests) TEST(DatabaseTests, reloadDbService) { auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test"); - std::vector<std::string> plugins{"dockv1", "dockv2"}; + std::vector<std::string> plugins{"dockv1"}; node->declare_parameter("dock_plugins", rclcpp::ParameterValue(plugins)); + node->declare_parameter( + "dockv1.plugin", + rclcpp::ParameterValue("opennav_docking::SimpleChargingDock")); opennav_docking::DockDatabase db; db.initialize(node, nullptr); From 90574567025cce3d7af0f0710bfabb4533c211fe Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Thu, 20 Jun 2024 22:42:41 +0200 Subject: [PATCH 16/18] Port system tests in nav2_system_tests (#4440) * wip, ported only test_bt_navigator Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * include test_bt_navigator_with dijlstra and test_bt_navigator_2 Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * uncomment some lines Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * More tests Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * Include end of line Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * move gz_sim cleanup process to utils nav2_simple_commander Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * fix linter Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * cleanup Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * removed unused path Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * cleanup Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * more cleanup Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * reduce set initial pose time Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * remove repeated variable Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * Remove log Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * Remove todo Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * use robot publisher Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * use robot publisher Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * include copyright Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * correct year Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> --------- Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> Signed-off-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> --- .../nav2_simple_commander/utils.py | 55 ++ nav2_system_tests/CMakeLists.txt | 7 +- nav2_system_tests/models/cardboard_box.sdf | 9 + nav2_system_tests/package.xml | 2 + .../localization/test_localization_launch.py | 37 +- nav2_system_tests/src/system/CMakeLists.txt | 24 +- .../src/system/nav2_system_params.yaml | 46 ++ .../src/system/nav_to_pose_tester_node.py | 14 +- .../src/system/test_system_launch.py | 77 ++- .../test_system_with_obstacle_launch.py | 190 ++++++ .../src/system/test_wrong_init_pose_launch.py | 81 ++- nav2_system_tests/urdf/common_properties.urdf | 45 -- nav2_system_tests/urdf/turtlebot3_burger.urdf | 155 ----- nav2_system_tests/urdf/turtlebot3_waffle.urdf | 279 --------- .../worlds/turtlebot3_ros2_demo.world | 497 ---------------- .../worlds/turtlebot3_ros2_demo_gps.world | 555 ----------------- .../turtlebot3_ros2_demo_obstacle.world | 558 ------------------ nav2_system_tests/worlds/world_only.model | 54 -- 18 files changed, 451 insertions(+), 2234 deletions(-) create mode 100644 nav2_simple_commander/nav2_simple_commander/utils.py create mode 100644 nav2_system_tests/models/cardboard_box.sdf create mode 100755 nav2_system_tests/src/system/test_system_with_obstacle_launch.py delete mode 100644 nav2_system_tests/urdf/common_properties.urdf delete mode 100644 nav2_system_tests/urdf/turtlebot3_burger.urdf delete mode 100644 nav2_system_tests/urdf/turtlebot3_waffle.urdf delete mode 100644 nav2_system_tests/worlds/turtlebot3_ros2_demo.world delete mode 100644 nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world delete mode 100644 nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world delete mode 100644 nav2_system_tests/worlds/world_only.model diff --git a/nav2_simple_commander/nav2_simple_commander/utils.py b/nav2_simple_commander/nav2_simple_commander/utils.py new file mode 100644 index 0000000000..d363901020 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/utils.py @@ -0,0 +1,55 @@ +#! /usr/bin/env python3 +# Copyright 2024 Open Navigation LLC +# Copyright 2024 Stevedan Ogochukwu Omodolor Omodia +# +# 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. + +"""General utility functions.""" + +import os +import signal +import subprocess + + +def find_os_processes(name): + """Find all the processes that are running gz sim.""" + ps_output = subprocess.check_output(['ps', 'aux'], text=True) + ps_lines = ps_output.split('\n') + gz_sim_processes = [] + for line in ps_lines: + if name in line: + columns = line.split() + pid = columns[1] + command = ' '.join(columns[10:]) + if command.startswith(name): + gz_sim_processes.append((pid, command)) + return gz_sim_processes + + +def kill_process(pid): + """Kill a process with a given PID.""" + try: + os.kill(int(pid), signal.SIGKILL) + print(f'Successfully killed process with PID: {pid}') + except Exception as e: + print(f'Failed to kill process with PID: {pid}. Error: {e}') + + +def kill_os_processes(name): + """Kill all processes that are running with name.""" + processes = find_os_processes(name) + if processes: + for pid, _ in processes: + kill_process(pid) + else: + print(f'No processes found starting with {name}') diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index d94298da77..baa1256d52 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(navigation2) find_package(angles REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(pluginlib REQUIRED) +find_package(nav2_minimal_tb3_sim REQUIRED) nav2_package() @@ -113,9 +114,9 @@ if(BUILD_TESTING) add_subdirectory(src/behavior_tree) add_subdirectory(src/planning) - # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634 add_subdirectory(src/localization) - # add_subdirectory(src/system) + add_subdirectory(src/system) + # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634 # add_subdirectory(src/system_failure) # add_subdirectory(src/updown) # add_subdirectory(src/waypoint_follower) @@ -127,7 +128,7 @@ if(BUILD_TESTING) # add_subdirectory(src/behaviors/assisted_teleop) # add_subdirectory(src/costmap_filters) add_subdirectory(src/error_codes) - install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) + install(DIRECTORY maps models DESTINATION share/${PROJECT_NAME}) endif() diff --git a/nav2_system_tests/models/cardboard_box.sdf b/nav2_system_tests/models/cardboard_box.sdf new file mode 100644 index 0000000000..97a40603c2 --- /dev/null +++ b/nav2_system_tests/models/cardboard_box.sdf @@ -0,0 +1,9 @@ +<sdf version="1.6"> + <model name='cardboard_box'> + <include> + <uri> + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Cardboard box + </uri> + </include> + </model> +</sdf> \ No newline at end of file diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 5fdbc230b7..2f3f45d191 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -30,6 +30,7 @@ <build_depend>launch_ros</build_depend> <build_depend>launch_testing</build_depend> <build_depend>nav2_planner</build_depend> + <build_depend>nav2_minimal_tb3_sim</build_depend> <exec_depend>launch_ros</exec_depend> <exec_depend>launch_testing</exec_depend> @@ -48,6 +49,7 @@ <exec_depend>nav2_amcl</exec_depend> <exec_depend>std_msgs</exec_depend> <exec_depend>tf2_geometry_msgs</exec_depend> + <exec_depend>nav2_minimal_tb3_sim</exec_depend> <!-- <exec_depend>gazebo_ros_pkgs</exec_depend> Remove/Replace after https://github.com/ros-navigation/navigation2/pull/3634 --> <exec_depend>navigation2</exec_depend> <exec_depend>lcov</exec_depend> diff --git a/nav2_system_tests/src/localization/test_localization_launch.py b/nav2_system_tests/src/localization/test_localization_launch.py index aedca4fcf7..e56ca77986 100755 --- a/nav2_system_tests/src/localization/test_localization_launch.py +++ b/nav2_system_tests/src/localization/test_localization_launch.py @@ -15,6 +15,7 @@ # limitations under the License. import os +from pathlib import Path import sys from ament_index_python.packages import get_package_share_directory @@ -26,6 +27,8 @@ import launch_ros.actions from launch_testing.legacy import LaunchTestService +from nav2_simple_commander.utils import kill_os_processes + def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') @@ -36,11 +39,19 @@ def main(argv=sys.argv[1:]): world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') set_env_vars_resources = AppendEnvironmentVariable( 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ) + set_env_vars_resources2 = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(sim_dir)).parent.resolve()) + ) start_gazebo_server = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -64,18 +75,14 @@ def main(argv=sys.argv[1:]): 'yaw': '0.0', }.items(), ) - - link_footprint = launch_ros.actions.Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], - ) - footprint_scan = launch_ros.actions.Node( - package='tf2_ros', - executable='static_transform_publisher', + run_robot_state_publisher = launch_ros.actions.Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], ) run_map_server = launch_ros.actions.Node( package='nav2_map_server', @@ -97,10 +104,10 @@ def main(argv=sys.argv[1:]): ld = LaunchDescription( [ set_env_vars_resources, + set_env_vars_resources2, start_gazebo_server, spawn_robot, - link_footprint, - footprint_scan, + run_robot_state_publisher, run_map_server, run_amcl, run_lifecycle_manager, @@ -115,7 +122,9 @@ def main(argv=sys.argv[1:]): lts.add_test_action(ld, test1_action) ls = LaunchService(argv=argv) ls.include_launch_description(ld) - return lts.run(ls) + return_code = lts.run(ls) + kill_os_processes('gz sim') + return return_code if __name__ == '__main__': diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index 39afa461c2..34ebf0e30e 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -5,9 +5,6 @@ ament_add_test(test_bt_navigator TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=True @@ -22,9 +19,6 @@ ament_add_test(test_bt_navigator_with_wrong_init_pose TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=True @@ -39,9 +33,6 @@ ament_add_test(test_bt_navigator_with_dijkstra TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=False @@ -56,9 +47,6 @@ ament_add_test(test_bt_navigator_2 TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=False @@ -68,14 +56,12 @@ ament_add_test(test_bt_navigator_2 ament_add_test(test_dynamic_obstacle GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_with_obstacle_launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_obstacle.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + ADD_OBSTACLE=True BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=False @@ -85,14 +71,12 @@ ament_add_test(test_dynamic_obstacle ament_add_test(test_nav_through_poses GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_with_obstacle_launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_obstacle.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + ADD_OBSTACLE=True BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml TESTER=nav_through_poses_tester_node.py ASTAR=False diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index a98d340f21..5ee29e6536 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -317,3 +317,49 @@ collision_monitor: min_height: 0.15 max_height: 2.0 enabled: True + +docking_server: + ros__parameters: + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + docks: ['home_dock'] # Input your docks here + home_dock: + type: 'simple_charging_dock' + frame: map + pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + \ No newline at end of file diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index 8fd1bc6034..0eca068039 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -59,6 +59,7 @@ def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): self.initial_pose_received = False self.initial_pose = initial_pose self.goal_pose = goal_pose + self.set_initial_pose_timeout = 15 self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') def info_msg(self, msg: str): @@ -212,11 +213,21 @@ def shutdown(self): def wait_for_initial_pose(self): self.initial_pose_received = False + # If the initial pose is not received within 100 seconds, return False + # this is because when setting a wrong initial pose, amcl_pose is not received + # and the test will hang indefinitely + start_time = time.time() + duration = 0 while not self.initial_pose_received: self.info_msg('Setting initial pose') self.setInitialPose() self.info_msg('Waiting for amcl_pose to be received') + duration = time.time() - start_time + if duration > self.set_initial_pose_timeout: + self.error_msg('Timeout waiting for initial pose to be set') + return False rclpy.spin_once(self, timeout_sec=1) + return True def test_RobotMovesToGoal(robot_tester): @@ -231,7 +242,8 @@ def run_all_tests(robot_tester): result = True if result: robot_tester.wait_for_node_active('amcl') - robot_tester.wait_for_initial_pose() + result = robot_tester.wait_for_initial_pose() + if result: robot_tester.wait_for_node_active('bt_navigator') result = robot_tester.runNavigateAction() diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 2f0546d156..a644072ebb 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -16,6 +16,7 @@ # limitations under the License. import os +from pathlib import Path import sys from ament_index_python.packages import get_package_share_directory @@ -23,6 +24,7 @@ from launch import LaunchDescription from launch import LaunchService from launch.actions import ( + AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable, @@ -33,11 +35,22 @@ from launch_testing.legacy import LaunchTestService from nav2_common.launch import RewrittenYaml +from nav2_simple_commander.utils import kill_os_processes def generate_launch_description(): - map_yaml_file = os.getenv('TEST_MAP') - world = os.getenv('TEST_WORLD') + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + ros_gz_sim_dir = get_package_share_directory('ros_gz_sim') + + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') bt_navigator_xml = os.path.join( get_package_share_directory('nav2_bt_navigator'), @@ -45,8 +58,6 @@ def generate_launch_description(): os.getenv('BT_NAVIGATOR_XML'), ) - bringup_dir = get_package_share_directory('nav2_bringup') - # Use local param file launch_dir = os.path.dirname(os.path.realpath(__file__)) params_file = os.path.join(launch_dir, 'nav2_system_params.yaml') @@ -79,34 +90,46 @@ def generate_launch_description(): [ SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - # Launch gazebo server for simulation - ExecuteProcess( - cmd=[ - 'gzserver', - '-s', - 'libgazebo_ros_init.so', - '--minimal_comms', - world, - ], - output='screen', + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ), - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(sim_dir)).parent.resolve()) + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-2.0', + 'y_pose': '-0.5', + 'z_pose': '0.01', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), ), Node( - package='tf2_ros', - executable='static_transform_publisher', + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') ), launch_arguments={ 'namespace': '', @@ -145,7 +168,9 @@ def main(argv=sys.argv[1:]): lts.add_test_action(ld, test1_action) ls = LaunchService(argv=argv) ls.include_launch_description(ld) - return lts.run(ls) + return_code = lts.run(ls) + kill_os_processes('gz sim') + return return_code if __name__ == '__main__': diff --git a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py new file mode 100755 index 0000000000..fb4468afef --- /dev/null +++ b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py @@ -0,0 +1,190 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# Copyright (c) 2020 Florian Gramss +# +# 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 pathlib import Path +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ( + AppendEnvironmentVariable, + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml +from nav2_simple_commander.utils import kill_os_processes + + +def generate_launch_description(): + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + ros_gz_sim_dir = get_package_share_directory('ros_gz_sim') + nav2_system_tests_dir = get_package_share_directory('nav2_system_tests') + + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') + + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) + + # Use local param file + launch_dir = os.path.dirname(os.path.realpath(__file__)) + params_file = os.path.join(launch_dir, 'nav2_system_params.yaml') + + # Replace the default parameter values for testing special features + # without having multiple params_files inside the nav2 stack + context = LaunchContext() + param_substitutions = {} + + if os.getenv('ASTAR') == 'True': + param_substitutions.update({'use_astar': 'True'}) + + param_substitutions.update( + {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')} + ) + param_substitutions.update( + {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')} + ) + + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True, + ) + + new_yaml = configured_params.perform(context) + + cardbox_sdf = os.path.join(nav2_system_tests_dir, 'models', 'cardboard_box.sdf') + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') + ), + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(sim_dir)).parent.resolve()) + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-2.0', + 'y_pose': '-0.5', + 'z_pose': '0.01', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), + ), + Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'cardboard_box', + '-file', cardbox_sdf, + '-x', '-1.0', '-y', '0.6', '-z', '0.15', + '-R', '0.0', '-P', '0.0', '-Y', '0.0',] + ), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + test1_action = ExecuteProcess( + cmd=[ + os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), + '-r', + '-2.0', + '-0.5', + '0.0', + '2.0', + '-e', + 'True', + ], + name='tester_node', + output='screen', + ) + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return_code = lts.run(ls) + kill_os_processes('gz sim') + return return_code + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index f42d276fcc..b0e2322692 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -16,13 +16,14 @@ # limitations under the License. import os +from pathlib import Path import sys from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription from launch import LaunchService from launch.actions import ( + AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable, @@ -33,11 +34,22 @@ from launch_testing.legacy import LaunchTestService from nav2_common.launch import RewrittenYaml +from nav2_simple_commander.utils import kill_os_processes def generate_launch_description(): - map_yaml_file = os.getenv('TEST_MAP') - world = os.getenv('TEST_WORLD') + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + ros_gz_sim_dir = get_package_share_directory('ros_gz_sim') + + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') bt_navigator_xml = os.path.join( get_package_share_directory('nav2_bt_navigator'), @@ -45,8 +57,9 @@ def generate_launch_description(): os.getenv('BT_NAVIGATOR_XML'), ) - bringup_dir = get_package_share_directory('nav2_bringup') - params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') + # Use local param file + launch_dir = os.path.dirname(os.path.realpath(__file__)) + params_file = os.path.join(launch_dir, 'nav2_system_params.yaml') # Replace the default parameter values for testing special features # without having multiple params_files inside the nav2 stack @@ -76,34 +89,46 @@ def generate_launch_description(): [ SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - # Launch gazebo server for simulation - ExecuteProcess( - cmd=[ - 'gzserver', - '-s', - 'libgazebo_ros_init.so', - '--minimal_comms', - world, - ], - output='screen', + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ), - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(sim_dir)).parent.resolve()) + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-2.0', + 'y_pose': '-0.5', + 'z_pose': '0.01', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), ), Node( - package='tf2_ros', - executable='static_transform_publisher', + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') ), launch_arguments={ 'namespace': '', @@ -142,7 +167,9 @@ def main(argv=sys.argv[1:]): lts.add_test_action(ld, test1_action) ls = LaunchService(argv=argv) ls.include_launch_description(ld) - return lts.run(ls) + return_code = lts.run(ls) + kill_os_processes('gz sim') + return return_code if __name__ == '__main__': diff --git a/nav2_system_tests/urdf/common_properties.urdf b/nav2_system_tests/urdf/common_properties.urdf deleted file mode 100644 index 9d457ea1d6..0000000000 --- a/nav2_system_tests/urdf/common_properties.urdf +++ /dev/null @@ -1,45 +0,0 @@ -<?xml version="1.0" ?> - -<robot name="xacro_properties" - xmlns:xacro="http://ros.org/wiki/xacro"> - - <!-- Init colour --> - <material name="black"> - <color rgba="0.0 0.0 0.0 1.0"/> - </material> - - <material name="dark"> - <color rgba="0.3 0.3 0.3 1.0"/> - </material> - <material name="light_black"> - <color rgba="0.4 0.4 0.4 1.0"/> - </material> - - <material name="blue"> - <color rgba="0.0 0.0 0.8 1.0"/> - </material> - - <material name="green"> - <color rgba="0.0 0.8 0.0 1.0"/> - </material> - - <material name="grey"> - <color rgba="0.5 0.5 0.5 1.0"/> - </material> - - <material name="orange"> - <color rgba="${255/255} ${108/255} ${10/255} 1.0"/> - </material> - - <material name="brown"> - <color rgba="${222/255} ${207/255} ${195/255} 1.0"/> - </material> - - <material name="red"> - <color rgba="0.8 0.0 0.0 1.0"/> - </material> - - <material name="white"> - <color rgba="1.0 1.0 1.0 1.0"/> - </material> -</robot> diff --git a/nav2_system_tests/urdf/turtlebot3_burger.urdf b/nav2_system_tests/urdf/turtlebot3_burger.urdf deleted file mode 100644 index 90f34dbbb8..0000000000 --- a/nav2_system_tests/urdf/turtlebot3_burger.urdf +++ /dev/null @@ -1,155 +0,0 @@ -<?xml version="1.0" ?> -<robot name="turtlebot3_burger" - xmlns:xacro="http://ros.org/wiki/xacro"> - <xacro:include filename="$(find turtlebot3_description)/urdf/common_properties.urdf"/> - - <link name="base_footprint"/> - - <joint name="base_joint" type="fixed"> - <parent link="base_footprint"/> - <child link="base_link"/> - <origin xyz="0.0 0.0 0.010" rpy="0 0 0"/> - </joint> - - <link name="base_link"> - <visual> - <origin xyz="-0.032 0 0.0" rpy="0 0 0"/> - <geometry> - <mesh filename="package://turtlebot3_description/meshes/bases/burger_base.stl" scale="0.001 0.001 0.001"/> - </geometry> - <material name="light_black"/> - </visual> - - <collision> - <origin xyz="-0.032 0 0.070" rpy="0 0 0"/> - <geometry> - <box size="0.140 0.140 0.143"/> - </geometry> - </collision> - - <inertial> - <origin xyz="0 0 0" rpy="0 0 0"/> - <mass value="8.2573504e-01"/> - <inertia ixx="2.2124416e-03" ixy="-1.2294101e-05" ixz="3.4938785e-05" iyy="2.1193702e-03" iyz="-5.0120904e-06" izz="2.0064271e-03" /> - </inertial> - </link> - - <joint name="wheel_left_joint" type="continuous"> - <parent link="base_link"/> - <child link="wheel_left_link"/> - <origin xyz="0.0 0.08 0.023" rpy="-1.57 0 0"/> - <axis xyz="0 0 1"/> - </joint> - - <link name="wheel_left_link"> - <visual> - <origin xyz="0 0 0" rpy="1.57 0 0"/> - <geometry> - <mesh filename="package://turtlebot3_description/meshes/wheels/left_tire.stl" scale="0.001 0.001 0.001"/> - </geometry> - <material name="dark"/> - </visual> - - <collision> - <origin xyz="0 0 0" rpy="0 0 0"/> - <geometry> - <cylinder length="0.018" radius="0.033"/> - </geometry> - </collision> - - <inertial> - <origin xyz="0 0 0" /> - <mass value="2.8498940e-02" /> - <inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09" iyy="1.1192413e-05" iyz="-1.4400107e-11" izz="2.0712558e-05" /> - </inertial> - </link> - - <joint name="wheel_right_joint" type="continuous"> - <parent link="base_link"/> - <child link="wheel_right_link"/> - <origin xyz="0.0 -0.080 0.023" rpy="-1.57 0 0"/> - <axis xyz="0 0 1"/> - </joint> - - <link name="wheel_right_link"> - <visual> - <origin xyz="0 0 0" rpy="1.57 0 0"/> - <geometry> - <mesh filename="package://turtlebot3_description/meshes/wheels/right_tire.stl" scale="0.001 0.001 0.001"/> - </geometry> - <material name="dark"/> - </visual> - - <collision> - <origin xyz="0 0 0" rpy="0 0 0"/> - <geometry> - <cylinder length="0.018" radius="0.033"/> - </geometry> - </collision> - - <inertial> - <origin xyz="0 0 0" /> - <mass value="2.8498940e-02" /> - <inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09" iyy="1.1192413e-05" iyz="-1.4400107e-11" izz="2.0712558e-05" /> - </inertial> - </link> - - <joint name="caster_back_joint" type="fixed"> - <parent link="base_link"/> - <child link="caster_back_link"/> - <origin xyz="-0.081 0 -0.004" rpy="-1.57 0 0"/> - </joint> - - <link name="caster_back_link"> - <collision> - <origin xyz="0 0.001 0" rpy="0 0 0"/> - <geometry> - <box size="0.030 0.009 0.020"/> - </geometry> - </collision> - - <inertial> - <origin xyz="0 0 0" /> - <mass value="0.005" /> - <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" /> - </inertial> - </link> - - <joint name="imu_joint" type="fixed"> - <parent link="base_link"/> - <child link="imu_link"/> - <origin xyz="-0.032 0 0.068" rpy="0 0 0"/> - </joint> - - <link name="imu_link"/> - - <joint name="scan_joint" type="fixed"> - <parent link="base_link"/> - <child link="base_scan"/> - <origin xyz="-0.032 0 0.172" rpy="0 0 0"/> - </joint> - - <link name="base_scan"> - <visual> - <origin xyz="0 0 0.0" rpy="0 0 0"/> - <geometry> - <mesh filename="package://turtlebot3_description/meshes/sensors/lds.stl" scale="0.001 0.001 0.001"/> - </geometry> - <material name="dark"/> - </visual> - - <collision> - <origin xyz="0.015 0 -0.0065" rpy="0 0 0"/> - <geometry> - <cylinder length="0.0315" radius="0.055"/> - </geometry> - </collision> - - <inertial> - <mass value="0.114" /> - <origin xyz="0 0 0" /> - <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" /> - </inertial> - </link> - -</robot> diff --git a/nav2_system_tests/urdf/turtlebot3_waffle.urdf b/nav2_system_tests/urdf/turtlebot3_waffle.urdf deleted file mode 100644 index 9a5bca1141..0000000000 --- a/nav2_system_tests/urdf/turtlebot3_waffle.urdf +++ /dev/null @@ -1,279 +0,0 @@ -<?xml version="1.0" ?> -<robot name="turtlebot3_waffle" xmlns:xacro="http://ros.org/wiki/xacro"> - <!-- <xacro:include filename="$(find turtlebot3_description)/urdf/common_properties.urdf"/> - - <xacro:property name="r200_cam_rgb_px" value="0.005"/> - <xacro:property name="r200_cam_rgb_py" value="0.018"/> - <xacro:property name="r200_cam_rgb_pz" value="0.013"/> - <xacro:property name="r200_cam_depth_offset" value="0.01"/> --> - - <!-- Init colour --> - <material name="black"> - <color rgba="0.0 0.0 0.0 1.0"/> - </material> - - <material name="dark"> - <color rgba="0.3 0.3 0.3 1.0"/> - </material> - - <material name="light_black"> - <color rgba="0.4 0.4 0.4 1.0"/> - </material> - - <material name="blue"> - <color rgba="0.0 0.0 0.8 1.0"/> - </material> - - <material name="green"> - <color rgba="0.0 0.8 0.0 1.0"/> - </material> - - <material name="grey"> - <color rgba="0.5 0.5 0.5 1.0"/> - </material> - - <material name="orange"> - <color rgba="1.0 0.4235 0.0392 1.0"/> - </material> - - <material name="brown"> - <color rgba="0.8706 0.8118 0.7647 1.0"/> - </material> - - <material name="red"> - <color rgba="0.8 0.0 0.0 1.0"/> - </material> - - <material name="white"> - <color rgba="1.0 1.0 1.0 1.0"/> - </material> - - <link name="base_footprint"/> - - <joint name="base_joint" type="fixed"> - <parent link="base_footprint"/> - <child link="base_link" /> - <origin xyz="0 0 0.010" rpy="0 0 0"/> - </joint> - - <link name="base_link"> - <visual> - <origin xyz="-0.064 0 0.047" rpy="0 0 0"/> - <geometry> - <box size="0.266 0.266 0.094"/> - </geometry> - <material name="blue"/> - </visual> - - <collision> - <origin xyz="-0.064 0 0.047" rpy="0 0 0"/> - <geometry> - <box size="0.266 0.266 0.094"/> - </geometry> - </collision> - - <inertial> - <origin xyz="0 0 0" rpy="0 0 0"/> - <mass value="1.3729096e+00"/> - <inertia ixx="8.7002718e-03" ixy="-4.7576583e-05" ixz="1.1160499e-04" - iyy="8.6195418e-03" iyz="-3.5422299e-06" - izz="1.4612727e-02" /> - </inertial> - </link> - - <joint name="wheel_left_joint" type="continuous"> - <parent link="base_link"/> - <child link="wheel_left_link"/> - <origin xyz="0.0 0.144 0.023" rpy="-1.57 0 0"/> - <axis xyz="0 0 1"/> - </joint> - - <link name="wheel_left_link"> - <visual> - <origin xyz="0 0 0" rpy="0 0 0"/> - <geometry> - <cylinder length="0.018" radius="0.033"/> - </geometry> - <material name="light_black"/> - </visual> - - <collision> - <origin xyz="0 0 0" rpy="0 0 0"/> - <geometry> - <cylinder length="0.018" radius="0.033"/> - </geometry> - </collision> - - <inertial> - <origin xyz="0 0 0" /> - <mass value="2.8498940e-02" /> - <inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09" - iyy="1.1192413e-05" iyz="-1.4400107e-11" - izz="2.0712558e-05" /> - </inertial> - </link> - - <joint name="wheel_right_joint" type="continuous"> - <parent link="base_link"/> - <child link="wheel_right_link"/> - <origin xyz="0.0 -0.144 0.023" rpy="-1.57 0 0"/> - <axis xyz="0 0 1"/> - </joint> - - <link name="wheel_right_link"> - <visual> - <origin xyz="0 0 0" rpy="0 0 0"/> - <geometry> - <cylinder length="0.018" radius="0.033"/> - </geometry> - <material name="light_black"/> - </visual> - - <collision> - <origin xyz="0 0 0" rpy="0 0 0"/> - <geometry> - <cylinder length="0.018" radius="0.033"/> - </geometry> - </collision> - - <inertial> - <origin xyz="0 0 0" /> - <mass value="2.8498940e-02" /> - <inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09" - iyy="1.1192413e-05" iyz="-1.4400107e-11" - izz="2.0712558e-05" /> - </inertial> - </link> - - <joint name="caster_back_right_joint" type="fixed"> - <parent link="base_link"/> - <child link="caster_back_right_link"/> - <origin xyz="-0.177 -0.064 -0.004" rpy="-1.57 0 0"/> - </joint> - - <link name="caster_back_right_link"> - <collision> - <origin xyz="0 0.001 0" rpy="0 0 0"/> - <geometry> - <box size="0.030 0.009 0.020"/> - </geometry> - </collision> - - <inertial> - <origin xyz="0 0 0" /> - <mass value="0.005" /> - <inertia ixx="0.001" ixy="0.0" ixz="0.0" - iyy="0.001" iyz="0.0" - izz="0.001" /> - </inertial> - </link> - - <joint name="caster_back_left_joint" type="fixed"> - <parent link="base_link"/> - <child link="caster_back_left_link"/> - <origin xyz="-0.177 0.064 -0.004" rpy="-1.57 0 0"/> - </joint> - - <link name="caster_back_left_link"> - <collision> - <origin xyz="0 0.001 0" rpy="0 0 0"/> - <geometry> - <box size="0.030 0.009 0.020"/> - </geometry> - </collision> - - <inertial> - <origin xyz="0 0 0" /> - <mass value="0.005" /> - <inertia ixx="0.001" ixy="0.0" ixz="0.0" - iyy="0.001" iyz="0.0" - izz="0.001" /> - </inertial> - </link> - - <joint name="imu_joint" type="fixed"> - <parent link="base_link"/> - <child link="imu_link"/> - <origin xyz="0.0 0 0.068" rpy="0 0 0"/> - </joint> - - <link name="imu_link"/> - - <joint name="scan_joint" type="fixed"> - <parent link="base_link"/> - <child link="base_scan"/> - <origin xyz="-0.064 0 0.122" rpy="0 0 0"/> - </joint> - - <link name="base_scan"> - <collision> - <origin xyz="0.015 0 -0.0065" rpy="0 0 0"/> - <geometry> - <cylinder length="0.0315" radius="0.055"/> - </geometry> - </collision> - - <inertial> - <mass value="0.114" /> - <origin xyz="0 0 0" /> - <inertia ixx="0.001" ixy="0.0" ixz="0.0" - iyy="0.001" iyz="0.0" - izz="0.001" /> - </inertial> - </link> - - <joint name="camera_joint" type="fixed"> - <origin xyz="0.064 -0.065 0.094" rpy="0 0 0"/> - <parent link="base_link"/> - <child link="camera_link"/> - </joint> - - <link name="camera_link"> - <collision> - <origin xyz="0.003 0.065 0.007" rpy="0 0 0"/> - <geometry> - <box size="0.012 0.132 0.020"/> - </geometry> - </collision> - - <!-- This inertial field needs doesn't contain reliable data!! --> -<!-- <inertial> - <mass value="0.564" /> - <origin xyz="0 0 0" /> - <inertia ixx="0.003881243" ixy="0.0" ixz="0.0" - iyy="0.000498940" iyz="0.0" - izz="0.003879257" /> - </inertial>--> - </link> - - <joint name="camera_rgb_joint" type="fixed"> - <origin xyz="0.005 0.018 0.013" rpy="-1.57 0 -1.57"/> - <!-- <origin xyz="${r200_cam_rgb_px} ${r200_cam_rgb_py} ${r200_cam_rgb_pz}" rpy="0 0 0"/> --> - <parent link="camera_link"/> - <child link="camera_rgb_frame"/> - </joint> - <link name="camera_rgb_frame"/> - - <joint name="camera_rgb_optical_joint" type="fixed"> - <origin xyz="0 0 0" rpy="-1.57 0 -1.57"/> - <parent link="camera_rgb_frame"/> - <child link="camera_rgb_optical_frame"/> - </joint> - <link name="camera_rgb_optical_frame"/> - - <joint name="camera_depth_joint" type="fixed"> - <origin xyz="0.005 0.028 0.013" rpy="-1.57 0 -1.57"/> - <!-- <origin xyz="${r200_cam_rgb_px} ${r200_cam_rgb_py + r200_cam_depth_offset} ${r200_cam_rgb_pz}" rpy="0 0 0"/> --> - <parent link="camera_link"/> - <child link="camera_depth_frame"/> - </joint> - <link name="camera_depth_frame"/> - - <joint name="camera_depth_optical_joint" type="fixed"> - <origin xyz="0 0 0" rpy="-1.57 0 -1.57"/> - <parent link="camera_depth_frame"/> - <child link="camera_depth_optical_frame"/> - </joint> - <link name="camera_depth_optical_frame"/> - -</robot> diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo.world deleted file mode 100644 index 9a9e1bdbe8..0000000000 --- a/nav2_system_tests/worlds/turtlebot3_ros2_demo.world +++ /dev/null @@ -1,497 +0,0 @@ -<?xml version="1.0"?> -<sdf version="1.6"> - <world name="default"> - - <include> - <uri>model://ground_plane</uri> - </include> - - <include> - <uri>model://sun</uri> - </include> - - <scene> - <shadows>false</shadows> - </scene> - - <gui fullscreen='0'> - <camera name='user_camera'> - <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose> - <view_controller>orbit</view_controller> - <projection_type>perspective</projection_type> - </camera> - </gui> - - <physics type="ode"> - <real_time_update_rate>1000.0</real_time_update_rate> - <max_step_size>0.001</max_step_size> - <real_time_factor>1</real_time_factor> - <ode> - <solver> - <type>quick</type> - <iters>150</iters> - <precon_iters>0</precon_iters> - <sor>1.400000</sor> - <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling> - </solver> - <constraints> - <cfm>0.00001</cfm> - <erp>0.2</erp> - <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel> - <contact_surface_layer>0.01000</contact_surface_layer> - </constraints> - </ode> - </physics> - - <model name="turtlebot3_world"> - <static>1</static> - <include> - <uri>model://turtlebot3_world</uri> - </include> - </model> - - <model name="turtlebot3"> - <pose>-2.0 -0.5 0.01 0.0 0.0 0.0</pose> - - <link name="base_footprint"/> - - <link name="base_link"> - - <inertial> - <pose>-0.064 0 0.048 0 0 0</pose> - <inertia> - <ixx>0.001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.001</iyy> - <iyz>0.000</iyz> - <izz>0.001</izz> - </inertia> - <mass>1.0</mass> - </inertial> - - <collision name="base_collision"> - <pose>-0.064 0 0.048 0 0 0</pose> - <geometry> - <box> - <size>0.265 0.265 0.089</size> - </box> - </geometry> - </collision> - - <visual name="base_visual"> - <pose>-0.064 0 0 0 0 0</pose> - <geometry> - <mesh> - <uri>model://turtlebot3_waffle/meshes/waffle_base.dae</uri> - <scale>0.001 0.001 0.001</scale> - </mesh> - </geometry> - </visual> - </link> - - <link name="imu_link"> - <sensor name="tb3_imu" type="imu"> - <always_on>true</always_on> - <update_rate>200</update_rate> - <imu> - <angular_velocity> - <x> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>2e-4</stddev> - </noise> - </x> - <y> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>2e-4</stddev> - </noise> - </y> - <z> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>2e-4</stddev> - </noise> - </z> - </angular_velocity> - <linear_acceleration> - <x> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>1.7e-2</stddev> - </noise> - </x> - <y> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>1.7e-2</stddev> - </noise> - </y> - <z> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>1.7e-2</stddev> - </noise> - </z> - </linear_acceleration> - </imu> - <plugin name="my_imu_plugin" filename="libgazebo_ros_imu_sensor.so"> - <initial_orientation_as_reference>false</initial_orientation_as_reference> - <ros> - <!-- <namespace>/tb3</namespace> --> - <remapping>~/out:=imu</remapping> - </ros> - </plugin> - </sensor> - </link> - - <link name="base_scan"> - <inertial> - <pose>-0.052 0 0.111 0 0 0</pose> - <inertia> - <ixx>0.001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.001</iyy> - <iyz>0.000</iyz> - <izz>0.001</izz> - </inertia> - <mass>0.125</mass> - </inertial> - - <collision name="lidar_sensor_collision"> - <pose>-0.064 0 0.121 0 0 0</pose> - <geometry> - <cylinder> - <radius>0.0508</radius> - <length>0.055</length> - </cylinder> - </geometry> - </collision> - - <visual name="lidar_sensor_visual"> - <pose>-0.032 0 0.171 0 0 0</pose> - <geometry> - <mesh> - <uri>model://turtlebot3_waffle/meshes/lds.dae</uri> - <scale>0.001 0.001 0.001</scale> - </mesh> - </geometry> - </visual> - - <sensor name="hls_lfcd_lds" type="ray"> - <always_on>true</always_on> - <visualize>false</visualize> - <pose>-0.064 0 0.121 0 0 0</pose> - <update_rate>5</update_rate> - <ray> - <scan> - <horizontal> - <samples>360</samples> - <resolution>1.000000</resolution> - <min_angle>0.000000</min_angle> - <max_angle>6.280000</max_angle> - </horizontal> - </scan> - <range> - <min>0.120000</min> - <max>3.5</max> - <resolution>0.015000</resolution> - </range> - <noise> - <type>gaussian</type> - <mean>0.0</mean> - <stddev>0.01</stddev> - </noise> - </ray> - <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so"> - <ros> - <!-- <namespace>/tb3</namespace> --> - <remapping>~/out:=scan</remapping> - </ros> - <output_type>sensor_msgs/LaserScan</output_type> - </plugin> - </sensor> - </link> - - <link name="wheel_left_link"> - - <inertial> - <pose>0.0 0.144 0.023 -1.57 0 0</pose> - <inertia> - <ixx>0.001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.001</iyy> - <iyz>0.000</iyz> - <izz>0.001</izz> - </inertia> - <mass>0.1</mass> - </inertial> - - <collision name="wheel_left_collision"> - <pose>0.0 0.144 0.023 -1.57 0 0</pose> - <geometry> - <cylinder> - <radius>0.033</radius> - <length>0.018</length> - </cylinder> - </geometry> - <surface> - <!-- This friction pamareter don't contain reliable data!! --> - <friction> - <ode> - <mu>100000.0</mu> - <mu2>100000.0</mu2> - <fdir1>0 0 0</fdir1> - <slip1>0.0</slip1> - <slip2>0.0</slip2> - </ode> - </friction> - <contact> - <ode> - <soft_cfm>0</soft_cfm> - <soft_erp>0.2</soft_erp> - <kp>1e+5</kp> - <kd>1</kd> - <max_vel>0.01</max_vel> - <min_depth>0.001</min_depth> - </ode> - </contact> - </surface> - </collision> - - <visual name="wheel_left_visual"> - <pose>0.0 0.144 0.023 0 0 0</pose> - <geometry> - <mesh> - <uri>model://turtlebot3_waffle/meshes/left_tire.dae</uri> - <scale>0.001 0.001 0.001</scale> - </mesh> - </geometry> - </visual> - </link> - - <link name="wheel_right_link"> - - <inertial> - <pose>0.0 -0.144 0.023 -1.57 0 0</pose> - <inertia> - <ixx>0.001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.001</iyy> - <iyz>0.000</iyz> - <izz>0.001</izz> - </inertia> - <mass>0.1</mass> - </inertial> - - <collision name="wheel_right_collision"> - <pose>0.0 -0.144 0.023 -1.57 0 0</pose> - <geometry> - <cylinder> - <radius>0.033</radius> - <length>0.018</length> - </cylinder> - </geometry> - <surface> - <!-- This friction pamareter don't contain reliable data!! --> - <friction> - <ode> - <mu>100000.0</mu> - <mu2>100000.0</mu2> - <fdir1>0 0 0</fdir1> - <slip1>0.0</slip1> - <slip2>0.0</slip2> - </ode> - </friction> - <contact> - <ode> - <soft_cfm>0</soft_cfm> - <soft_erp>0.2</soft_erp> - <kp>1e+5</kp> - <kd>1</kd> - <max_vel>0.01</max_vel> - <min_depth>0.001</min_depth> - </ode> - </contact> - </surface> - </collision> - - <visual name="wheel_right_visual"> - <pose>0.0 -0.144 0.023 0 0 0</pose> - <geometry> - <mesh> - <uri>model://turtlebot3_waffle/meshes/right_tire.dae</uri> - <scale>0.001 0.001 0.001</scale> - </mesh> - </geometry> - </visual> - </link> - - <link name='caster_back_right_link'> - <pose>-0.177 -0.064 -0.004 0 0 0</pose> - <inertial> - <mass>0.001</mass> - <inertia> - <ixx>0.00001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.00001</iyy> - <iyz>0.000</iyz> - <izz>0.00001</izz> - </inertia> - </inertial> - <collision name='collision'> - <geometry> - <sphere> - <radius>0.005000</radius> - </sphere> - </geometry> - <surface> - <contact> - <ode> - <soft_cfm>0</soft_cfm> - <soft_erp>0.2</soft_erp> - <kp>1e+5</kp> - <kd>1</kd> - <max_vel>0.01</max_vel> - <min_depth>0.001</min_depth> - </ode> - </contact> - </surface> - </collision> - </link> - - <link name='caster_back_left_link'> - <pose>-0.177 0.064 -0.004 0 0 0</pose> - <inertial> - <mass>0.001</mass> - <inertia> - <ixx>0.00001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.00001</iyy> - <iyz>0.000</iyz> - <izz>0.00001</izz> - </inertia> - </inertial> - <collision name='collision'> - <geometry> - <sphere> - <radius>0.005000</radius> - </sphere> - </geometry> - <surface> - <contact> - <ode> - <soft_cfm>0</soft_cfm> - <soft_erp>0.2</soft_erp> - <kp>1e+5</kp> - <kd>1</kd> - <max_vel>0.01</max_vel> - <min_depth>0.001</min_depth> - </ode> - </contact> - </surface> - </collision> - </link> - - <joint name="base_joint" type="fixed"> - <parent>base_footprint</parent> - <child>base_link</child> - <pose>0.0 0.0 0.010 0 0 0</pose> - </joint> - - <joint name="wheel_left_joint" type="revolute"> - <parent>base_link</parent> - <child>wheel_left_link</child> - <pose>0.0 0.144 0.023 -1.57 0 0</pose> - <axis> - <xyz>0 0 1</xyz> - </axis> - </joint> - - <joint name="wheel_right_joint" type="revolute"> - <parent>base_link</parent> - <child>wheel_right_link</child> - <pose>0.0 -0.144 0.023 -1.57 0 0</pose> - <axis> - <xyz>0 0 1</xyz> - </axis> - </joint> - - <joint name='caster_back_right_joint' type='ball'> - <parent>base_link</parent> - <child>caster_back_right_link</child> - </joint> - - <joint name='caster_back_left_joint' type='ball'> - <parent>base_link</parent> - <child>caster_back_left_link</child> - </joint> - - <joint name="imu_joint" type="fixed"> - <parent>base_link</parent> - <child>imu_link</child> - <pose>-0.032 0 0.068 0 0 0</pose> - <axis> - <xyz>0 0 1</xyz> - </axis> - </joint> - - <joint name="lidar_joint" type="fixed"> - <parent>base_link</parent> - <child>base_scan</child> - <pose>-0.064 0 0.121 0 0 0</pose> - <axis> - <xyz>0 0 1</xyz> - </axis> - </joint> - - <plugin name="turtlebot3_diff_drive" filename="libgazebo_ros_diff_drive.so"> - - <ros> - <!-- <namespace>/tb3</namespace> --> - <remapping>/tf:=tf</remapping> - </ros> - - <!-- wheels --> - <left_joint>wheel_left_joint</left_joint> - <right_joint>wheel_right_joint</right_joint> - - <!-- kinematics --> - <wheel_separation>0.287</wheel_separation> - <wheel_diameter>0.066</wheel_diameter> - - <!-- limits --> - <max_wheel_torque>20</max_wheel_torque> - <max_wheel_acceleration>1.0</max_wheel_acceleration> - - <!-- output --> - <publish_odom>true</publish_odom> - <publish_odom_tf>true</publish_odom_tf> - <publish_wheel_tf>false</publish_wheel_tf> - - <odometry_frame>odom</odometry_frame> - <robot_base_frame>base_footprint</robot_base_frame> - - </plugin> - - <plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so"> - <ros> - <!-- <namespace>/tb3</namespace> --> - <remapping>~/out:=joint_states</remapping> - </ros> - <update_rate>250</update_rate> - <joint_name>wheel_left_joint</joint_name> - <joint_name>wheel_right_joint</joint_name> - </plugin> - - </model> - - </world> -</sdf> - diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world deleted file mode 100644 index 94b72499a2..0000000000 --- a/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world +++ /dev/null @@ -1,555 +0,0 @@ -<?xml version="1.0"?> -<sdf version="1.6"> - <world name="default"> - - <spherical_coordinates> - <!-- currently gazebo has a bug: instead of outputing lat, long, altitude in ENU - (x = East, y = North and z = Up) as the default configurations, it's outputting (-E)(-N)U, - therefore we rotate the default frame 180 so that it would go back to ENU - see: https://github.com/osrf/gazebo/issues/2022 --> - <surface_model>EARTH_WGS84</surface_model> - <world_frame_orientation>ENU</world_frame_orientation> - <latitude_deg>55.944831</latitude_deg> - <longitude_deg>-3.186998</longitude_deg> - <elevation>0.0</elevation> - <heading_deg>180.0</heading_deg> - </spherical_coordinates> - - <include> - <uri>model://ground_plane</uri> - </include> - - <include> - <uri>model://sun</uri> - </include> - - <scene> - <shadows>false</shadows> - </scene> - - <gui fullscreen='0'> - <camera name='user_camera'> - <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose> - <view_controller>orbit</view_controller> - <projection_type>perspective</projection_type> - </camera> - </gui> - - <physics type="ode"> - <real_time_update_rate>1000.0</real_time_update_rate> - <max_step_size>0.001</max_step_size> - <real_time_factor>1</real_time_factor> - <ode> - <solver> - <type>quick</type> - <iters>150</iters> - <precon_iters>0</precon_iters> - <sor>1.400000</sor> - <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling> - </solver> - <constraints> - <cfm>0.00001</cfm> - <erp>0.2</erp> - <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel> - <contact_surface_layer>0.01000</contact_surface_layer> - </constraints> - </ode> - </physics> - - <model name="turtlebot3"> - <pose>-2.0 -0.5 0.01 0.0 0.0 0.0</pose> - - <link name="base_footprint"/> - - <link name="base_link"> - - <inertial> - <pose>-0.064 0 0.048 0 0 0</pose> - <inertia> - <ixx>0.001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.001</iyy> - <iyz>0.000</iyz> - <izz>0.001</izz> - </inertia> - <mass>1.0</mass> - </inertial> - - <collision name="base_collision"> - <pose>-0.064 0 0.048 0 0 0</pose> - <geometry> - <box> - <size>0.265 0.265 0.089</size> - </box> - </geometry> - </collision> - - <visual name="base_visual"> - <pose>-0.064 0 0 0 0 0</pose> - <geometry> - <mesh> - <uri>model://turtlebot3_waffle/meshes/waffle_base.dae</uri> - <scale>0.001 0.001 0.001</scale> - </mesh> - </geometry> - </visual> - </link> - - <link name="imu_link"> - <sensor name="tb3_imu" type="imu"> - <always_on>true</always_on> - <update_rate>200</update_rate> - <imu> - <angular_velocity> - <x> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>2e-4</stddev> - </noise> - </x> - <y> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>2e-4</stddev> - </noise> - </y> - <z> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>2e-4</stddev> - </noise> - </z> - </angular_velocity> - <linear_acceleration> - <x> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>1.7e-2</stddev> - </noise> - </x> - <y> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>1.7e-2</stddev> - </noise> - </y> - <z> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>1.7e-2</stddev> - </noise> - </z> - </linear_acceleration> - </imu> - <plugin name="my_imu_plugin" filename="libgazebo_ros_imu_sensor.so"> - <initial_orientation_as_reference>false</initial_orientation_as_reference> - <ros> - <!-- <namespace>/tb3</namespace> --> - <remapping>~/out:=/imu/data</remapping> - </ros> - </plugin> - </sensor> - </link> - - <link name="gps_link"> - <!-- Some weird bug occurs if this joint gets the default inertia --> - <inertial> - <pose>-0.052 0 0.111 0 0 0</pose> - <inertia> - <mass>0.001</mass> - <ixx>0.001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.001</iyy> - <iyz>0.000</iyz> - <izz>0.001</izz> - </inertia> - <mass>0.125</mass> - </inertial> - <sensor name="tb3_gps" type="gps"> - <always_on>true</always_on> - <update_rate>1</update_rate> - <pose>0 0 0 0 0 0</pose> - <gps> - <position_sensing> - <horizontal> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>0.1</stddev> - </noise> - </horizontal> - <vertical> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>0.0</stddev> - </noise> - </vertical> - </position_sensing> - </gps> - <plugin name="my_gps_plugin" filename="libgazebo_ros_gps_sensor.so"> - <ros> - <remapping>~/out:=/gps/fix</remapping> - </ros> - </plugin> - </sensor> - </link> - - <link name="base_scan"> - <inertial> - <pose>-0.052 0 0.111 0 0 0</pose> - <inertia> - <ixx>0.001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.001</iyy> - <iyz>0.000</iyz> - <izz>0.001</izz> - </inertia> - <mass>0.125</mass> - </inertial> - - <collision name="lidar_sensor_collision"> - <pose>-0.064 0 0.121 0 0 0</pose> - <geometry> - <cylinder> - <radius>0.0508</radius> - <length>0.055</length> - </cylinder> - </geometry> - </collision> - - <visual name="lidar_sensor_visual"> - <pose>-0.032 0 0.171 0 0 0</pose> - <geometry> - <mesh> - <uri>model://turtlebot3_waffle/meshes/lds.dae</uri> - <scale>0.001 0.001 0.001</scale> - </mesh> - </geometry> - </visual> - - <sensor name="hls_lfcd_lds" type="ray"> - <always_on>true</always_on> - <visualize>false</visualize> - <pose>-0.064 0 0.121 0 0 0</pose> - <update_rate>5</update_rate> - <ray> - <scan> - <horizontal> - <samples>360</samples> - <resolution>1.000000</resolution> - <min_angle>0.000000</min_angle> - <max_angle>6.280000</max_angle> - </horizontal> - </scan> - <range> - <min>0.120000</min> - <max>3.5</max> - <resolution>0.015000</resolution> - </range> - <noise> - <type>gaussian</type> - <mean>0.0</mean> - <stddev>0.01</stddev> - </noise> - </ray> - <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so"> - <ros> - <!-- <namespace>/tb3</namespace> --> - <remapping>~/out:=scan</remapping> - </ros> - <output_type>sensor_msgs/LaserScan</output_type> - </plugin> - </sensor> - </link> - - <link name="wheel_left_link"> - - <inertial> - <pose>0.0 0.144 0.023 -1.57 0 0</pose> - <inertia> - <ixx>0.001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.001</iyy> - <iyz>0.000</iyz> - <izz>0.001</izz> - </inertia> - <mass>0.1</mass> - </inertial> - - <collision name="wheel_left_collision"> - <pose>0.0 0.144 0.023 -1.57 0 0</pose> - <geometry> - <cylinder> - <radius>0.033</radius> - <length>0.018</length> - </cylinder> - </geometry> - <surface> - <!-- This friction pamareter don't contain reliable data!! --> - <friction> - <ode> - <mu>100000.0</mu> - <mu2>100000.0</mu2> - <fdir1>0 0 0</fdir1> - <slip1>0.0</slip1> - <slip2>0.0</slip2> - </ode> - </friction> - <contact> - <ode> - <soft_cfm>0</soft_cfm> - <soft_erp>0.2</soft_erp> - <kp>1e+5</kp> - <kd>1</kd> - <max_vel>0.01</max_vel> - <min_depth>0.001</min_depth> - </ode> - </contact> - </surface> - </collision> - - <visual name="wheel_left_visual"> - <pose>0.0 0.144 0.023 0 0 0</pose> - <geometry> - <mesh> - <uri>model://turtlebot3_waffle/meshes/left_tire.dae</uri> - <scale>0.001 0.001 0.001</scale> - </mesh> - </geometry> - </visual> - </link> - - <link name="wheel_right_link"> - - <inertial> - <pose>0.0 -0.144 0.023 -1.57 0 0</pose> - <inertia> - <ixx>0.001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.001</iyy> - <iyz>0.000</iyz> - <izz>0.001</izz> - </inertia> - <mass>0.1</mass> - </inertial> - - <collision name="wheel_right_collision"> - <pose>0.0 -0.144 0.023 -1.57 0 0</pose> - <geometry> - <cylinder> - <radius>0.033</radius> - <length>0.018</length> - </cylinder> - </geometry> - <surface> - <!-- This friction pamareter don't contain reliable data!! --> - <friction> - <ode> - <mu>100000.0</mu> - <mu2>100000.0</mu2> - <fdir1>0 0 0</fdir1> - <slip1>0.0</slip1> - <slip2>0.0</slip2> - </ode> - </friction> - <contact> - <ode> - <soft_cfm>0</soft_cfm> - <soft_erp>0.2</soft_erp> - <kp>1e+5</kp> - <kd>1</kd> - <max_vel>0.01</max_vel> - <min_depth>0.001</min_depth> - </ode> - </contact> - </surface> - </collision> - - <visual name="wheel_right_visual"> - <pose>0.0 -0.144 0.023 0 0 0</pose> - <geometry> - <mesh> - <uri>model://turtlebot3_waffle/meshes/right_tire.dae</uri> - <scale>0.001 0.001 0.001</scale> - </mesh> - </geometry> - </visual> - </link> - - <link name='caster_back_right_link'> - <pose>-0.177 -0.064 -0.004 0 0 0</pose> - <inertial> - <mass>0.001</mass> - <inertia> - <ixx>0.00001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.00001</iyy> - <iyz>0.000</iyz> - <izz>0.00001</izz> - </inertia> - </inertial> - <collision name='collision'> - <geometry> - <sphere> - <radius>0.005000</radius> - </sphere> - </geometry> - <surface> - <contact> - <ode> - <soft_cfm>0</soft_cfm> - <soft_erp>0.2</soft_erp> - <kp>1e+5</kp> - <kd>1</kd> - <max_vel>0.01</max_vel> - <min_depth>0.001</min_depth> - </ode> - </contact> - </surface> - </collision> - </link> - - <link name='caster_back_left_link'> - <pose>-0.177 0.064 -0.004 0 0 0</pose> - <inertial> - <mass>0.001</mass> - <inertia> - <ixx>0.00001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.00001</iyy> - <iyz>0.000</iyz> - <izz>0.00001</izz> - </inertia> - </inertial> - <collision name='collision'> - <geometry> - <sphere> - <radius>0.005000</radius> - </sphere> - </geometry> - <surface> - <contact> - <ode> - <soft_cfm>0</soft_cfm> - <soft_erp>0.2</soft_erp> - <kp>1e+5</kp> - <kd>1</kd> - <max_vel>0.01</max_vel> - <min_depth>0.001</min_depth> - </ode> - </contact> - </surface> - </collision> - </link> - - <joint name="base_joint" type="fixed"> - <parent>base_footprint</parent> - <child>base_link</child> - <pose>0.0 0.0 0.010 0 0 0</pose> - </joint> - - <joint name="wheel_left_joint" type="revolute"> - <parent>base_link</parent> - <child>wheel_left_link</child> - <pose>0.0 0.144 0.023 -1.57 0 0</pose> - <axis> - <xyz>0 0 1</xyz> - </axis> - </joint> - - <joint name="wheel_right_joint" type="revolute"> - <parent>base_link</parent> - <child>wheel_right_link</child> - <pose>0.0 -0.144 0.023 -1.57 0 0</pose> - <axis> - <xyz>0 0 1</xyz> - </axis> - </joint> - - <joint name='caster_back_right_joint' type='ball'> - <parent>base_link</parent> - <child>caster_back_right_link</child> - </joint> - - <joint name='caster_back_left_joint' type='ball'> - <parent>base_link</parent> - <child>caster_back_left_link</child> - </joint> - - <joint name="imu_joint" type="fixed"> - <parent>base_link</parent> - <child>imu_link</child> - <pose>-0.032 0 0.068 0 0 0</pose> - <axis> - <xyz>0 0 1</xyz> - </axis> - </joint> - - <joint name="gps_sensor_joint" type="fixed"> - <parent>base_link</parent> - <child>gps_link</child> - <pose>-0.05 0 0.05 0 0 0</pose> - <axis> - <xyz>0 0 1</xyz> - </axis> - </joint> - - <joint name="lidar_joint" type="fixed"> - <parent>base_link</parent> - <child>base_scan</child> - <pose>-0.064 0 0.121 0 0 0</pose> - <axis> - <xyz>0 0 1</xyz> - </axis> - </joint> - - <plugin name="turtlebot3_diff_drive" filename="libgazebo_ros_diff_drive.so"> - - <ros> - <!-- <namespace>/tb3</namespace> --> - <remapping>/tf:=tf</remapping> - </ros> - - <!-- wheels --> - <left_joint>wheel_left_joint</left_joint> - <right_joint>wheel_right_joint</right_joint> - - <!-- kinematics --> - <wheel_separation>0.287</wheel_separation> - <wheel_diameter>0.066</wheel_diameter> - - <!-- limits --> - <max_wheel_torque>20</max_wheel_torque> - <max_wheel_acceleration>1.0</max_wheel_acceleration> - - <!-- output --> - <publish_odom>true</publish_odom> - <publish_odom_tf>false</publish_odom_tf> - <publish_wheel_tf>false</publish_wheel_tf> - - <odometry_frame>odom</odometry_frame> - <robot_base_frame>base_link</robot_base_frame> - - </plugin> - - <plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so"> - <ros> - <!-- <namespace>/tb3</namespace> --> - <remapping>~/out:=joint_states</remapping> - </ros> - <update_rate>250</update_rate> - <joint_name>wheel_left_joint</joint_name> - <joint_name>wheel_right_joint</joint_name> - </plugin> - - </model> - - </world> -</sdf> - diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world deleted file mode 100644 index b8c0d0d128..0000000000 --- a/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world +++ /dev/null @@ -1,558 +0,0 @@ -<?xml version="1.0"?> -<sdf version="1.6"> - <world name="default"> - - <include> - <uri>model://ground_plane</uri> - </include> - - <include> - <uri>model://sun</uri> - </include> - - <scene> - <shadows>false</shadows> - </scene> - - <gui fullscreen='0'> - <camera name='user_camera'> - <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose> - <view_controller>orbit</view_controller> - <projection_type>perspective</projection_type> - </camera> - </gui> - - <physics type="ode"> - <real_time_update_rate>1000.0</real_time_update_rate> - <max_step_size>0.001</max_step_size> - <real_time_factor>1</real_time_factor> - <ode> - <solver> - <type>quick</type> - <iters>150</iters> - <precon_iters>0</precon_iters> - <sor>1.400000</sor> - <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling> - </solver> - <constraints> - <cfm>0.00001</cfm> - <erp>0.2</erp> - <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel> - <contact_surface_layer>0.01000</contact_surface_layer> - </constraints> - </ode> - </physics> - - <model name="turtlebot3_world"> - <static>1</static> - <include> - <uri>model://turtlebot3_world</uri> - </include> - </model> - - <model name="turtlebot3"> - <pose>-2.0 -0.5 0.01 0.0 0.0 0.0</pose> - - <link name="base_footprint"/> - - <link name="base_link"> - - <inertial> - <pose>-0.064 0 0.048 0 0 0</pose> - <inertia> - <ixx>0.001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.001</iyy> - <iyz>0.000</iyz> - <izz>0.001</izz> - </inertia> - <mass>1.0</mass> - </inertial> - - <collision name="base_collision"> - <pose>-0.064 0 0.048 0 0 0</pose> - <geometry> - <box> - <size>0.265 0.265 0.089</size> - </box> - </geometry> - </collision> - - <visual name="base_visual"> - <pose>-0.064 0 0 0 0 0</pose> - <geometry> - <mesh> - <uri>model://turtlebot3_waffle/meshes/waffle_base.dae</uri> - <scale>0.001 0.001 0.001</scale> - </mesh> - </geometry> - </visual> - </link> - - <link name="imu_link"> - <sensor name="tb3_imu" type="imu"> - <always_on>true</always_on> - <update_rate>200</update_rate> - <imu> - <angular_velocity> - <x> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>2e-4</stddev> - </noise> - </x> - <y> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>2e-4</stddev> - </noise> - </y> - <z> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>2e-4</stddev> - </noise> - </z> - </angular_velocity> - <linear_acceleration> - <x> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>1.7e-2</stddev> - </noise> - </x> - <y> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>1.7e-2</stddev> - </noise> - </y> - <z> - <noise type="gaussian"> - <mean>0.0</mean> - <stddev>1.7e-2</stddev> - </noise> - </z> - </linear_acceleration> - </imu> - <plugin name="my_imu_plugin" filename="libgazebo_ros_imu_sensor.so"> - <initial_orientation_as_reference>false</initial_orientation_as_reference> - <ros> - <!-- <namespace>/tb3</namespace> --> - <remapping>~/out:=imu</remapping> - </ros> - </plugin> - </sensor> - </link> - - <link name="base_scan"> - <inertial> - <pose>-0.052 0 0.111 0 0 0</pose> - <inertia> - <ixx>0.001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.001</iyy> - <iyz>0.000</iyz> - <izz>0.001</izz> - </inertia> - <mass>0.125</mass> - </inertial> - - <collision name="lidar_sensor_collision"> - <pose>-0.020 0 0.161 0 0 0</pose> - <geometry> - <cylinder> - <radius>0.0508</radius> - <length>0.055</length> - </cylinder> - </geometry> - </collision> - - <visual name="lidar_sensor_visual"> - <pose>-0.064 0 0.121 0 0 0</pose> - <geometry> - <mesh> - <uri>model://turtlebot3_waffle/meshes/lds.dae</uri> - <scale>0.001 0.001 0.001</scale> - </mesh> - </geometry> - </visual> - - <sensor name="hls_lfcd_lds" type="ray"> - <always_on>true</always_on> - <visualize>false</visualize> - <pose>-0.064 0 0.121 0 0 0</pose> - <update_rate>5</update_rate> - <ray> - <scan> - <horizontal> - <samples>360</samples> - <resolution>1.000000</resolution> - <min_angle>0.000000</min_angle> - <max_angle>6.280000</max_angle> - </horizontal> - </scan> - <range> - <min>0.120000</min> - <max>3.5</max> - <resolution>0.015000</resolution> - </range> - <noise> - <type>gaussian</type> - <mean>0.0</mean> - <stddev>0.01</stddev> - </noise> - </ray> - <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so"> - <ros> - <!-- <namespace>/tb3</namespace> --> - <remapping>~/out:=scan</remapping> - </ros> - <output_type>sensor_msgs/LaserScan</output_type> - </plugin> - </sensor> - </link> - - <link name="wheel_left_link"> - - <inertial> - <pose>0.0 0.144 0.023 -1.57 0 0</pose> - <inertia> - <ixx>0.001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.001</iyy> - <iyz>0.000</iyz> - <izz>0.001</izz> - </inertia> - <mass>0.1</mass> - </inertial> - - <collision name="wheel_left_collision"> - <pose>0.0 0.144 0.023 -1.57 0 0</pose> - <geometry> - <cylinder> - <radius>0.033</radius> - <length>0.018</length> - </cylinder> - </geometry> - <surface> - <!-- This friction pamareter don't contain reliable data!! --> - <friction> - <ode> - <mu>100000.0</mu> - <mu2>100000.0</mu2> - <fdir1>0 0 0</fdir1> - <slip1>0.0</slip1> - <slip2>0.0</slip2> - </ode> - </friction> - <contact> - <ode> - <soft_cfm>0</soft_cfm> - <soft_erp>0.2</soft_erp> - <kp>1e+5</kp> - <kd>1</kd> - <max_vel>0.01</max_vel> - <min_depth>0.001</min_depth> - </ode> - </contact> - </surface> - </collision> - - <visual name="wheel_left_visual"> - <pose>0.0 0.144 0.023 0 0 0</pose> - <geometry> - <mesh> - <uri>model://turtlebot3_waffle/meshes/left_tire.dae</uri> - <scale>0.001 0.001 0.001</scale> - </mesh> - </geometry> - </visual> - </link> - - <link name="wheel_right_link"> - - <inertial> - <pose>0.0 -0.144 0.023 -1.57 0 0</pose> - <inertia> - <ixx>0.001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.001</iyy> - <iyz>0.000</iyz> - <izz>0.001</izz> - </inertia> - <mass>0.1</mass> - </inertial> - - <collision name="wheel_right_collision"> - <pose>0.0 -0.144 0.023 -1.57 0 0</pose> - <geometry> - <cylinder> - <radius>0.033</radius> - <length>0.018</length> - </cylinder> - </geometry> - <surface> - <!-- This friction pamareter don't contain reliable data!! --> - <friction> - <ode> - <mu>100000.0</mu> - <mu2>100000.0</mu2> - <fdir1>0 0 0</fdir1> - <slip1>0.0</slip1> - <slip2>0.0</slip2> - </ode> - </friction> - <contact> - <ode> - <soft_cfm>0</soft_cfm> - <soft_erp>0.2</soft_erp> - <kp>1e+5</kp> - <kd>1</kd> - <max_vel>0.01</max_vel> - <min_depth>0.001</min_depth> - </ode> - </contact> - </surface> - </collision> - - <visual name="wheel_right_visual"> - <pose>0.0 -0.144 0.023 0 0 0</pose> - <geometry> - <mesh> - <uri>model://turtlebot3_waffle/meshes/right_tire.dae</uri> - <scale>0.001 0.001 0.001</scale> - </mesh> - </geometry> - </visual> - </link> - - <link name='caster_back_right_link'> - <pose>-0.177 -0.064 -0.004 0 0 0</pose> - <inertial> - <mass>0.001</mass> - <inertia> - <ixx>0.00001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.00001</iyy> - <iyz>0.000</iyz> - <izz>0.00001</izz> - </inertia> - </inertial> - <collision name='collision'> - <geometry> - <sphere> - <radius>0.005000</radius> - </sphere> - </geometry> - <surface> - <contact> - <ode> - <soft_cfm>0</soft_cfm> - <soft_erp>0.2</soft_erp> - <kp>1e+5</kp> - <kd>1</kd> - <max_vel>0.01</max_vel> - <min_depth>0.001</min_depth> - </ode> - </contact> - </surface> - </collision> - </link> - - <link name='caster_back_left_link'> - <pose>-0.177 0.064 -0.004 0 0 0</pose> - <inertial> - <mass>0.001</mass> - <inertia> - <ixx>0.00001</ixx> - <ixy>0.000</ixy> - <ixz>0.000</ixz> - <iyy>0.00001</iyy> - <iyz>0.000</iyz> - <izz>0.00001</izz> - </inertia> - </inertial> - <collision name='collision'> - <geometry> - <sphere> - <radius>0.005000</radius> - </sphere> - </geometry> - <surface> - <contact> - <ode> - <soft_cfm>0</soft_cfm> - <soft_erp>0.2</soft_erp> - <kp>1e+5</kp> - <kd>1</kd> - <max_vel>0.01</max_vel> - <min_depth>0.001</min_depth> - </ode> - </contact> - </surface> - </collision> - </link> - - <joint name="base_joint" type="fixed"> - <parent>base_footprint</parent> - <child>base_link</child> - <pose>0.0 0.0 0.010 0 0 0</pose> - </joint> - - <joint name="wheel_left_joint" type="revolute"> - <parent>base_link</parent> - <child>wheel_left_link</child> - <pose>0.0 0.144 0.023 -1.57 0 0</pose> - <axis> - <xyz>0 0 1</xyz> - </axis> - </joint> - - <joint name="wheel_right_joint" type="revolute"> - <parent>base_link</parent> - <child>wheel_right_link</child> - <pose>0.0 -0.144 0.023 -1.57 0 0</pose> - <axis> - <xyz>0 0 1</xyz> - </axis> - </joint> - - <joint name='caster_back_right_joint' type='ball'> - <parent>base_link</parent> - <child>caster_back_right_link</child> - </joint> - - <joint name='caster_back_left_joint' type='ball'> - <parent>base_link</parent> - <child>caster_back_left_link</child> - </joint> - - <joint name="imu_joint" type="fixed"> - <parent>base_link</parent> - <child>imu_link</child> - <pose>-0.032 0 0.068 0 0 0</pose> - <axis> - <xyz>0 0 1</xyz> - </axis> - </joint> - - <joint name="lidar_joint" type="fixed"> - <parent>base_link</parent> - <child>base_scan</child> - <pose>-0.064 0 0.121 0 0 0</pose> - <axis> - <xyz>0 0 1</xyz> - </axis> - </joint> - - <plugin name="turtlebot3_diff_drive" filename="libgazebo_ros_diff_drive.so"> - - <ros> - <!-- <namespace>/tb3</namespace> --> - <remapping>/tf:=tf</remapping> - </ros> - - <!-- wheels --> - <left_joint>wheel_left_joint</left_joint> - <right_joint>wheel_right_joint</right_joint> - - <!-- kinematics --> - <wheel_separation>0.287</wheel_separation> - <wheel_diameter>0.066</wheel_diameter> - - <!-- limits --> - <max_wheel_torque>20</max_wheel_torque> - <max_wheel_acceleration>1.0</max_wheel_acceleration> - - <!-- output --> - <publish_odom>true</publish_odom> - <publish_odom_tf>true</publish_odom_tf> - <publish_wheel_tf>false</publish_wheel_tf> - - <odometry_frame>odom</odometry_frame> - <robot_base_frame>base_footprint</robot_base_frame> - - </plugin> - - <plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so"> - <ros> - <!-- <namespace>/tb3</namespace> --> - <remapping>~/out:=joint_states</remapping> - </ros> - <update_rate>250</update_rate> - <joint_name>wheel_left_joint</joint_name> - <joint_name>wheel_right_joint</joint_name> - </plugin> - - </model> - - <model name='cardboard_box'> - <pose frame=''>-1.0 0.6 0.15 0 -0 0</pose> - - <link name='link'> - <inertial> - <mass>20</mass> - <inertia> - <ixx>0.0416667</ixx> - <ixy>0</ixy> - <ixz>0</ixz> - <iyy>0.0566667</iyy> - <iyz>0</iyz> - <izz>0.0683333</izz> - </inertia> - <pose frame=''>0 0 0 0 -0 0</pose> - </inertial> - - <collision name='collision'> - <geometry> - <box> - <size>0.5 0.4 0.3</size> - </box> - </geometry> - <surface> - <friction> - <ode> - <mu>1</mu> - <mu2>1</mu2> - </ode> - <torsional> - <ode/> - </torsional> - </friction> - <contact> - <ode> - <kp>1e+07</kp> - <kd>1</kd> - <min_depth>0.001</min_depth> - <max_vel>0.1</max_vel> - </ode> - </contact> - <bounce/> - </surface> - <max_contacts>10</max_contacts> - </collision> - - <visual name='visual'> - <pose frame=''>0 0 -0.15 0 -0 0</pose> - <geometry> - <mesh> - <uri>model://cardboard_box/meshes/cardboard_box.dae</uri> - <scale>1.25932 1.00745 0.755591</scale> - </mesh> - </geometry> - </visual> - - <self_collide>0</self_collide> - <enable_wind>0</enable_wind> - <kinematic>0</kinematic> - </link> - </model> - - </world> -</sdf> diff --git a/nav2_system_tests/worlds/world_only.model b/nav2_system_tests/worlds/world_only.model deleted file mode 100644 index 4c45d4e2f9..0000000000 --- a/nav2_system_tests/worlds/world_only.model +++ /dev/null @@ -1,54 +0,0 @@ -<?xml version="1.0"?> -<sdf version="1.6"> - <world name="default"> - - <include> - <uri>model://ground_plane</uri> - </include> - - <include> - <uri>model://sun</uri> - </include> - - <scene> - <shadows>false</shadows> - </scene> - - <gui fullscreen='0'> - <camera name='user_camera'> - <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose> - <view_controller>orbit</view_controller> - <projection_type>perspective</projection_type> - </camera> - </gui> - - <physics type="ode"> - <real_time_update_rate>1000.0</real_time_update_rate> - <max_step_size>0.001</max_step_size> - <real_time_factor>1</real_time_factor> - <ode> - <solver> - <type>quick</type> - <iters>150</iters> - <precon_iters>0</precon_iters> - <sor>1.400000</sor> - <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling> - </solver> - <constraints> - <cfm>0.00001</cfm> - <erp>0.2</erp> - <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel> - <contact_surface_layer>0.01000</contact_surface_layer> - </constraints> - </ode> - </physics> - - <model name="turtlebot3_world"> - <static>1</static> - <include> - <uri>model://turtlebot3_world</uri> - </include> - </model> - - </world> -</sdf> From a82090f5f1dde9caf9161dc77cbdcd4849a38691 Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Thu, 20 Jun 2024 23:26:24 +0200 Subject: [PATCH 17/18] Feat/port systems tests failure to new gazebo (#4453) * migrate system failures test to new gazebo Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * cleanly kill gz sim in gazebo Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * use robot publisher and kill gz sim Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> --------- Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> --- nav2_system_tests/CMakeLists.txt | 2 +- .../src/system_failure/CMakeLists.txt | 3 - .../test_system_failure_launch.py | 77 +++++++++++++------ 3 files changed, 53 insertions(+), 29 deletions(-) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index baa1256d52..acd3d0c2a7 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -116,8 +116,8 @@ if(BUILD_TESTING) add_subdirectory(src/planning) add_subdirectory(src/localization) add_subdirectory(src/system) + add_subdirectory(src/system_failure) # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634 - # add_subdirectory(src/system_failure) # add_subdirectory(src/updown) # add_subdirectory(src/waypoint_follower) # add_subdirectory(src/gps_navigation) diff --git a/nav2_system_tests/src/system_failure/CMakeLists.txt b/nav2_system_tests/src/system_failure/CMakeLists.txt index 6bc4620535..babd49fd5b 100644 --- a/nav2_system_tests/src/system_failure/CMakeLists.txt +++ b/nav2_system_tests/src/system_failure/CMakeLists.txt @@ -5,8 +5,5 @@ ament_add_test(test_failure_navigator TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml ) diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py index 01a7d50bc9..052ec50b31 100755 --- a/nav2_system_tests/src/system_failure/test_system_failure_launch.py +++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py @@ -16,6 +16,7 @@ # limitations under the License. import os +from pathlib import Path import sys from ament_index_python.packages import get_package_share_directory @@ -23,6 +24,7 @@ from launch import LaunchDescription from launch import LaunchService from launch.actions import ( + AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable, @@ -33,11 +35,22 @@ from launch_testing.legacy import LaunchTestService from nav2_common.launch import RewrittenYaml +from nav2_simple_commander.utils import kill_os_processes def generate_launch_description(): - map_yaml_file = os.getenv('TEST_MAP') - world = os.getenv('TEST_WORLD') + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + ros_gz_sim_dir = get_package_share_directory('ros_gz_sim') + + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') bt_navigator_xml = os.path.join( get_package_share_directory('nav2_bt_navigator'), @@ -45,8 +58,7 @@ def generate_launch_description(): os.getenv('BT_NAVIGATOR_XML'), ) - bringup_dir = get_package_share_directory('nav2_bringup') - params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') + params_file = os.path.join(nav2_bringup_dir, 'params', 'nav2_params.yaml') # Replace the `use_astar` setting on the params file param_substitutions = { @@ -66,33 +78,46 @@ def generate_launch_description(): SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), # Launch gazebo server for simulation - ExecuteProcess( - cmd=[ - 'gzserver', - '-s', - 'libgazebo_ros_init.so', - '--minimal_comms', - world, - ], - output='screen', + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ), - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(sim_dir)).parent.resolve()) + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-2.0', + 'y_pose': '-0.5', + 'z_pose': '0.01', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), ), Node( - package='tf2_ros', - executable='static_transform_publisher', + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') ), launch_arguments={ 'namespace': '', @@ -129,7 +154,9 @@ def main(argv=sys.argv[1:]): lts.add_test_action(ld, test1_action) ls = LaunchService(argv=argv) ls.include_launch_description(ld) - return lts.run(ls) + return_code = lts.run(ls) + kill_os_processes('gz sim') + return return_code if __name__ == '__main__': From dd7e1fc088f20501cac0241bb8fa3e09d38672ce Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Thu, 20 Jun 2024 23:45:09 +0200 Subject: [PATCH 18/18] Feat/port upown tests to new gazebo (#4454) * minor speeling check Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * minor changes Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * minor changes Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> --------- Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> --- nav2_system_tests/CMakeLists.txt | 2 +- nav2_system_tests/src/updown/README.md | 2 +- nav2_system_tests/src/updown/test_updown_launch.py | 11 +++++------ 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index acd3d0c2a7..13576a6cdd 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -117,8 +117,8 @@ if(BUILD_TESTING) add_subdirectory(src/localization) add_subdirectory(src/system) add_subdirectory(src/system_failure) + add_subdirectory(src/updown) # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634 - # add_subdirectory(src/updown) # add_subdirectory(src/waypoint_follower) # add_subdirectory(src/gps_navigation) # add_subdirectory(src/behaviors/spin) diff --git a/nav2_system_tests/src/updown/README.md b/nav2_system_tests/src/updown/README.md index 6756d52465..ff84569c3a 100644 --- a/nav2_system_tests/src/updown/README.md +++ b/nav2_system_tests/src/updown/README.md @@ -14,7 +14,7 @@ If the test passes, you should see this comment in the output: To run the test in a loop 1000x, run the `test_updown_reliablity` script and log the output: ``` -./test_updown_reliablity |& tee /tmp/updown.log +./test_updown_reliability |& tee /tmp/updown.log ``` When the test is completed, pipe the log to the `updownresults.py` script to get a summary of the results: ``` diff --git a/nav2_system_tests/src/updown/test_updown_launch.py b/nav2_system_tests/src/updown/test_updown_launch.py index 458779411b..760f458d57 100755 --- a/nav2_system_tests/src/updown/test_updown_launch.py +++ b/nav2_system_tests/src/updown/test_updown_launch.py @@ -25,11 +25,9 @@ def generate_launch_description(): # Configuration parameters for the launch - launch_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') - map_yaml_file = os.path.join( - get_package_share_directory('nav2_system_tests'), 'maps/map_circular.yaml' - ) + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') # Specify the actions start_tf_cmd_1 = Node( @@ -61,7 +59,8 @@ def generate_launch_description(): ) nav2_bringup = launch.actions.IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), launch_arguments={ 'map': map_yaml_file, 'use_sim_time': 'True', @@ -77,7 +76,7 @@ def generate_launch_description(): 'lib/nav2_system_tests/test_updown', ) ], - cwd=[launch_dir], + cwd=[nav2_bringup_dir], output='screen', )