From 65dab923acb60e7b6f56470a154a80965cb7dd25 Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Wed, 6 Nov 2019 22:12:15 +0800
Subject: [PATCH] change planner name to planner property
---
.../compute_path_to_pose_action.hpp | 4 +-
.../launch/nav2_localization_launch.py | 6 +-
.../nav2_multi_tb3_simulation_launch.py | 20 +-
.../bringup/launch/nav2_navigation_launch.py | 6 +-
.../launch/nav2_tb3_simulation_launch.py | 49 ++---
.../params/nav2_multirobot_params_1.yaml | 171 ++++++++++++++++++
.../params/nav2_multirobot_params_2.yaml | 171 ++++++++++++++++++
nav2_bringup/bringup/params/nav2_params.yaml | 45 ++++-
nav2_bt_navigator/README.md | 4 +-
.../behavior_trees/navigate_w_replanning.xml | 2 +-
.../navigate_w_replanning_and_recovery.xml | 2 +-
nav2_bt_navigator/src/bt_navigator.cpp | 2 +-
nav2_controller/README.md | 8 +
nav2_controller/src/nav2_controller.cpp | 2 +-
nav2_core/README.md | 7 +
nav2_core/include/nav2_core/local_planner.hpp | 2 +-
nav2_costmap_2d/CMakeLists.txt | 6 +-
nav2_costmap_2d/README.md | 79 ++++++++
.../include/nav2_costmap_2d/voxel_layer.hpp | 2 +-
nav2_costmap_2d/plugins/obstacle_layer.cpp | 3 +-
nav2_costmap_2d/plugins/voxel_layer.cpp | 5 +-
nav2_costmap_2d/src/costmap_2d_markers.cpp | 4 +-
nav2_costmap_2d/src/costmap_2d_publisher.cpp | 47 ++---
nav2_costmap_2d/src/costmap_2d_ros.cpp | 2 +-
.../test/integration/inflation_tests.cpp | 2 +-
.../test/integration/obstacle_tests.cpp | 2 +-
.../integration/test_collision_checker.cpp | 2 +-
nav2_msgs/action/ComputePathToPose.action | 2 +-
.../include/nav2_planner/planner_server.hpp | 4 +-
nav2_planner/src/planner_server.cpp | 34 ++--
nav2_system_tests/package.xml | 2 -
.../localization/test_localization_node.cpp | 2 +-
.../src/planning/planner_tester.hpp | 2 +-
.../src/system/test_multi_robot_launch.py | 7 +-
tools/ros2_dependencies.repos | 2 +-
tools/run_test_suite.bash | 1 +
36 files changed, 590 insertions(+), 121 deletions(-)
create mode 100644 nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml
create mode 100644 nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml
create mode 100644 nav2_controller/README.md
create mode 100644 nav2_core/README.md
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp
index e85f4eee..c6df35c1 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp
@@ -38,7 +38,7 @@ public:
void on_tick() override
{
getInput("goal", goal_.pose);
- getInput("planner_name", goal_.planner_name);
+ getInput("planner_property", goal_.planner_property);
}
void on_success() override
@@ -57,7 +57,7 @@ public:
return providedBasicPorts({
BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
- BT::InputPort<std::string>("planner_name", "")
+ BT::InputPort<std::string>("planner_property", "")
});
}
diff --git a/nav2_bringup/bringup/launch/nav2_localization_launch.py b/nav2_bringup/bringup/launch/nav2_localization_launch.py
index a460e0bb..9a92ba4c 100644
--- a/nav2_bringup/bringup/launch/nav2_localization_launch.py
+++ b/nav2_bringup/bringup/launch/nav2_localization_launch.py
@@ -41,12 +41,8 @@ def generate_launch_description():
# https://github.com/ros2/launch_ros/issues/56
remappings = [((namespace, '/tf'), '/tf'),
((namespace, '/tf_static'), '/tf_static'),
- ('/scan', 'scan'),
('/tf', 'tf'),
- ('/tf_static', 'tf_static'),
- ('/cmd_vel', 'cmd_vel'),
- ('/map', 'map'),
- ('/goal_pose', 'goal_pose')]
+ ('/tf_static', 'tf_static')]
# Create our own temporary YAML files that include substitutions
param_substitutions = {
diff --git a/nav2_bringup/bringup/launch/nav2_multi_tb3_simulation_launch.py b/nav2_bringup/bringup/launch/nav2_multi_tb3_simulation_launch.py
index 49e306ef..abcc87c4 100644
--- a/nav2_bringup/bringup/launch/nav2_multi_tb3_simulation_launch.py
+++ b/nav2_bringup/bringup/launch/nav2_multi_tb3_simulation_launch.py
@@ -50,7 +50,7 @@ def generate_launch_description():
# On this example all robots are launched with the same settings
map_yaml_file = LaunchConfiguration('map')
- params_file = LaunchConfiguration('params_file')
+
bt_xml_file = LaunchConfiguration('bt_xml_file')
autostart = LaunchConfiguration('autostart')
rviz_config_file = LaunchConfiguration('rviz_config')
@@ -74,10 +74,15 @@ def generate_launch_description():
default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.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_params.yaml'),
- description='Full path to the ROS2 parameters file to use for all launched nodes')
+ 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_bt_xml_cmd = DeclareLaunchArgument(
'bt_xml_file',
@@ -132,6 +137,8 @@ def generate_launch_description():
source_file=rviz_config_file,
replacements={'<robot_namespace>': ('/' + robot['name'])})
+ params_file = LaunchConfiguration(robot['name'] + '_params_file')
+
group = GroupAction([
# TODO(orduno)
# Each `action.Node` within the `localization` and `navigation` launch
@@ -204,7 +211,8 @@ def generate_launch_description():
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_world_cmd)
ld.add_action(declare_map_yaml_cmd)
- ld.add_action(declare_params_file_cmd)
+ ld.add_action(declare_robot1_params_file_cmd)
+ ld.add_action(declare_robot2_params_file_cmd)
ld.add_action(declare_bt_xml_cmd)
ld.add_action(declare_use_rviz_cmd)
ld.add_action(declare_autostart_cmd)
diff --git a/nav2_bringup/bringup/launch/nav2_navigation_launch.py b/nav2_bringup/bringup/launch/nav2_navigation_launch.py
index 20c2377e..dc3f2a58 100644
--- a/nav2_bringup/bringup/launch/nav2_navigation_launch.py
+++ b/nav2_bringup/bringup/launch/nav2_navigation_launch.py
@@ -42,12 +42,8 @@ def generate_launch_description():
# https://github.com/ros2/launch_ros/issues/56
remappings = [((namespace, '/tf'), '/tf'),
((namespace, '/tf_static'), '/tf_static'),
- ('/scan', 'scan'),
('/tf', 'tf'),
- ('/tf_static', 'tf_static'),
- ('/cmd_vel', 'cmd_vel'),
- ('/map', 'map'),
- ('/goal_pose', 'goal_pose')]
+ ('/tf_static', 'tf_static')]
# Create our own temporary YAML files that include substitutions
param_substitutions = {
diff --git a/nav2_bringup/bringup/launch/nav2_tb3_simulation_launch.py b/nav2_bringup/bringup/launch/nav2_tb3_simulation_launch.py
index 08fbe1a2..c6534a50 100644
--- a/nav2_bringup/bringup/launch/nav2_tb3_simulation_launch.py
+++ b/nav2_bringup/bringup/launch/nav2_tb3_simulation_launch.py
@@ -16,7 +16,7 @@
import os
-from ament_index_python.packages import get_package_prefix, get_package_share_directory
+from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (DeclareLaunchArgument, EmitEvent, ExecuteProcess,
@@ -56,11 +56,8 @@ def generate_launch_description():
# https://github.com/ros2/launch_ros/issues/56
remappings = [((namespace, '/tf'), '/tf'),
((namespace, '/tf_static'), '/tf_static'),
- ('/scan', 'scan'),
('/tf', 'tf'),
- ('/tf_static', 'tf_static'),
- ('/cmd_vel', 'cmd_vel'),
- ('/map', 'map')]
+ ('/tf_static', 'tf_static')]
# Declare the launch arguments
declare_namespace_cmd = DeclareLaunchArgument(
@@ -152,37 +149,19 @@ def generate_launch_description():
remappings=remappings,
arguments=[urdf])
- # TODO(orduno) RVIZ crashing if launched as a node: https://github.com/ros2/rviz/issues/442
- # Launching as node works after applying the change described on the github issue.
- # Once fixed, launch by providing the remappings:
- # rviz_remappings = [('/tf', 'tf'),
- # ('/tf_static', 'tf_static'),
- # ('goal_pose', 'goal_pose'),
- # ('/clicked_point', 'clicked_point'),
- # ('/initialpose', 'initialpose'),
- # ('/parameter_events', 'parameter_events'),
- # ('/rosout', 'rosout')]
-
- # start_rviz_cmd = Node(
- # package='rviz2',
- # node_executable='rviz2',
- # node_name='rviz2',
- # arguments=['-d', rviz_config_file],
- # output='screen',
- # use_remappings=IfCondition(use_remappings),
- # remappings=rviz_remappings)
-
- start_rviz_cmd = ExecuteProcess(
+ start_rviz_cmd = Node(
condition=IfCondition(use_rviz),
- cmd=[os.path.join(get_package_prefix('rviz2'), 'lib/rviz2/rviz2'),
- ['-d', rviz_config_file],
- ['__ns:=/', namespace],
- '/tf:=tf',
- '/tf_static:=tf_static',
- '/goal_pose:=goal_pose',
- '/clicked_point:=clicked_point',
- '/initialpose:=initialpose'],
- cwd=[launch_dir], output='screen')
+ package='rviz2',
+ node_executable='rviz2',
+ node_name='rviz2',
+ arguments=['-d', rviz_config_file],
+ output='screen',
+ use_remappings=IfCondition(use_remappings),
+ remappings=[('/tf', 'tf'),
+ ('/tf_static', 'tf_static'),
+ ('goal_pose', 'goal_pose'),
+ ('/clicked_point', 'clicked_point'),
+ ('/initialpose', 'initialpose')])
exit_event_handler = RegisterEventHandler(
event_handler=OnProcessExit(
diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml
new file mode 100644
index 00000000..6c138d09
--- /dev/null
+++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml
@@ -0,0 +1,171 @@
+amcl:
+ ros__parameters:
+ use_sim_time: True
+ alpha1: 0.2
+ alpha2: 0.2
+ alpha3: 0.2
+ alpha4: 0.2
+ alpha5: 0.2
+ base_frame_id: "base_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: "differential"
+ 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
+
+amcl_map_client:
+ ros__parameters:
+ use_sim_time: True
+
+amcl_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+bt_navigator:
+ ros__parameters:
+ use_sim_time: True
+ bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
+
+bt_navigator_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+controller_server:
+ ros__parameters:
+ use_sim_time: True
+ 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
+ min_x_velocity_threshold: 0.001
+ # Add high threshold velocity for turtlebot 3 issue.
+ # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
+ min_y_velocity_threshold: 0.5
+ min_theta_velocity_threshold: 0.001
+ 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
+ xy_goal_tolerance: 0.25
+ transform_tolerance: 0.2
+ 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
+
+controller_server_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+local_costmap:
+ local_costmap:
+ ros__parameters:
+ use_sim_time: True
+ global_frame: odom
+ plugin_names: ["obstacle_layer", "inflation_layer"]
+ plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"]
+ rolling_window: true
+ width: 3
+ height: 3
+ resolution: 0.05
+ robot_radius: 0.22
+ inflation_layer.cost_scaling_factor: 3.0
+ obstacle_layer:
+ enabled: True
+ scan:
+ topic: /robot1/scan
+ max_obstacle_height: 2.0
+ clearing: True
+ marking: True
+ static_layer:
+ map_subscribe_transient_local: True
+ always_send_full_costmap: True
+ observation_sources: scan
+ local_costmap_client:
+ ros__parameters:
+ use_sim_time: True
+ local_costmap_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+global_costmap:
+ global_costmap:
+ ros__parameters:
+ use_sim_time: True
+ robot_radius: 0.22
+ obstacle_layer:
+ enabled: True
+ scan:
+ topic: /robot1/scan
+ max_obstacle_height: 2.0
+ clearing: True
+ marking: True
+ static_layer:
+ map_subscribe_transient_local: True
+ always_send_full_costmap: True
+ observation_sources: scan
+ global_costmap_client:
+ ros__parameters:
+ use_sim_time: True
+ global_costmap_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+map_server:
+ ros__parameters:
+ use_sim_time: True
+ yaml_filename: "turtlebot3_world.yaml"
+
+planner_server:
+ ros__parameters:
+ use_sim_time: True
+ tolerance: 0.0
+ use_astar: false
+
+planner_server_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+robot_state_publisher:
+ ros__parameters:
+ use_sim_time: True
diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml
new file mode 100644
index 00000000..d1f52ba2
--- /dev/null
+++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml
@@ -0,0 +1,171 @@
+amcl:
+ ros__parameters:
+ use_sim_time: True
+ alpha1: 0.2
+ alpha2: 0.2
+ alpha3: 0.2
+ alpha4: 0.2
+ alpha5: 0.2
+ base_frame_id: "base_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: "differential"
+ 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
+
+amcl_map_client:
+ ros__parameters:
+ use_sim_time: True
+
+amcl_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+bt_navigator:
+ ros__parameters:
+ use_sim_time: True
+ bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
+
+bt_navigator_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+controller_server:
+ ros__parameters:
+ use_sim_time: True
+ 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
+ min_x_velocity_threshold: 0.001
+ # Add high threshold velocity for turtlebot 3 issue.
+ # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
+ min_y_velocity_threshold: 0.5
+ min_theta_velocity_threshold: 0.001
+ 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
+ xy_goal_tolerance: 0.25
+ transform_tolerance: 0.2
+ 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
+
+controller_server_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+local_costmap:
+ local_costmap:
+ ros__parameters:
+ use_sim_time: True
+ global_frame: odom
+ plugin_names: ["obstacle_layer", "inflation_layer"]
+ plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"]
+ rolling_window: true
+ width: 3
+ height: 3
+ resolution: 0.05
+ robot_radius: 0.22
+ inflation_layer.cost_scaling_factor: 3.0
+ obstacle_layer:
+ enabled: True
+ scan:
+ topic: /robot2/scan
+ max_obstacle_height: 2.0
+ clearing: True
+ marking: True
+ static_layer:
+ map_subscribe_transient_local: True
+ always_send_full_costmap: True
+ observation_sources: scan
+ local_costmap_client:
+ ros__parameters:
+ use_sim_time: True
+ local_costmap_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+global_costmap:
+ global_costmap:
+ ros__parameters:
+ use_sim_time: True
+ robot_radius: 0.22
+ obstacle_layer:
+ enabled: True
+ scan:
+ topic: /robot2/scan
+ max_obstacle_height: 2.0
+ clearing: True
+ marking: True
+ static_layer:
+ map_subscribe_transient_local: True
+ always_send_full_costmap: True
+ observation_sources: scan
+ global_costmap_client:
+ ros__parameters:
+ use_sim_time: True
+ global_costmap_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+map_server:
+ ros__parameters:
+ use_sim_time: True
+ yaml_filename: "turtlebot3_world.yaml"
+
+planner_server:
+ ros__parameters:
+ use_sim_time: True
+ tolerance: 0.0
+ use_astar: false
+
+planner_server_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+robot_state_publisher:
+ ros__parameters:
+ use_sim_time: True
diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml
index d8c522f3..634fced8 100644
--- a/nav2_bringup/bringup/params/nav2_params.yaml
+++ b/nav2_bringup/bringup/params/nav2_params.yaml
@@ -102,25 +102,42 @@ local_costmap:
ros__parameters:
use_sim_time: True
global_frame: odom
- plugin_names: ["obstacle_layer", "inflation_layer"]
- plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"]
+ plugin_names: ["obstacle_layer", "voxel_layer", "inflation_layer"]
+ plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_costmap_2d::InflationLayer"]
rolling_window: true
width: 3
height: 3
resolution: 0.05
robot_radius: 0.22
- inflation_layer.cost_scaling_factor: 3.0
+ inflation_layer:
+ cost_scaling_factor: 3.0
obstacle_layer:
enabled: True
+ observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
+ data_type: "LaserScan"
+ voxel_layer:
+ enabled: True
+ publish_voxel_map: True
+ origin_z: 0.0
+ z_resolution: 0.2
+ z_voxels: 10
+ max_obstacle_height: 2.0
+ mark_threshold: 0
+ observation_sources: pointcloud
+ pointcloud:
+ topic: /intel_realsense_r200_depth/points
+ max_obstacle_height: 2.0
+ clearing: True
+ marking: True
+ data_type: "PointCloud2"
static_layer:
map_subscribe_transient_local: True
always_send_full_costmap: True
- observation_sources: scan
local_costmap_client:
ros__parameters:
use_sim_time: True
@@ -132,18 +149,36 @@ global_costmap:
global_costmap:
ros__parameters:
use_sim_time: True
+ plugin_names: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
+ plugin_types: ["nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_costmap_2d::InflationLayer"]
robot_radius: 0.22
obstacle_layer:
enabled: True
+ observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
+ data_type: "LaserScan"
+ voxel_layer:
+ enabled: True
+ publish_voxel_map: True
+ origin_z: 0.0
+ z_resolution: 0.2
+ z_voxels: 10
+ max_obstacle_height: 2.0
+ mark_threshold: 0
+ observation_sources: pointcloud
+ pointcloud:
+ topic: /intel_realsense_r200_depth/points
+ max_obstacle_height: 2.0
+ clearing: True
+ marking: True
+ data_type: "PointCloud2"
static_layer:
map_subscribe_transient_local: True
always_send_full_costmap: True
- observation_sources: scan
global_costmap_client:
ros__parameters:
use_sim_time: True
diff --git a/nav2_bt_navigator/README.md b/nav2_bt_navigator/README.md
index daca77cd..394efd02 100644
--- a/nav2_bt_navigator/README.md
+++ b/nav2_bt_navigator/README.md
@@ -37,7 +37,7 @@ The BT Navigator package has two sample XML-based descriptions of BTs. These tr
<RateController hz="1.0">
<Fallback>
<GoalReached/>
- <ComputePathToPose goal="${goal}" planner_name="ComputePathToPose"/>
+ <ComputePathToPose goal="${goal}" planner_property="GridBased"/>
</Fallback>
</RateController>
<FollowPath path="${path}"/>
@@ -55,7 +55,7 @@ Navigate with replanning is composed of the following custom decorator, conditio
* GoalReached: Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, which in that case the `ComputePathToPose` action node will not get ticked.
#### Action Nodes
-* ComputePathToPose: When this node is ticked, the goal will be placed on the blackboard which will be shared to the Behavior tree. The bt action node would then utilizes the action server to send a request to the global planner to recompute the global path. Once the global path is recomputed, the result will be sent back via action server and then the updated path will be placed on the blackboard. The `planner` parameter will tell the planning server which of the loaded planning plugins to utilize, in case of desiring different parameters, planners, or behaviors. The name of the planner should correspond with the high level task it accomplishes and align with the `planner_plugin_names` name given to it in the planner server. If no planner name is provided, it will use the only planner in the planner server when only one is available.
+* ComputePathToPose: When this node is ticked, the goal will be placed on the blackboard which will be shared to the Behavior tree. The bt action node would then utilizes the action server to send a request to the global planner to recompute the global path. Once the global path is recomputed, the result will be sent back via action server and then the updated path will be placed on the blackboard. The `planner` parameter will tell the planning server which of the loaded planning plugins to utilize, in case of desiring different parameters, planners, or behaviors. The name of the planner should correspond with the high level task it accomplishes and align with the `planner_property_names` name given to it in the planner server. If no planner name is provided, it will use the only planner in the planner server when only one is available.
The graphical version of this Behavior Tree:
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml
index 9b67ebe0..53fdd642 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml
+++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml
@@ -8,7 +8,7 @@
<RateController hz="1.0">
<Fallback>
<GoalReached/>
- <ComputePathToPose goal="{goal}" planner_name="ComputePathToPose"/>
+ <ComputePathToPose goal="{goal}" planner_property="GridBased"/>
</Fallback>
</RateController>
<FollowPath path="{path}"/>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
index 0e07710b..eb406607 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
+++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
@@ -10,7 +10,7 @@
<Fallback name="GoalReached">
<GoalReached/>
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
- <ComputePathToPose goal="{goal}" path="{path}" planner_name="ComputePathToPose"/>
+ <ComputePathToPose goal="{goal}" path="{path}" planner_property="GridBased"/>
<ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
</RecoveryNode>
</Fallback>
diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
index 9a9a0ad1..ee63854d 100644
--- a/nav2_bt_navigator/src/bt_navigator.cpp
+++ b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -62,7 +62,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_, this, false);
goal_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
- "/goal_pose",
+ "goal_pose",
rclcpp::SystemDefaultsQoS(),
std::bind(&BtNavigator::onGoalPoseReceived, this, std::placeholders::_1));
diff --git a/nav2_controller/README.md b/nav2_controller/README.md
new file mode 100644
index 00000000..83352682
--- /dev/null
+++ b/nav2_controller/README.md
@@ -0,0 +1,8 @@
+# Nav2 Controller
+
+The Nav2 Controller is an [Execution Module](../doc/requirements/requirements.md) that implements the `nav2_msgs::action::FollowPath` action server.
+
+An execution module implementing the `nav2_msgs::action::FollowPath` action server is responsible for generating command velocities for the robot, given the computed path from the planner module in `nav2_planner`. The nav2_controller package is designed to be loaded with plugins for path execution. The plugins need to implement functions in the virtual base class defined in the `local_planner` header file in `nav2_core` package.
+
+
+Currently available controller plugins are: DWB, and [TEB (dashing release)](https://github.com/rst-tu-dortmund/teb_local_planner/tree/dashing-devel).
\ No newline at end of file
diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp
index da63aff0..8bc61733 100644
--- a/nav2_controller/src/nav2_controller.cpp
+++ b/nav2_controller/src/nav2_controller.cpp
@@ -76,7 +76,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
odom_sub_ = std::make_unique<nav_2d_utils::OdomSubscriber>(node);
- vel_publisher_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 1);
+ vel_publisher_ = create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
// Create the action server that we implement with our followPath method
action_server_ = std::make_unique<ActionServer>(rclcpp_node_, "FollowPath",
diff --git a/nav2_core/README.md b/nav2_core/README.md
new file mode 100644
index 00000000..72a8630c
--- /dev/null
+++ b/nav2_core/README.md
@@ -0,0 +1,7 @@
+# Nav2 Core
+
+This package hosts the abstract interface (virtual base classes) for plugins to be used with the following:
+- global planner (e.g., `nav2_navfn_planner`)
+- local planner (i.e, path execution controller, e.g `nav2_dwb_controller`)
+- goal checker
+- recovery behaviors
diff --git a/nav2_core/include/nav2_core/local_planner.hpp b/nav2_core/include/nav2_core/local_planner.hpp
index 14b953c8..6fd7691d 100644
--- a/nav2_core/include/nav2_core/local_planner.hpp
+++ b/nav2_core/include/nav2_core/local_planner.hpp
@@ -54,7 +54,7 @@ namespace nav2_core
/**
* @class LocalPlanner
- * @brief planner interface that will be inherited by all local planners
+ * @brief planner interface that acts as a virtual base class for all local planner plugins
*/
class LocalPlanner
{
diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt
index ca42f6f9..56b40564 100644
--- a/nav2_costmap_2d/CMakeLists.txt
+++ b/nav2_costmap_2d/CMakeLists.txt
@@ -110,6 +110,10 @@ target_link_libraries(nav2_costmap_2d_markers
nav2_costmap_2d_core
)
+ament_target_dependencies(nav2_costmap_2d_markers
+ ${dependencies}
+)
+
add_executable(nav2_costmap_2d_cloud src/costmap_2d_cloud.cpp)
target_link_libraries(nav2_costmap_2d_cloud
nav2_costmap_2d_core
@@ -125,7 +129,7 @@ target_link_libraries(nav2_costmap_2d
layers
)
-install(TARGETS nav2_costmap_2d_core nav2_costmap_2d layers nav2_costmap_2d_client
+install(TARGETS nav2_costmap_2d_core nav2_costmap_2d layers nav2_costmap_2d_client nav2_costmap_2d_markers
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}
diff --git a/nav2_costmap_2d/README.md b/nav2_costmap_2d/README.md
index 405b9683..9dd12247 100644
--- a/nav2_costmap_2d/README.md
+++ b/nav2_costmap_2d/README.md
@@ -10,7 +10,86 @@ ROS2 port of the ROS1 navigation stack version, with minimal noteable changes ne
widespread discussion throughout the navigation stack (see issue https://github.com/ros-planning/navigation2/issues/177) and
general ROS2 community. A proposal temporary replacement has been submitted as a PR here: https://github.com/ros-planning/navigation2/pull/196
+## How to configure using Voxel Layer plugin:
+By default, the navigation stack uses the _Obstacle Layer_ to avoid obstacles in 2D. The _Voxel Layer_ allows for detecting obstacles in 3D using Pointcloud2 data. It requires Pointcloud2 data being published on some topic. For simulation, a Gazebo model of the robot with depth camera enabled will publish a pointcloud topic.
+
+The Voxel Layer plugin can be used to update the local costmap, glocal costmap or both, depending on how you define it in the `nav2_params.yaml` file in the nav2_bringup directory. The voxel layer plugin has to be added to the list of ```plugin_names``` and ```plugin_types``` in the global/local costmap scopes in the param file. If these are not defined in the param file, the default plugins set in nav2_costmap_2d will be used.
+
+Inside each costmap layer (voxel, obstacle, etc) define `observation_sources` param. Here you can define multiple sources to be used with the layer. The param configuration example below shows the way you can configure costmaps to use voxel layer.
+
+The `voxel_layer` param has `observation_source: pointcloud` in it's scope, and the param `pointcloud` has `topic`, and `data_type` inside it's scope. By default, the data_type is `LaserScan`, thus you need to specify `PointCloud2` if you are using PointCould2 data from a depth sensor.
+
+Example param configuration snippet for enabling voxel layer in local costmap is shown below (not all params are shown):
+```
+local_costmap:
+ local_costmap:
+ ros__parameters:
+ plugin_names: ["obstacle_layer", "voxel_layer", "inflation_layer"]
+ plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_costmap_2d::InflationLayer"]
+ obstacle_layer:
+ enabled: True
+ observation_sources: scan
+ scan:
+ topic: /scan
+ max_obstacle_height: 2.0
+ clearing: True
+ marking: True
+ data_type: "LaserScan"
+ voxel_layer:
+ enabled: True
+ publish_voxel_map: True
+ origin_z: 0.0
+ z_resolution: 0.2
+ z_voxels: 10
+ max_obstacle_height: 2.0
+ mark_threshold: 0
+ observation_sources: pointcloud
+ pointcloud:
+ topic: /intel_realsense_r200_depth/points
+ max_obstacle_height: 2.0
+ clearing: True
+ marking: True
+ data_type: "PointCloud2"
+```
+Please note that not all params needed to run the navigation stack are shown here. This example only shows how you can add different costmap layers, with multiple input sources of different types.
+
+### To visualize the voxels in RVIZ:
+- Make sure `publish_voxel_map` in `voxel_layer` param's scope is set to `True`.
+- Open a new terminal and run:
+ ```ros2 run nav2_costmap_2d nav2_costmap_2d_markers voxel_grid:=/local_costmap/voxel_grid visualization_marker:=/my_marker```
+ Here you can change `my_marker` to any topic name you like for the markers to be published on.
+
+- Then add `my_marker` to RVIZ using the GUI.
+
+
+####Errata:
+- To see the markers in 3D, you will need to change the _view_ in RVIZ to a 3 dimensional view (e.g. orbit) from the RVIZ GUI.
+- Currently due to some bug in rviz, you need to set the `fixed_frame` in the rviz display, to `odom` frame.
+- Using pointcloud data from a saved bag file while using gazebo simulation can be troublesome due to the clock time skipping to an earlier time.
+
+## How to use multiple sensor sources:
+Multiple sources can be added into a costmap layer (e.g., obstacle layer), by listing them under the 'observation_sources' for that layer.
+For example, to add laser scan and pointcloud as two different sources of inputs to the obstacle layer, you can define them in the params file as shown below for the local costmap:
+```
+local_costmap:
+ local_costmap:
+ ros__parameters:
+ plugin_names: ["obstacle_layer", "inflation_layer"]
+ plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"]
+ obstacle_layer:
+ enabled: True
+ observation_sources: scan pointcloud
+ scan:
+ topic: /scan
+ data_type: "LaserScan"
+ pointcloud:
+ topic: /intel_realsense_r200_depth/points
+ data_type: "PointCloud2"
+```
+In order to add multiple sources to the global costmap, follow the same procedure shown in the example above, but now adding the sources and their specific params under the `global_costmap` scope.
+
## Future Plans
- Conceptually, the costmap_2d model acts as a world model of what is known from the map, sensor, robot pose, etc. We'd like
to broaden this world model concept and use costmap's layer concept as motivation for providing a service-style interface to
potential clients needing information about the world (see issue https://github.com/ros-planning/navigation2/issues/18)
+
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
index 22dc74fd..5e963c4f 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
@@ -94,7 +94,7 @@ private:
double * max_y);
bool publish_voxel_;
- rclcpp::Publisher<nav2_msgs::msg::VoxelGrid>::SharedPtr voxel_pub_;
+ rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::VoxelGrid>::SharedPtr voxel_pub_;
nav2_voxel_grid::VoxelGrid voxel_grid_;
double z_resolution_, origin_z_;
int unknown_threshold_, mark_threshold_, size_z_;
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
index 347a139b..e40b0203 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -80,6 +80,7 @@ void ObstacleLayer::onInitialize()
rclcpp::ParameterValue(true));
declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));
declareParameter("combination_method", rclcpp::ParameterValue(1));
+ declareParameter("observation_sources", rclcpp::ParameterValue(std::string("")));
node_->get_parameter(name_ + "." + "enabled", enabled_);
node_->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
@@ -87,7 +88,7 @@ void ObstacleLayer::onInitialize()
node_->get_parameter(name_ + "." + "combination_method", combination_method_);
node_->get_parameter("track_unknown_space", track_unknown_space);
node_->get_parameter("transform_tolerance", transform_tolerance);
- node_->get_parameter("observation_sources", topics_string);
+ node_->get_parameter(name_ + "." + "observation_sources", topics_string);
RCLCPP_INFO(node_->get_logger(), "Subscribed to Topics: %s", topics_string.c_str());
diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp
index 105b88a1..526b6200 100644
--- a/nav2_costmap_2d/plugins/voxel_layer.cpp
+++ b/nav2_costmap_2d/plugins/voxel_layer.cpp
@@ -88,6 +88,8 @@ void VoxelLayer::onInitialize()
"voxel_grid", custom_qos);
}
+ voxel_pub_->on_activate();
+
clearing_endpoints_pub_ = node_->create_publisher<sensor_msgs::msg::PointCloud>(
"clearing_endpoints", custom_qos);
@@ -108,11 +110,12 @@ void VoxelLayer::matchSize()
void VoxelLayer::reset()
{
+ voxel_pub_->on_deactivate();
deactivate();
resetMaps();
- voxel_grid_.reset();
activate();
undeclareAllParameters();
+ voxel_pub_->on_activate();
}
void VoxelLayer::resetMaps()
diff --git a/nav2_costmap_2d/src/costmap_2d_markers.cpp b/nav2_costmap_2d/src/costmap_2d_markers.cpp
index 42357321..2f1af4f7 100644
--- a/nav2_costmap_2d/src/costmap_2d_markers.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_markers.cpp
@@ -62,7 +62,8 @@ float g_colors_a[] = {0.0f, 0.5f, 1.0f};
V_Cell g_cells;
nav2_util::LifecycleNode::SharedPtr g_node;
-rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub;
+rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>::SharedPtr pub;
+
void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
{
if (grid->data.empty()) {
@@ -150,6 +151,7 @@ int main(int argc, char ** argv)
pub = g_node->create_publisher<visualization_msgs::msg::Marker>(
"visualization_marker", 1);
+ pub->on_activate();
auto sub = g_node->create_subscription<nav2_msgs::msg::VoxelGrid>(
"voxel_grid", rclcpp::SystemDefaultsQoS(), voxelCallback);
diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp
index 32499c8b..5e6e414c 100644
--- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp
@@ -165,8 +165,10 @@ void Costmap2DPublisher::prepareCostmap()
void Costmap2DPublisher::publishCostmap()
{
- prepareCostmap();
- costmap_raw_pub_->publish(costmap_raw_);
+ if (node_->count_subscribers(costmap_raw_pub_->get_topic_name()) > 0) {
+ prepareCostmap();
+ costmap_raw_pub_->publish(costmap_raw_);
+ }
float resolution = costmap_->getResolution();
if (always_send_full_costmap_ || grid_.info.resolution != resolution ||
@@ -175,28 +177,31 @@ void Costmap2DPublisher::publishCostmap()
saved_origin_x_ != costmap_->getOriginX() ||
saved_origin_y_ != costmap_->getOriginY())
{
- prepareGrid();
- costmap_pub_->publish(grid_);
+ if (node_->count_subscribers(costmap_pub_->get_topic_name()) > 0) {
+ prepareGrid();
+ costmap_pub_->publish(grid_);
+ }
} else if (x0_ < xn_) {
- std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
- // Publish Just an Update
- map_msgs::msg::OccupancyGridUpdate update;
- update.header.stamp = rclcpp::Time();
- update.header.frame_id = global_frame_;
- update.x = x0_;
- update.y = y0_;
- update.width = xn_ - x0_;
- update.height = yn_ - y0_;
- update.data.resize(update.width * update.height);
-
- unsigned int i = 0;
- for (unsigned int y = y0_; y < yn_; y++) {
- for (unsigned int x = x0_; x < xn_; x++) {
- unsigned char cost = costmap_->getCost(x, y);
- update.data[i++] = cost_translation_table_[cost];
+ if (node_->count_subscribers(costmap_update_pub_->get_topic_name()) > 0) {
+ std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+ // Publish Just an Update
+ map_msgs::msg::OccupancyGridUpdate update;
+ update.header.stamp = rclcpp::Time();
+ update.header.frame_id = global_frame_;
+ update.x = x0_;
+ update.y = y0_;
+ update.width = xn_ - x0_;
+ update.height = yn_ - y0_;
+ update.data.resize(update.width * update.height);
+ unsigned int i = 0;
+ for (unsigned int y = y0_; y < yn_; y++) {
+ for (unsigned int x = x0_; x < xn_; x++) {
+ unsigned char cost = costmap_->getCost(x, y);
+ update.data[i++] = cost_translation_table_[cost];
+ }
}
+ costmap_update_pub_->publish(update);
}
- costmap_update_pub_->publish(update);
}
xn_ = yn_ = 0;
diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
index 3c846558..c6789f04 100644
--- a/nav2_costmap_2d/src/costmap_2d_ros.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -89,7 +89,7 @@ Costmap2DROS::Costmap2DROS(
declare_parameter("height", rclcpp::ParameterValue(10));
declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
declare_parameter("map_topic", rclcpp::ParameterValue(
- (parent_namespace_ == "/" ? "" : parent_namespace_ ) + std::string("/map")));
+ (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map")));
declare_parameter("observation_sources", rclcpp::ParameterValue(std::string("")));
declare_parameter("origin_x", rclcpp::ParameterValue(0.0));
declare_parameter("origin_y", rclcpp::ParameterValue(0.0));
diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp
index cd7e1c89..357852b6 100644
--- a/nav2_costmap_2d/test/integration/inflation_tests.cpp
+++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp
@@ -183,7 +183,7 @@ void TestNode::initNode(double inflation_radius)
"inflation_test_node", "", false, options);
// Declare non-plugin specific costmap parameters
- node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("/map")));
+ node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map")));
node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false));
node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false));
node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp
index 25b41e5d..0f1ddd39 100644
--- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp
+++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp
@@ -104,7 +104,7 @@ public:
TestNode()
{
node_ = std::make_shared<TestLifecycleNode>("obstacle_test_node");
- node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("/map")));
+ node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map")));
node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false));
node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false));
node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
diff --git a/nav2_costmap_2d/test/integration/test_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_collision_checker.cpp
index a7a79c07..01e78ffc 100644
--- a/nav2_costmap_2d/test/integration/test_collision_checker.cpp
+++ b/nav2_costmap_2d/test/integration/test_collision_checker.cpp
@@ -90,7 +90,7 @@ public:
global_frame_("map")
{
// Declare non-plugin specific costmap parameters
- declare_parameter("map_topic", rclcpp::ParameterValue(std::string("/map")));
+ declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map")));
declare_parameter("track_unknown_space", rclcpp::ParameterValue(true));
declare_parameter("use_maximum", rclcpp::ParameterValue(false));
declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action
index f5e90427..40a1b937 100644
--- a/nav2_msgs/action/ComputePathToPose.action
+++ b/nav2_msgs/action/ComputePathToPose.action
@@ -1,6 +1,6 @@
#goal definition
geometry_msgs/PoseStamped pose
-string planner_name
+string planner_property
---
#result definition
nav_msgs/Path path
diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp
index 5914785b..23c2f882 100644
--- a/nav2_planner/include/nav2_planner/planner_server.hpp
+++ b/nav2_planner/include/nav2_planner/planner_server.hpp
@@ -115,8 +115,8 @@ protected:
// Planner
PlannerMap planners_;
pluginlib::ClassLoader<nav2_core::GlobalPlanner> gp_loader_;
- std::vector<std::string> plugin_names_, plugin_types_;
- std::string planner_names_concat_;
+ std::vector<std::string> plugin_properties_, plugin_types_;
+ std::string planner_properties_concat_;
// TF buffer
std::shared_ptr<tf2_ros::Buffer> tf_;
diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
index 9eeb5d7a..c3bffd49 100644
--- a/nav2_planner/src/planner_server.cpp
+++ b/nav2_planner/src/planner_server.cpp
@@ -41,10 +41,10 @@ PlannerServer::PlannerServer()
RCLCPP_INFO(get_logger(), "Creating");
// Declare this node's parameters
- std::vector<std::string> default_name, default_type;
- default_name.push_back("ComputePathToPose");
+ std::vector<std::string> default_property, default_type;
+ default_property.push_back("GridBased");
default_type.push_back("nav2_navfn_planner/NavfnPlanner");
- declare_parameter("planner_plugin_names", default_name);
+ declare_parameter("planner_plugin_properties", default_property);
declare_parameter("planner_plugin_types", default_type);
// Setup the global costmap
@@ -77,11 +77,11 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state)
tf_ = costmap_ros_->getTfBuffer();
- get_parameter("planner_plugin_names", plugin_names_);
+ get_parameter("planner_plugin_properties", plugin_properties_);
get_parameter("planner_plugin_types", plugin_types_);
auto node = shared_from_this();
- if (plugin_names_.size() != plugin_types_.size()) {
+ if (plugin_properties_.size() != plugin_types_.size()) {
RCLCPP_FATAL(get_logger(),
"Planner plugin names and types sizes do not match!");
exit(-1);
@@ -92,9 +92,9 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state)
nav2_core::GlobalPlanner::Ptr planner =
gp_loader_.createUniqueInstance(plugin_types_[i]);
RCLCPP_INFO(get_logger(), "Created global planner plugin %s of type %s",
- plugin_names_[i].c_str(), plugin_types_[i].c_str());
- planner->configure(node, plugin_names_[i], tf_, costmap_ros_);
- planners_.insert({plugin_names_[i], planner});
+ plugin_properties_[i].c_str(), plugin_types_[i].c_str());
+ planner->configure(node, plugin_properties_[i], tf_, costmap_ros_);
+ planners_.insert({plugin_properties_[i], planner});
} catch (const pluginlib::PluginlibException & ex) {
RCLCPP_FATAL(get_logger(), "Failed to create global planner. Exception: %s",
ex.what());
@@ -103,7 +103,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state)
}
for (uint i = 0; i != plugin_types_.size(); i++) {
- planner_names_concat_ += plugin_names_[i] + std::string(" ");
+ planner_properties_concat_ += plugin_properties_[i] + std::string(" ");
}
// Initialize pubs & subs
@@ -224,27 +224,27 @@ PlannerServer::computePlan()
"(%.2f, %.2f).", start.pose.position.x, start.pose.position.y,
goal->pose.pose.position.x, goal->pose.pose.position.y);
- if (planners_.find(goal->planner_name) != planners_.end()) {
- result->path = planners_[goal->planner_name]->createPlan(start, goal->pose);
+ if (planners_.find(goal->planner_property) != planners_.end()) {
+ result->path = planners_[goal->planner_property]->createPlan(start, goal->pose);
} else {
- if (planners_.size() == 1 && goal->planner_name.empty()) {
+ if (planners_.size() == 1 && goal->planner_property.empty()) {
if (!single_planner_warning_given_) {
single_planner_warning_given_ = true;
RCLCPP_WARN(get_logger(), "No planners specified in action call. "
"Server will use only plugin %s in server."
- " This warning will appear once.", planner_names_concat_);
+ " This warning will appear once.", planner_properties_concat_.c_str());
}
result->path = planners_[planners_.begin()->first]->createPlan(start, goal->pose);
} else {
RCLCPP_ERROR(get_logger(), "planner %s is not a valid planner. "
- "Planner names are: %s", goal->planner_name.c_str(),
- planner_names_concat_.c_str());
+ "Planner names are: %s", goal->planner_property.c_str(),
+ planner_properties_concat_.c_str());
}
}
if (result->path.poses.size() == 0) {
RCLCPP_WARN(get_logger(), "Planning algorithm %s failed to generate a valid"
- " path to (%.2f, %.2f)", goal->planner_name.c_str(),
+ " path to (%.2f, %.2f)", goal->planner_property.c_str(),
goal->pose.pose.position.x, goal->pose.pose.position.y);
// TODO(orduno): define behavior if a preemption is available
action_server_->terminate_goals();
@@ -264,7 +264,7 @@ PlannerServer::computePlan()
return;
} catch (std::exception & ex) {
RCLCPP_WARN(get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"",
- goal->planner_name.c_str(), goal->pose.pose.position.x,
+ goal->planner_property.c_str(), goal->pose.pose.position.x,
goal->pose.pose.position.y, ex.what());
// TODO(orduno): provide information about fail error to parent task,
diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml
index ada78f78..afd4b166 100644
--- a/nav2_system_tests/package.xml
+++ b/nav2_system_tests/package.xml
@@ -45,8 +45,6 @@
<exec_depend>std_msgs</exec_depend>
<exec_depend>tf2_geometry_msgs</exec_depend>
<exec_depend>gazebo_ros_pkgs</exec_depend>
- <exec_depend>launch_ros</exec_depend>
- <exec_depend>launch_testing</exec_depend>
<exec_depend>navigation2</exec_depend>
<exec_depend>lcov</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
diff --git a/nav2_system_tests/src/localization/test_localization_node.cpp b/nav2_system_tests/src/localization/test_localization_node.cpp
index bf543f19..32ecf214 100644
--- a/nav2_system_tests/src/localization/test_localization_node.cpp
+++ b/nav2_system_tests/src/localization/test_localization_node.cpp
@@ -43,7 +43,7 @@ public:
node = rclcpp::Node::make_shared("localization_test");
- while (node->count_subscribers("/scan") < 1) {
+ while (node->count_subscribers("scan") < 1) {
std::this_thread::sleep_for(100ms);
rclcpp::spin_some(node);
}
diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp
index 4c4ac2d5..ce26d268 100644
--- a/nav2_system_tests/src/planning/planner_tester.hpp
+++ b/nav2_system_tests/src/planning/planner_tester.hpp
@@ -80,7 +80,7 @@ public:
if (!nav2_util::getCurrentPose(start, *tf_, "map", "base_link", 0.1)) {
return false;
}
- path = planners_["ComputePathToPose"]->createPlan(start, goal);
+ path = planners_["GridBased"]->createPlan(start, goal);
return true;
}
diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py
index 292b1d40..dc8270a7 100755
--- a/nav2_system_tests/src/system/test_multi_robot_launch.py
+++ b/nav2_system_tests/src/system/test_multi_robot_launch.py
@@ -40,7 +40,10 @@ 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')
+ robot1_params_file = os.path.join(bringup_dir, # noqa: F841
+ 'params/nav2_multirobot_params_1.yaml')
+ robot2_params_file = os.path.join(bringup_dir, # noqa: F841
+ 'params/nav2_multirobot_params_2.yaml')
# Names and poses of the robots
robots = [
@@ -86,6 +89,8 @@ def generate_launch_description():
# Define commands for launching the navigation instances
nav_instances_cmds = []
for robot in robots:
+ params_file = eval(robot['name'] + '_params_file')
+
group = GroupAction([
# Instances use the robot's name for namespace
PushRosNamespace(robot['name']),
diff --git a/tools/ros2_dependencies.repos b/tools/ros2_dependencies.repos
index 6b58b64a..71a7cdf3 100644
--- a/tools/ros2_dependencies.repos
+++ b/tools/ros2_dependencies.repos
@@ -2,7 +2,7 @@ repositories:
BehaviorTree.CPP:
type: git
url: https://github.com/BehaviorTree/BehaviorTree.CPP.git
- version: ros2-dev
+ version: 3.0.9
angles:
type: git
url: https://github.com/ros/angles.git
diff --git a/tools/run_test_suite.bash b/tools/run_test_suite.bash
index df3480e5..b4e2c45a 100755
--- a/tools/run_test_suite.bash
+++ b/tools/run_test_suite.bash
@@ -25,3 +25,4 @@ $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_planner_ran
$SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_bt_navigator$
$SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_bt_navigator_with_dijkstra$
$SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_dynamic_obstacle$
+$SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_multi_robot$
--
GitLab