Nav2 route server start pose orientation scorer

nav2_core/include/nav2_core/route_exceptions.hpp

@@ -71,6 +71,13 @@ class IndeterminantNodesOnGraph : public RouteException
   : RouteException(description) {}
 };
 
+class InvalidEdgeScorerUse : public RouteException
+{
+public:
+  explicit InvalidEdgeScorerUse(const std::string & description)
+  : RouteException(description) {}
+};
+
 }  // namespace nav2_core
 
 #endif  // NAV2_CORE__ROUTE_EXCEPTIONS_HPP_

nav2_msgs/action/ComputeAndTrackRoute.action

@@ -6,6 +6,7 @@ uint16 INDETERMINANT_NODES_ON_GRAPH=403
 uint16 TIMEOUT=404
 uint16 NO_VALID_ROUTE=405
 uint16 OPERATION_FAILED=406
+uint16 INVALID_EDGE_SCORER_USE=407
 
 #goal definition
 uint16 start_id

nav2_msgs/action/ComputeRoute.action

@@ -5,6 +5,7 @@ uint16 NO_VALID_GRAPH=402
 uint16 INDETERMINANT_NODES_ON_GRAPH=403
 uint16 TIMEOUT=404
 uint16 NO_VALID_ROUTE=405
+uint16 INVALID_EDGE_SCORER_USE=407
 
 #goal definition
 uint16 start_id

nav2_route/CMakeLists.txt

@@ -88,6 +88,7 @@ add_library(edge_scorers SHARED
   src/plugins/edge_cost_functions/costmap_scorer.cpp
   src/plugins/edge_cost_functions/semantic_scorer.cpp
   src/plugins/edge_cost_functions/goal_orientation_scorer.cpp
+  src/plugins/edge_cost_functions/start_pose_orientation_scorer.cpp
 )
 target_include_directories(edge_scorers
   PUBLIC

nav2_route/include/nav2_route/edge_scorer.hpp

@@ -19,6 +19,7 @@
 #include <memory>
 #include <vector>
 
+#include "tf2_ros/buffer.h"
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "nav2_util/node_utils.hpp"
@@ -47,7 +48,9 @@ class EdgeScorer
   /**
    * @brief Constructor
    */
-  explicit EdgeScorer(nav2_util::LifecycleNode::SharedPtr node);
+  explicit EdgeScorer(
+    nav2_util::LifecycleNode::SharedPtr node,
+    const std::shared_ptr<tf2_ros::Buffer> tf_buffer);
 
   /**
    * @brief Destructor
@@ -63,7 +66,8 @@ class EdgeScorer
    * @return If edge is valid
    */
   bool score(
-    const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge,
+    const EdgePtr edge, const RouteRequest & route_request,
+    const EdgeType & edge_type,
     float & score);
 
   /**

nav2_route/include/nav2_route/goal_intent_extractor.hpp

@@ -111,6 +111,12 @@ class GoalIntentExtractor
    */
   void overrideStart(const geometry_msgs::msg::PoseStamped & start_pose);
 
+  /**
+   * @brief gets the desired start pose
+   * @return PoseStamped of start pose
+   */
+  geometry_msgs::msg::PoseStamped getStart();
+
 protected:
   rclcpp::Logger logger_{rclcpp::get_logger("GoalIntentExtractor")};
   std::shared_ptr<NodeSpatialTree> node_spatial_tree_;

nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp

@@ -18,6 +18,7 @@
 #include <memory>
 #include <string>
 
+#include "tf2_ros/buffer.h"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "pluginlib/class_loader.hpp"
@@ -55,6 +56,7 @@ class EdgeCostFunction
    */
   virtual void configure(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+    const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
     const std::string & name) = 0;
 
   /**
@@ -63,8 +65,8 @@ class EdgeCostFunction
    * start/end nodes and their associated metadata and actions
    */
   virtual bool score(
-    const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose,
-    bool final_edge, float & cost) = 0;
+    const EdgePtr edge, const RouteRequest & route_request,
+    const EdgeType & edge_type, float & cost) = 0;
 
   /**
    * @brief Get name of the plugin for parameter scope mapping

nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp

@@ -51,6 +51,7 @@ class CostmapScorer : public EdgeCostFunction
    */
   void configure(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+    const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
     const std::string & name) override;
 
   /**
@@ -61,8 +62,8 @@ class CostmapScorer : public EdgeCostFunction
    * @return bool if this edge is open valid to traverse
    */
   bool score(
-    const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge,
-    float & cost) override;
+    const EdgePtr edge, const RouteRequest & route_request,
+    const EdgeType & edge_type, float & cost) override;
 
   /**
    * @brief Get name of the plugin for parameter scope mapping

nav2_route/include/nav2_route/plugins/edge_cost_functions/distance_scorer.hpp

@@ -49,6 +49,7 @@ class DistanceScorer : public EdgeCostFunction
    */
   void configure(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+    const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
     const std::string & name) override;
 
   /**
@@ -59,8 +60,8 @@ class DistanceScorer : public EdgeCostFunction
    * @return bool if this edge is open valid to traverse
    */
   bool score(
-    const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge,
-    float & cost) override;
+    const EdgePtr edge, const RouteRequest & route_request,
+    const EdgeType & edge_type, float & cost) override;
 
   /**
    * @brief Get name of the plugin for parameter scope mapping

nav2_route/include/nav2_route/plugins/edge_cost_functions/dynamic_edges_scorer.hpp

@@ -52,6 +52,7 @@ class DynamicEdgesScorer : public EdgeCostFunction
    */
   void configure(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+    const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
     const std::string & name) override;
 
   /**
@@ -62,8 +63,8 @@ class DynamicEdgesScorer : public EdgeCostFunction
    * @return bool if this edge is open valid to traverse
    */
   bool score(
-    const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge,
-    float & cost) override;
+    const EdgePtr edge, const RouteRequest & route_request,
+    const EdgeType & edge_type, float & cost) override;
 
   /**
    * @brief Get name of the plugin for parameter scope mapping

nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp

@@ -20,6 +20,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "nav2_core/route_exceptions.hpp"
 #include "nav2_route/interfaces/edge_cost_function.hpp"
 #include "nav2_util/line_iterator.hpp"
 #include "nav2_util/node_utils.hpp"
@@ -33,7 +34,11 @@ namespace nav2_route
 
 /**
  * @class GoalOrientationScorer
- * @brief Scores final edge by comparing the
+ * @brief Scores an edge leading to the goal node by comparing the orientation of the route 
+ * pose and the orientation of the edge by multiplying the deviation from the desired 
+ * orientation with a user defined weight. An alternative method can be selected, with 
+ * the use_orientation_threshold flag, which rejects the edge it is greater than some 
+ * tolerance
  */
 class GoalOrientationScorer : public EdgeCostFunction
 {
@@ -53,6 +58,7 @@ class GoalOrientationScorer : public EdgeCostFunction
    */
   void configure(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+    const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
     const std::string & name) override;
 
   /**
@@ -63,8 +69,8 @@ class GoalOrientationScorer : public EdgeCostFunction
    * @return bool if this edge is open valid to traverse
    */
   bool score(
-    const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge,
-    float & cost) override;
+    const EdgePtr edge, const RouteRequest & route_request,
+    const EdgeType & edge_type, float & cost) override;
 
   /**
    * @brief Get name of the plugin for parameter scope mapping
@@ -76,6 +82,8 @@ class GoalOrientationScorer : public EdgeCostFunction
   rclcpp::Logger logger_{rclcpp::get_logger("GoalOrientationScorer")};
   std::string name_;
   double orientation_tolerance_;
+  float orientation_weight_;
+  bool use_orientation_threshold_;
 };
 
 }  // namespace nav2_route

nav2_route/include/nav2_route/plugins/edge_cost_functions/penalty_scorer.hpp

@@ -48,6 +48,7 @@ class PenaltyScorer : public EdgeCostFunction
    */
   void configure(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+    const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
     const std::string & name) override;
 
   /**
@@ -58,8 +59,8 @@ class PenaltyScorer : public EdgeCostFunction
    * @return bool if this edge is open valid to traverse
    */
   bool score(
-    const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge,
-    float & cost) override;
+    const EdgePtr edge, const RouteRequest & route_request,
+    const EdgeType & edge_type, float & cost) override;
 
   /**
    * @brief Get name of the plugin for parameter scope mapping

nav2_route/include/nav2_route/plugins/edge_cost_functions/semantic_scorer.hpp

@@ -50,6 +50,7 @@ class SemanticScorer : public EdgeCostFunction
    */
   void configure(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+    const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
     const std::string & name) override;
 
   /**
@@ -60,8 +61,8 @@ class SemanticScorer : public EdgeCostFunction
    * @return bool if this edge is open valid to traverse
    */
   bool score(
-    const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge,
-    float & cost) override;
+    const EdgePtr edge, const RouteRequest & route_request,
+    const EdgeType & edge_type, float & cost) override;
 
   /**
    * @brief Scores graph object based on metadata's semantic value at key

nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp

@@ -0,0 +1,93 @@
+// Copyright (c) 2025, Polymath Robotics Inc.
+//
+// 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_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__START_POSE_ORIENTATION_SCORER_HPP_
+#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__START_POSE_ORIENTATION_SCORER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "nav2_core/route_exceptions.hpp"
+#include "nav2_route/interfaces/edge_cost_function.hpp"
+#include "nav2_util/line_iterator.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2/utils.h"
+#include "angles/angles.h"
+
+namespace nav2_route
+{
+
+/**
+ * @class StartPoseOrientationScorer
+ * @brief Scores initial edge by comparing the orientation of the robot's current 
+ * pose and the orientation of the edge by multiplying the deviation from the desired 
+ * orientation with a user defined weight. An alternative method can be selected, with 
+ * the use_orientation_threshold flag, which rejects the edge it is greater than some 
+ * tolerance
+ */
+class StartPoseOrientationScorer : public EdgeCostFunction
+{
+public:
+  /**
+   * @brief Constructor
+   */
+  StartPoseOrientationScorer() = default;
+
+  /**
+   * @brief destructor
+   */
+  virtual ~StartPoseOrientationScorer() = default;
+
+  /**
+   * @brief Configure
+   */
+  void configure(
+    const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+    const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+    const std::string & name) override;
+
+  /**
+   * @brief Main scoring plugin API
+   * @param edge The edge pointer to score, which has access to the
+   * start/end nodes and their associated metadata and actions
+   * @param cost of the edge scored
+   * @return bool if this edge is open valid to traverse
+   */
+  bool score(
+    const EdgePtr edge, const RouteRequest & route_request,
+    const EdgeType & edge_type, float & cost) override;
+
+  /**
+   * @brief Get name of the plugin for parameter scope mapping
+   * @return Name
+   */
+  std::string getName() override;
+
+protected:
+  rclcpp::Logger logger_{rclcpp::get_logger("StartPoseOrientationScorer")};
+  std::string name_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  double orientation_tolerance_;
+  float orientation_weight_;
+  bool use_orientation_threshold_;
+};
+
+}  // namespace nav2_route
+
+#endif  // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__START_POSE_ORIENTATION_SCORER_HPP_

nav2_route/include/nav2_route/plugins/edge_cost_functions/time_scorer.hpp

@@ -50,6 +50,7 @@ class TimeScorer : public EdgeCostFunction
    */
   void configure(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+    const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
     const std::string & name) override;
 
   /**
@@ -60,8 +61,8 @@ class TimeScorer : public EdgeCostFunction
    * @return bool if this edge is open valid to traverse
    */
   bool score(
-    const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge,
-    float & cost) override;
+    const EdgePtr edge, const RouteRequest & route_request,
+    const EdgeType & edge_type, float & cost) override;
 
   /**
    * @brief Get name of the plugin for parameter scope mapping