Nav2 Route Server

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/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_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_cancel_node.hpp

@@ -0,0 +1,61 @@
+// 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_CANCEL_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_CANCEL_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "nav2_msgs/action/compute_and_track_route.hpp"
+
+#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A nav2_behavior_tree::BtActionNode class
+ * that wraps nav2_msgs::action::ComputeAndTrackRoute cancellation
+ */
+class ComputeAndTrackRouteCancel
+  : public BtCancelActionNode<nav2_msgs::action::ComputeAndTrackRoute>
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::ComputeAndTrackRouteCancel
+   * @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
+   */
+  ComputeAndTrackRouteCancel(
+    const std::string & xml_tag_name,
+    const std::string & action_name,
+    const BT::NodeConfiguration & conf);
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing basic ports along with node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts(
+      {
+      });
+  }
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_CANCEL_NODE_HPP_