From ff6af68b11b45d1e85904c45415027c1a367001a Mon Sep 17 00:00:00 2001 From: Alexander Yuen Date: Thu, 16 Jan 2025 21:06:15 +0000 Subject: [PATCH 01/11] added goal pose and bool to check for last edge for all scorers Signed-off-by: Alexander Yuen --- nav2_route/include/nav2_route/edge_scorer.hpp | 6 +- .../interfaces/edge_cost_function.hpp | 3 +- .../adjust_edges_scorer.hpp | 2 +- .../edge_cost_functions/costmap_scorer.hpp | 2 +- .../edge_cost_functions/distance_scorer.hpp | 2 +- .../edge_cost_functions/penalty_scorer.hpp | 2 +- .../edge_cost_functions/semantic_scorer.hpp | 2 +- .../edge_cost_functions/time_scorer.hpp | 2 +- .../include/nav2_route/route_planner.hpp | 13 ++-- nav2_route/src/edge_scorer.cpp | 4 +- .../adjust_edges_scorer.cpp | 2 +- .../edge_cost_functions/costmap_scorer.cpp | 2 +- .../edge_cost_functions/distance_scorer.cpp | 2 +- .../edge_cost_functions/penalty_scorer.cpp | 2 +- .../edge_cost_functions/semantic_scorer.cpp | 2 +- .../edge_cost_functions/time_scorer.cpp | 2 +- nav2_route/src/route_planner.cpp | 28 ++++---- nav2_route/src/route_server.cpp | 2 +- nav2_route/test/test_edge_scorers.cpp | 69 +++++++++++-------- nav2_route/test/test_route_planner.cpp | 24 ++++--- 20 files changed, 100 insertions(+), 73 deletions(-) diff --git a/nav2_route/include/nav2_route/edge_scorer.hpp b/nav2_route/include/nav2_route/edge_scorer.hpp index 944d8ab40c2..7582259a5bc 100644 --- a/nav2_route/include/nav2_route/edge_scorer.hpp +++ b/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,12 @@ 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, float & score, bool final_edge); /** * @brief Provide the number of plugisn in the scorer loaded diff --git a/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp b/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp index 6ac9545f864..924937c5cb6 100644 --- a/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp +++ b/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, float & cost, bool final_edge) = 0; /** * @brief Get name of the plugin for parameter scope mapping diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/adjust_edges_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/adjust_edges_scorer.hpp index e190694e7bc..2eab691155e 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/adjust_edges_scorer.hpp +++ b/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, float & cost, bool final_edge) override; /** * @brief Get name of the plugin for parameter scope mapping diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp index 758852a09ac..2364bd4fd1a 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp +++ b/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, float & cost, bool final_edge) override; /** * @brief Get name of the plugin for parameter scope mapping diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/distance_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/distance_scorer.hpp index 7f5a0947a4a..528f500c908 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/distance_scorer.hpp +++ b/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, float & cost, bool final_edge) override; /** * @brief Get name of the plugin for parameter scope mapping diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/penalty_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/penalty_scorer.hpp index ce3e641a708..af8c1d84e10 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/penalty_scorer.hpp +++ b/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, float & cost, bool final_edge) override; /** * @brief Get name of the plugin for parameter scope mapping diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/semantic_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/semantic_scorer.hpp index 79d4d0ff2da..69458c836ea 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/semantic_scorer.hpp +++ b/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, float & cost, bool final_edge) override; /** * @brief Scores graph object based on metadata's semantic value at key diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/time_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/time_scorer.hpp index bec6b100b6b..5f2f564ae05 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/time_scorer.hpp +++ b/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, float & cost, bool final_edge) override; /** * @brief Get name of the plugin for parameter scope mapping diff --git a/nav2_route/include/nav2_route/route_planner.hpp b/nav2_route/include/nav2_route/route_planner.hpp index 5c1179e1769..506c886c889 100644 --- a/nav2_route/include/nav2_route/route_planner.hpp +++ b/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 & blocked_ids); + Graph & graph, unsigned int start_index, unsigned int goal_index, + const std::vector & 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 & blocked_ids); + Graph & graph, const NodePtr start_node, const NodePtr goal_node, + const std::vector & blocked_ids, + const geometry_msgs::msg::PoseStamped goal); /** * @brief Gets the traversal cost for an edge using edge scorers @@ -90,7 +93,7 @@ class RoutePlanner * @return if this edge is valid for search */ inline bool getTraversalCost( - const EdgePtr edge, float & score, const std::vector & blocked_ids); + const EdgePtr edge, float & score, const std::vector & blocked_ids, const geometry_msgs::msg::PoseStamped goal); /** * @brief Gets the next node in the priority queue for search diff --git a/nav2_route/src/edge_scorer.cpp b/nav2_route/src/edge_scorer.cpp index 968ff52e40c..14569732aef 100644 --- a/nav2_route/src/edge_scorer.cpp +++ b/nav2_route/src/edge_scorer.cpp @@ -60,7 +60,7 @@ 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, float & total_score, bool final_edge) { total_score = 0.0; float curr_score = 0.0; @@ -71,7 +71,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, curr_score, final_edge)) { total_score += curr_score; } else { return false; diff --git a/nav2_route/src/plugins/edge_cost_functions/adjust_edges_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/adjust_edges_scorer.cpp index ef9846cbe04..ca9b3bf0469 100644 --- a/nav2_route/src/plugins/edge_cost_functions/adjust_edges_scorer.cpp +++ b/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 */, float & cost, bool /* final_edge */) { // Find if this edge is in the closed set of edges if (closed_edges_.find(edge->edgeid) != closed_edges_.end()) { diff --git a/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp index 1c8918f503f..932f7ffa5f1 100644 --- a/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp +++ b/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 */, float & cost, bool /* final_edge */) { if (!costmap_) { RCLCPP_WARN(logger_, "No costmap yet received!"); diff --git a/nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp index f7b72a687b0..1e2f429574b 100644 --- a/nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp @@ -38,7 +38,7 @@ void DistanceScorer::configure( weight_ = static_cast(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 */, float & cost, bool /* final_edge */) { // Get the speed limit, if set for an edge float speed_val = 1.0f; diff --git a/nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp index a1f4aeff640..b28114ea591 100644 --- a/nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp @@ -38,7 +38,7 @@ void PenaltyScorer::configure( weight_ = static_cast(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 */, float & cost, bool /* final_edge */) { // Get the speed limit, if set for an edge float penalty_val = 0.0f; diff --git a/nav2_route/src/plugins/edge_cost_functions/semantic_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/semantic_scorer.cpp index 56e28f6367d..9f8e1fd6085 100644 --- a/nav2_route/src/plugins/edge_cost_functions/semantic_scorer.cpp +++ b/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 */, float & cost, bool /* final_edge */) { float score = 0.0; Metadata & node_mdata = edge->end->metadata; diff --git a/nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp index 85fc65bdaf3..05de02a1194 100644 --- a/nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp @@ -46,7 +46,7 @@ void TimeScorer::configure( max_vel_ = static_cast(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 */, float & cost, bool /* final_edge */) { // We have a previous actual time to utilize for a refined estimate float time = 0.0; diff --git a/nav2_route/src/route_planner.cpp b/nav2_route/src/route_planner.cpp index 813ecfddd08..1b31b751419 100644 --- a/nav2_route/src/route_planner.cpp +++ b/nav2_route/src/route_planner.cpp @@ -38,8 +38,9 @@ void RoutePlanner::configure(nav2_util::LifecycleNode::SharedPtr node) } Route RoutePlanner::findRoute( - Graph & graph, unsigned int start, unsigned int goal, - const std::vector & blocked_ids) + Graph & graph, unsigned int start_index, unsigned int goal_index, + const std::vector & blocked_ids, + geometry_msgs::msg::PoseStamped goal_pose) { if (graph.empty()) { throw nav2_core::NoValidGraph("Graph is invalid for routing!"); @@ -48,9 +49,9 @@ Route RoutePlanner::findRoute( // Find the start and goal pointers, it is important in this function // that these are the actual pointers, so that copied addresses are // not lost in the route when this function goes out of scope. - const NodePtr & start_node = &graph.at(start); - const NodePtr & goal_node = &graph.at(goal); - findShortestGraphTraversal(graph, start_node, goal_node, blocked_ids); + const NodePtr & start_node = &graph.at(start_index); + const NodePtr & goal_node = &graph.at(goal_index); + findShortestGraphTraversal(graph, start_node, goal_node, blocked_ids, goal_pose); EdgePtr & parent_edge = goal_node->search_state.parent_edge; if (!parent_edge) { @@ -80,14 +81,15 @@ void RoutePlanner::resetSearchStates(Graph & graph) } void RoutePlanner::findShortestGraphTraversal( - Graph & graph, const NodePtr start, const NodePtr goal, - const std::vector & blocked_ids) + Graph & graph, const NodePtr start_node, const NodePtr goal_node, + const std::vector & blocked_ids, + geometry_msgs::msg::PoseStamped goal_pose) { // Setup the Dijkstra's search resetSearchStates(graph); - goal_id_ = goal->nodeid; - start->search_state.integrated_cost = 0.0; - addNode(0.0, start); + goal_id_ = goal_node->nodeid; + start_node->search_state.integrated_cost = 0.0; + addNode(0.0, start_node); NodePtr neighbor{nullptr}; EdgePtr edge{nullptr}; @@ -116,7 +118,7 @@ void RoutePlanner::findShortestGraphTraversal( neighbor = edge->end; // If edge is invalid (lane closed, occupied, etc), don't expand - if (!getTraversalCost(edge, traversal_cost, blocked_ids)) { + if (!getTraversalCost(edge, traversal_cost, blocked_ids, goal_pose)) { continue; } @@ -139,7 +141,7 @@ void RoutePlanner::findShortestGraphTraversal( } bool RoutePlanner::getTraversalCost( - const EdgePtr edge, float & score, const std::vector & blocked_ids) + const EdgePtr edge, float & score, const std::vector & blocked_ids, const geometry_msgs::msg::PoseStamped goal) { // If edge or node is in the blocked list, as long as its not blocking the goal itself auto idBlocked = [&](unsigned int id) {return id == edge->edgeid || id == edge->end->nodeid;}; @@ -158,7 +160,7 @@ bool RoutePlanner::getTraversalCost( return true; } - return edge_scorer_->score(edge, score); + return edge_scorer_->score(edge, goal, score, isGoal(edge->end)); } NodeElement RoutePlanner::getNextNode() diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp index 86c1a19d923..da3f539bd22 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -215,7 +215,7 @@ Route RouteServer::findRoute( route.start_node = &graph_.at(start_route); } else { // Compute the route via graph-search, returns a node-edge sequence - route = route_planner_->findRoute(graph_, start_route, end_route, rerouting_info.blocked_ids); + route = route_planner_->findRoute(graph_, start_route, end_route, rerouting_info.blocked_ids, goal->goal); } return goal_intent_extractor_->pruneStartandGoal(route, goal, rerouting_info); diff --git a/nav2_route/test/test_edge_scorers.cpp b/nav2_route/test/test_edge_scorers.cpp index df20f397d69..6484de4f0c0 100644 --- a/nav2_route/test/test_edge_scorers.cpp +++ b/nav2_route/test/test_edge_scorers.cpp @@ -58,19 +58,20 @@ TEST(EdgeScorersTest, test_api) edge.edgeid = 10; edge.start = &n1; edge.end = &n2; + const geometry_msgs::msg::PoseStamped goal_pose; float traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 0.0); // Because nodes coords are 0/0 n1.coords.x = 1.0; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 1.0); // Distance is now 1m // For full coverage, add in a speed limit tag to make sure it is applied appropriately float speed_limit = 0.8f; edge.metadata.setValue("speed_limit", speed_limit); - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 1.25); // 1m / 0.8 = 1.25 } @@ -125,15 +126,17 @@ TEST(EdgeScorersTest, test_invalid_edge_scoring) edge.start = &n1; edge.end = &n2; + const geometry_msgs::msg::PoseStamped goal_pose; + // The score function should return false since closed float traversal_cost = -1; - EXPECT_FALSE(scorer.score(&edge, traversal_cost)); + EXPECT_FALSE(scorer.score(&edge, goal_pose, traversal_cost, false)); // The score function should return true since no longer the problematic edge ID // and edgeid 42 as the dynamic cost of 42 assigned to it traversal_cost = -1; edge.edgeid = 11; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 42.0); // Try to re-open this edge @@ -145,7 +148,7 @@ TEST(EdgeScorersTest, test_invalid_edge_scoring) // The score function should return true since now opened up traversal_cost = -1; edge.edgeid = 10; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); node_thread.reset(); } @@ -163,6 +166,7 @@ TEST(EdgeScorersTest, test_penalty_scoring) EdgeScorer scorer(node); EXPECT_EQ(scorer.numPlugins(), 1); // PenaltyScorer + const geometry_msgs::msg::PoseStamped goal_pose; // Create edge to score Node n1, n2; @@ -179,7 +183,7 @@ TEST(EdgeScorersTest, test_penalty_scoring) // The score function should return 10.0 from penalty value float traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 10.0); } @@ -208,10 +212,11 @@ TEST(EdgeScorersTest, test_costmap_scoring) edge.edgeid = 10; edge.start = &n1; edge.end = &n2; + const geometry_msgs::msg::PoseStamped goal_pose; // The score function should return false because no costmap given float traversal_cost = -1; - EXPECT_FALSE(scorer.score(&edge, traversal_cost)); + EXPECT_FALSE(scorer.score(&edge, goal_pose, traversal_cost, false)); // Create a demo costmap: * = 100, - = 0, / = 254 // * * * * - - - - - - - - @@ -251,7 +256,7 @@ TEST(EdgeScorersTest, test_costmap_scoring) n2.coords.x = 8.0; n2.coords.y = 8.0; traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); // Segment in freespace EXPECT_EQ(traversal_cost, 0.0); @@ -260,7 +265,7 @@ TEST(EdgeScorersTest, test_costmap_scoring) n2.coords.x = 2.0; n2.coords.y = 8.0; traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); // Segment in 100 space EXPECT_NEAR(traversal_cost, 100.0 / 254.0, 0.01); @@ -270,14 +275,14 @@ TEST(EdgeScorersTest, test_costmap_scoring) n2.coords.y = 5.9; traversal_cost = -1; // Segment in lethal space, won't fill in - EXPECT_FALSE(scorer.score(&edge, traversal_cost)); + EXPECT_FALSE(scorer.score(&edge, goal_pose, traversal_cost, false)); n1.coords.x = 1.0; n1.coords.y = 1.0; n2.coords.x = 6.0; n2.coords.y = 1.0; traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); // Segment in 0 and 100 space, use_max so 100 (normalized) EXPECT_NEAR(traversal_cost, 100.0 / 254.0, 0.01); @@ -287,7 +292,7 @@ TEST(EdgeScorersTest, test_costmap_scoring) n2.coords.y = 11.0; traversal_cost = -1; // Off map, so invalid - EXPECT_FALSE(scorer.score(&edge, traversal_cost)); + EXPECT_FALSE(scorer.score(&edge, goal_pose, traversal_cost, false)); node_thread.reset(); } @@ -357,6 +362,8 @@ TEST(EdgeScorersTest, test_costmap_scoring_alt_profile) rclcpp::Rate r(1); r.sleep(); + const geometry_msgs::msg::PoseStamped goal_pose; + // Off map n1.coords.x = -1.0; n1.coords.y = -1.0; @@ -364,7 +371,7 @@ TEST(EdgeScorersTest, test_costmap_scoring_alt_profile) n2.coords.y = 11.0; float traversal_cost = -1; // Off map, so cannot score - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 0.0); n1.coords.x = 4.1; @@ -373,7 +380,7 @@ TEST(EdgeScorersTest, test_costmap_scoring_alt_profile) n2.coords.y = 5.9; traversal_cost = -1; // Segment in lethal space, so score is maximum (1) - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_NEAR(traversal_cost, 1.0, 0.01); n1.coords.x = 1.0; @@ -381,7 +388,7 @@ TEST(EdgeScorersTest, test_costmap_scoring_alt_profile) n2.coords.x = 6.0; n2.coords.y = 1.0; traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); // Segment in 0 and 100 space, 3m @ 100, 2m @ 0, averaged is 60 EXPECT_NEAR(traversal_cost, 60.0 / 254.0, 0.01); @@ -415,28 +422,30 @@ TEST(EdgeScorersTest, test_time_scoring) float time_taken = 10.0f; edge.metadata.setValue("abs_time_taken", time_taken); + const geometry_msgs::msg::PoseStamped goal_pose; + // The score function should return 10.0 from time taken float traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 10.0); // 10.0 * 1.0 weight // Without time taken or abs speed limit set, uses default max speed of 0.5 m/s edge.metadata.data.clear(); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 2.0); // 1.0 m / 0.5 m/s * 1.0 weight // Use speed limit if set float speed_limit = 0.85; edge.metadata.setValue("abs_speed_limit", speed_limit); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_NEAR(traversal_cost, 1.1764, 0.001); // 1.0 m / 0.85 m/s * 1.0 weight // Still use time taken measurements if given first edge.metadata.setValue("abs_time_taken", time_taken); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 10.0); // 10.0 * 1.0 weight } @@ -468,6 +477,8 @@ TEST(EdgeScorersTest, test_semantic_scoring_key) EdgeScorer scorer(node); EXPECT_EQ(scorer.numPlugins(), 1); // SemanticScorer + const geometry_msgs::msg::PoseStamped goal_pose; + // Create edge to score Node n1, n2; n1.nodeid = 1; @@ -481,21 +492,21 @@ TEST(EdgeScorersTest, test_semantic_scoring_key) // Should fail, since both nothing under key `class` nor metadata set at all float traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 0.0); // nothing is set in semantics // Should be valid under the right key std::string test_n = "Test1"; edge.metadata.setValue("class", test_n); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 1.0); // 1.0 * 1.0 weight test_n = "Test2"; edge.metadata.setValue("class", test_n); n2.metadata.setValue("class", test_n); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 4.0); // (2.0 + 2.0) * 1.0 weight // Cannot find, doesn't exist @@ -503,7 +514,7 @@ TEST(EdgeScorersTest, test_semantic_scoring_key) edge.metadata.setValue("class", test_n); n2.metadata.setValue("class", test_n); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 0.0); // 0.0 * 1.0 weight } @@ -549,9 +560,11 @@ TEST(EdgeScorersTest, test_semantic_scoring_keys) edge.start = &n1; edge.end = &n2; + const geometry_msgs::msg::PoseStamped goal_pose; + // Should fail, since both nothing under key `class` nor metadata set at all float traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 0.0); // nothing is set in semantics // Should fail, since under the class key when the semantic key is empty string @@ -559,7 +572,7 @@ TEST(EdgeScorersTest, test_semantic_scoring_keys) std::string test_n = "Test1"; edge.metadata.setValue("class", test_n); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 0.0); // 0.0 * 1.0 weight // Should succeed, since now actual class is a key, not a value of the `class` key @@ -567,7 +580,7 @@ TEST(EdgeScorersTest, test_semantic_scoring_keys) edge.metadata.setValue(test_n, test_n); n2.metadata.setValue(test_n, test_n); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 4.0); // (2.0 + 2.0) * 1.0 weight // Cannot find, doesn't exist @@ -576,6 +589,6 @@ TEST(EdgeScorersTest, test_semantic_scoring_keys) test_n = "Test4"; edge.metadata.setValue(test_n, test_n); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 0.0); // 0.0 * 1.0 weight } diff --git a/nav2_route/test/test_route_planner.cpp b/nav2_route/test/test_route_planner.cpp index 219a85954da..b54ad4e70d9 100644 --- a/nav2_route/test/test_route_planner.cpp +++ b/nav2_route/test/test_route_planner.cpp @@ -21,6 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_route/route_planner.hpp" +#include "nav2_msgs/action/compute_route.hpp" class RclCppFixture { @@ -126,6 +127,9 @@ inline Graph create4x4Graph() TEST(RoutePlannerTest, test_route_planner_positive) { + + geometry_msgs::msg::PoseStamped goal_pose; + auto node = std::make_shared("router_test"); RoutePlanner planner; planner.configure(node); @@ -138,7 +142,7 @@ TEST(RoutePlannerTest, test_route_planner_positive) // Plan across diagonal, should be length 6 start = 0u; goal = 15u; - Route route = planner.findRoute(graph, start, goal, blocked_ids); + Route route = planner.findRoute(graph, start, goal, blocked_ids, goal_pose); EXPECT_NEAR(route.route_cost, 6.0, 0.001); EXPECT_EQ(route.edges.size(), 6u); @@ -147,19 +151,19 @@ TEST(RoutePlannerTest, test_route_planner_positive) start = 15; goal = 0; EXPECT_THROW( - planner.findRoute(graph, start, goal, blocked_ids), nav2_core::NoValidRouteCouldBeFound); + planner.findRoute(graph, start, goal, blocked_ids, goal_pose), nav2_core::NoValidRouteCouldBeFound); // Lets find a simple plan and then mess with the planner with blocking edges start = 0; goal = 12; - route = planner.findRoute(graph, start, goal, blocked_ids); + route = planner.findRoute(graph, start, goal, blocked_ids, goal_pose); EXPECT_NEAR(route.route_cost, 3.0, 0.001); EXPECT_EQ(route.edges.size(), 3u); // Now block an edge as closed along the chain, should find the next best path unsigned int key_edgeid = 19u; blocked_ids.push_back(key_edgeid); // Edge between node 0-4 in the 0-4-9-12 chain - route = planner.findRoute(graph, start, goal, blocked_ids); + route = planner.findRoute(graph, start, goal, blocked_ids, goal_pose); EXPECT_NEAR(route.route_cost, 5.0, 0.001); EXPECT_EQ(route.edges.size(), 5u); for (auto & edge : route.edges) { @@ -171,13 +175,15 @@ TEST(RoutePlannerTest, test_route_planner_positive) blocked_ids.clear(); graph[0].neighbors[1].edge_cost.cost = 1e6; graph[0].neighbors[1].edge_cost.overridable = false; - route = planner.findRoute(graph, start, goal, blocked_ids); + route = planner.findRoute(graph, start, goal, blocked_ids, goal_pose); EXPECT_NEAR(route.route_cost, 5.0, 0.001); EXPECT_EQ(route.edges.size(), 5u); } TEST(RoutePlannerTest, test_route_planner_negative) -{ +{ + geometry_msgs::msg::PoseStamped goal_pose; + auto node = std::make_shared("router_test"); node->declare_parameter("max_iterations", rclcpp::ParameterValue(5)); RoutePlanner planner; @@ -188,17 +194,17 @@ TEST(RoutePlannerTest, test_route_planner_negative) Graph graph; // No graph yet, should fail - EXPECT_THROW(planner.findRoute(graph, start, goal, blocked_ids), nav2_core::NoValidGraph); + EXPECT_THROW(planner.findRoute(graph, start, goal, blocked_ids, goal_pose), nav2_core::NoValidGraph); // Create a graph to test routing upon. graph = create4x4Graph(); // Try with a stupidly low number of iterations graph[0].neighbors[1].edge_cost.overridable = true; - EXPECT_THROW(planner.findRoute(graph, start, goal, blocked_ids), nav2_core::TimedOut); + EXPECT_THROW(planner.findRoute(graph, start, goal, blocked_ids, goal_pose), nav2_core::TimedOut); // If we try to plan but we both cannot override and has 0 cost, will throw graph[0].neighbors[1].edge_cost.overridable = false; graph[0].neighbors[1].edge_cost.cost = 0.0; - EXPECT_THROW(planner.findRoute(graph, start, goal, blocked_ids), nav2_core::NoValidGraph); + EXPECT_THROW(planner.findRoute(graph, start, goal, blocked_ids, goal_pose), nav2_core::NoValidGraph); } From 822da7867f969f3e5ecd5ec98262e1f6a51d1037 Mon Sep 17 00:00:00 2001 From: Alexander Yuen Date: Sat, 18 Jan 2025 07:28:41 +0000 Subject: [PATCH 02/11] added goal_orientation scorer Signed-off-by: Alexander Yuen --- nav2_route/CMakeLists.txt | 1 + .../goal_orientation_scorer.hpp | 80 +++++++++++++++++++ nav2_route/plugins.xml | 5 ++ .../goal_orientation_scorer.cpp | 64 +++++++++++++++ 4 files changed, 150 insertions(+) create mode 100644 nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp create mode 100644 nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index f4a4a477db9..fc88c72baa7 100644 --- a/nav2_route/CMakeLists.txt +++ b/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 diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp new file mode 100644 index 00000000000..b64593815db --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp @@ -0,0 +1,80 @@ +// 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 +#include + +#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" + +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, float & cost, bool final_edge) 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_ \ No newline at end of file diff --git a/nav2_route/plugins.xml b/nav2_route/plugins.xml index 946c2eaee59..e392ef379ae 100644 --- a/nav2_route/plugins.xml +++ b/nav2_route/plugins.xml @@ -29,6 +29,11 @@ Cost function for adding costs to edges time to complete the edge based on previous runs and/or estimation with absolute speed limits. + + + Cost function for checking if the orientation of route matches orientation of the goal + + diff --git a/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp new file mode 100644 index 00000000000..9de91b35423 --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp @@ -0,0 +1,64 @@ +// 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 +#include + +#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(2*M_PI)); + orientation_tolerance_ = node->get_parameter(getName() + ".orientation_tolerance").as_double(); + +} + +bool GoalOrientationScorer::score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped goal_pose, float & cost, bool final_edge) +{ + if(final_edge){ + + double d_yaw = std::abs(std::atan2(edge->end->coords.y - edge->start->coords.y, edge->end->coords.x - edge->start->coords.x)-tf2::getYaw(goal_pose.pose.orientation)); + + if(d_yaw > M_PI){ + d_yaw = 2*M_PI-d_yaw; + } + + if(d_yaw > orientation_tolerance_){ + cost = 255.0; + 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) From 29ee71e3d37121c758e8aca4c1e970aaeb91c537 Mon Sep 17 00:00:00 2001 From: Alexander Yuen Date: Sat, 18 Jan 2025 07:44:30 +0000 Subject: [PATCH 03/11] added test for GoalOrientationScorer Signed-off-by: Alexander Yuen --- nav2_route/test/test_edge_scorers.cpp | 43 +++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/nav2_route/test/test_edge_scorers.cpp b/nav2_route/test/test_edge_scorers.cpp index 6484de4f0c0..acba666923f 100644 --- a/nav2_route/test/test_edge_scorers.cpp +++ b/nav2_route/test/test_edge_scorers.cpp @@ -592,3 +592,46 @@ TEST(EdgeScorersTest, test_semantic_scoring_keys) EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); EXPECT_EQ(traversal_cost, 0.0); // 0.0 * 1.0 weight } + +TEST(EdgeScorersTest, test_goal_orientation_scoring) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"GoalOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::GoalOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.orientation_tolerance", + rclcpp::ParameterValue(1.57)); + + EdgeScorer scorer(node); + EXPECT_EQ(scorer.numPlugins(), 1); // GoalOrientationScorer + + geometry_msgs::msg::PoseStamped goal_pose; + goal_pose.pose.orientation.x = 0.0; + goal_pose.pose.orientation.y = 0.0; + goal_pose.pose.orientation.z = 0.0; + goal_pose.pose.orientation.w = 1.0; + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 0.0; + n1.coords.y = 0.0; + n2.coords.x = 1.0; + n2.coords.y = 0.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + // The score function should return 10.0 from penalty value + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, true)); + // EXPECT_EQ(traversal_cost, 10.0); +} \ No newline at end of file From b83594cd053b3afd6f107f7130bf56addd628466 Mon Sep 17 00:00:00 2001 From: Alexander Yuen Date: Wed, 22 Jan 2025 17:11:44 +0000 Subject: [PATCH 04/11] changed goal pose to a const ref, and moved score to end as implicit return Signed-off-by: Alexander Yuen --- nav2_route/include/nav2_route/edge_scorer.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/nav2_route/include/nav2_route/edge_scorer.hpp b/nav2_route/include/nav2_route/edge_scorer.hpp index 7582259a5bc..58f1a68407c 100644 --- a/nav2_route/include/nav2_route/edge_scorer.hpp +++ b/nav2_route/include/nav2_route/edge_scorer.hpp @@ -62,7 +62,9 @@ class EdgeScorer * @param final_edge whether this edge brings us to the goal or not * @return If edge is valid */ - bool score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped goal_pose, float & score, bool final_edge); + 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 From 3ed9bc42269538066a2c6bbcd2ab69542f0565c4 Mon Sep 17 00:00:00 2001 From: Alexander Yuen Date: Wed, 22 Jan 2025 17:13:22 +0000 Subject: [PATCH 05/11] changed goal arguments to const ref Signed-off-by: Alexander Yuen --- nav2_route/include/nav2_route/route_planner.hpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/nav2_route/include/nav2_route/route_planner.hpp b/nav2_route/include/nav2_route/route_planner.hpp index 506c886c889..4d3c27b303b 100644 --- a/nav2_route/include/nav2_route/route_planner.hpp +++ b/nav2_route/include/nav2_route/route_planner.hpp @@ -64,7 +64,7 @@ class RoutePlanner Route findRoute( Graph & graph, unsigned int start_index, unsigned int goal_index, const std::vector & blocked_ids, - const geometry_msgs::msg::PoseStamped goal); + const geometry_msgs::msg::PoseStamped & goal); protected: /** @@ -83,7 +83,7 @@ class RoutePlanner void findShortestGraphTraversal( Graph & graph, const NodePtr start_node, const NodePtr goal_node, const std::vector & blocked_ids, - const geometry_msgs::msg::PoseStamped goal); + const geometry_msgs::msg::PoseStamped & goal); /** * @brief Gets the traversal cost for an edge using edge scorers @@ -93,7 +93,8 @@ class RoutePlanner * @return if this edge is valid for search */ inline bool getTraversalCost( - const EdgePtr edge, float & score, const std::vector & blocked_ids, const geometry_msgs::msg::PoseStamped goal); + const EdgePtr edge, float & score, const std::vector & blocked_ids, + const geometry_msgs::msg::PoseStamped & goal); /** * @brief Gets the next node in the priority queue for search From 054a5561137ef7c82655ff1997b5aeb60c4bd7d0 Mon Sep 17 00:00:00 2001 From: Alexander Yuen Date: Wed, 22 Jan 2025 17:14:50 +0000 Subject: [PATCH 06/11] using const ref for goal pose, rearranged total_score to match header Signed-off-by: Alexander Yuen --- nav2_route/src/edge_scorer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/nav2_route/src/edge_scorer.cpp b/nav2_route/src/edge_scorer.cpp index 14569732aef..00101930cbd 100644 --- a/nav2_route/src/edge_scorer.cpp +++ b/nav2_route/src/edge_scorer.cpp @@ -60,7 +60,9 @@ EdgeScorer::EdgeScorer(nav2_util::LifecycleNode::SharedPtr node) } } -bool EdgeScorer::score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped goal_pose, float & total_score, bool final_edge) +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; From 205d81d7994d5049268dfcaadc697a61ca3c8d11 Mon Sep 17 00:00:00 2001 From: Alexander Yuen Date: Wed, 22 Jan 2025 17:15:58 +0000 Subject: [PATCH 07/11] linting on goal_orientation_scorer.hpp Signed-off-by: Alexander Yuen --- .../edge_cost_functions/goal_orientation_scorer.hpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp index b64593815db..f7f5149dc63 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp @@ -26,13 +26,14 @@ #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 + * @brief Scores final edge by comparing the */ class GoalOrientationScorer : public EdgeCostFunction { @@ -61,7 +62,9 @@ class GoalOrientationScorer : public EdgeCostFunction * @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, float & cost, bool final_edge) override; + bool score( + const EdgePtr edge, const geometry_msgs::msg::PoseStamped goal_pose, float & cost, + bool final_edge) override; /** * @brief Get name of the plugin for parameter scope mapping @@ -77,4 +80,4 @@ class GoalOrientationScorer : public EdgeCostFunction } // namespace nav2_route -#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__GOAL_ORIENTATION_SCORER_HPP_ \ No newline at end of file +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__GOAL_ORIENTATION_SCORER_HPP_ From 606b9b473e091b1c92fcbd8b131c03b2a824b628 Mon Sep 17 00:00:00 2001 From: Alexander Yuen Date: Wed, 22 Jan 2025 17:17:38 +0000 Subject: [PATCH 08/11] using M_PI as default threshold, fixed angle wrapping by using angles library, no longer modifying cost Signed-off-by: Alexander Yuen --- .../goal_orientation_scorer.cpp | 27 +++++++++---------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp index 9de91b35423..0c49d1c8b83 100644 --- a/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp @@ -29,26 +29,25 @@ void GoalOrientationScorer::configure( logger_ = node->get_logger(); nav2_util::declare_parameter_if_not_declared( - node, getName() + ".orientation_tolerance", rclcpp::ParameterValue(2*M_PI)); + node, getName() + ".orientation_tolerance", rclcpp::ParameterValue(M_PI)); orientation_tolerance_ = node->get_parameter(getName() + ".orientation_tolerance").as_double(); - } -bool GoalOrientationScorer::score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped goal_pose, float & cost, bool final_edge) +bool GoalOrientationScorer::score( + const EdgePtr edge, + const geometry_msgs::msg::PoseStamped goal_pose, float & /*cost*/, + bool final_edge) { - if(final_edge){ - - double d_yaw = std::abs(std::atan2(edge->end->coords.y - edge->start->coords.y, edge->end->coords.x - edge->start->coords.x)-tf2::getYaw(goal_pose.pose.orientation)); - - if(d_yaw > M_PI){ - d_yaw = 2*M_PI-d_yaw; - } - - if(d_yaw > orientation_tolerance_){ - cost = 255.0; + 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; } From b09a6c738f89499b9a5443d999935e0c07b059b4 Mon Sep 17 00:00:00 2001 From: Alexander Yuen Date: Wed, 22 Jan 2025 17:19:15 +0000 Subject: [PATCH 09/11] changed arguments to use const refs, changed argument order in score function to matach header Signed-off-by: Alexander Yuen --- nav2_route/src/route_planner.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/nav2_route/src/route_planner.cpp b/nav2_route/src/route_planner.cpp index 1b31b751419..c46df666a11 100644 --- a/nav2_route/src/route_planner.cpp +++ b/nav2_route/src/route_planner.cpp @@ -40,7 +40,7 @@ void RoutePlanner::configure(nav2_util::LifecycleNode::SharedPtr node) Route RoutePlanner::findRoute( Graph & graph, unsigned int start_index, unsigned int goal_index, const std::vector & blocked_ids, - geometry_msgs::msg::PoseStamped goal_pose) + const geometry_msgs::msg::PoseStamped & goal_pose) { if (graph.empty()) { throw nav2_core::NoValidGraph("Graph is invalid for routing!"); @@ -83,7 +83,7 @@ void RoutePlanner::resetSearchStates(Graph & graph) void RoutePlanner::findShortestGraphTraversal( Graph & graph, const NodePtr start_node, const NodePtr goal_node, const std::vector & blocked_ids, - geometry_msgs::msg::PoseStamped goal_pose) + const geometry_msgs::msg::PoseStamped & goal_pose) { // Setup the Dijkstra's search resetSearchStates(graph); @@ -141,7 +141,8 @@ void RoutePlanner::findShortestGraphTraversal( } bool RoutePlanner::getTraversalCost( - const EdgePtr edge, float & score, const std::vector & blocked_ids, const geometry_msgs::msg::PoseStamped goal) + const EdgePtr edge, float & score, const std::vector & blocked_ids, + const geometry_msgs::msg::PoseStamped & goal) { // If edge or node is in the blocked list, as long as its not blocking the goal itself auto idBlocked = [&](unsigned int id) {return id == edge->edgeid || id == edge->end->nodeid;}; @@ -160,7 +161,7 @@ bool RoutePlanner::getTraversalCost( return true; } - return edge_scorer_->score(edge, goal, score, isGoal(edge->end)); + return edge_scorer_->score(edge, goal, isGoal(edge->end), score); } NodeElement RoutePlanner::getNextNode() From aa62ddf4144f752b000f36ca4d4d21f7a0aa030a Mon Sep 17 00:00:00 2001 From: Alexander Yuen Date: Wed, 22 Jan 2025 17:21:52 +0000 Subject: [PATCH 10/11] changed calling of score to match argument sequence, changed GoalOrientaitonScorer to test the opposite direction and check the return value Signed-off-by: Alexander Yuen --- nav2_route/test/test_edge_scorers.cpp | 76 +++++++++++++++------------ 1 file changed, 41 insertions(+), 35 deletions(-) diff --git a/nav2_route/test/test_edge_scorers.cpp b/nav2_route/test/test_edge_scorers.cpp index acba666923f..fdbee591334 100644 --- a/nav2_route/test/test_edge_scorers.cpp +++ b/nav2_route/test/test_edge_scorers.cpp @@ -61,17 +61,17 @@ TEST(EdgeScorersTest, test_api) const geometry_msgs::msg::PoseStamped goal_pose; float traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 0.0); // Because nodes coords are 0/0 n1.coords.x = 1.0; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 1.0); // Distance is now 1m // For full coverage, add in a speed limit tag to make sure it is applied appropriately float speed_limit = 0.8f; edge.metadata.setValue("speed_limit", speed_limit); - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 1.25); // 1m / 0.8 = 1.25 } @@ -126,17 +126,17 @@ TEST(EdgeScorersTest, test_invalid_edge_scoring) edge.start = &n1; edge.end = &n2; - const geometry_msgs::msg::PoseStamped goal_pose; + const geometry_msgs::msg::PoseStamped goal_pose; // The score function should return false since closed float traversal_cost = -1; - EXPECT_FALSE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_FALSE(scorer.score(&edge, goal_pose, false, traversal_cost)); // The score function should return true since no longer the problematic edge ID // and edgeid 42 as the dynamic cost of 42 assigned to it traversal_cost = -1; edge.edgeid = 11; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 42.0); // Try to re-open this edge @@ -148,7 +148,7 @@ TEST(EdgeScorersTest, test_invalid_edge_scoring) // The score function should return true since now opened up traversal_cost = -1; edge.edgeid = 10; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); node_thread.reset(); } @@ -183,7 +183,7 @@ TEST(EdgeScorersTest, test_penalty_scoring) // The score function should return 10.0 from penalty value float traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 10.0); } @@ -216,7 +216,7 @@ TEST(EdgeScorersTest, test_costmap_scoring) // The score function should return false because no costmap given float traversal_cost = -1; - EXPECT_FALSE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_FALSE(scorer.score(&edge, goal_pose, false, traversal_cost)); // Create a demo costmap: * = 100, - = 0, / = 254 // * * * * - - - - - - - - @@ -256,7 +256,7 @@ TEST(EdgeScorersTest, test_costmap_scoring) n2.coords.x = 8.0; n2.coords.y = 8.0; traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); // Segment in freespace EXPECT_EQ(traversal_cost, 0.0); @@ -265,7 +265,7 @@ TEST(EdgeScorersTest, test_costmap_scoring) n2.coords.x = 2.0; n2.coords.y = 8.0; traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); // Segment in 100 space EXPECT_NEAR(traversal_cost, 100.0 / 254.0, 0.01); @@ -275,14 +275,14 @@ TEST(EdgeScorersTest, test_costmap_scoring) n2.coords.y = 5.9; traversal_cost = -1; // Segment in lethal space, won't fill in - EXPECT_FALSE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_FALSE(scorer.score(&edge, goal_pose, false, traversal_cost)); n1.coords.x = 1.0; n1.coords.y = 1.0; n2.coords.x = 6.0; n2.coords.y = 1.0; traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); // Segment in 0 and 100 space, use_max so 100 (normalized) EXPECT_NEAR(traversal_cost, 100.0 / 254.0, 0.01); @@ -292,7 +292,7 @@ TEST(EdgeScorersTest, test_costmap_scoring) n2.coords.y = 11.0; traversal_cost = -1; // Off map, so invalid - EXPECT_FALSE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_FALSE(scorer.score(&edge, goal_pose, false, traversal_cost)); node_thread.reset(); } @@ -371,7 +371,7 @@ TEST(EdgeScorersTest, test_costmap_scoring_alt_profile) n2.coords.y = 11.0; float traversal_cost = -1; // Off map, so cannot score - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 0.0); n1.coords.x = 4.1; @@ -380,7 +380,7 @@ TEST(EdgeScorersTest, test_costmap_scoring_alt_profile) n2.coords.y = 5.9; traversal_cost = -1; // Segment in lethal space, so score is maximum (1) - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_NEAR(traversal_cost, 1.0, 0.01); n1.coords.x = 1.0; @@ -388,7 +388,7 @@ TEST(EdgeScorersTest, test_costmap_scoring_alt_profile) n2.coords.x = 6.0; n2.coords.y = 1.0; traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); // Segment in 0 and 100 space, 3m @ 100, 2m @ 0, averaged is 60 EXPECT_NEAR(traversal_cost, 60.0 / 254.0, 0.01); @@ -426,26 +426,26 @@ TEST(EdgeScorersTest, test_time_scoring) // The score function should return 10.0 from time taken float traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 10.0); // 10.0 * 1.0 weight // Without time taken or abs speed limit set, uses default max speed of 0.5 m/s edge.metadata.data.clear(); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 2.0); // 1.0 m / 0.5 m/s * 1.0 weight // Use speed limit if set float speed_limit = 0.85; edge.metadata.setValue("abs_speed_limit", speed_limit); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_NEAR(traversal_cost, 1.1764, 0.001); // 1.0 m / 0.85 m/s * 1.0 weight // Still use time taken measurements if given first edge.metadata.setValue("abs_time_taken", time_taken); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 10.0); // 10.0 * 1.0 weight } @@ -492,21 +492,21 @@ TEST(EdgeScorersTest, test_semantic_scoring_key) // Should fail, since both nothing under key `class` nor metadata set at all float traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 0.0); // nothing is set in semantics // Should be valid under the right key std::string test_n = "Test1"; edge.metadata.setValue("class", test_n); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 1.0); // 1.0 * 1.0 weight test_n = "Test2"; edge.metadata.setValue("class", test_n); n2.metadata.setValue("class", test_n); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 4.0); // (2.0 + 2.0) * 1.0 weight // Cannot find, doesn't exist @@ -514,7 +514,7 @@ TEST(EdgeScorersTest, test_semantic_scoring_key) edge.metadata.setValue("class", test_n); n2.metadata.setValue("class", test_n); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 0.0); // 0.0 * 1.0 weight } @@ -564,7 +564,7 @@ TEST(EdgeScorersTest, test_semantic_scoring_keys) // Should fail, since both nothing under key `class` nor metadata set at all float traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 0.0); // nothing is set in semantics // Should fail, since under the class key when the semantic key is empty string @@ -572,7 +572,7 @@ TEST(EdgeScorersTest, test_semantic_scoring_keys) std::string test_n = "Test1"; edge.metadata.setValue("class", test_n); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 0.0); // 0.0 * 1.0 weight // Should succeed, since now actual class is a key, not a value of the `class` key @@ -580,7 +580,7 @@ TEST(EdgeScorersTest, test_semantic_scoring_keys) edge.metadata.setValue(test_n, test_n); n2.metadata.setValue(test_n, test_n); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 4.0); // (2.0 + 2.0) * 1.0 weight // Cannot find, doesn't exist @@ -589,7 +589,7 @@ TEST(EdgeScorersTest, test_semantic_scoring_keys) test_n = "Test4"; edge.metadata.setValue(test_n, test_n); traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, false)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); EXPECT_EQ(traversal_cost, 0.0); // 0.0 * 1.0 weight } @@ -599,7 +599,8 @@ TEST(EdgeScorersTest, test_goal_orientation_scoring) auto node = std::make_shared("edge_scorer_test"); node->declare_parameter( - "edge_cost_functions", rclcpp::ParameterValue(std::vector{"GoalOrientationScorer"})); + "edge_cost_functions", + rclcpp::ParameterValue(std::vector{"GoalOrientationScorer"})); nav2_util::declare_parameter_if_not_declared( node, "GoalOrientationScorer.plugin", rclcpp::ParameterValue(std::string{"nav2_route::GoalOrientationScorer"})); @@ -609,7 +610,7 @@ TEST(EdgeScorersTest, test_goal_orientation_scoring) EdgeScorer scorer(node); EXPECT_EQ(scorer.numPlugins(), 1); // GoalOrientationScorer - + geometry_msgs::msg::PoseStamped goal_pose; goal_pose.pose.orientation.x = 0.0; goal_pose.pose.orientation.y = 0.0; @@ -630,8 +631,13 @@ TEST(EdgeScorersTest, test_goal_orientation_scoring) edge.start = &n1; edge.end = &n2; - // The score function should return 10.0 from penalty value float traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, traversal_cost, true)); - // EXPECT_EQ(traversal_cost, 10.0); -} \ No newline at end of file + EXPECT_TRUE(scorer.score(&edge, goal_pose, true, traversal_cost)); + EXPECT_EQ(traversal_cost, -1); + + edge.start = &n2; + edge.end = &n1; + + EXPECT_FALSE(scorer.score(&edge, goal_pose, true, traversal_cost)); + EXPECT_EQ(traversal_cost, -1); +} From e782c894955757dc5d9ba4367fa01424ae847f2a Mon Sep 17 00:00:00 2001 From: Alexander Yuen Date: Wed, 22 Jan 2025 22:04:00 +0000 Subject: [PATCH 11/11] switched cost edge pairs to imply return of cost, default orientation as M_PI / 2.0 Signed-off-by: Alexander Yuen --- .../include/nav2_route/interfaces/edge_cost_function.hpp | 2 +- .../plugins/edge_cost_functions/adjust_edges_scorer.hpp | 2 +- .../plugins/edge_cost_functions/costmap_scorer.hpp | 2 +- .../plugins/edge_cost_functions/distance_scorer.hpp | 2 +- .../plugins/edge_cost_functions/goal_orientation_scorer.hpp | 4 +--- .../plugins/edge_cost_functions/penalty_scorer.hpp | 2 +- .../plugins/edge_cost_functions/semantic_scorer.hpp | 2 +- .../nav2_route/plugins/edge_cost_functions/time_scorer.hpp | 2 +- nav2_route/src/edge_scorer.cpp | 2 +- .../src/plugins/edge_cost_functions/adjust_edges_scorer.cpp | 2 +- .../src/plugins/edge_cost_functions/costmap_scorer.cpp | 2 +- .../src/plugins/edge_cost_functions/distance_scorer.cpp | 2 +- .../plugins/edge_cost_functions/goal_orientation_scorer.cpp | 5 ++--- .../src/plugins/edge_cost_functions/penalty_scorer.cpp | 2 +- .../src/plugins/edge_cost_functions/semantic_scorer.cpp | 2 +- nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp | 2 +- 16 files changed, 17 insertions(+), 20 deletions(-) diff --git a/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp b/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp index 924937c5cb6..166199932e4 100644 --- a/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp +++ b/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp @@ -62,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, const geometry_msgs::msg::PoseStamped goal_pose, float & cost, bool final_edge) = 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 diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/adjust_edges_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/adjust_edges_scorer.hpp index 2eab691155e..0f17a20b5e5 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/adjust_edges_scorer.hpp +++ b/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, const geometry_msgs::msg::PoseStamped goal_pose, float & cost, bool final_edge) 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 diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp index 2364bd4fd1a..f7ccf37b6cc 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp +++ b/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, const geometry_msgs::msg::PoseStamped goal_pose, float & cost, bool final_edge) 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 diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/distance_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/distance_scorer.hpp index 528f500c908..68c377a03a8 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/distance_scorer.hpp +++ b/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, const geometry_msgs::msg::PoseStamped goal_pose, float & cost, bool final_edge) 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 diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp index f7f5149dc63..775505ac093 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp @@ -62,9 +62,7 @@ class GoalOrientationScorer : public EdgeCostFunction * @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, float & cost, - bool final_edge) 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 diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/penalty_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/penalty_scorer.hpp index af8c1d84e10..503d14ff80b 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/penalty_scorer.hpp +++ b/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, const geometry_msgs::msg::PoseStamped goal_pose, float & cost, bool final_edge) 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 diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/semantic_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/semantic_scorer.hpp index 69458c836ea..01509f6008d 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/semantic_scorer.hpp +++ b/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, const geometry_msgs::msg::PoseStamped goal_pose, float & cost, bool final_edge) 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 diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/time_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/time_scorer.hpp index 5f2f564ae05..a6a6066ec93 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/time_scorer.hpp +++ b/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, const geometry_msgs::msg::PoseStamped goal_pose, float & cost, bool final_edge) 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 diff --git a/nav2_route/src/edge_scorer.cpp b/nav2_route/src/edge_scorer.cpp index 00101930cbd..d6504d42594 100644 --- a/nav2_route/src/edge_scorer.cpp +++ b/nav2_route/src/edge_scorer.cpp @@ -73,7 +73,7 @@ bool EdgeScorer::score( for (auto & plugin : plugins_) { curr_score = 0.0; - if (plugin->score(edge, goal_pose, curr_score, final_edge)) { + if (plugin->score(edge, goal_pose, final_edge, curr_score)) { total_score += curr_score; } else { return false; diff --git a/nav2_route/src/plugins/edge_cost_functions/adjust_edges_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/adjust_edges_scorer.cpp index ca9b3bf0469..37bcc9c33b1 100644 --- a/nav2_route/src/plugins/edge_cost_functions/adjust_edges_scorer.cpp +++ b/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, const geometry_msgs::msg::PoseStamped /* goal_pose */, float & cost, bool /* final_edge */) +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()) { diff --git a/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp index 932f7ffa5f1..6f88af7defc 100644 --- a/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp +++ b/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, const geometry_msgs::msg::PoseStamped /* goal_pose */, float & cost, bool /* final_edge */) +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!"); diff --git a/nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp index 1e2f429574b..816d2a1cfa7 100644 --- a/nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp @@ -38,7 +38,7 @@ void DistanceScorer::configure( weight_ = static_cast(node->get_parameter(getName() + ".weight").as_double()); } -bool DistanceScorer::score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped /* goal_pose */, float & cost, bool /* final_edge */) +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; diff --git a/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp index 0c49d1c8b83..3ab1447ba31 100644 --- a/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp @@ -29,14 +29,13 @@ void GoalOrientationScorer::configure( logger_ = node->get_logger(); nav2_util::declare_parameter_if_not_declared( - node, getName() + ".orientation_tolerance", rclcpp::ParameterValue(M_PI)); + 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, float & /*cost*/, - bool final_edge) + const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, float & /*cost*/) { if (final_edge) { double edge_orientation = std::atan2( diff --git a/nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp index b28114ea591..5c104e446c2 100644 --- a/nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp @@ -38,7 +38,7 @@ void PenaltyScorer::configure( weight_ = static_cast(node->get_parameter(getName() + ".weight").as_double()); } -bool PenaltyScorer::score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped /* goal_pose */, float & cost, bool /* final_edge */) +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; diff --git a/nav2_route/src/plugins/edge_cost_functions/semantic_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/semantic_scorer.cpp index 9f8e1fd6085..4f787ec6da7 100644 --- a/nav2_route/src/plugins/edge_cost_functions/semantic_scorer.cpp +++ b/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, const geometry_msgs::msg::PoseStamped /* goal_pose */, float & cost, bool /* final_edge */) +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; diff --git a/nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp index 05de02a1194..333cfb73743 100644 --- a/nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp @@ -46,7 +46,7 @@ void TimeScorer::configure( max_vel_ = static_cast(node->get_parameter(getName() + ".max_vel").as_double()); } -bool TimeScorer::score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped /* goal_pose */, float & cost, bool /* final_edge */) +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;