Merge branch 'main' into smac_shortcutting

Signed-off-by: Steve Macenski <[email protected]>

.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 }}\

.github/PULL_REQUEST_TEMPLATE.md

@@ -9,6 +9,7 @@
 | Ticket(s) this addresses   | (add tickets here #1) |
 | Primary OS tested on | (Ubuntu, MacOS, Windows) |
 | Robotic platform tested on | (Steve's Robot, gazebo simulation of Tally, hardware turtlebot) |
+| Does this PR contain AI generated software? | (No; Yes and it is marked inline in the code) |
 
 ---
 

.github/workflows/lint.yml

@@ -0,0 +1,21 @@
+name: Lint
+on:
+  pull_request:
+
+jobs:
+  ament_lint_general:
+    name: ament_${{ matrix.linter }}
+    runs-on: ubuntu-latest
+    container:
+      image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest
+    strategy:
+      fail-fast: false
+      matrix:
+          linter: [xmllint, cpplint, uncrustify, pep257, flake8]
+    steps:
+    - uses: actions/checkout@v4
+    - uses: ros-tooling/[email protected]
+      with:
+        linter: ${{ matrix.linter }}
+        distribution: rolling
+        package-name: "*"

CONTRIBUTING.md

@@ -0,0 +1,18 @@
+Any contribution that you make to this repository will
+be under the Apache 2 License, as dictated by that
+[license](http://www.apache.org/licenses/LICENSE-2.0.html):
+
+~~~
+5. Submission of Contributions. Unless You explicitly state otherwise,
+   any Contribution intentionally submitted for inclusion in the Work
+   by You to the Licensor shall be under the terms and conditions of
+   this License, without any additional terms or conditions.
+   Notwithstanding the above, nothing herein shall supersede or modify
+   the terms of any separate license agreement you may have executed
+   with Licensor regarding such Contributions.
+~~~
+
+Contributors must sign-off each commit by adding a `Signed-off-by: ...`
+line to commit messages to certify that they have the right to submit
+the code they are contributing to the project according to the
+[Developer Certificate of Origin (DCO)](https://developercertificate.org/).

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_amcl/src/amcl_node.cpp

@@ -1167,7 +1167,7 @@ AmclNode::dynamicParametersCallback(
     if (param_type == ParameterType::PARAMETER_DOUBLE) {
       if (param_name == "alpha1") {
         alpha1_ = parameter.as_double();
-        //alpha restricted to be non-negative
+        // alpha restricted to be non-negative
         if (alpha1_ < 0.0) {
           RCLCPP_WARN(
             get_logger(), "You've set alpha1 to be negative,"
@@ -1177,7 +1177,7 @@ AmclNode::dynamicParametersCallback(
         reinit_odom = true;
       } else if (param_name == "alpha2") {
         alpha2_ = parameter.as_double();
-        //alpha restricted to be non-negative
+        // alpha restricted to be non-negative
         if (alpha2_ < 0.0) {
           RCLCPP_WARN(
             get_logger(), "You've set alpha2 to be negative,"
@@ -1187,7 +1187,7 @@ AmclNode::dynamicParametersCallback(
         reinit_odom = true;
       } else if (param_name == "alpha3") {
         alpha3_ = parameter.as_double();
-        //alpha restricted to be non-negative
+        // alpha restricted to be non-negative
         if (alpha3_ < 0.0) {
           RCLCPP_WARN(
             get_logger(), "You've set alpha3 to be negative,"
@@ -1197,7 +1197,7 @@ AmclNode::dynamicParametersCallback(
         reinit_odom = true;
       } else if (param_name == "alpha4") {
         alpha4_ = parameter.as_double();
-        //alpha restricted to be non-negative
+        // alpha restricted to be non-negative
         if (alpha4_ < 0.0) {
           RCLCPP_WARN(
             get_logger(), "You've set alpha4 to be negative,"
@@ -1207,7 +1207,7 @@ AmclNode::dynamicParametersCallback(
         reinit_odom = true;
       } else if (param_name == "alpha5") {
         alpha5_ = parameter.as_double();
-        //alpha restricted to be non-negative
+        // alpha restricted to be non-negative
         if (alpha5_ < 0.0) {
           RCLCPP_WARN(
             get_logger(), "You've set alpha5 to be negative,"

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/bt_action_node.hpp

@@ -61,6 +61,8 @@ class BtActionNode : public BT::ActionNodeBase
     server_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
     getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+    wait_for_service_timeout_ =
+      config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
 
     // Initialize the input and output messages
     goal_ = typename ActionT::Goal();
@@ -93,7 +95,7 @@ class BtActionNode : public BT::ActionNodeBase
 
     // Make sure the server is actually there before continuing
     RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
-    if (!action_client_->wait_for_action_server(1s)) {
+    if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
       RCLCPP_ERROR(
         node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
         action_name.c_str());
@@ -462,6 +464,9 @@ class BtActionNode : public BT::ActionNodeBase
   // The timeout value for BT loop execution
   std::chrono::milliseconds bt_loop_duration_;
 
+  // The timeout value for waiting for a service to response
+  std::chrono::milliseconds wait_for_service_timeout_;
+
   // To track the action server acknowledgement when a new goal is sent
   std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>
   future_goal_handle_;

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -248,6 +248,9 @@ class BtActionServer
   // Default timeout value while waiting for response from a server
   std::chrono::milliseconds default_server_timeout_;
 
+  // The timeout value for waiting for a service to response
+  std::chrono::milliseconds wait_for_service_timeout_;
+
   // User-provided callbacks
   OnGoalReceivedCallback on_goal_received_callback_;
   OnLoopCallback on_loop_callback_;

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -135,7 +135,7 @@ bool BtActionServer<ActionT>::on_configure()
     node, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link")));
   nav2_util::declare_parameter_if_not_declared(
     node, "transform_tolerance", rclcpp::ParameterValue(0.1));
-  nav2_util::copy_all_parameters(node, client_node_);
+  rclcpp::copy_all_parameter_values(node, client_node_);
 
   // set the timeout in seconds for the action server to discard goal handles if not finished
   double action_server_result_timeout;
@@ -158,6 +158,9 @@ bool BtActionServer<ActionT>::on_configure()
   int default_server_timeout;
   node->get_parameter("default_server_timeout", default_server_timeout);
   default_server_timeout_ = std::chrono::milliseconds(default_server_timeout);
+  int wait_for_service_timeout;
+  node->get_parameter("wait_for_service_timeout", wait_for_service_timeout);
+  wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout);
 
   // Get error code id names to grab off of the blackboard
   error_code_names_ = node->get_parameter("error_code_names").as_string_array();
@@ -172,6 +175,9 @@ bool BtActionServer<ActionT>::on_configure()
   blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_);  // NOLINT
   blackboard_->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);  // NOLINT
   blackboard_->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);  // NOLINT
+  blackboard_->set<std::chrono::milliseconds>(
+    "wait_for_service_timeout",
+    wait_for_service_timeout_);
 
   return true;
 }
@@ -235,6 +241,9 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
       blackboard->set<rclcpp::Node::SharedPtr>("node", client_node_);
       blackboard->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);
       blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
+      blackboard->set<std::chrono::milliseconds>(
+        "wait_for_service_timeout",
+        wait_for_service_timeout_);
     }
   } catch (const std::exception & e) {
     RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what());

nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp

@@ -59,6 +59,8 @@ class BtCancelActionNode : public BT::ActionNodeBase
     server_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
     getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+    wait_for_service_timeout_ =
+      config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
 
     std::string remapped_action_name;
     if (getInput("server_name", remapped_action_name)) {
@@ -89,7 +91,7 @@ class BtCancelActionNode : public BT::ActionNodeBase
 
     // Make sure the server is actually there before continuing
     RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
-    if (!action_client_->wait_for_action_server(1s)) {
+    if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
       RCLCPP_ERROR(
         node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
         action_name.c_str());
@@ -168,6 +170,8 @@ class BtCancelActionNode : public BT::ActionNodeBase
   // The timeout value while waiting for response from a server when a
   // new action goal is canceled
   std::chrono::milliseconds server_timeout_;
+  // The timeout value for waiting for a service to response
+  std::chrono::milliseconds wait_for_service_timeout_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -62,6 +62,8 @@ class BtServiceNode : public BT::ActionNodeBase
     server_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
     getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+    wait_for_service_timeout_ =
+      config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
 
     // Now that we have node_ to use, create the service client for this BT service
     getInput("service_name", service_name_);
@@ -77,7 +79,7 @@ class BtServiceNode : public BT::ActionNodeBase
     RCLCPP_DEBUG(
       node_->get_logger(), "Waiting for \"%s\" service",
       service_name_.c_str());
-    if (!service_client_->wait_for_service(1s)) {
+    if (!service_client_->wait_for_service(wait_for_service_timeout_)) {
       RCLCPP_ERROR(
         node_->get_logger(), "\"%s\" service server not available after waiting for 1 s",
         service_node_name.c_str());
@@ -249,6 +251,9 @@ class BtServiceNode : public BT::ActionNodeBase
   // The timeout value for BT loop execution
   std::chrono::milliseconds bt_loop_duration_;
 
+  // The timeout value for waiting for a service to response
+  std::chrono::milliseconds wait_for_service_timeout_;
+
   // To track the server response when a new request is sent
   std::shared_future<typename ServiceT::Response::SharedPtr> future_result_;
   bool request_sent_{false};

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