Nav2 route server goal orientation scorer

nav2_route/CMakeLists.txt

@@ -81,6 +81,7 @@ add_library(edge_scorers SHARED
   src/plugins/edge_cost_functions/penalty_scorer.cpp
   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
 )
 
 ament_target_dependencies(edge_scorers

nav2_route/include/nav2_route/edge_scorer.hpp

@@ -26,7 +26,7 @@
 #include "nav2_route/types.hpp"
 #include "nav2_route/utils.hpp"
 #include "nav2_route/interfaces/edge_cost_function.hpp"
-
+#include "geometry_msgs/msg/pose_stamped.hpp"
 namespace nav2_route
 {
 
@@ -57,10 +57,14 @@ class EdgeScorer
   /**
    * @brief Score the edge with the set of plugins
    * @param edge Ptr to edge for scoring
+   * @param goal_pose Pose Stamped of desired goal
    * @param score of edge
+   * @param final_edge whether this edge brings us to the goal or not
    * @return If edge is valid
    */
-  bool score(const EdgePtr edge, float & score);
+  bool score(
+    const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge,
+    float & score);
 
   /**
    * @brief Provide the number of plugisn in the scorer loaded

nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp

@@ -22,6 +22,7 @@
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "nav2_route/types.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 
 namespace nav2_route
 {
@@ -61,7 +62,7 @@ class EdgeCostFunction
    * @param edge The edge pointer to score, which has access to the
    * start/end nodes and their associated metadata and actions
    */
-  virtual bool score(const EdgePtr edge, float & cost) = 0;
+  virtual bool score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, float & cost) = 0;
 
   /**
    * @brief Get name of the plugin for parameter scope mapping

nav2_route/include/nav2_route/plugins/edge_cost_functions/adjust_edges_scorer.hpp

@@ -61,7 +61,7 @@ class AdjustEdgesScorer : public EdgeCostFunction
    * @param cost of the edge scored
    * @return bool if this edge is open valid to traverse
    */
-  bool score(const EdgePtr edge, float & cost) override;
+  bool score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, float & cost) override;
 
   /**
    * @brief Get name of the plugin for parameter scope mapping

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

@@ -60,7 +60,7 @@ class CostmapScorer : public EdgeCostFunction
    * @param cost of the edge scored
    * @return bool if this edge is open valid to traverse
    */
-  bool score(const EdgePtr edge, float & cost) override;
+  bool score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, 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

@@ -58,7 +58,7 @@ class DistanceScorer : public EdgeCostFunction
    * @param cost of the edge scored
    * @return bool if this edge is open valid to traverse
    */
-  bool score(const EdgePtr edge, float & cost) override;
+  bool score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, 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

@@ -0,0 +1,81 @@
+// Copyright (c) 2024, 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__GOAl_ORIENTATION_SCORER_HPP_
+#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__GOAL_ORIENTATION_SCORER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "nav2_route/interfaces/edge_cost_function.hpp"
+#include "nav2_util/line_iterator.hpp"
+#include "nav2_util/node_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 GoalOrientationScorer
+ * @brief Scores final edge by comparing the
+ */
+class GoalOrientationScorer : public EdgeCostFunction
+{
+public:
+  /**
+   * @brief Constructor
+   */
+  GoalOrientationScorer() = default;
+
+  /**
+   * @brief destructor
+   */
+  virtual ~GoalOrientationScorer() = default;
+
+  /**
+   * @brief Configure
+   */
+  void configure(
+    const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+    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 geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, 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("GoalOrientationScorer")};
+  std::string name_;
+  double orientation_tolerance_;
+};
+
+}  // namespace nav2_route
+
+#endif  // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__GOAL_ORIENTATION_SCORER_HPP_

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

@@ -57,7 +57,7 @@ class PenaltyScorer : public EdgeCostFunction
    * @param cost of the edge scored
    * @return bool if this edge is open valid to traverse
    */
-  bool score(const EdgePtr edge, float & cost) override;
+  bool score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, 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

@@ -59,7 +59,7 @@ class SemanticScorer : public EdgeCostFunction
    * @param cost of the edge scored
    * @return bool if this edge is open valid to traverse
    */
-  bool score(const EdgePtr edge, float & cost) override;
+  bool score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, float & cost) override;
 
   /**
    * @brief Scores graph object based on metadata's semantic value at key

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

@@ -59,7 +59,7 @@ class TimeScorer : public EdgeCostFunction
    * @param cost of the edge scored
    * @return bool if this edge is open valid to traverse
    */
-  bool score(const EdgePtr edge, float & cost) override;
+  bool score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, float & cost) override;
 
   /**
    * @brief Get name of the plugin for parameter scope mapping

nav2_route/include/nav2_route/route_planner.hpp

@@ -26,6 +26,7 @@
 #include "nav2_route/utils.hpp"
 #include "nav2_route/edge_scorer.hpp"
 #include "nav2_core/route_exceptions.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 
 namespace nav2_route
 {
@@ -61,8 +62,9 @@ class RoutePlanner
    * @return Route object containing the navigation graph route
    */
   Route findRoute(
-    Graph & graph, unsigned int start, unsigned int goal,
-    const std::vector<unsigned int> & blocked_ids);
+    Graph & graph, unsigned int start_index, unsigned int goal_index,
+    const std::vector<unsigned int> & blocked_ids,
+    const geometry_msgs::msg::PoseStamped & goal);
 
 protected:
   /**
@@ -79,8 +81,9 @@ class RoutePlanner
    * @param blocked_ids A set of blocked node and edge IDs not to traverse
    */
   void findShortestGraphTraversal(
-    Graph & graph, const NodePtr start, const NodePtr goal,
-    const std::vector<unsigned int> & blocked_ids);
+    Graph & graph, const NodePtr start_node, const NodePtr goal_node,
+    const std::vector<unsigned int> & blocked_ids,
+    const geometry_msgs::msg::PoseStamped & goal);
 
   /**
    * @brief Gets the traversal cost for an edge using edge scorers
@@ -90,7 +93,8 @@ class RoutePlanner
    * @return if this edge is valid for search
    */
   inline bool getTraversalCost(
-    const EdgePtr edge, float & score, const std::vector<unsigned int> & blocked_ids);
+    const EdgePtr edge, float & score, const std::vector<unsigned int> & blocked_ids,
+    const geometry_msgs::msg::PoseStamped & goal);
 
   /**
    * @brief Gets the next node in the priority queue for search

nav2_route/plugins.xml

@@ -29,6 +29,11 @@
       <description>Cost function for adding costs to edges time to complete the edge based on previous runs and/or estimation with absolute speed limits.</description>
     </class>
   </library>
+  <library path="edge_scorers">
+    <class type="nav2_route::GoalOrientationScorer" base_class_type="nav2_route::EdgeCostFunction">
+      <description>Cost function for checking if the orientation of route matches orientation of the goal</description>
+    </class>
+  </library>
 
   <library path="route_operations">
     <class type="nav2_route::AdjustSpeedLimit" base_class_type="nav2_route::RouteOperation">

nav2_route/src/edge_scorer.cpp

@@ -60,7 +60,9 @@ EdgeScorer::EdgeScorer(nav2_util::LifecycleNode::SharedPtr node)
   }
 }
 
-bool EdgeScorer::score(const EdgePtr edge, float & total_score)
+bool EdgeScorer::score(
+  const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose,
+  bool final_edge, float & total_score)
 {
   total_score = 0.0;
   float curr_score = 0.0;
@@ -71,7 +73,7 @@ bool EdgeScorer::score(const EdgePtr edge, float & total_score)
 
   for (auto & plugin : plugins_) {
     curr_score = 0.0;
-    if (plugin->score(edge, curr_score)) {
+    if (plugin->score(edge, goal_pose, final_edge, curr_score)) {
       total_score += curr_score;
     } else {
       return false;

nav2_route/src/plugins/edge_cost_functions/adjust_edges_scorer.cpp

@@ -63,7 +63,7 @@ void AdjustEdgesScorer::closedEdgesCb(
   response->success = true;
 }
 
-bool AdjustEdgesScorer::score(const EdgePtr edge, float & cost)
+bool AdjustEdgesScorer::score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped & /* goal_pose */, bool /* final_edge */, float & cost)
 {
   // Find if this edge is in the closed set of edges
   if (closed_edges_.find(edge->edgeid) != closed_edges_.end()) {

nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp

@@ -74,7 +74,7 @@ void CostmapScorer::prepare()
 
 // TODO(sm) does this critic make efficiency sense at a
 // reasonable sized graph / node distance? Lower iterator density?
-bool CostmapScorer::score(const EdgePtr edge, float & cost)
+bool CostmapScorer::score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped & /* goal_pose */, bool /* final_edge */, float & cost)
 {
   if (!costmap_) {
     RCLCPP_WARN(logger_, "No costmap yet received!");

nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp

@@ -38,7 +38,7 @@ void DistanceScorer::configure(
   weight_ = static_cast<float>(node->get_parameter(getName() + ".weight").as_double());
 }
 
-bool DistanceScorer::score(const EdgePtr edge, float & cost)
+bool DistanceScorer::score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped & /* goal_pose */, bool /* final_edge */, float & cost)
 {
   // Get the speed limit, if set for an edge
   float speed_val = 1.0f;

nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp

@@ -0,0 +1,62 @@
+// Copyright (c) 2024, 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.
+
+#include <memory>
+#include <string>
+
+#include "nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp"
+
+namespace nav2_route
+{
+
+void GoalOrientationScorer::configure(
+  const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+  const std::string & name)
+{
+  RCLCPP_INFO(node->get_logger(), "Configuring goal orientation scorer.");
+  name_ = name;
+  logger_ = node->get_logger();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, getName() + ".orientation_tolerance", rclcpp::ParameterValue(M_PI/2.0));
+  orientation_tolerance_ = node->get_parameter(getName() + ".orientation_tolerance").as_double();
+}
+
+bool GoalOrientationScorer::score(
+  const EdgePtr edge,
+  const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, float & /*cost*/)
+{
+  if (final_edge) {
+    double edge_orientation = std::atan2(
+      edge->end->coords.y - edge->start->coords.y,
+      edge->end->coords.x - edge->start->coords.x);
+    double goal_orientation = tf2::getYaw(goal_pose.pose.orientation);
+    double d_yaw = std::abs(angles::shortest_angular_distance(edge_orientation, goal_orientation));
+
+    if (d_yaw > orientation_tolerance_) {
+      return false;
+    }
+  }
+  return true;
+}
+
+std::string GoalOrientationScorer::getName()
+{
+  return name_;
+}
+
+}  // namespace nav2_route
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_route::GoalOrientationScorer, nav2_route::EdgeCostFunction)

nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp

@@ -38,7 +38,7 @@ void PenaltyScorer::configure(
   weight_ = static_cast<float>(node->get_parameter(getName() + ".weight").as_double());
 }
 
-bool PenaltyScorer::score(const EdgePtr edge, float & cost)
+bool PenaltyScorer::score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped & /* goal_pose */, bool /* final_edge */, float & cost)
 {
   // Get the speed limit, if set for an edge
   float penalty_val = 0.0f;

nav2_route/src/plugins/edge_cost_functions/semantic_scorer.cpp

@@ -69,7 +69,7 @@ void SemanticScorer::metadataValueScorer(Metadata & mdata, float & score)
   }
 }
 
-bool SemanticScorer::score(const EdgePtr edge, float & cost)
+bool SemanticScorer::score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped & /* goal_pose */, bool /* final_edge */, float & cost)
 {
   float score = 0.0;
   Metadata & node_mdata = edge->end->metadata;

nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp

@@ -46,7 +46,7 @@ void TimeScorer::configure(
   max_vel_ = static_cast<float>(node->get_parameter(getName() + ".max_vel").as_double());
 }
 
-bool TimeScorer::score(const EdgePtr edge, float & cost)
+bool TimeScorer::score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped & /* goal_pose */, bool /* final_edge */, float & cost)
 {
   // We have a previous actual time to utilize for a refined estimate
   float time = 0.0;