diff --git a/nav2_core/include/nav2_core/route_exceptions.hpp b/nav2_core/include/nav2_core/route_exceptions.hpp index f0ef3094d64..aed9b304279 100644 --- a/nav2_core/include/nav2_core/route_exceptions.hpp +++ b/nav2_core/include/nav2_core/route_exceptions.hpp @@ -71,6 +71,13 @@ class IndeterminantNodesOnGraph : public RouteException : RouteException(description) {} }; +class InvalidEdgeScorerUse : public RouteException +{ +public: + explicit InvalidEdgeScorerUse(const std::string & description) + : RouteException(description) {} +}; + } // namespace nav2_core #endif // NAV2_CORE__ROUTE_EXCEPTIONS_HPP_ diff --git a/nav2_msgs/action/ComputeAndTrackRoute.action b/nav2_msgs/action/ComputeAndTrackRoute.action index d4539545763..bca31386889 100644 --- a/nav2_msgs/action/ComputeAndTrackRoute.action +++ b/nav2_msgs/action/ComputeAndTrackRoute.action @@ -6,6 +6,7 @@ uint16 INDETERMINANT_NODES_ON_GRAPH=403 uint16 TIMEOUT=404 uint16 NO_VALID_ROUTE=405 uint16 OPERATION_FAILED=406 +uint16 INVALID_EDGE_SCORER_USE=407 #goal definition uint16 start_id diff --git a/nav2_msgs/action/ComputeRoute.action b/nav2_msgs/action/ComputeRoute.action index 96c947bc93a..c251bf58ba0 100644 --- a/nav2_msgs/action/ComputeRoute.action +++ b/nav2_msgs/action/ComputeRoute.action @@ -5,6 +5,7 @@ uint16 NO_VALID_GRAPH=402 uint16 INDETERMINANT_NODES_ON_GRAPH=403 uint16 TIMEOUT=404 uint16 NO_VALID_ROUTE=405 +uint16 INVALID_EDGE_SCORER_USE=407 #goal definition uint16 start_id diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index 29354e617f6..c504096bcc4 100644 --- a/nav2_route/CMakeLists.txt +++ b/nav2_route/CMakeLists.txt @@ -88,6 +88,7 @@ add_library(edge_scorers SHARED src/plugins/edge_cost_functions/costmap_scorer.cpp src/plugins/edge_cost_functions/semantic_scorer.cpp src/plugins/edge_cost_functions/goal_orientation_scorer.cpp + src/plugins/edge_cost_functions/start_pose_orientation_scorer.cpp ) target_include_directories(edge_scorers PUBLIC diff --git a/nav2_route/include/nav2_route/edge_scorer.hpp b/nav2_route/include/nav2_route/edge_scorer.hpp index 9d91254d6e8..5cd764cf5d6 100644 --- a/nav2_route/include/nav2_route/edge_scorer.hpp +++ b/nav2_route/include/nav2_route/edge_scorer.hpp @@ -19,6 +19,7 @@ #include #include +#include "tf2_ros/buffer.h" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" #include "nav2_util/node_utils.hpp" @@ -47,7 +48,9 @@ class EdgeScorer /** * @brief Constructor */ - explicit EdgeScorer(nav2_util::LifecycleNode::SharedPtr node); + explicit EdgeScorer( + nav2_util::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer); /** * @brief Destructor @@ -63,7 +66,8 @@ class EdgeScorer * @return If edge is valid */ bool score( - const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & score); /** diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp index 475d1dcebc7..21906fcbcf3 100644 --- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -111,6 +111,12 @@ class GoalIntentExtractor */ void overrideStart(const geometry_msgs::msg::PoseStamped & start_pose); + /** + * @brief gets the desired start pose + * @return PoseStamped of start pose + */ + geometry_msgs::msg::PoseStamped getStart(); + protected: rclcpp::Logger logger_{rclcpp::get_logger("GoalIntentExtractor")}; std::shared_ptr node_spatial_tree_; 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 0ae457b78b5..83f640f4c4b 100644 --- a/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp +++ b/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp @@ -18,6 +18,7 @@ #include #include +#include "tf2_ros/buffer.h" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "pluginlib/class_loader.hpp" @@ -55,6 +56,7 @@ class EdgeCostFunction */ virtual void configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, const std::string & name) = 0; /** @@ -63,8 +65,8 @@ class EdgeCostFunction * start/end nodes and their associated metadata and actions */ virtual bool score( - const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, - bool final_edge, float & cost) = 0; + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) = 0; /** * @brief Get name of the plugin for parameter scope mapping 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 558faaaf78d..3f9ded08d00 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 @@ -51,6 +51,7 @@ class CostmapScorer : public EdgeCostFunction */ void configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, const std::string & name) override; /** @@ -61,8 +62,8 @@ class CostmapScorer : public EdgeCostFunction * @return bool if this edge is open valid to traverse */ bool score( - const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, - float & cost) override; + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; /** * @brief Get name of the plugin for parameter scope mapping 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 315e2394fce..f40ca835ae5 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 @@ -49,6 +49,7 @@ class DistanceScorer : public EdgeCostFunction */ void configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, const std::string & name) override; /** @@ -59,8 +60,8 @@ class DistanceScorer : public EdgeCostFunction * @return bool if this edge is open valid to traverse */ bool score( - const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, - float & cost) override; + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; /** * @brief Get name of the plugin for parameter scope mapping diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/dynamic_edges_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/dynamic_edges_scorer.hpp index dbe2bda4200..cfab91e8ac0 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/dynamic_edges_scorer.hpp +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/dynamic_edges_scorer.hpp @@ -52,6 +52,7 @@ class DynamicEdgesScorer : public EdgeCostFunction */ void configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, const std::string & name) override; /** @@ -62,8 +63,8 @@ class DynamicEdgesScorer : public EdgeCostFunction * @return bool if this edge is open valid to traverse */ bool score( - const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, - float & cost) override; + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; /** * @brief Get name of the plugin for parameter scope mapping 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 de60ea6cf04..af741be2dee 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 @@ -20,6 +20,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_core/route_exceptions.hpp" #include "nav2_route/interfaces/edge_cost_function.hpp" #include "nav2_util/line_iterator.hpp" #include "nav2_util/node_utils.hpp" @@ -33,7 +34,11 @@ namespace nav2_route /** * @class GoalOrientationScorer - * @brief Scores final edge by comparing the + * @brief Scores an edge leading to the goal node by comparing the orientation of the route + * pose and the orientation of the edge by multiplying the deviation from the desired + * orientation with a user defined weight. An alternative method can be selected, with + * the use_orientation_threshold flag, which rejects the edge it is greater than some + * tolerance */ class GoalOrientationScorer : public EdgeCostFunction { @@ -53,6 +58,7 @@ class GoalOrientationScorer : public EdgeCostFunction */ void configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, const std::string & name) override; /** @@ -63,8 +69,8 @@ class GoalOrientationScorer : public EdgeCostFunction * @return bool if this edge is open valid to traverse */ bool score( - const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, - float & cost) override; + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; /** * @brief Get name of the plugin for parameter scope mapping @@ -76,6 +82,8 @@ class GoalOrientationScorer : public EdgeCostFunction rclcpp::Logger logger_{rclcpp::get_logger("GoalOrientationScorer")}; std::string name_; double orientation_tolerance_; + float orientation_weight_; + bool use_orientation_threshold_; }; } // namespace nav2_route 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 08db78e12a2..055c4bc2346 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 @@ -48,6 +48,7 @@ class PenaltyScorer : public EdgeCostFunction */ void configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, const std::string & name) override; /** @@ -58,8 +59,8 @@ class PenaltyScorer : public EdgeCostFunction * @return bool if this edge is open valid to traverse */ bool score( - const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, - float & cost) override; + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; /** * @brief Get name of the plugin for parameter scope mapping 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 d1986656558..0546e683848 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 @@ -50,6 +50,7 @@ class SemanticScorer : public EdgeCostFunction */ void configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, const std::string & name) override; /** @@ -60,8 +61,8 @@ class SemanticScorer : public EdgeCostFunction * @return bool if this edge is open valid to traverse */ bool score( - const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, - float & cost) override; + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; /** * @brief Scores graph object based on metadata's semantic value at key diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp new file mode 100644 index 00000000000..12a2630ac38 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp @@ -0,0 +1,93 @@ +// Copyright (c) 2025, Polymath Robotics Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__START_POSE_ORIENTATION_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__START_POSE_ORIENTATION_SCORER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_util/line_iterator.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" +#include "angles/angles.h" + +namespace nav2_route +{ + +/** + * @class StartPoseOrientationScorer + * @brief Scores initial edge by comparing the orientation of the robot's current + * pose and the orientation of the edge by multiplying the deviation from the desired + * orientation with a user defined weight. An alternative method can be selected, with + * the use_orientation_threshold flag, which rejects the edge it is greater than some + * tolerance + */ +class StartPoseOrientationScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + StartPoseOrientationScorer() = default; + + /** + * @brief destructor + */ + virtual ~StartPoseOrientationScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + +protected: + rclcpp::Logger logger_{rclcpp::get_logger("StartPoseOrientationScorer")}; + std::string name_; + std::shared_ptr tf_buffer_; + double orientation_tolerance_; + float orientation_weight_; + bool use_orientation_threshold_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__START_POSE_ORIENTATION_SCORER_HPP_ 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 551cf9af300..72b942129af 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 @@ -50,6 +50,7 @@ class TimeScorer : public EdgeCostFunction */ void configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, const std::string & name) override; /** @@ -60,8 +61,8 @@ class TimeScorer : public EdgeCostFunction * @return bool if this edge is open valid to traverse */ bool score( - const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, - float & cost) override; + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; /** * @brief Get name of the plugin for parameter scope mapping diff --git a/nav2_route/include/nav2_route/route_planner.hpp b/nav2_route/include/nav2_route/route_planner.hpp index 99f5840a4c1..e1abc9aeed6 100644 --- a/nav2_route/include/nav2_route/route_planner.hpp +++ b/nav2_route/include/nav2_route/route_planner.hpp @@ -22,6 +22,8 @@ #include #include +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" #include "nav2_route/types.hpp" #include "nav2_route/utils.hpp" #include "nav2_route/edge_scorer.hpp" @@ -51,7 +53,9 @@ class RoutePlanner * @brief Configure the route planner, get parameters * @param node Node object to get parametersfrom */ - void configure(nav2_util::LifecycleNode::SharedPtr node); + void configure( + nav2_util::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer); /** * @brief Find the route from start to goal on the graph @@ -64,7 +68,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 RouteRequest & route_request); protected: /** @@ -83,7 +87,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 RouteRequest & route_request); /** * @brief Gets the traversal cost for an edge using edge scorers @@ -94,7 +98,7 @@ class RoutePlanner */ inline bool getTraversalCost( const EdgePtr edge, float & score, const std::vector & blocked_ids, - const geometry_msgs::msg::PoseStamped & goal); + const RouteRequest & route_request); /** * @brief Gets the next node in the priority queue for search @@ -128,10 +132,26 @@ class RoutePlanner */ inline bool isGoal(const NodePtr node); + /** + * @brief Checks if a given node is the start node + * @param node Node to check + * @return bool If this node is the start + */ + inline bool isStart(const NodePtr node); + + /** + * @brief Checks edge is a start or end edge + * @param edge Edge to check + * @return EdgeType identifying whether the edge is start, end or none + */ + nav2_route::EdgeType classifyEdge(const EdgePtr edge); + int max_iterations_{0}; + unsigned int start_id_{0}; unsigned int goal_id_{0}; NodeQueue queue_; std::unique_ptr edge_scorer_; + std::shared_ptr tf_buffer_; }; } // namespace nav2_route diff --git a/nav2_route/include/nav2_route/types.hpp b/nav2_route/include/nav2_route/types.hpp index 2c5e83f716a..254a52c9a37 100644 --- a/nav2_route/include/nav2_route/types.hpp +++ b/nav2_route/include/nav2_route/types.hpp @@ -203,6 +203,21 @@ struct Route float route_cost{0.0}; }; +/** + * @struct nav2_route::RouteRequest + * @brief An object to store salient features of the route request including its start + * and goal node ids, start and goal pose, and a flag to indicate if the start and goal + * poses are relevant + */ +struct RouteRequest +{ + unsigned int start_nodeid; //node id of start node + unsigned int goal_nodeid; //node id of goal node + geometry_msgs::msg::PoseStamped start_pose; //pose of start + geometry_msgs::msg::PoseStamped goal_pose; //pose of goal + bool use_poses; //whether the start and goal poses are used +}; + /** * @enum nav2_route::TrackerResult * @brief Return result of the route tracker to the main server for processing @@ -271,6 +286,17 @@ struct ReroutingState } }; +/** + * @enum nav2_route::EdgeType + * @brief An enum class describing what type of edge connecting two nodes is + */ +enum class EdgeType +{ + NONE = 0, + START = 1, + END = 2 +}; + } // namespace nav2_route #endif // NAV2_ROUTE__TYPES_HPP_ diff --git a/nav2_route/plugins.xml b/nav2_route/plugins.xml index 610c6846072..dba5c93f807 100644 --- a/nav2_route/plugins.xml +++ b/nav2_route/plugins.xml @@ -34,6 +34,11 @@ Cost function for checking if the orientation of route matches orientation of the goal + + + Cost function for checking if the orientation of robot matches with the orientation of the edge + + diff --git a/nav2_route/src/edge_scorer.cpp b/nav2_route/src/edge_scorer.cpp index b1c3080dff7..52406e0a64c 100644 --- a/nav2_route/src/edge_scorer.cpp +++ b/nav2_route/src/edge_scorer.cpp @@ -22,7 +22,9 @@ namespace nav2_route { -EdgeScorer::EdgeScorer(nav2_util::LifecycleNode::SharedPtr node) +EdgeScorer::EdgeScorer( + nav2_util::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer) : plugin_loader_("nav2_route", "nav2_route::EdgeCostFunction") { // load plugins with a default of the DistanceScorer @@ -49,7 +51,7 @@ EdgeScorer::EdgeScorer(nav2_util::LifecycleNode::SharedPtr node) RCLCPP_INFO( node->get_logger(), "Created edge cost function plugin %s of type %s", edge_cost_function_ids[i].c_str(), type.c_str()); - scorer->configure(node, edge_cost_function_ids[i]); + scorer->configure(node, tf_buffer, edge_cost_function_ids[i]); plugins_.push_back(std::move(scorer)); } catch (const pluginlib::PluginlibException & ex) { RCLCPP_FATAL( @@ -61,8 +63,8 @@ EdgeScorer::EdgeScorer(nav2_util::LifecycleNode::SharedPtr node) } bool EdgeScorer::score( - const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, - bool final_edge, float & total_score) + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & total_score) { total_score = 0.0; float curr_score = 0.0; @@ -73,7 +75,7 @@ bool EdgeScorer::score( for (auto & plugin : plugins_) { curr_score = 0.0; - if (plugin->score(edge, goal_pose, final_edge, curr_score)) { + if (plugin->score(edge, route_request, edge_type, curr_score)) { total_score += curr_score; } else { return false; diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp index 2b625c55f3d..eb155e1c309 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -195,6 +195,11 @@ Route GoalIntentExtractor::pruneStartandGoal( return pruned_route; } +geometry_msgs::msg::PoseStamped GoalIntentExtractor::getStart() +{ + return start_; +} + template Route GoalIntentExtractor::pruneStartandGoal( const Route & input_route, const std::shared_ptr goal, 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 5cbca6d2405..93cc4b56a3e 100644 --- a/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp @@ -22,6 +22,7 @@ namespace nav2_route void CostmapScorer::configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, const std::string & name) { RCLCPP_INFO(node->get_logger(), "Configuring costmap scorer."); @@ -74,8 +75,8 @@ void CostmapScorer::prepare() bool CostmapScorer::score( const EdgePtr edge, - const geometry_msgs::msg::PoseStamped & /* goal_pose */, - bool /* final_edge */, float & cost) + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, 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 c9bd6b72f22..1175b56a6d8 100644 --- a/nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp @@ -22,6 +22,7 @@ namespace nav2_route void DistanceScorer::configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, const std::string & name) { RCLCPP_INFO(node->get_logger(), "Configuring distance scorer."); @@ -40,8 +41,8 @@ void DistanceScorer::configure( bool DistanceScorer::score( const EdgePtr edge, - const geometry_msgs::msg::PoseStamped & /* goal_pose */, - bool /* final_edge */, float & cost) + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, 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/dynamic_edges_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/dynamic_edges_scorer.cpp index 16b05c82d88..e2b1986e31a 100644 --- a/nav2_route/src/plugins/edge_cost_functions/dynamic_edges_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/dynamic_edges_scorer.cpp @@ -22,6 +22,7 @@ namespace nav2_route void DynamicEdgesScorer::configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, const std::string & name) { RCLCPP_INFO(node->get_logger(), "Configuring adjust edges scorer."); @@ -63,10 +64,10 @@ void DynamicEdgesScorer::closedEdgesCb( response->success = true; } -bool DynamicEdgesScorer::score( +bool AdjustEdgesScorer::score( const EdgePtr edge, - const geometry_msgs::msg::PoseStamped & /* goal_pose */, - bool /* final_edge */, float & cost) + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, 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/goal_orientation_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp index ee436e5bc30..9194556af8b 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 @@ -22,6 +22,7 @@ namespace nav2_route void GoalOrientationScorer::configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, const std::string & name) { RCLCPP_INFO(node->get_logger(), "Configuring goal orientation scorer."); @@ -30,23 +31,44 @@ void GoalOrientationScorer::configure( nav2_util::declare_parameter_if_not_declared( node, getName() + ".orientation_tolerance", rclcpp::ParameterValue(M_PI / 2.0)); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".orientation_weight", rclcpp::ParameterValue(1.0)); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".use_orientation_threshold", rclcpp::ParameterValue(false)); + orientation_tolerance_ = node->get_parameter(getName() + ".orientation_tolerance").as_double(); + orientation_weight_ = + static_cast(node->get_parameter(getName() + ".orientation_weight").as_double()); + use_orientation_threshold_ = + node->get_parameter(getName() + ".use_orientation_threshold").as_bool(); } bool GoalOrientationScorer::score( const EdgePtr edge, - const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, float & /*cost*/) + const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) { - if (final_edge) { + + if (!route_request.use_poses) { + throw nav2_core::InvalidEdgeScorerUse("Cannot use goal orientation scorer without goal pose specified!"); + } + + if (edge_type == EdgeType::END) { 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 goal_orientation = tf2::getYaw(route_request.goal_pose.pose.orientation); double d_yaw = std::abs(angles::shortest_angular_distance(edge_orientation, goal_orientation)); - if (d_yaw > orientation_tolerance_) { - return false; + if (use_orientation_threshold_) { + if (d_yaw > orientation_tolerance_) { + return false; + } } + + cost = orientation_weight_ * static_cast(d_yaw); } return true; } 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 362be028b62..1eb20872d36 100644 --- a/nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp @@ -22,6 +22,7 @@ namespace nav2_route void PenaltyScorer::configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, const std::string & name) { RCLCPP_INFO(node->get_logger(), "Configuring penalty scorer."); @@ -40,8 +41,8 @@ void PenaltyScorer::configure( bool PenaltyScorer::score( const EdgePtr edge, - const geometry_msgs::msg::PoseStamped & /* goal_pose */, - bool /* final_edge */, float & cost) + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, 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 51361be2981..6fdcf05be28 100644 --- a/nav2_route/src/plugins/edge_cost_functions/semantic_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/semantic_scorer.cpp @@ -22,6 +22,7 @@ namespace nav2_route void SemanticScorer::configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, const std::string & name) { RCLCPP_INFO(node->get_logger(), "Configuring semantic scorer."); @@ -71,8 +72,8 @@ void SemanticScorer::metadataValueScorer(Metadata & mdata, float & score) bool SemanticScorer::score( const EdgePtr edge, - const geometry_msgs::msg::PoseStamped & /* goal_pose */, - bool /* final_edge */, float & cost) + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, float & cost) { float score = 0.0; Metadata & node_mdata = edge->end->metadata; diff --git a/nav2_route/src/plugins/edge_cost_functions/start_pose_orientation_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/start_pose_orientation_scorer.cpp new file mode 100644 index 00000000000..1c78de36489 --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/start_pose_orientation_scorer.cpp @@ -0,0 +1,87 @@ +// Copyright (c) 2025, Polymath Robotics Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp" + +namespace nav2_route +{ + +void StartPoseOrientationScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring start pose 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)); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".orientation_weight", rclcpp::ParameterValue(1.0)); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".use_orientation_threshold", rclcpp::ParameterValue(false)); + + orientation_tolerance_ = node->get_parameter(getName() + ".orientation_tolerance").as_double(); + orientation_weight_ = + static_cast(node->get_parameter(getName() + ".orientation_weight").as_double()); + use_orientation_threshold_ = + node->get_parameter(getName() + ".use_orientation_threshold").as_bool(); + + tf_buffer_ = tf_buffer; +} + +bool StartPoseOrientationScorer::score( + const EdgePtr edge, + const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) +{ + + if (!route_request.use_poses) { + throw nav2_core::InvalidEdgeScorerUse("Cannot use start pose orientation scorer without start pose specified!"); + } + + if (edge_type == EdgeType::START) { + double edge_orientation = std::atan2( + edge->end->coords.y - edge->start->coords.y, + edge->end->coords.x - edge->start->coords.x); + double start_orientation = tf2::getYaw(route_request.start_pose.pose.orientation); + double d_yaw = std::abs(angles::shortest_angular_distance(edge_orientation, start_orientation)); + + if (use_orientation_threshold_) { + if (d_yaw > orientation_tolerance_) { + return false; + } + } + + cost = orientation_weight_ * static_cast(d_yaw); + } + return true; +} + +std::string StartPoseOrientationScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::StartPoseOrientationScorer, nav2_route::EdgeCostFunction) 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 4f757aed58c..8cf91b66d8a 100644 --- a/nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp @@ -22,6 +22,7 @@ namespace nav2_route void TimeScorer::configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, const std::string & name) { RCLCPP_INFO(node->get_logger(), "Configuring time scorer."); @@ -47,8 +48,9 @@ void TimeScorer::configure( } bool TimeScorer::score( - const EdgePtr edge, const geometry_msgs::msg::PoseStamped & /* goal_pose */, - bool /* final_edge */, float & cost) + const EdgePtr edge, + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, 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 7ea10c4e010..2e535243064 100644 --- a/nav2_route/src/route_planner.cpp +++ b/nav2_route/src/route_planner.cpp @@ -24,7 +24,9 @@ namespace nav2_route { -void RoutePlanner::configure(nav2_util::LifecycleNode::SharedPtr node) +void RoutePlanner::configure( + nav2_util::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer) { nav2_util::declare_parameter_if_not_declared( node, "max_iterations", rclcpp::ParameterValue(0)); @@ -34,13 +36,13 @@ void RoutePlanner::configure(nav2_util::LifecycleNode::SharedPtr node) max_iterations_ = std::numeric_limits::max(); } - edge_scorer_ = std::make_unique(node); + edge_scorer_ = std::make_unique(node, tf_buffer); } Route RoutePlanner::findRoute( Graph & graph, unsigned int start_index, unsigned int goal_index, const std::vector & blocked_ids, - const geometry_msgs::msg::PoseStamped & goal_pose) + const RouteRequest & route_request) { if (graph.empty()) { throw nav2_core::NoValidGraph("Graph is invalid for routing!"); @@ -51,7 +53,7 @@ Route RoutePlanner::findRoute( // is valid when this function goes out of scope 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); + findShortestGraphTraversal(graph, start_node, goal_node, blocked_ids, route_request); EdgePtr & parent_edge = goal_node->search_state.parent_edge; if (!parent_edge) { @@ -83,10 +85,11 @@ void RoutePlanner::resetSearchStates(Graph & graph) void RoutePlanner::findShortestGraphTraversal( Graph & graph, const NodePtr start_node, const NodePtr goal_node, const std::vector & blocked_ids, - const geometry_msgs::msg::PoseStamped & goal_pose) + const RouteRequest & route_request) { // Setup the Dijkstra's search resetSearchStates(graph); + start_id_ = start_node->nodeid; goal_id_ = goal_node->nodeid; start_node->search_state.integrated_cost = 0.0; addNode(0.0, start_node); @@ -120,7 +123,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, goal_pose)) { + if (!getTraversalCost(edge, traversal_cost, blocked_ids, route_request)) { continue; } @@ -143,7 +146,7 @@ void RoutePlanner::findShortestGraphTraversal( bool RoutePlanner::getTraversalCost( const EdgePtr edge, float & score, const std::vector & blocked_ids, - const geometry_msgs::msg::PoseStamped & goal) + const RouteRequest & route_request) { // If edge or node is in the blocked list, don't expand auto is_blocked = std::find_if( @@ -165,7 +168,7 @@ bool RoutePlanner::getTraversalCost( return true; } - return edge_scorer_->score(edge, goal, isGoal(edge->end), score); + return edge_scorer_->score(edge, route_request, classifyEdge(edge), score); } NodeElement RoutePlanner::getNextNode() @@ -196,4 +199,19 @@ bool RoutePlanner::isGoal(const NodePtr node) return node->nodeid == goal_id_; } +bool RoutePlanner::isStart(const NodePtr node) +{ + return node->nodeid == start_id_; +} + +nav2_route::EdgeType RoutePlanner::classifyEdge(const EdgePtr edge) +{ + if (isStart(edge->start)) { + return EdgeType::START; + } else if (isGoal(edge->end)) { + return EdgeType::END; + } + return nav2_route::EdgeType::NONE; +} + } // namespace nav2_route diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp index 5bea9b7de4b..f0bfea3dea5 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -80,7 +80,7 @@ RouteServer::on_configure(const rclcpp_lifecycle::State & /*state*/) node, graph_, &id_to_graph_map_, tf_, route_frame_, base_frame_); route_planner_ = std::make_shared(); - route_planner_->configure(node); + route_planner_->configure(node, tf_); route_tracker_ = std::make_shared(); route_tracker_->configure( @@ -215,10 +215,17 @@ Route RouteServer::findRoute( route.route_cost = 0.0; route.start_node = &graph_.at(start_route); } else { + // Populate request data (start & goal id, start & goal pose, if set) for routing + RouteRequest route_request; + route_request.start_nodeid = start_route; + route_request.goal_nodeid = end_route; + route_request.start_pose = goal_intent_extractor_->getStart(); + route_request.goal_pose = goal->goal; + route_request.use_poses = goal->use_poses; + // Compute the route via graph-search, returns a node-edge sequence route = route_planner_->findRoute( - graph_, start_route, end_route, rerouting_info.blocked_ids, - goal->goal); + graph_, start_route, end_route, rerouting_info.blocked_ids, route_request); } return goal_intent_extractor_->pruneStartandGoal(route, goal, rerouting_info); @@ -275,44 +282,49 @@ RouteServer::processRouteRequest( } catch (nav2_core::NoValidRouteCouldBeFound & ex) { exceptionWarning(goal, ex); result->error_code = ActionT::Goal::NO_VALID_ROUTE; - result->error_msg = ex.what(); + result->error_msg = ex.what(); action_server->terminate_current(result); } catch (nav2_core::TimedOut & ex) { exceptionWarning(goal, ex); result->error_code = ActionT::Goal::TIMEOUT; - result->error_msg = ex.what(); + result->error_msg = ex.what(); action_server->terminate_current(result); } catch (nav2_core::RouteTFError & ex) { exceptionWarning(goal, ex); result->error_code = ActionT::Goal::TF_ERROR; - result->error_msg = ex.what(); + result->error_msg = ex.what(); action_server->terminate_current(result); } catch (nav2_core::NoValidGraph & ex) { exceptionWarning(goal, ex); result->error_code = ActionT::Goal::NO_VALID_GRAPH; - result->error_msg = ex.what(); + result->error_msg = ex.what(); action_server->terminate_current(result); } catch (nav2_core::IndeterminantNodesOnGraph & ex) { exceptionWarning(goal, ex); result->error_code = ActionT::Goal::INDETERMINANT_NODES_ON_GRAPH; - result->error_msg = ex.what(); + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::InvalidEdgeScorerUse & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Goal::INVALID_EDGE_SCORER_USE; + result->error_msg = ex.what(); action_server->terminate_current(result); } catch (nav2_core::OperationFailed & ex) { // A special case since Operation Failed is only in Compute & Track // actions, specifying it to allow otherwise fully shared code exceptionWarning(goal, ex); result->error_code = ComputeAndTrackRoute::Goal::OPERATION_FAILED; - result->error_msg = ex.what(); + result->error_msg = ex.what(); action_server->terminate_current(result); } catch (nav2_core::RouteException & ex) { exceptionWarning(goal, ex); result->error_code = ActionT::Goal::UNKNOWN; - result->error_msg = ex.what(); + result->error_msg = ex.what(); action_server->terminate_current(result); } catch (std::exception & ex) { exceptionWarning(goal, ex); result->error_code = ActionT::Goal::UNKNOWN; - result->error_msg = ex.what(); + result->error_msg = ex.what(); action_server->terminate_current(result); } } diff --git a/nav2_route/test/performance_benchmarking.cpp b/nav2_route/test/performance_benchmarking.cpp index 8c4bd560db3..a29c0289bdb 100644 --- a/nav2_route/test/performance_benchmarking.cpp +++ b/nav2_route/test/performance_benchmarking.cpp @@ -69,6 +69,7 @@ int main(int argc, char const * argv[]) { rclcpp::init(argc, argv); auto node = std::make_shared("route_benchmarking2"); + std::shared_ptr tf_buffer; Graph graph = createGraph(); @@ -78,7 +79,7 @@ int main(int argc, char const * argv[]) // graph_vis_pub->publish(utils::toMsg(graph, "map", node->now())); RoutePlanner planner; - planner.configure(node); + planner.configure(node, tf_buffer); std::vector blocked_ids; Route route; diff --git a/nav2_route/test/test_edge_scorers.cpp b/nav2_route/test/test_edge_scorers.cpp index 8aa74878bdd..ca437995cea 100644 --- a/nav2_route/test/test_edge_scorers.cpp +++ b/nav2_route/test/test_edge_scorers.cpp @@ -26,6 +26,9 @@ #include "nav2_msgs/srv/dynamic_edges.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_2d_publisher.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "tf2_ros/static_transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" class RclCppFixture { @@ -40,15 +43,17 @@ using namespace nav2_route; // NOLINT TEST(EdgeScorersTest, test_lifecycle) { auto node = std::make_shared("edge_scorer_test"); - EdgeScorer scorer(node); + std::shared_ptr tf_buffer; + EdgeScorer scorer(node, tf_buffer); } TEST(EdgeScorersTest, test_api) { // Tests basic API and default behavior. Also covers the DistanceScorer plugin. auto node = std::make_shared("edge_scorer_test"); - EdgeScorer scorer(node); - EXPECT_EQ(scorer.numPlugins(), 2); // default DistanceScorer, DynamicEdgesScorer + std::shared_ptr tf_buffer; + EdgeScorer scorer(node, tf_buffer); + EXPECT_EQ(scorer.numPlugins(), 2); // default DistanceScorer, AdjustEdgesScorer Node n1, n2; n1.nodeid = 1; @@ -58,20 +63,22 @@ TEST(EdgeScorersTest, test_api) edge.edgeid = 10; edge.start = &n1; edge.end = &n2; - const geometry_msgs::msg::PoseStamped goal_pose; + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; float traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, 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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, 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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); EXPECT_EQ(traversal_cost, 1.25); // 1m / 0.8 = 1.25 } @@ -79,11 +86,14 @@ TEST(EdgeScorersTest, test_failed_api) { // Expect failure since plugin does not exist auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + node->declare_parameter( "edge_cost_functions", rclcpp::ParameterValue(std::vector{"FakeScorer"})); nav2_util::declare_parameter_if_not_declared( node, "FakeScorer.plugin", rclcpp::ParameterValue(std::string{"FakePluginPath"})); - EXPECT_THROW(EdgeScorer scorer(node), pluginlib::PluginlibException); + + EXPECT_THROW(EdgeScorer scorer(node, tf_buffer), pluginlib::PluginlibException); } TEST(EdgeScorersTest, test_invalid_edge_scoring) @@ -93,6 +103,7 @@ TEST(EdgeScorersTest, test_invalid_edge_scoring) auto node = std::make_shared("route_server"); auto node_thread = std::make_unique(node); auto node2 = std::make_shared("my_node2"); + std::shared_ptr tf_buffer; node->declare_parameter( "edge_cost_functions", rclcpp::ParameterValue(std::vector{"DynamicEdgesScorer"})); @@ -100,8 +111,8 @@ TEST(EdgeScorersTest, test_invalid_edge_scoring) node, "DynamicEdgesScorer.plugin", rclcpp::ParameterValue(std::string{"nav2_route::DynamicEdgesScorer"})); - EdgeScorer scorer(node); - EXPECT_EQ(scorer.numPlugins(), 1); // DynamicEdgesScorer + EdgeScorer scorer(node, tf_buffer); + EXPECT_EQ(scorer.numPlugins(), 1); // AdjustEdgesScorer // Send service to set an edge as invalid auto srv_client = @@ -126,17 +137,19 @@ TEST(EdgeScorersTest, test_invalid_edge_scoring) edge.start = &n1; edge.end = &n2; - const geometry_msgs::msg::PoseStamped goal_pose; + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; // The score function should return false since closed float traversal_cost = -1; - EXPECT_FALSE(scorer.score(&edge, goal_pose, false, traversal_cost)); + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, 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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); EXPECT_EQ(traversal_cost, 42.0); // Try to re-open this edge @@ -148,7 +161,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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); node_thread.reset(); } @@ -157,6 +170,7 @@ TEST(EdgeScorersTest, test_penalty_scoring) { // Test Penalty scorer plugin loading + penalizing on metadata values auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; node->declare_parameter( "edge_cost_functions", rclcpp::ParameterValue(std::vector{"PenaltyScorer"})); @@ -164,9 +178,11 @@ TEST(EdgeScorersTest, test_penalty_scoring) node, "PenaltyScorer.plugin", rclcpp::ParameterValue(std::string{"nav2_route::PenaltyScorer"})); - EdgeScorer scorer(node); + EdgeScorer scorer(node, tf_buffer); EXPECT_EQ(scorer.numPlugins(), 1); // PenaltyScorer - const geometry_msgs::msg::PoseStamped goal_pose; + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; // Create edge to score Node n1, n2; @@ -183,7 +199,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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); EXPECT_EQ(traversal_cost, 10.0); } @@ -192,6 +208,7 @@ TEST(EdgeScorersTest, test_costmap_scoring) // Test Penalty scorer plugin loading + penalizing on metadata values auto node = std::make_shared("edge_scorer_test"); auto node_thread = std::make_unique(node); + std::shared_ptr tf_buffer; node->declare_parameter( "edge_cost_functions", rclcpp::ParameterValue(std::vector{"CostmapScorer"})); @@ -199,7 +216,7 @@ TEST(EdgeScorersTest, test_costmap_scoring) node, "CostmapScorer.plugin", rclcpp::ParameterValue(std::string{"nav2_route::CostmapScorer"})); - EdgeScorer scorer(node); + EdgeScorer scorer(node, tf_buffer); EXPECT_EQ(scorer.numPlugins(), 1); // CostmapScorer // Create edge to score @@ -212,11 +229,13 @@ TEST(EdgeScorersTest, test_costmap_scoring) edge.edgeid = 10; edge.start = &n1; edge.end = &n2; - const geometry_msgs::msg::PoseStamped goal_pose; + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; // The score function should return false because no costmap given float traversal_cost = -1; - EXPECT_FALSE(scorer.score(&edge, goal_pose, false, traversal_cost)); + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, traversal_cost)); // Create a demo costmap: * = 100, - = 0, / = 254 // * * * * - - - - - - - - @@ -256,7 +275,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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); // Segment in freespace EXPECT_EQ(traversal_cost, 0.0); @@ -265,7 +284,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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); // Segment in 100 space EXPECT_NEAR(traversal_cost, 100.0 / 254.0, 0.01); @@ -275,14 +294,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, false, traversal_cost)); + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, 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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, 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 +311,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, false, traversal_cost)); + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, traversal_cost)); node_thread.reset(); } @@ -302,6 +321,7 @@ TEST(EdgeScorersTest, test_costmap_scoring_alt_profile) // Test Penalty scorer plugin loading + penalizing on metadata values auto node = std::make_shared("edge_scorer_test"); auto node_thread = std::make_unique(node); + std::shared_ptr tf_buffer; node->declare_parameter( "edge_cost_functions", rclcpp::ParameterValue(std::vector{"CostmapScorer"})); @@ -315,7 +335,7 @@ TEST(EdgeScorersTest, test_costmap_scoring_alt_profile) node->declare_parameter( "CostmapScorer.invalid_off_map", rclcpp::ParameterValue(false)); - EdgeScorer scorer(node); + EdgeScorer scorer(node, tf_buffer); EXPECT_EQ(scorer.numPlugins(), 1); // CostmapScorer // Create edge to score @@ -362,7 +382,9 @@ TEST(EdgeScorersTest, test_costmap_scoring_alt_profile) rclcpp::Rate r(1); r.sleep(); - const geometry_msgs::msg::PoseStamped goal_pose; + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; // Off map n1.coords.x = -1.0; @@ -371,7 +393,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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); EXPECT_EQ(traversal_cost, 0.0); n1.coords.x = 4.1; @@ -380,7 +402,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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); EXPECT_NEAR(traversal_cost, 1.0, 0.01); n1.coords.x = 1.0; @@ -388,7 +410,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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, 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); @@ -399,6 +421,7 @@ TEST(EdgeScorersTest, test_time_scoring) { // Test Time scorer plugin loading auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; node->declare_parameter( "edge_cost_functions", rclcpp::ParameterValue(std::vector{"TimeScorer"})); @@ -406,7 +429,7 @@ TEST(EdgeScorersTest, test_time_scoring) node, "TimeScorer.plugin", rclcpp::ParameterValue(std::string{"nav2_route::TimeScorer"})); - EdgeScorer scorer(node); + EdgeScorer scorer(node, tf_buffer); EXPECT_EQ(scorer.numPlugins(), 1); // TimeScorer // Create edge to score @@ -422,30 +445,32 @@ 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; + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; // The score function should return 10.0 from time taken float traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, 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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, 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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, 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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); EXPECT_EQ(traversal_cost, 10.0); // 10.0 * 1.0 weight } @@ -453,6 +478,7 @@ TEST(EdgeScorersTest, test_semantic_scoring_key) { // Test Time scorer plugin loading auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; node->declare_parameter( "edge_cost_functions", rclcpp::ParameterValue(std::vector{"SemanticScorer"})); @@ -474,10 +500,12 @@ TEST(EdgeScorersTest, test_semantic_scoring_key) rclcpp::ParameterValue(static_cast(i))); } - EdgeScorer scorer(node); + EdgeScorer scorer(node, tf_buffer); EXPECT_EQ(scorer.numPlugins(), 1); // SemanticScorer - const geometry_msgs::msg::PoseStamped goal_pose; + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; // Create edge to score Node n1, n2; @@ -492,21 +520,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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, 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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, 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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); EXPECT_EQ(traversal_cost, 4.0); // (2.0 + 2.0) * 1.0 weight // Cannot find, doesn't exist @@ -514,7 +542,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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); EXPECT_EQ(traversal_cost, 0.0); // 0.0 * 1.0 weight } @@ -522,6 +550,7 @@ TEST(EdgeScorersTest, test_semantic_scoring_keys) { // Test Time scorer plugin loading auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; node->declare_parameter( "edge_cost_functions", rclcpp::ParameterValue(std::vector{"SemanticScorer"})); @@ -546,7 +575,7 @@ TEST(EdgeScorersTest, test_semantic_scoring_keys) rclcpp::ParameterValue(static_cast(i))); } - EdgeScorer scorer(node); + EdgeScorer scorer(node, tf_buffer); EXPECT_EQ(scorer.numPlugins(), 1); // SemanticScorer // Create edge to score @@ -560,11 +589,13 @@ TEST(EdgeScorersTest, test_semantic_scoring_keys) edge.start = &n1; edge.end = &n2; - const geometry_msgs::msg::PoseStamped goal_pose; + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; // Should fail, since both nothing under key `class` nor metadata set at all float traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, 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 +603,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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, 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 +611,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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); EXPECT_EQ(traversal_cost, 4.0); // (2.0 + 2.0) * 1.0 weight // Cannot find, doesn't exist @@ -589,14 +620,82 @@ 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, false, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); EXPECT_EQ(traversal_cost, 0.0); // 0.0 * 1.0 weight } +TEST(EdgeScorersTest, test_goal_orientation_threshold) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + + 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)); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.use_orientation_threshold", + rclcpp::ParameterValue(true)); + + EdgeScorer scorer(node, tf_buffer); + EXPECT_EQ(scorer.numPlugins(), 1); // GoalOrientationScorer + + geometry_msgs::msg::PoseStamped start_pose, 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; + + RouteRequest route_request; + route_request.goal_pose = goal_pose; + route_request.use_poses = true; + + EdgeType edge_type = EdgeType::END; + + // 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, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + edge.start = &n2; + edge.end = &n1; + + + traversal_cost = -1; + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + route_request.use_poses = false; + + EXPECT_THROW(scorer.score(&edge, route_request, edge_type, traversal_cost), nav2_core::InvalidEdgeScorerUse); +} + TEST(EdgeScorersTest, test_goal_orientation_scoring) { // Test Penalty scorer plugin loading + penalizing on metadata values auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + + double orientation_weight = 100.0; node->declare_parameter( "edge_cost_functions", @@ -607,16 +706,29 @@ TEST(EdgeScorersTest, test_goal_orientation_scoring) nav2_util::declare_parameter_if_not_declared( node, "GoalOrientationScorer.orientation_tolerance", rclcpp::ParameterValue(1.57)); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.use_orientation_thershold", + rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.orientation_weight", + rclcpp::ParameterValue(orientation_weight)); - EdgeScorer scorer(node); + + EdgeScorer scorer(node, tf_buffer); EXPECT_EQ(scorer.numPlugins(), 1); // GoalOrientationScorer - geometry_msgs::msg::PoseStamped goal_pose; + geometry_msgs::msg::PoseStamped start_pose, 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; + RouteRequest route_request; + route_request.goal_pose = goal_pose; + route_request.use_poses = true; + + EdgeType edge_type = EdgeType::END; + // Create edge to score Node n1, n2; n1.nodeid = 1; @@ -632,13 +744,189 @@ TEST(EdgeScorersTest, test_goal_orientation_scoring) edge.end = &n2; float traversal_cost = -1; - EXPECT_TRUE(scorer.score(&edge, goal_pose, true, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); EXPECT_EQ(traversal_cost, 0.0); edge.start = &n2; edge.end = &n1; + traversal_cost = -1; - EXPECT_FALSE(scorer.score(&edge, goal_pose, true, traversal_cost)); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_NEAR(traversal_cost, orientation_weight * M_PI, 0.001); + + route_request.use_poses = false; + + EXPECT_THROW(scorer.score(&edge, route_request, edge_type, traversal_cost), nav2_core::InvalidEdgeScorerUse); +} + +TEST(EdgeScorersTest, test_start_pose_orientation_threshold) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer = std::make_shared(node->get_clock()); + std::shared_ptr tf_listener = + std::make_shared(*tf_buffer); + + node->declare_parameter( + "edge_cost_functions", + rclcpp::ParameterValue(std::vector{"StartPoseOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::StartPoseOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.orientation_tolerance", + rclcpp::ParameterValue(1.57)); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.use_orientation_threshold", + rclcpp::ParameterValue(true)); + + EdgeScorer scorer(node, tf_buffer); + EXPECT_EQ(scorer.numPlugins(), 1); // GoalOrientationScorer + + double yaw = 0.0; + + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + + geometry_msgs::msg::PoseStamped start_pose, 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; + + start_pose.header.frame_id = "map"; + start_pose.header.stamp = node->get_clock()->now(); + + start_pose.pose.position.x = 0.0; + start_pose.pose.position.x = 0.0; + start_pose.pose.position.z = 0.0; + + start_pose.pose.orientation.x = q.getX(); + start_pose.pose.orientation.y = q.getY(); + start_pose.pose.orientation.z = q.getZ(); + start_pose.pose.orientation.w = q.getW(); + + RouteRequest route_request; + route_request.start_pose = start_pose; + route_request.use_poses = true; + + EdgeType edge_type = EdgeType::START; + + // 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, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + edge.start = &n2; + edge.end = &n1; + + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + route_request.use_poses = false; + + EXPECT_THROW(scorer.score(&edge, route_request, edge_type, traversal_cost), nav2_core::InvalidEdgeScorerUse); +} + +TEST(EdgeScorersTest, test_start_pose_orientation_scoring) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer = std::make_shared(node->get_clock()); + std::shared_ptr tf_listener = + std::make_shared(*tf_buffer); + + double orientation_weight = 100.0; + + node->declare_parameter( + "edge_cost_functions", + rclcpp::ParameterValue(std::vector{"StartPoseOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::StartPoseOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.orientation_tolerance", + rclcpp::ParameterValue(1.57)); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.use_orientation_thershold", + rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.orientation_weight", + rclcpp::ParameterValue(orientation_weight)); + + EdgeScorer scorer(node, tf_buffer); + EXPECT_EQ(scorer.numPlugins(), 1); // GoalOrientationScorer + + double yaw = 0.0; + + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + + geometry_msgs::msg::PoseStamped start_pose, 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; + + start_pose.header.frame_id = "map"; + start_pose.header.stamp = node->get_clock()->now(); + + start_pose.pose.position.x = 0.0; + start_pose.pose.position.x = 0.0; + start_pose.pose.position.z = 0.0; + + start_pose.pose.orientation.x = q.getX(); + start_pose.pose.orientation.y = q.getY(); + start_pose.pose.orientation.z = q.getZ(); + start_pose.pose.orientation.w = q.getW(); + + + RouteRequest route_request; + route_request.start_pose = start_pose; + route_request.use_poses = true; + + EdgeType edge_type = EdgeType::START; + + // 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, route_request, edge_type, traversal_cost)); EXPECT_EQ(traversal_cost, 0.0); + + edge.start = &n2; + edge.end = &n1; + + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_NEAR(traversal_cost, orientation_weight * M_PI, 0.001); + + route_request.use_poses = false; + + EXPECT_THROW(scorer.score(&edge, route_request, edge_type, traversal_cost), nav2_core::InvalidEdgeScorerUse); + } diff --git a/nav2_route/test/test_route_planner.cpp b/nav2_route/test/test_route_planner.cpp index 3d3760d53a5..1da32c61c2b 100644 --- a/nav2_route/test/test_route_planner.cpp +++ b/nav2_route/test/test_route_planner.cpp @@ -127,11 +127,13 @@ inline Graph create4x4Graph() TEST(RoutePlannerTest, test_route_planner_positive) { - geometry_msgs::msg::PoseStamped goal_pose; + geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; auto node = std::make_shared("router_test"); + std::shared_ptr tf_buffer; RoutePlanner planner; - planner.configure(node); + planner.configure(node, tf_buffer); std::vector blocked_ids; unsigned int start, goal; @@ -141,7 +143,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, goal_pose); + Route route = planner.findRoute(graph, start, goal, blocked_ids, route_request); EXPECT_NEAR(route.route_cost, 6.0, 0.001); EXPECT_EQ(route.edges.size(), 6u); @@ -152,19 +154,19 @@ TEST(RoutePlannerTest, test_route_planner_positive) EXPECT_THROW( planner.findRoute( graph, start, goal, blocked_ids, - goal_pose), nav2_core::NoValidRouteCouldBeFound); + route_request), 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, goal_pose); + route = planner.findRoute(graph, start, goal, blocked_ids, route_request); 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, goal_pose); + route = planner.findRoute(graph, start, goal, blocked_ids, route_request); EXPECT_NEAR(route.route_cost, 5.0, 0.001); EXPECT_EQ(route.edges.size(), 5u); for (auto & edge : route.edges) { @@ -176,19 +178,21 @@ 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, goal_pose); + route = planner.findRoute(graph, start, goal, blocked_ids, route_request); 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; + geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; auto node = std::make_shared("router_test"); + std::shared_ptr tf_buffer; node->declare_parameter("max_iterations", rclcpp::ParameterValue(5)); RoutePlanner planner; - planner.configure(node); + planner.configure(node, tf_buffer); std::vector blocked_ids; unsigned int start = 0; unsigned int goal = 15; @@ -198,14 +202,14 @@ TEST(RoutePlannerTest, test_route_planner_negative) EXPECT_THROW( planner.findRoute( graph, start, goal, blocked_ids, - goal_pose), nav2_core::NoValidGraph); + route_request), 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, goal_pose), nav2_core::TimedOut); + EXPECT_THROW(planner.findRoute(graph, start, goal, blocked_ids, route_request), 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; @@ -213,5 +217,5 @@ TEST(RoutePlannerTest, test_route_planner_negative) EXPECT_THROW( planner.findRoute( graph, start, goal, blocked_ids, - goal_pose), nav2_core::NoValidGraph); + route_request), nav2_core::NoValidGraph); }