Merge branch 'main' into add_polygon_velocity

.circleci/config.yml

@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v16\
+              - "<< parameters.key >>-v18\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v16\
+              - "<< parameters.key >>-v18\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v16\
+            key: "<< parameters.key >>-v18\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\

README.md

@@ -20,6 +20,8 @@ For detailed instructions on how to:
 
 Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate).
 
+If you need professional services related to Nav2, please contact Open Navigation at [email protected].
+
 ## Our Sponsors
 
 Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community. 

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -25,6 +25,7 @@
 #include "behaviortree_cpp_v3/xml_parsing.h"
 #include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
 
+#include "rclcpp/rclcpp.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -46,7 +47,8 @@ class BehaviorTreeEngine
    * @brief A constructor for nav2_behavior_tree::BehaviorTreeEngine
    * @param plugin_libraries vector of BT plugin library names to load
    */
-  explicit BehaviorTreeEngine(const std::vector<std::string> & plugin_libraries);
+  explicit BehaviorTreeEngine(
+    const std::vector<std::string> & plugin_libraries);
   virtual ~BehaviorTreeEngine() {}
 
   /**

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp

@@ -64,6 +64,11 @@ class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTele
    */
   BT::NodeStatus on_cancelled() override;
 
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -81,6 +86,7 @@ class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTele
 
 private:
   bool is_recovery_;
+  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp

@@ -64,6 +64,11 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
    */
   BT::NodeStatus on_cancelled() override;
 
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -79,6 +84,9 @@ 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

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp

@@ -43,6 +43,11 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
     const std::string & action_name,
     const BT::NodeConfiguration & conf);
 
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -59,6 +64,11 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
       });
   }
 
+  /**
+ * @brief Function to perform some user-defined operation on tick
+ */
+  void on_tick() override;
+
   /**
  * @brief Function to perform some user-defined operation upon successful completion of the action
  */
@@ -73,6 +83,9 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
    * @brief Function to perform some user-defined operation upon cancellation of the action
    */
   BT::NodeStatus on_cancelled() override;
+
+private:
+  bool initalized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp

@@ -36,6 +36,10 @@ class RemovePassedGoals : public BT::ActionNodeBase
     const std::string & xml_tag_name,
     const BT::NodeConfiguration & conf);
 
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
 
   static BT::PortsList providedPorts()
   {
@@ -56,6 +60,7 @@ 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

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp

@@ -48,6 +48,11 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
    */
   void on_tick() override;
 
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -81,6 +86,7 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
 
 private:
   bool is_recovery_;
+  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp

@@ -45,6 +45,11 @@ class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
    */
   void on_tick() override;
 
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -56,6 +61,9 @@ class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
         BT::InputPort<double>("wait_duration", 1.0, "Wait time")
       });
   }
+
+private:
+  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp

@@ -52,6 +52,11 @@ class DistanceTraveledCondition : public BT::ConditionNode
    */
   BT::NodeStatus tick() override;
 
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing node-specific ports
@@ -74,6 +79,7 @@ class DistanceTraveledCondition : public BT::ConditionNode
   double distance_;
   double transform_tolerance_;
   std::string global_frame_, robot_base_frame_;
+  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp

@@ -51,6 +51,11 @@ class IsBatteryLowCondition : public BT::ConditionNode
    */
   BT::NodeStatus tick() override;
 
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing node-specific ports
@@ -81,6 +86,7 @@ class IsBatteryLowCondition : public BT::ConditionNode
   double min_battery_;
   bool is_voltage_;
   bool is_battery_low_;
+  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp

@@ -50,6 +50,11 @@ class IsPathValidCondition : public BT::ConditionNode
    */
   BT::NodeStatus tick() override;
 
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing node-specific ports
@@ -68,6 +73,7 @@ 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

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp

@@ -48,6 +48,11 @@ class TimeExpiredCondition : public BT::ConditionNode
    */
   BT::NodeStatus tick() override;
 
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing node-specific ports
@@ -63,6 +68,7 @@ class TimeExpiredCondition : public BT::ConditionNode
   rclcpp::Node::SharedPtr node_;
   rclcpp::Time start_;
   double period_;
+  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp

@@ -55,6 +55,11 @@ class TransformAvailableCondition : public BT::ConditionNode
    */
   BT::NodeStatus tick() override;
 
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing node-specific ports
@@ -75,6 +80,7 @@ class TransformAvailableCondition : public BT::ConditionNode
 
   std::string child_frame_;
   std::string parent_frame_;
+  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp

@@ -18,6 +18,7 @@
 #include <string>
 
 #include "nav2_msgs/action/compute_path_to_pose.hpp"
+#include "nav2_msgs/action/compute_path_through_poses.hpp"
 #include "nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp"
 
 namespace nav2_behavior_tree
@@ -27,6 +28,8 @@ class WouldAPlannerRecoveryHelp : public AreErrorCodesPresent
 {
   using Action = nav2_msgs::action::ComputePathToPose;
   using ActionResult = Action::Result;
+  using ThroughAction = nav2_msgs::action::ComputePathThroughPoses;
+  using ThroughActionResult = Action::Result;
 
 public:
   WouldAPlannerRecoveryHelp(

nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp

@@ -38,6 +38,11 @@ class RateController : public BT::DecoratorNode
     const std::string & name,
     const BT::NodeConfiguration & conf);
 
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing node-specific ports
@@ -59,6 +64,7 @@ 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

nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp

@@ -24,18 +24,27 @@ 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)
+: BtActionNode<nav2_msgs::action::AssistedTeleop>(xml_tag_name, action_name, conf),
+  initialized_(false)
+{}
+
+void AssistedTeleopAction::initialize()
 {
   double time_allowance;
   getInput("time_allowance", time_allowance);
   getInput("is_recovery", is_recovery_);
 
   // Populate the input message
   goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
+  initialized_ = true;
 }
 
 void AssistedTeleopAction::on_tick()
 {
+  if (!initialized_) {
+    initialize();
+  }
+
   if (is_recovery_) {
     increment_recovery_count();
   }

nav2_behavior_tree/plugins/action/back_up_action.cpp

@@ -24,7 +24,12 @@ 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)
+: BtActionNode<nav2_msgs::action::BackUp>(xml_tag_name, action_name, conf),
+  initialized_(false)
+{
+}
+
+void nav2_behavior_tree::BackUpAction::initialize()
 {
   double dist;
   getInput("backup_dist", dist);
@@ -39,10 +44,15 @@ BackUpAction::BackUpAction(
   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_) {
+    initialize();
+  }
+
   increment_recovery_count();
 }