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/edge_scorer.hpp b/nav2_route/include/nav2_route/edge_scorer.hpp index 944d8ab40c2..58f1a68407c 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,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 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..166199932e4 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, 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 e190694e7bc..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, 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 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..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, 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 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..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, 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 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..775505ac093 --- /dev/null +++ b/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 +#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" +#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_ 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..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, 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 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..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, 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 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..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, 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 diff --git a/nav2_route/include/nav2_route/route_planner.hpp b/nav2_route/include/nav2_route/route_planner.hpp index 5c1179e1769..4d3c27b303b 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,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 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/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/edge_scorer.cpp b/nav2_route/src/edge_scorer.cpp index 968ff52e40c..d6504d42594 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, 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; 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..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, 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()) { 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..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, 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!"); 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..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, 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; 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..3ab1447ba31 --- /dev/null +++ b/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 +#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(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) 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..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, 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; 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..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, 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; 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..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, 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; diff --git a/nav2_route/src/route_planner.cpp b/nav2_route/src/route_planner.cpp index 813ecfddd08..c46df666a11 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, + const 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, + const 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,8 @@ 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 +161,7 @@ bool RoutePlanner::getTraversalCost( return true; } - return edge_scorer_->score(edge, score); + return edge_scorer_->score(edge, goal, isGoal(edge->end), score); } 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..fdbee591334 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, 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, traversal_cost)); + 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, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); 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, 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, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); 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, false, traversal_cost)); 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, false, traversal_cost)); 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, false, traversal_cost)); // 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, false, traversal_cost)); // 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, false, traversal_cost)); // 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, 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, traversal_cost)); + 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); @@ -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, false, traversal_cost)); 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, false, traversal_cost)); 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, false, traversal_cost)); 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, 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); @@ -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, 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, traversal_cost)); + 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, traversal_cost)); + 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, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); 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, 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, traversal_cost)); + 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, traversal_cost)); + 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 @@ -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, false, traversal_cost)); 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, 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 @@ -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, 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 @@ -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, false, traversal_cost)); EXPECT_EQ(traversal_cost, 4.0); // (2.0 + 2.0) * 1.0 weight // Cannot find, doesn't exist @@ -576,6 +589,55 @@ 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, false, traversal_cost)); 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; + + float traversal_cost = -1; + 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); +} 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); }