From c8e8b235181829105ec65ae44ea7bc00540a4ec9 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 13 Dec 2024 14:10:02 -0800 Subject: [PATCH] Jazzy Sync 4: Dec 13, 2024 (#4797) * Pass IDLE to on_tick, use that for initialize condition (#4744) * Pass IDLE to on_tick, use that for initialize condition Signed-off-by: redvinaa * Fix battery sub creation bug Signed-off-by: redvinaa --------- Signed-off-by: redvinaa * nav2_costmap_2d: add missing default_value_ copy in Costmap2D operator= (#4753) default_value_ is an attribute of the Costmap2D class and should be copied along with the other attributes. Signed-off-by: Dylan De Coeyer * nav2_costmap_2d plugin container layer (#4781) * updated CMakeLists.txt to include plugin_container_layer Signed-off-by: alexander * added plugin container layer to costmap_plugins.xml Signed-off-by: alexander * initial commit of plugin container layer Signed-off-by: alexander * fixed plugin namespace Signed-off-by: alexander * fixed message_filter Signed-off-by: alexander * linting Signed-off-by: alexander * modified addPlugin method to also take layer name Signed-off-by: alexander * reverted default plugins to be empty, free costmap_ during layer destruction, changed addPlugin to take layer name as an argument Signed-off-by: alexander * CMake changes to test plugin container layer Signed-off-by: alexander * added helper method to add plugin container layer Signed-off-by: alexander * added initial implementation of plugin container tests Signed-off-by: alexander * added enable to dynamic params, removed unecessary comments, removed unecessary members Signed-off-by: alexander * cleaned up and added additional tests Signed-off-by: alexander * added Apache copyrights Signed-off-by: alexander * linting for ament_cpplint Signed-off-by: alexander * added example file for plugin_container_layer to nav2_bringup Signed-off-by: alexander * removed unused rolling_window_ member variable Signed-off-by: alexander * removed default plugins and plugin types Signed-off-by: alexander * switched to using CombinationMethod enum, added updateWithMaxWithoutUnknownOverwrite case Signed-off-by: alexander * removed combined_costmap_ Signed-off-by: alexander * fixed layer naming and accomodating tests Signed-off-by: alexander * removed nav2_params_plugin_container_layer.yaml Signed-off-by: alexander * added more comprehensive checks for checking if layers are clearable Signed-off-by: alexander * added dynamics parameter handling, fixed current_ setting, increased test coverage Signed-off-by: alexander * removed unnecessary locks, added default value Signed-off-by: alexander * removed unecessary resetMap Signed-off-by: alexander * added layer resetting when reset method is called Signed-off-by: alexander * swapped logic for isClearable Signed-off-by: alexander * fixed breaking tests, removed unnecessary combined_costmap_ Signed-off-by: alexander * consolidated initialization for loops Signed-off-by: alexander * switched default_value_ to NO_INFORMATION Signed-off-by: alexander * added clearArea function Signed-off-by: alexander * added clearArea test Signed-off-by: alexander * removed TODO message Signed-off-by: alexander * removed constructor and destructors since they do nothing Signed-off-by: alexander * added check on costmap layer to see if it is clearable first Signed-off-by: alexander * fixed tests for clearing functionality Signed-off-by: alexander * added try catch around initialization of plugins Signed-off-by: alexander * fixed indents Signed-off-by: alexander --------- Signed-off-by: alexander * bumping to 1.3.3 for jazzy sync Signed-off-by: Steve Macenski * Fix backporting error: renamed header files Signed-off-by: Steve Macenski --------- Signed-off-by: redvinaa Signed-off-by: Dylan De Coeyer Signed-off-by: alexander Signed-off-by: Steve Macenski Co-authored-by: Vince Reda <60265874+redvinaa@users.noreply.github.com> Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com> Co-authored-by: alexanderjyuen <103065090+alexanderjyuen@users.noreply.github.com> --- nav2_amcl/package.xml | 2 +- .../nav2_behavior_tree/bt_action_node.hpp | 6 +- .../plugins/action/assisted_teleop_action.hpp | 1 - .../plugins/action/back_up_action.hpp | 3 - .../action/remove_passed_goals_action.hpp | 1 - .../plugins/action/spin_action.hpp | 1 - .../plugins/action/wait_action.hpp | 3 - .../condition/distance_traveled_condition.hpp | 1 - .../condition/goal_reached_condition.hpp | 1 - .../condition/is_battery_low_condition.hpp | 1 - .../condition/is_path_valid_condition.hpp | 1 - .../condition/time_expired_condition.hpp | 1 - .../transform_available_condition.hpp | 1 - .../plugins/decorator/rate_controller.hpp | 1 - nav2_behavior_tree/package.xml | 2 +- .../plugins/action/assisted_teleop_action.cpp | 6 +- .../plugins/action/back_up_action.cpp | 6 +- .../action/remove_passed_goals_action.cpp | 7 +- .../plugins/action/spin_action.cpp | 9 +- .../plugins/action/wait_action.cpp | 6 +- .../condition/distance_traveled_condition.cpp | 6 +- .../condition/goal_reached_condition.cpp | 7 +- .../condition/is_battery_low_condition.cpp | 39 +- .../condition/is_path_valid_condition.cpp | 6 +- .../condition/time_expired_condition.cpp | 6 +- .../transform_available_condition.cpp | 6 +- .../plugins/decorator/rate_controller.cpp | 6 +- nav2_behaviors/package.xml | 2 +- nav2_bringup/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_collision_monitor/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- nav2_costmap_2d/CMakeLists.txt | 1 + nav2_costmap_2d/costmap_plugins.xml | 3 + .../plugin_container_layer.hpp | 128 ++++ nav2_costmap_2d/package.xml | 2 +- .../plugins/plugin_container_layer.cpp | 234 ++++++ nav2_costmap_2d/src/costmap_2d.cpp | 1 + nav2_costmap_2d/src/costmap_2d_ros.cpp | 11 +- .../test/integration/CMakeLists.txt | 19 + .../integration/plugin_container_tests.cpp | 688 ++++++++++++++++++ nav2_costmap_2d/test/testing_helper.hpp | 13 + nav2_docking/opennav_docking/package.xml | 2 +- nav2_docking/opennav_docking_bt/package.xml | 2 +- nav2_docking/opennav_docking_core/package.xml | 2 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- .../nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_graceful_controller/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_loopback_sim/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_mppi_controller/package.xml | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- .../package.xml | 2 +- nav2_rotation_shim_controller/package.xml | 2 +- nav2_rviz_plugins/package.xml | 2 +- nav2_simple_commander/package.xml | 2 +- nav2_smac_planner/package.xml | 2 +- nav2_smoother/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_theta_star_planner/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_velocity_smoother/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- 77 files changed, 1185 insertions(+), 131 deletions(-) create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp create mode 100644 nav2_costmap_2d/plugins/plugin_container_layer.cpp create mode 100644 nav2_costmap_2d/test/integration/plugin_container_tests.cpp diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index bf1d10858c4..2ded6e5136e 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.3.3 + 1.3.4

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 4e723f1ac5f..505a7895c51 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -191,15 +191,15 @@ class BtActionNode : public BT::ActionNodeBase { // first step to be done only at the beginning of the Action if (!BT::isStatusActive(status())) { - // setting the status to RUNNING to notify the BT Loggers (if any) - setStatus(BT::NodeStatus::RUNNING); - // reset the flag to send the goal or not, allowing the user the option to set it in on_tick should_send_goal_ = true; // user defined callback, may modify "should_send_goal_". on_tick(); + // setting the status to RUNNING to notify the BT Loggers (if any) + setStatus(BT::NodeStatus::RUNNING); + if (!should_send_goal_) { return BT::NodeStatus::FAILURE; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp index 97511608d50..6466948b313 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp @@ -86,7 +86,6 @@ class AssistedTeleopAction : public BtActionNode "error_code_id", "The back up behavior server error code") }); } - -private: - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index 33552a24f06..fcfade6d17b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -61,7 +61,6 @@ class RemovePassedGoals : public BT::ActionNodeBase double transform_tolerance_; std::shared_ptr tf_; std::string robot_base_frame_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp index 4f9b300e445..cad5e8ce4a8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp @@ -86,7 +86,6 @@ class SpinAction : public BtActionNode private: bool is_recovery_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp index fdc320c16b0..1749a032cb1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp @@ -61,9 +61,6 @@ class WaitAction : public BtActionNode BT::InputPort("wait_duration", 1.0, "Wait time") }); } - -private: - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp index 77a80728ddf..dd200f318b3 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp @@ -80,7 +80,6 @@ class DistanceTraveledCondition : public BT::ConditionNode double distance_; double transform_tolerance_; std::string global_frame_, robot_base_frame_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp index 44e582c5f53..ff65da09a18 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp @@ -90,7 +90,6 @@ class GoalReachedCondition : public BT::ConditionNode rclcpp::Node::SharedPtr node_; std::shared_ptr tf_; - bool initialized_; double goal_reached_tol_; double transform_tolerance_; std::string robot_base_frame_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp index 59a023ff3c2..40f0ac5804b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -86,7 +86,6 @@ class IsBatteryLowCondition : public BT::ConditionNode double min_battery_; bool is_voltage_; bool is_battery_low_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index 5a9f255a9b7..a22942f9a56 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -73,7 +73,6 @@ class IsPathValidCondition : public BT::ConditionNode // The timeout value while waiting for a responce from the // is path valid service std::chrono::milliseconds server_timeout_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp index 26a3431b5db..2e34689c56a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp @@ -68,7 +68,6 @@ class TimeExpiredCondition : public BT::ConditionNode rclcpp::Node::SharedPtr node_; rclcpp::Time start_; double period_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp index 511c381321b..732e862ff4c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp @@ -80,7 +80,6 @@ class TransformAvailableCondition : public BT::ConditionNode std::string child_frame_; std::string parent_frame_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp index c390357b342..ccc346eb16d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp @@ -64,7 +64,6 @@ class RateController : public BT::DecoratorNode std::chrono::time_point start_; double period_; bool first_time_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 04397ef742b..6c152f16c0a 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.3.3 + 1.3.4 Nav2 behavior tree wrappers, nodes, and utilities Michael Jeronimo Carlos Orduno diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp index e876d2ce40b..7354b05f370 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -24,8 +24,7 @@ AssistedTeleopAction::AssistedTeleopAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf), - initialized_(false) +: BtActionNode(xml_tag_name, action_name, conf) {} void AssistedTeleopAction::initialize() @@ -36,12 +35,11 @@ void AssistedTeleopAction::initialize() // Populate the input message goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); - initialized_ = true; } void AssistedTeleopAction::on_tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index e17580fe71c..3df77f98d7c 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -24,8 +24,7 @@ BackUpAction::BackUpAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf), - initialized_(false) +: BtActionNode(xml_tag_name, action_name, conf) { } @@ -44,12 +43,11 @@ void nav2_behavior_tree::BackUpAction::initialize() goal_.target.z = 0.0; goal_.speed = speed; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); - initialized_ = true; } void BackUpAction::on_tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index 7e771523002..723e704e437 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -28,8 +28,7 @@ RemovePassedGoals::RemovePassedGoals( const std::string & name, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(name, conf), - viapoint_achieved_radius_(0.5), - initialized_(false) + viapoint_achieved_radius_(0.5) {} void RemovePassedGoals::initialize() @@ -46,9 +45,7 @@ void RemovePassedGoals::initialize() inline BT::NodeStatus RemovePassedGoals::tick() { - setStatus(BT::NodeStatus::RUNNING); - - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index d10bb83f32b..cb3459006fe 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -23,10 +23,7 @@ SpinAction::SpinAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf), - initialized_(false) -{ -} +: BtActionNode(xml_tag_name, action_name, conf) {} void SpinAction::initialize() { @@ -37,13 +34,11 @@ void SpinAction::initialize() goal_.target_yaw = dist; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); getInput("is_recovery", is_recovery_); - - initialized_ = true; } void SpinAction::on_tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index b607d026e59..03a18c9f947 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -25,8 +25,7 @@ WaitAction::WaitAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf), - initialized_(false) +: BtActionNode(xml_tag_name, action_name, conf) { } @@ -42,12 +41,11 @@ void WaitAction::initialize() } goal_.time = rclcpp::Duration::from_seconds(duration); - initialized_ = true; } void WaitAction::on_tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp index 991e0ab7cf2..2ebf9c486ea 100644 --- a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp @@ -29,8 +29,7 @@ DistanceTraveledCondition::DistanceTraveledCondition( const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), distance_(1.0), - transform_tolerance_(0.1), - initialized_(false) + transform_tolerance_(0.1) { } @@ -46,12 +45,11 @@ void DistanceTraveledCondition::initialize() node_, "global_frame", this); robot_base_frame_ = BT::deconflictPortAndParamFrame( node_, "robot_base_frame", this); - initialized_ = true; } BT::NodeStatus DistanceTraveledCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index e93ba5cc360..4b5e20a733e 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -27,8 +27,7 @@ namespace nav2_behavior_tree GoalReachedCondition::GoalReachedCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) -: BT::ConditionNode(condition_name, conf), - initialized_(false) +: BT::ConditionNode(condition_name, conf) { auto node = config().blackboard->get("node"); @@ -52,13 +51,11 @@ void GoalReachedCondition::initialize() tf_ = config().blackboard->get>("tf_buffer"); node_->get_parameter("transform_tolerance", transform_tolerance_); - - initialized_ = true; } BT::NodeStatus GoalReachedCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp index a0e3761f28c..eb0a8be4301 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -27,35 +27,40 @@ IsBatteryLowCondition::IsBatteryLowCondition( battery_topic_("/battery_status"), min_battery_(0.0), is_voltage_(false), - is_battery_low_(false), - initialized_(false) + is_battery_low_(false) { } void IsBatteryLowCondition::initialize() { getInput("min_battery", min_battery_); - getInput("battery_topic", battery_topic_); + std::string battery_topic_new; + getInput("battery_topic", battery_topic_new); getInput("is_voltage", is_voltage_); - node_ = config().blackboard->get("node"); - callback_group_ = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, - false); - callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - rclcpp::SubscriptionOptions sub_option; - sub_option.callback_group = callback_group_; - battery_sub_ = node_->create_subscription( - battery_topic_, - rclcpp::SystemDefaultsQoS(), - std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1), - sub_option); - initialized_ = true; + // Only create a new subscriber if the topic has changed or subscriber is empty + if (battery_topic_new != battery_topic_ || !battery_sub_) { + battery_topic_ = battery_topic_new; + + node_ = config().blackboard->get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + battery_sub_ = node_->create_subscription( + battery_topic_, + rclcpp::SystemDefaultsQoS(), + std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1), + sub_option); + } } BT::NodeStatus IsBatteryLowCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 7274743e1e9..cd76df97495 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -23,8 +23,7 @@ namespace nav2_behavior_tree IsPathValidCondition::IsPathValidCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) -: BT::ConditionNode(condition_name, conf), - initialized_(false) +: BT::ConditionNode(condition_name, conf) { node_ = config().blackboard->get("node"); client_ = node_->create_client("is_path_valid"); @@ -35,12 +34,11 @@ IsPathValidCondition::IsPathValidCondition( void IsPathValidCondition::initialize() { getInput("server_timeout", server_timeout_); - initialized_ = true; } BT::NodeStatus IsPathValidCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp index f03c9711aa8..23fd614cb7c 100644 --- a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp @@ -27,8 +27,7 @@ TimeExpiredCondition::TimeExpiredCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - period_(1.0), - initialized_(false) + period_(1.0) { } @@ -37,12 +36,11 @@ void TimeExpiredCondition::initialize() getInput("seconds", period_); node_ = config().blackboard->get("node"); start_ = node_->now(); - initialized_ = true; } BT::NodeStatus TimeExpiredCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp index 0ee491fbfd2..7e5b7b345e9 100644 --- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp @@ -27,8 +27,7 @@ TransformAvailableCondition::TransformAvailableCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - was_found_(false), - initialized_(false) + was_found_(false) { node_ = config().blackboard->get("node"); tf_ = config().blackboard->get>("tf_buffer"); @@ -52,12 +51,11 @@ void TransformAvailableCondition::initialize() } RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node"); - initialized_ = true; } BT::NodeStatus TransformAvailableCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp index 9592d119c96..5263da09eb2 100644 --- a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp @@ -24,8 +24,7 @@ RateController::RateController( const std::string & name, const BT::NodeConfiguration & conf) : BT::DecoratorNode(name, conf), - first_time_(false), - initialized_(false) + first_time_(false) { } @@ -34,12 +33,11 @@ void RateController::initialize() double hz = 1.0; getInput("hz", hz); period_ = 1.0 / hz; - initialized_ = true; } BT::NodeStatus RateController::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 09ee0b13060..af5c5abe15a 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.3.3 + 1.3.4 Nav2 behavior server Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 2ac03353696..ae826003a2e 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.3.3 + 1.3.4 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 503e27d42e5..54ffcb6e6ee 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.3.3 + 1.3.4 Nav2 BT Navigator Server Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 1b5e8a34501..29220e934bf 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.3.3 + 1.3.4 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 68f0c8e6f3f..1d92b77acc7 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.3.3 + 1.3.4 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index b781f16b575..8d9e3e5113a 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.3.3 + 1.3.4 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 4bab7d28ef6..e0595725030 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.3.3 + 1.3.4 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index de852df2d62..3ee8d8a5595 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.3.3 + 1.3.4 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 723f81f0f9b..46d795b2408 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -88,6 +88,7 @@ add_library(layers SHARED plugins/voxel_layer.cpp plugins/range_sensor_layer.cpp plugins/denoise_layer.cpp + plugins/plugin_container_layer.cpp ) add_library(${PROJECT_NAME}::layers ALIAS layers) ament_target_dependencies(layers diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index 6748cd5fcae..16e2f4ba61a 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -18,6 +18,9 @@ Filters noise-induced freestanding obstacles or small obstacles groups + + Plugin to group different layers within the same costmap + diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp new file mode 100644 index 00000000000..e98c7b8136f --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp @@ -0,0 +1,128 @@ +// Copyright (c) 2024 Polymath Robotics, Inc. +// +// 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. + +#ifndef NAV2_COSTMAP_2D__PLUGIN_CONTAINER_LAYER_HPP_ +#define NAV2_COSTMAP_2D__PLUGIN_CONTAINER_LAYER_HPP_ + +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/layer.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "nav2_costmap_2d/costmap_layer.hpp" +#include "nav2_costmap_2d/observation_buffer.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" +#include "tf2_ros/message_filter.h" +#include "message_filters/subscriber.h" +#include "pluginlib/class_loader.hpp" + +using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; +using nav2_costmap_2d::NO_INFORMATION; +using rcl_interfaces::msg::ParameterType; +namespace nav2_costmap_2d +{ +/** + * @class PluginContainerLayer + * @brief Holds a list of plugins and applies them only to the specific layer + */ +class PluginContainerLayer : public CostmapLayer +{ +public: + /** + * @brief Initialization process of layer on startup + */ + virtual void onInitialize(); + /** + * @brief Update the bounds of the master costmap by this layer's update + *dimensions + * @param robot_x X pose of robot + * @param robot_y Y pose of robot + * @param robot_yaw Robot orientation + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ + virtual void updateBounds( + double robot_x, + double robot_y, + double robot_yaw, + double * min_x, + double * min_y, + double * max_x, + double * max_y); + /** + * @brief Update the costs in the master costmap in the window + * @param master_grid The master costmap grid to update + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ + virtual void updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, + int min_j, + int max_i, + int max_j); + virtual void onFootprintChanged(); + /** @brief Update the footprint to match size of the parent costmap. */ + virtual void matchSize(); + /** + * @brief Deactivate the layer + */ + virtual void deactivate(); + /** + * @brief Activate the layer + */ + virtual void activate(); + /** + * @brief Reset this costmap + */ + virtual void reset(); + /** + * @brief If clearing operations should be processed on this layer or not + */ + virtual bool isClearable(); + /** + * @brief Clear an area in the constituent costmaps with the given dimension + * if invert, then clear everything except these dimensions + */ + void clearArea(int start_x, int start_y, int end_x, int end_y, bool invert) override; + + void addPlugin(std::shared_ptr plugin, std::string layer_name); + pluginlib::ClassLoader plugin_loader_{"nav2_costmap_2d", "nav2_costmap_2d::Layer"}; + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); + +private: + /// @brief Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr + dyn_params_handler_; + + nav2_costmap_2d::CombinationMethod combination_method_; + std::vector> plugins_; + std::vector plugin_names_; + std::vector plugin_types_; +}; +} // namespace nav2_costmap_2d +#endif // NAV2_COSTMAP_2D__PLUGIN_CONTAINER_LAYER_HPP_ diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 61354722020..1ff54d88299 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.3.3 + 1.3.4 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_costmap_2d/plugins/plugin_container_layer.cpp b/nav2_costmap_2d/plugins/plugin_container_layer.cpp new file mode 100644 index 00000000000..7d171f6b6a1 --- /dev/null +++ b/nav2_costmap_2d/plugins/plugin_container_layer.cpp @@ -0,0 +1,234 @@ +// Copyright (c) 2024 Polymath Robotics, Inc. +// +// 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. + +#include "nav2_costmap_2d/plugin_container_layer.hpp" + +#include "nav2_costmap_2d/costmap_math.hpp" +#include "nav2_costmap_2d/footprint.hpp" +#include "nav2_util/node_utils.hpp" +#include "rclcpp/parameter_events_filter.hpp" +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::PluginContainerLayer, nav2_costmap_2d::Layer) + +using std::vector; + +namespace nav2_costmap_2d +{ + +void PluginContainerLayer::onInitialize() +{ + auto node = node_.lock(); + + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + nav2_util::declare_parameter_if_not_declared(node, name_ + "." + "enabled", + rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared(node, name_ + "." + "plugins", + rclcpp::ParameterValue(std::vector{})); + nav2_util::declare_parameter_if_not_declared(node, name_ + "." + "combination_method", + rclcpp::ParameterValue(1)); + + node->get_parameter(name_ + "." + "enabled", enabled_); + node->get_parameter(name_ + "." + "plugins", plugin_names_); + + int combination_method_param{}; + node->get_parameter(name_ + "." + "combination_method", combination_method_param); + combination_method_ = combination_method_from_int(combination_method_param); + + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &PluginContainerLayer::dynamicParametersCallback, + this, + std::placeholders::_1)); + + plugin_types_.resize(plugin_names_.size()); + + for (unsigned int i = 0; i < plugin_names_.size(); ++i) { + plugin_types_[i] = nav2_util::get_plugin_type_param(node, name_ + "." + plugin_names_[i]); + std::shared_ptr plugin = plugin_loader_.createSharedInstance(plugin_types_[i]); + addPlugin(plugin, plugin_names_[i]); + } + + default_value_ = nav2_costmap_2d::NO_INFORMATION; + + PluginContainerLayer::matchSize(); + current_ = true; +} + +void PluginContainerLayer::addPlugin(std::shared_ptr plugin, std::string layer_name) +{ + plugins_.push_back(plugin); + auto node = node_.lock(); + plugin->initialize(layered_costmap_, name_ + "." + layer_name, tf_, node, callback_group_); +} + +void PluginContainerLayer::updateBounds( + double robot_x, + double robot_y, + double robot_yaw, + double * min_x, + double * min_y, + double * max_x, + double * max_y) +{ + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); + } +} + +void PluginContainerLayer::updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, + int min_j, + int max_i, + int max_j) +{ + std::lock_guard guard(*getMutex()); + if (!enabled_) { + return; + } + + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->updateCosts(*this, min_i, min_j, max_i, max_j); + } + + switch (combination_method_) { + case CombinationMethod::Overwrite: + updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); + break; + case CombinationMethod::Max: + updateWithMax(master_grid, min_i, min_j, max_i, max_j); + break; + case CombinationMethod::MaxWithoutUnknownOverwrite: + updateWithMaxWithoutUnknownOverwrite(master_grid, min_i, min_j, max_i, max_j); + break; + default: // Nothing + break; + } + + current_ = true; +} + +void PluginContainerLayer::activate() +{ + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->activate(); + } +} + +void PluginContainerLayer::deactivate() +{ + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->deactivate(); + } +} + +void PluginContainerLayer::reset() +{ + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->reset(); + } + resetMaps(); + current_ = false; +} + +void PluginContainerLayer::onFootprintChanged() +{ + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->onFootprintChanged(); + } +} + +void PluginContainerLayer::matchSize() +{ + std::lock_guard guard(*getMutex()); + Costmap2D * master = layered_costmap_->getCostmap(); + resizeMap( + master->getSizeInCellsX(), master->getSizeInCellsY(), + master->getResolution(), master->getOriginX(), master->getOriginY()); + + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->matchSize(); + } +} + +bool PluginContainerLayer::isClearable() +{ + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + if((*plugin)->isClearable()) { + return true; + } + } + return false; +} + +void PluginContainerLayer::clearArea(int start_x, int start_y, int end_x, int end_y, bool invert) +{ + CostmapLayer::clearArea(start_x, start_y, end_x, end_y, invert); + for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + auto costmap_layer = std::dynamic_pointer_cast(*plugin); + if ((*plugin)->isClearable() && costmap_layer != nullptr) { + costmap_layer->clearArea(start_x, start_y, end_x, end_y, invert); + } + } +} + +rcl_interfaces::msg::SetParametersResult PluginContainerLayer::dynamicParametersCallback( + std::vector parameters) +{ + std::lock_guard guard(*getMutex()); + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == name_ + "." + "combination_method") { + combination_method_ = combination_method_from_int(parameter.as_int()); + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) { + enabled_ = parameter.as_bool(); + current_ = false; + } + } + } + + result.successful = true; + return result; +} + +} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index 9cb7ce19816..9ddbcf5e9d6 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -219,6 +219,7 @@ Costmap2D & Costmap2D::operator=(const Costmap2D & map) resolution_ = map.resolution_; origin_x_ = map.origin_x_; origin_y_ = map.origin_y_; + default_value_ = map.default_value_; // initialize our various maps initMaps(size_x_, size_y_); diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 0fdfb5d2b8c..cb18c33dbf2 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -186,9 +186,14 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) layered_costmap_->addPlugin(plugin); // TODO(mjeronimo): instead of get(), use a shared ptr - plugin->initialize( - layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(), - shared_from_this(), callback_group_); + try { + plugin->initialize(layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(), + shared_from_this(), callback_group_); + } catch (const std::exception & e) { + RCLCPP_ERROR(get_logger(), "Failed to initialize costmap plugin %s! %s.", + plugin_names_[i].c_str(), e.what()); + return nav2_util::CallbackReturn::FAILURE; + } lock.unlock(); diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 7fbef3d41db..59d6ea65ffb 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -39,6 +39,15 @@ target_link_libraries(range_tests_exec ${PROJECT_NAME}::layers ) +ament_add_gtest_executable(plugin_container_tests_exec + plugin_container_tests.cpp +) +target_link_libraries(plugin_container_tests_exec + nav2_costmap_2d_core + layers +) + + ament_add_gtest(dyn_params_tests dyn_params_tests.cpp ) @@ -113,6 +122,16 @@ ament_add_test(range_tests TEST_EXECUTABLE=$ ) +ament_add_test(plugin_container_tests + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) + ament_add_test(test_costmap_publisher_exec GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" diff --git a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp new file mode 100644 index 00000000000..9a7bf160388 --- /dev/null +++ b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp @@ -0,0 +1,688 @@ +// Copyright (c) 2024 Polymath Robotics, Inc. +// +// 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. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "nav2_costmap_2d/observation_buffer.hpp" +#include "../testing_helper.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/plugin_container_layer.hpp" + +using std::begin; +using std::end; +using std::for_each; +using std::all_of; +using std::none_of; +using std::pair; +using std::string; + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class TestLifecycleNode : public nav2_util::LifecycleNode +{ +public: + explicit TestLifecycleNode(const string & name) + : nav2_util::LifecycleNode(name) + { + } + + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } +}; + +class TestNode : public ::testing::Test +{ +public: + TestNode() + { + node_ = std::make_shared("plugin_container_test_node"); + 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)); + node_->declare_parameter("unknown_cost_value", + rclcpp::ParameterValue(static_cast(0xff))); + node_->declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); + node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3)); + node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string(""))); + } + + ~TestNode() {} + + void waitForMap(std::shared_ptr & slayer) + { + while (!slayer->isCurrent()) { + rclcpp::spin_some(node_->get_node_base_interface()); + } + } + + std::vector setRadii( + nav2_costmap_2d::LayeredCostmap & layers, + double length, double width) + { + std::vector polygon; + geometry_msgs::msg::Point p; + p.x = width; + p.y = length; + polygon.push_back(p); + p.x = width; + p.y = -length; + polygon.push_back(p); + p.x = -width; + p.y = -length; + polygon.push_back(p); + p.x = -width; + p.y = length; + polygon.push_back(p); + layers.setFootprint(polygon); + + return polygon; + } + +protected: + std::shared_ptr node_; +}; + +/* + * For reference, the static map looks like this: + * + * 0 0 0 0 0 0 0 254 254 254 + * + * 0 0 0 0 0 0 0 254 254 254 + * + * 0 0 0 254 254 254 0 0 0 0 + * + * 0 0 0 0 0 0 0 0 0 0 + * + * 0 0 0 0 0 0 0 0 0 0 + * + * 0 0 0 0 254 0 0 254 254 254 + * + * 0 0 0 0 254 0 0 254 254 254 + * + * 0 0 0 0 0 0 0 254 254 254 + * + * 0 0 0 0 0 0 0 0 0 0 + * + * 0 0 0 0 0 0 0 0 0 0 + * + * upper left is 0,0, lower right is 9,9 + */ + +/** + * Test if combining layers different plugin container layers works + */ + +TEST_F(TestNode, testObstacleLayers) { + tf2_ros::Buffer tf(node_->get_clock()); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + std::shared_ptr olayer_a = + std::make_shared(); + pclayer_a->addPlugin(olayer_a, "pclayer_a.obstacles"); + + std::shared_ptr olayer_b = + std::make_shared(); + pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles"); + + addObservation(olayer_a, 5.0, 5.0); + addObservation(olayer_b, 3.0, 8.0); + + layers.updateMap(0, 0, 0); + + nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); + + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 2); +} + +TEST_F(TestNode, testObstacleAndStaticLayers) { + tf2_ros::Buffer tf(node_->get_clock()); + + node_->declare_parameter("pclayer_a.static.map_topic", + rclcpp::ParameterValue(std::string("map"))); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + std::shared_ptr slayer = + std::make_shared(); + pclayer_a->addPlugin(slayer, "pclayer_a.static"); + + std::shared_ptr olayer_b = + std::make_shared(); + pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles"); + + addObservation(olayer_b, 3.0, 8.0); + + waitForMap(slayer); + + layers.updateMap(0, 0, 0); + + nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); + + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21); +} + +TEST_F(TestNode, testDifferentInflationLayers) { + tf2_ros::Buffer tf(node_->get_clock()); + + node_->declare_parameter("pclayer_a.static.map_topic", + rclcpp::ParameterValue(std::string("map"))); + + node_->declare_parameter("pclayer_b.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + std::shared_ptr slayer = + std::make_shared(); + pclayer_a->addPlugin(slayer, "pclayer_a.static"); + + std::shared_ptr olayer_b = + std::make_shared(); + pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles"); + + std::shared_ptr ilayer_b = + std::make_shared(); + pclayer_b->addPlugin(ilayer_b, "pclayer_b.inflation"); + + std::vector polygon = setRadii(layers, 1, 1); + layers.setFootprint(polygon); + + addObservation(olayer_b, 5.0, 4.0); + + waitForMap(slayer); + + layers.updateMap(0, 0, 0); + + nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); + + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 4); +} + +TEST_F(TestNode, testDifferentInflationLayers2) { + tf2_ros::Buffer tf(node_->get_clock()); + + node_->declare_parameter("pclayer_a.static.map_topic", + rclcpp::ParameterValue(std::string("map"))); + + node_->declare_parameter("pclayer_a.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_a.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + std::shared_ptr slayer = + std::make_shared(); + pclayer_a->addPlugin(slayer, "pclayer_a.static"); + + std::shared_ptr ilayer_a = + std::make_shared(); + pclayer_a->addPlugin(ilayer_a, "pclayer_a.inflation"); + + std::shared_ptr olayer_b = + std::make_shared(); + pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles"); + + std::vector polygon = setRadii(layers, 1, 1); + layers.setFootprint(polygon); + + addObservation(olayer_b, 9.0, 9.0); + + waitForMap(slayer); + + layers.updateMap(0, 0, 0); + + nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); + + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 28); +} + +TEST_F(TestNode, testResetting) { + tf2_ros::Buffer tf(node_->get_clock()); + + node_->declare_parameter("pclayer_a.static.map_topic", + rclcpp::ParameterValue(std::string("map"))); + + node_->declare_parameter("pclayer_a.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_a.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + std::shared_ptr slayer = + std::make_shared(); + pclayer_a->addPlugin(slayer, "pclayer_a.static"); + + std::shared_ptr ilayer_a = + std::make_shared(); + pclayer_a->addPlugin(ilayer_a, "pclayer_a.inflation"); + + std::shared_ptr olayer_b = + std::make_shared(); + pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles"); + + std::shared_ptr ilayer_b = + std::make_shared(); + pclayer_b->addPlugin(ilayer_b, "pclayer_a.inflation"); + + std::vector polygon = setRadii(layers, 1, 1); + layers.setFootprint(polygon); + + addObservation(olayer_b, 9.0, 9.0); + + waitForMap(slayer); + + ASSERT_EQ(pclayer_a->isCurrent(), true); + ASSERT_EQ(pclayer_b->isCurrent(), true); + + ASSERT_EQ(slayer->isCurrent(), true); + ASSERT_EQ(ilayer_a->isCurrent(), true); + ASSERT_EQ(olayer_b->isCurrent(), true); + ASSERT_EQ(ilayer_b->isCurrent(), true); + + pclayer_a->reset(); + pclayer_b->reset(); + + ASSERT_EQ(pclayer_a->isCurrent(), false); + ASSERT_EQ(pclayer_b->isCurrent(), false); + + ASSERT_EQ(slayer->isCurrent(), false); + ASSERT_EQ(ilayer_a->isCurrent(), false); + ASSERT_EQ(olayer_b->isCurrent(), false); + ASSERT_EQ(ilayer_b->isCurrent(), false); +} + +TEST_F(TestNode, testClearing) { + tf2_ros::Buffer tf(node_->get_clock()); + + node_->declare_parameter("pclayer_a.static.map_topic", + rclcpp::ParameterValue(std::string("map"))); + + node_->declare_parameter("pclayer_a.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_a.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + std::shared_ptr slayer = + std::make_shared(); + pclayer_a->addPlugin(slayer, "pclayer_a.static"); + + std::shared_ptr ilayer_a = + std::make_shared(); + pclayer_a->addPlugin(ilayer_a, "pclayer_a.inflation"); + + std::shared_ptr olayer_b = + std::make_shared(); + pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles"); + + std::shared_ptr ilayer_b = + std::make_shared(); + pclayer_b->addPlugin(ilayer_b, "pclayer_a.inflation"); + + std::vector polygon = setRadii(layers, 1, 1); + layers.setFootprint(polygon); + + addObservation(olayer_b, 9.0, 9.0); + + waitForMap(slayer); + + layers.updateMap(0, 0, 0); + + nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); + + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 29); + ASSERT_EQ(olayer_b->getCost(9, 9), nav2_costmap_2d::LETHAL_OBSTACLE); + + pclayer_a->clearArea(-1, -1, 10, 10, false); + pclayer_b->clearArea(-1, -1, 10, 10, false); + + ASSERT_EQ(slayer->getCost(9, 0), nav2_costmap_2d::LETHAL_OBSTACLE); + ASSERT_EQ(olayer_b->getCost(9, 9), nav2_costmap_2d::NO_INFORMATION); +} + +TEST_F(TestNode, testOverwriteCombinationMethods) { + tf2_ros::Buffer tf(node_->get_clock()); + + node_->declare_parameter("pclayer_a.static.map_topic", + rclcpp::ParameterValue(std::string("map"))); + + node_->declare_parameter("pclayer_a.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_a.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + node_->set_parameter(rclcpp::Parameter("pclayer_b.combination_method", 0)); + + std::shared_ptr slayer = + std::make_shared(); + pclayer_a->addPlugin(slayer, "pclayer_a.static"); + + std::shared_ptr ilayer_a = + std::make_shared(); + pclayer_a->addPlugin(ilayer_a, "pclayer_a.inflation"); + + std::shared_ptr olayer_b = + std::make_shared(); + pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles"); + + std::shared_ptr ilayer_b = + std::make_shared(); + pclayer_b->addPlugin(ilayer_b, "pclayer_a.inflation"); + + std::vector polygon = setRadii(layers, 1, 1); + layers.setFootprint(polygon); + + addObservation(olayer_b, 9.0, 9.0); + + waitForMap(slayer); + + layers.updateMap(0, 0, 0); + + nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); + + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 1); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 2); +} + +TEST_F(TestNode, testWithoutUnknownOverwriteCombinationMethods) { + tf2_ros::Buffer tf(node_->get_clock()); + + node_->declare_parameter("pclayer_a.static.map_topic", + rclcpp::ParameterValue(std::string("map"))); + + node_->declare_parameter("pclayer_a.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_a.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.cost_scaling_factor", + rclcpp::ParameterValue(1.0)); + + node_->declare_parameter("pclayer_b.inflation.inflation_radius", + rclcpp::ParameterValue(1.0)); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, true); + + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + node_->set_parameter(rclcpp::Parameter("pclayer_a.combination_method", 2)); + node_->set_parameter(rclcpp::Parameter("pclayer_b.combination_method", 2)); + + std::shared_ptr slayer = + std::make_shared(); + pclayer_a->addPlugin(slayer, "pclayer_a.static"); + + std::shared_ptr ilayer_a = + std::make_shared(); + pclayer_a->addPlugin(ilayer_a, "pclayer_a.inflation"); + + std::shared_ptr olayer_b = + std::make_shared(); + pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles"); + + std::shared_ptr ilayer_b = + std::make_shared(); + pclayer_b->addPlugin(ilayer_b, "pclayer_a.inflation"); + + std::vector polygon = setRadii(layers, 1, 1); + layers.setFootprint(polygon); + + addObservation(olayer_b, 9.0, 9.0); + + waitForMap(slayer); + + layers.updateMap(0, 0, 0); + + nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); + + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::NO_INFORMATION), 100); +} + +TEST_F(TestNode, testClearable) { + tf2_ros::Buffer tf(node_->get_clock()); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + std::shared_ptr pclayer_a = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); + + std::shared_ptr pclayer_b = nullptr; + addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b"); + + std::shared_ptr slayer = + std::make_shared(); + pclayer_a->addPlugin(slayer, "pclayer_a.static"); + + std::shared_ptr olayer_a = + std::make_shared(); + pclayer_a->addPlugin(olayer_a, "pclayer_a.obstacles"); + + std::shared_ptr slayer_b = + std::make_shared(); + pclayer_b->addPlugin(slayer_b, "pclayer_b.obstacles"); + + waitForMap(slayer); + + ASSERT_EQ(pclayer_a->isClearable(), true); + ASSERT_EQ(pclayer_b->isClearable(), false); +} + +TEST_F(TestNode, testDynParamsSetPluginContainerLayer) +{ + auto costmap = std::make_shared("test_costmap"); + + // Add obstacle layer + std::vector plugins_str; + plugins_str.push_back("plugin_container_layer_a"); + plugins_str.push_back("plugin_container_layer_b"); + costmap->set_parameter(rclcpp::Parameter("plugins", plugins_str)); + + costmap->declare_parameter("plugin_container_layer_a.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::PluginContainerLayer"))); + costmap->declare_parameter("plugin_container_layer_b.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::PluginContainerLayer"))); + + std::vector plugins_str_a; + plugins_str_a.push_back("obstacle_layer"); + plugins_str_a.push_back("inflation_layer"); + costmap->declare_parameter("plugin_container_layer_a.plugins", + rclcpp::ParameterValue(plugins_str_a)); + + costmap->declare_parameter("plugin_container_layer_a.obstacle_layer.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::ObstacleLayer"))); + costmap->declare_parameter("plugin_container_layer_a.inflation_layer.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::InflationLayer"))); + + std::vector plugins_str_b; + plugins_str_b.push_back("static_layer"); + plugins_str_b.push_back("inflation_layer"); + costmap->declare_parameter("plugin_container_layer_b.plugins", + rclcpp::ParameterValue(plugins_str_b)); + + costmap->declare_parameter("plugin_container_layer_b.static_layer.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::StaticLayer"))); + costmap->declare_parameter("plugin_container_layer_b.inflation_layer.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::InflationLayer"))); + + + costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("map"))); + costmap->set_parameter(rclcpp::Parameter("robot_base_frame", std::string("base_link"))); + + costmap->on_configure(rclcpp_lifecycle::State()); + + costmap->on_activate(rclcpp_lifecycle::State()); + + auto parameter_client = std::make_shared( + costmap->get_node_base_interface(), costmap->get_node_topics_interface(), + costmap->get_node_graph_interface(), + costmap->get_node_services_interface()); + + auto results = parameter_client->set_parameters_atomically( + { + rclcpp::Parameter("plugin_container_layer_a.combination_method", 2), + rclcpp::Parameter("plugin_container_layer_b.combination_method", 1), + rclcpp::Parameter("plugin_container_layer_a.enabled", false), + rclcpp::Parameter("plugin_container_layer_b.enabled", true) + }); + + rclcpp::spin_until_future_complete( + costmap->get_node_base_interface(), + results); + + EXPECT_EQ(costmap->get_parameter("plugin_container_layer_a.combination_method").as_int(), 2); + EXPECT_EQ(costmap->get_parameter("plugin_container_layer_b.combination_method").as_int(), 1); + + EXPECT_EQ(costmap->get_parameter("plugin_container_layer_a.enabled").as_bool(), false); + EXPECT_EQ(costmap->get_parameter("plugin_container_layer_b.enabled").as_bool(), true); + + costmap->on_deactivate(rclcpp_lifecycle::State()); + costmap->on_cleanup(rclcpp_lifecycle::State()); + costmap->on_shutdown(rclcpp_lifecycle::State()); +} diff --git a/nav2_costmap_2d/test/testing_helper.hpp b/nav2_costmap_2d/test/testing_helper.hpp index 8ab95e6d444..03e1d6a0bd8 100644 --- a/nav2_costmap_2d/test/testing_helper.hpp +++ b/nav2_costmap_2d/test/testing_helper.hpp @@ -17,6 +17,7 @@ #define TESTING_HELPER_HPP_ #include +#include #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" @@ -26,6 +27,7 @@ #include "nav2_costmap_2d/range_sensor_layer.hpp" #include "nav2_costmap_2d/obstacle_layer.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" +#include "nav2_costmap_2d/plugin_container_layer.hpp" #include "nav2_util/lifecycle_node.hpp" const double MAX_Z(1.0); @@ -142,5 +144,16 @@ void addInflationLayer( layers.addPlugin(ipointer); } +void addPluginContainerLayer( + nav2_costmap_2d::LayeredCostmap & layers, + tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr & pclayer, + std::string name, + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr) +{ + pclayer = std::make_shared(); + pclayer->initialize(&layers, name, &tf, node, callback_group); + layers.addPlugin(std::shared_ptr(pclayer)); +} #endif // TESTING_HELPER_HPP_ diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml index 43d89f5c64a..eb3711eb476 100644 --- a/nav2_docking/opennav_docking/package.xml +++ b/nav2_docking/opennav_docking/package.xml @@ -2,7 +2,7 @@ opennav_docking - 1.3.3 + 1.3.4 A Task Server for robot charger docking Steve Macenski Apache-2.0 diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/package.xml index 77d1a394eb1..39a2a3cbf74 100644 --- a/nav2_docking/opennav_docking_bt/package.xml +++ b/nav2_docking/opennav_docking_bt/package.xml @@ -2,7 +2,7 @@ opennav_docking_bt - 1.3.3 + 1.3.4 A set of BT nodes and XMLs for docking Steve Macenski Apache-2.0 diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/package.xml index f0d782d8d3c..895d4f68cdb 100644 --- a/nav2_docking/opennav_docking_core/package.xml +++ b/nav2_docking/opennav_docking_core/package.xml @@ -2,7 +2,7 @@ opennav_docking_core - 1.3.3 + 1.3.4 A set of headers for plugins core to the opennav docking framework Steve Macenski Apache-2.0 diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 82173a877b1..cf0df0bda41 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.3.3 + 1.3.4 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 68d858ce0c9..3ac03b05692 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.3.3 + 1.3.4 DWB core interfaces package Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index d79f8097ca5..6e2976b0ecd 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.3.3 + 1.3.4 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 3fc7a1e2679..753fe239c07 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.3.3 + 1.3.4 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 50926c98ed0..d795771b147 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.3.3 + 1.3.4 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 9aae228a247..c57410066e3 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.3.3 + 1.3.4 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 6ebf841ab6e..bcbaee02e00 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.3.3 + 1.3.4 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index cb6994259d5..bf0f68c58a2 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.3.3 + 1.3.4 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_graceful_controller/package.xml b/nav2_graceful_controller/package.xml index 95ef103d776..d84e35ab80b 100644 --- a/nav2_graceful_controller/package.xml +++ b/nav2_graceful_controller/package.xml @@ -2,7 +2,7 @@ nav2_graceful_controller - 1.3.3 + 1.3.4 Graceful motion controller Alberto Tudela Apache-2.0 diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 057552ef2f5..6a2df2f39a0 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.3.3 + 1.3.4 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml index 0fffe1a4cb0..df450febd99 100644 --- a/nav2_loopback_sim/package.xml +++ b/nav2_loopback_sim/package.xml @@ -2,7 +2,7 @@ nav2_loopback_sim - 1.3.3 + 1.3.4 A loopback simulator to replace physics simulation steve macenski Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index f658ddbdf8b..2b27d23e5e1 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.3.3 + 1.3.4 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 13afd1e58c6..fa105867d4d 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.3.3 + 1.3.4 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index cf4cdbf43fa..c30ea21c1ff 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.3.3 + 1.3.4 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 3277aea7449..d8745e96dd7 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.3.3 + 1.3.4 Nav2 NavFn planner Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 9bba6f0b2d7..ba9a85b78e0 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.3.3 + 1.3.4 Nav2 planner server package Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index f45f4dd48f0..06ca9ccfba2 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.3.3 + 1.3.4 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index c8930eddd6e..87b6f4776de 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.3.3 + 1.3.4 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index b7a46b1ea94..5cc51009da1 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.3.3 + 1.3.4 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index d369aa0093a..e1dd961ac40 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.3.3 + 1.3.4 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 2077602b69f..9fc728ad34d 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.3.3 + 1.3.4 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 963f52e3778..cb7702903ce 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.3.3 + 1.3.4 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 36bdfd3f60a..91b1b846705 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.3.3 + 1.3.4 A sets of system-level tests for Nav2 usually involving full robot simulation Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 417a74eab9b..420c5322f4a 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.3.3 + 1.3.4 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 1e1441234d9..d43a35fdd0d 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.3.3 + 1.3.4 Nav2 utilities Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index d28af826b8e..2f925f1d414 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.3.3 + 1.3.4 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 4fa5eba26af..78cd7d1d4b0 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.3.3 + 1.3.4 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 10a416a5be9..38d2f9c72c1 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.3.3 + 1.3.4 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index eab0074b3c2..dcabfa10b18 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.3.3 + 1.3.4 ROS2 Navigation Stack