Merge main into humble_main

.github/PULL_REQUEST_TEMPLATE.md

@@ -39,6 +39,7 @@
 <!--
 * I wrote unit tests that cover 90%+ of changes and extensively tested on my physical robot platform in production for 1 week
 * I wrote unit tests and tested in simulation for 10 minutes
+* Performed linting validation using pre-commit run --all or colcon test
 -->
 
 ---

.github/workflows/update_ci_image.yaml

@@ -8,6 +8,8 @@ on:
   push:
     branches:
       - main
+      - jazzy
+      - humble
     paths:
       - '**/package.xml'
       - '**/*.repos'

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -28,13 +28,7 @@
 #include <utility>
 #include <vector>
 
-// For compatibility with Jazzy
-#include "rclcpp/version.h"
-#if RCLCPP_VERSION_GTE(29, 0, 0)
 #include "message_filters/subscriber.hpp"
-#else
-#include "message_filters/subscriber.h"
-#endif
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_util/lifecycle_node.hpp"

nav2_amcl/src/amcl_node.cpp

@@ -1567,22 +1567,19 @@ AmclNode::initServices()
   global_loc_srv_ = std::make_shared<nav2_util::ServiceServer<std_srvs::srv::Empty,
       std::shared_ptr<nav2_util::LifecycleNode>>>(
     "reinitialize_global_localization", shared_from_this(),
-    std::bind(
-      &AmclNode::globalLocalizationCallback, this, std::placeholders::_1,
+    std::bind(&AmclNode::globalLocalizationCallback, this, std::placeholders::_1,
       std::placeholders::_2, std::placeholders::_3));
 
   initial_guess_srv_ = std::make_shared<nav2_util::ServiceServer<nav2_msgs::srv::SetInitialPose,
       std::shared_ptr<nav2_util::LifecycleNode>>>(
     "set_initial_pose", shared_from_this(),
-    std::bind(
-      &AmclNode::initialPoseReceivedSrv, this, std::placeholders::_1, std::placeholders::_2,
+    std::bind(&AmclNode::initialPoseReceivedSrv, this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3));
 
   nomotion_update_srv_ = std::make_shared<nav2_util::ServiceServer<std_srvs::srv::Empty,
       std::shared_ptr<nav2_util::LifecycleNode>>>(
     "request_nomotion_update", shared_from_this(),
-    std::bind(
-      &AmclNode::nomotionUpdateCallback, this, std::placeholders::_1, std::placeholders::_2,
+    std::bind(&AmclNode::nomotionUpdateCallback, this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3));
 }
 

nav2_amcl/src/map/map_cspace.cpp

@@ -132,8 +132,7 @@ void enqueue(
 
   map->cells[map_index].occ_dist = distance * map->scale;
 
-  Q.emplace(
-    CellData{map, static_cast<unsigned int>(i), static_cast<unsigned int>(j),
+  Q.emplace(CellData{map, static_cast<unsigned int>(i), static_cast<unsigned int>(j),
       static_cast<unsigned int>(src_i), static_cast<unsigned int>(src_j),
       map->cells[map_index].occ_dist});
 
@@ -180,27 +179,27 @@ void map_update_cspace(map_t * map, double max_occ_dist)
     CellData current_cell = Q.top();
     if (current_cell.i_ > 0) {
       enqueue(
-        map, current_cell.i_ - 1, current_cell.j_,
-        current_cell.src_i_, current_cell.src_j_,
-        Q, cdm, marked);
+          map, current_cell.i_ - 1, current_cell.j_,
+          current_cell.src_i_, current_cell.src_j_,
+          Q, cdm, marked);
     }
     if (current_cell.j_ > 0) {
       enqueue(
-        map, current_cell.i_, current_cell.j_ - 1,
-        current_cell.src_i_, current_cell.src_j_,
-        Q, cdm, marked);
+          map, current_cell.i_, current_cell.j_ - 1,
+          current_cell.src_i_, current_cell.src_j_,
+          Q, cdm, marked);
     }
     if (static_cast<int>(current_cell.i_) < map->size_x - 1) {
       enqueue(
-        map, current_cell.i_ + 1, current_cell.j_,
-        current_cell.src_i_, current_cell.src_j_,
-        Q, cdm, marked);
+          map, current_cell.i_ + 1, current_cell.j_,
+          current_cell.src_i_, current_cell.src_j_,
+          Q, cdm, marked);
     }
     if (static_cast<int>(current_cell.j_) < map->size_y - 1) {
       enqueue(
-        map, current_cell.i_, current_cell.j_ + 1,
-        current_cell.src_i_, current_cell.src_j_,
-        Q, cdm, marked);
+          map, current_cell.i_, current_cell.j_ + 1,
+          current_cell.src_i_, current_cell.src_j_,
+          Q, cdm, marked);
     }
 
     Q.pop();

nav2_behavior_tree/CMakeLists.txt

@@ -104,6 +104,9 @@ list(APPEND plugin_libs nav2_transform_available_condition_bt_node)
 add_library(nav2_goal_reached_condition_bt_node SHARED plugins/condition/goal_reached_condition.cpp)
 list(APPEND plugin_libs nav2_goal_reached_condition_bt_node)
 
+add_library(nav2_are_poses_near_condition_bt_node SHARED plugins/condition/are_poses_near_condition.cpp)
+list(APPEND plugin_libs nav2_are_poses_near_condition_bt_node)
+
 add_library(nav2_globally_updated_goal_condition_bt_node SHARED plugins/condition/globally_updated_goal_condition.cpp)
 list(APPEND plugin_libs nav2_globally_updated_goal_condition_bt_node)
 
@@ -143,6 +146,9 @@ list(APPEND plugin_libs nav2_would_a_planner_recovery_help_condition_bt_node)
 add_library(nav2_would_a_smoother_recovery_help_condition_bt_node SHARED plugins/condition/would_a_smoother_recovery_help_condition.cpp)
 list(APPEND plugin_libs nav2_would_a_smoother_recovery_help_condition_bt_node)
 
+add_library(nav2_would_a_route_recovery_help_condition_bt_node SHARED plugins/condition/would_a_route_recovery_help_condition.cpp)
+list(APPEND plugin_libs nav2_would_a_route_recovery_help_condition_bt_node)
+
 add_library(nav2_reinitialize_global_localization_service_bt_node SHARED plugins/action/reinitialize_global_localization_service.cpp)
 list(APPEND plugin_libs nav2_reinitialize_global_localization_service_bt_node)
 
@@ -158,6 +164,9 @@ list(APPEND plugin_libs nav2_speed_controller_bt_node)
 add_library(nav2_truncate_path_action_bt_node SHARED plugins/action/truncate_path_action.cpp)
 list(APPEND plugin_libs nav2_truncate_path_action_bt_node)
 
+add_library(nav2_concatenate_paths_action_bt_node SHARED plugins/action/concatenate_paths_action.cpp)
+list(APPEND plugin_libs nav2_concatenate_paths_action_bt_node)
+
 add_library(nav2_truncate_path_local_action_bt_node SHARED plugins/action/truncate_path_local_action.cpp)
 list(APPEND plugin_libs nav2_truncate_path_local_action_bt_node)
 
@@ -182,9 +191,21 @@ list(APPEND plugin_libs nav2_remove_passed_goals_action_bt_node)
 add_library(nav2_remove_in_collision_goals_action_bt_node SHARED plugins/action/remove_in_collision_goals_action.cpp)
 list(APPEND plugin_libs nav2_remove_in_collision_goals_action_bt_node)
 
+add_library(nav2_append_goal_pose_to_goals_action_bt_node SHARED plugins/action/append_goal_pose_to_goals_action.cpp)
+list(APPEND plugin_libs nav2_append_goal_pose_to_goals_action_bt_node)
+
 add_library(nav2_get_pose_from_path_action_bt_node SHARED plugins/action/get_pose_from_path_action.cpp)
 list(APPEND plugin_libs nav2_get_pose_from_path_action_bt_node)
 
+add_library(nav2_extract_route_nodes_as_goals_action_bt_node SHARED plugins/action/extract_route_nodes_as_goals_action.cpp)
+list(APPEND plugin_libs nav2_extract_route_nodes_as_goals_action_bt_node)
+
+add_library(nav2_get_next_few_goals_action_bt_node SHARED plugins/action/get_next_few_goals_action.cpp)
+list(APPEND plugin_libs nav2_get_next_few_goals_action_bt_node)
+
+add_library(nav2_get_current_pose_action_bt_node SHARED plugins/action/get_current_pose_action.cpp)
+list(APPEND plugin_libs nav2_get_current_pose_action_bt_node)
+
 add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp)
 list(APPEND plugin_libs nav2_pipeline_sequence_bt_node)
 
@@ -212,6 +233,15 @@ list(APPEND plugin_libs nav2_progress_checker_selector_bt_node)
 add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp)
 list(APPEND plugin_libs nav2_goal_updated_controller_bt_node)
 
+add_library(nav2_compute_and_track_route_cancel_bt_node SHARED plugins/action/compute_and_track_route_cancel_node.cpp)
+list(APPEND plugin_libs nav2_compute_and_track_route_cancel_bt_node)
+
+add_library(nav2_compute_and_track_route_bt_node SHARED plugins/action/compute_and_track_route_action.cpp)
+list(APPEND plugin_libs nav2_compute_and_track_route_bt_node)
+
+add_library(nav2_compute_route_bt_node SHARED plugins/action/compute_route_action.cpp)
+list(APPEND plugin_libs nav2_compute_route_bt_node)
+
 foreach(bt_plugin ${plugin_libs})
   target_include_directories(${bt_plugin}
     PRIVATE

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -115,7 +115,7 @@ class BtServiceNode : public BT::ActionNodeBase
       service_name_ = service_new;
       node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
       service_client_ = std::make_shared<nav2_util::ServiceClient<ServiceT>>(
-        service_name_, node_, true /*creates and spins an internal executor*/);
+      service_name_, node_, true /*creates and spins an internal executor*/);
     }
   }
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/append_goal_pose_to_goals_action.hpp

@@ -0,0 +1,56 @@
+// Copyright (c) 2025 Open Navigation LLC
+//
+// 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_BEHAVIOR_TREE__PLUGINS__ACTION__APPEND_GOAL_POSE_TO_GOALS_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__APPEND_GOAL_POSE_TO_GOALS_ACTION_HPP_
+
+#include <vector>
+#include <memory>
+#include <string>
+
+#include "nav_msgs/msg/goals.hpp"
+#include "nav2_msgs/msg/route.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "behaviortree_cpp/action_node.h"
+
+namespace nav2_behavior_tree
+{
+
+class AppendGoalPoseToGoals : public BT::ActionNodeBase
+{
+public:
+  AppendGoalPoseToGoals(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf);
+
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<geometry_msgs::msg::PoseStamped>("goal_pose", "Goal pose to append"),
+      BT::InputPort<nav_msgs::msg::Goals>("input_goals", "Input goals to append to"),
+      BT::OutputPort<nav_msgs::msg::Goals>("output_goals", "Output goals after appending")
+    };
+  }
+
+private:
+  void halt() override {}
+  BT::NodeStatus tick() override;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__APPEND_GOAL_POSE_TO_GOALS_ACTION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp

@@ -0,0 +1,107 @@
+// Copyright (c) 2025 Open Navigation LLC
+//
+// 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_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_ACTION_HPP_
+
+#include <string>
+#include <memory>
+
+#include "nav2_msgs/action/compute_and_track_route.hpp"
+#include "nav2_behavior_tree/bt_action_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputeAndTrackRoute
+ */
+class ComputeAndTrackRouteAction : public BtActionNode<nav2_msgs::action::ComputeAndTrackRoute>
+{
+  using Action = nav2_msgs::action::ComputeAndTrackRoute;
+  using ActionResult = Action::Result;
+
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::ComputeAndTrackRouteAction
+   * @param xml_tag_name Name for the XML tag for this node
+   * @param action_name Action name this node creates a client for
+   * @param conf BT node configuration
+   */
+  ComputeAndTrackRouteAction(
+    const std::string & xml_tag_name,
+    const std::string & action_name,
+    const BT::NodeConfiguration & conf);
+
+  /**
+   * @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
+   */
+  BT::NodeStatus on_success() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon abortion of the action
+   */
+  BT::NodeStatus on_aborted() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon cancellation of the action
+   */
+  BT::NodeStatus on_cancelled() override;
+
+  /**
+   * @brief Function to perform some user-defined operation after a timeout
+   * waiting for a result that hasn't been received yet
+   * @param feedback shared_ptr to latest feedback message
+   */
+  void on_wait_for_result(
+    std::shared_ptr<const Action::Feedback> feedback) override;
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing basic ports along with node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts(
+      {
+        BT::InputPort<unsigned int>("start_id", "ID of the start node"),
+        BT::InputPort<unsigned int>("goal_id", "ID of the goal node"),
+        BT::InputPort<geometry_msgs::msg::PoseStamped>(
+          "start",
+          "Start pose of the path if overriding current robot pose and using poses over IDs"),
+        BT::InputPort<geometry_msgs::msg::PoseStamped>(
+          "goal", "Goal pose of the path if using poses over IDs"),
+        BT::InputPort<bool>(
+          "use_start", false,
+          "Whether to use the start pose or the robot's current pose"),
+        BT::InputPort<bool>(
+          "use_poses", false, "Whether to use poses or IDs for start and goal"),
+        BT::OutputPort<builtin_interfaces::msg::Duration>("execution_duration",
+          "Time taken to compute and track route"),
+        BT::OutputPort<ActionResult::_error_code_type>(
+          "error_code_id", "The compute route error code"),
+        BT::OutputPort<std::string>(
+          "error_msg", "The compute route error msg"),
+      });
+  }
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_ACTION_HPP_