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 <redvinaa@gmail.com> * Fix battery sub creation bug Signed-off-by: redvinaa <redvinaa@gmail.com> --------- Signed-off-by: redvinaa <redvinaa@gmail.com> * 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 <dylan.decoeyer@quimesis.be> * nav2_costmap_2d plugin container layer (#4781) * updated CMakeLists.txt to include plugin_container_layer Signed-off-by: alexander <alex@polymathrobotics.com> * added plugin container layer to costmap_plugins.xml Signed-off-by: alexander <alex@polymathrobotics.com> * initial commit of plugin container layer Signed-off-by: alexander <alex@polymathrobotics.com> * fixed plugin namespace Signed-off-by: alexander <alex@polymathrobotics.com> * fixed message_filter Signed-off-by: alexander <alex@polymathrobotics.com> * linting Signed-off-by: alexander <alex@polymathrobotics.com> * modified addPlugin method to also take layer name Signed-off-by: alexander <alex@polymathrobotics.com> * reverted default plugins to be empty, free costmap_ during layer destruction, changed addPlugin to take layer name as an argument Signed-off-by: alexander <alex@polymathrobotics.com> * CMake changes to test plugin container layer Signed-off-by: alexander <alex@polymathrobotics.com> * added helper method to add plugin container layer Signed-off-by: alexander <alex@polymathrobotics.com> * added initial implementation of plugin container tests Signed-off-by: alexander <alex@polymathrobotics.com> * added enable to dynamic params, removed unecessary comments, removed unecessary members Signed-off-by: alexander <alex@polymathrobotics.com> * cleaned up and added additional tests Signed-off-by: alexander <alex@polymathrobotics.com> * added Apache copyrights Signed-off-by: alexander <alex@polymathrobotics.com> * linting for ament_cpplint Signed-off-by: alexander <alex@polymathrobotics.com> * added example file for plugin_container_layer to nav2_bringup Signed-off-by: alexander <alex@polymathrobotics.com> * removed unused rolling_window_ member variable Signed-off-by: alexander <alex@polymathrobotics.com> * removed default plugins and plugin types Signed-off-by: alexander <alex@polymathrobotics.com> * switched to using CombinationMethod enum, added updateWithMaxWithoutUnknownOverwrite case Signed-off-by: alexander <alex@polymathrobotics.com> * removed combined_costmap_ Signed-off-by: alexander <alex@polymathrobotics.com> * fixed layer naming and accomodating tests Signed-off-by: alexander <alex@polymathrobotics.com> * removed nav2_params_plugin_container_layer.yaml Signed-off-by: alexander <alex@polymathrobotics.com> * added more comprehensive checks for checking if layers are clearable Signed-off-by: alexander <alex@polymathrobotics.com> * added dynamics parameter handling, fixed current_ setting, increased test coverage Signed-off-by: alexander <alex@polymathrobotics.com> * removed unnecessary locks, added default value Signed-off-by: alexander <alex@polymathrobotics.com> * removed unecessary resetMap Signed-off-by: alexander <alex@polymathrobotics.com> * added layer resetting when reset method is called Signed-off-by: alexander <alex@polymathrobotics.com> * swapped logic for isClearable Signed-off-by: alexander <alex@polymathrobotics.com> * fixed breaking tests, removed unnecessary combined_costmap_ Signed-off-by: alexander <alex@polymathrobotics.com> * consolidated initialization for loops Signed-off-by: alexander <alex@polymathrobotics.com> * switched default_value_ to NO_INFORMATION Signed-off-by: alexander <alex@polymathrobotics.com> * added clearArea function Signed-off-by: alexander <alex@polymathrobotics.com> * added clearArea test Signed-off-by: alexander <alex@polymathrobotics.com> * removed TODO message Signed-off-by: alexander <alex@polymathrobotics.com> * removed constructor and destructors since they do nothing Signed-off-by: alexander <alex@polymathrobotics.com> * added check on costmap layer to see if it is clearable first Signed-off-by: alexander <alex@polymathrobotics.com> * fixed tests for clearing functionality Signed-off-by: alexander <alex@polymathrobotics.com> * added try catch around initialization of plugins Signed-off-by: alexander <alex@polymathrobotics.com> * fixed indents Signed-off-by: alexander <alex@polymathrobotics.com> --------- Signed-off-by: alexander <alex@polymathrobotics.com> * bumping to 1.3.3 for jazzy sync Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix backporting error: renamed header files Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: redvinaa <redvinaa@gmail.com> Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> Signed-off-by: alexander <alex@polymathrobotics.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> 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>
ros-navigation · Dec 13, 2024 · c8e8b23 · c8e8b23
1 parent 2da6aa8
commit c8e8b23
Show file tree

Hide file tree

Showing 77 changed files with 1,185 additions and 131 deletions.
diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>1.3.3</version>
+  <version>1.3.4</version>
   <description>
     <p>
       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
@@ -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
@@ -86,7 +86,6 @@ class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTele
 
 private:
   bool is_recovery_;
-  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp
@@ -84,9 +84,6 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
           "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
@@ -61,7 +61,6 @@ class RemovePassedGoals : public BT::ActionNodeBase
   double transform_tolerance_;
   std::shared_ptr<tf2_ros::Buffer> 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
@@ -86,7 +86,6 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
 
 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
@@ -61,9 +61,6 @@ class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
         BT::InputPort<double>("wait_duration", 1.0, "Wait time")
       });
   }
-
-private:
-  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

diff --git a/...ehavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/...ehavior_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
@@ -90,7 +90,6 @@ class GoalReachedCondition : public BT::ConditionNode
   rclcpp::Node::SharedPtr node_;
   std::shared_ptr<tf2_ros::Buffer> 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
@@ -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
@@ -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
@@ -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/...avior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp b/...avior_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
@@ -64,7 +64,6 @@ class RateController : public BT::DecoratorNode
   std::chrono::time_point<std::chrono::high_resolution_clock> 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
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behavior_tree</name>
-  <version>1.3.3</version>
+  <version>1.3.4</version>
   <description>Nav2 behavior tree wrappers, nodes, and utilities</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

diff --git 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<nav2_msgs::action::AssistedTeleop>(xml_tag_name, action_name, conf),
-  initialized_(false)
+: BtActionNode<nav2_msgs::action::AssistedTeleop>(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
@@ -24,8 +24,7 @@ BackUpAction::BackUpAction(
   const std::string & xml_tag_name,
   const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: BtActionNode<nav2_msgs::action::BackUp>(xml_tag_name, action_name, conf),
-  initialized_(false)
+: BtActionNode<nav2_msgs::action::BackUp>(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
@@ -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
@@ -23,10 +23,7 @@ SpinAction::SpinAction(
   const std::string & xml_tag_name,
   const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: BtActionNode<nav2_msgs::action::Spin>(xml_tag_name, action_name, conf),
-  initialized_(false)
-{
-}
+: BtActionNode<nav2_msgs::action::Spin>(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
@@ -25,8 +25,7 @@ WaitAction::WaitAction(
   const std::string & xml_tag_name,
   const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: BtActionNode<nav2_msgs::action::Wait>(xml_tag_name, action_name, conf),
-  initialized_(false)
+: BtActionNode<nav2_msgs::action::Wait>(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
@@ -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<std::string>(
     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
@@ -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<rclcpp::Node::SharedPtr>("node");
 
@@ -52,13 +51,11 @@ void GoalReachedCondition::initialize()
   tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("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
@@ -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<rclcpp::Node::SharedPtr>("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<sensor_msgs::msg::BatteryState>(
-    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<rclcpp::Node::SharedPtr>("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<sensor_msgs::msg::BatteryState>(
+      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
@@ -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<rclcpp::Node::SharedPtr>("node");
   client_ = node_->create_client<nav2_msgs::srv::IsPathValid>("is_path_valid");
@@ -35,12 +34,11 @@ IsPathValidCondition::IsPathValidCondition(
 void IsPathValidCondition::initialize()
 {
   getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
-  initialized_ = true;
 }
 
 BT::NodeStatus IsPathValidCondition::tick()
 {
-  if (!initialized_) {
+  if (!BT::isStatusActive(status())) {
     initialize();
   }