Nav2 route server start pose orientation scorer #4950

alexanderjyuen · 2025-02-25T18:16:50Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	N/A
Primary OS tested on	Ubuntu 20.04
Robotic platform tested on	Polymath Robotics' Ackermann agriculture client
Does this PR contain AI generated software?	No

Description of contribution in a few bullet points

Presently there is no edge scorer that restricts or penalizes routes that start with an orientation that is very different from the robot start pose. A new edge scorer has been added to do that comparison and return false if a route pose is very different from the start pose

In order to do this, two major additions were made outside of including start_pose_orientation_scorer.cpp and start_pose_orientation_scorer.hpp` this includes:

Adding a function to check if the node ID is the start node
Passing the bool identifying an edge being the start edge all the way from the route server to the edge scorers
Passing the tf buffer from route planner down to the edge scorers
Modifying all edge scorers to take new arguments

Description of documentation updates required from your changes

Need to add configuration guide on full release of route_server

Description of how this change was tested

Unit tests were written to cover the major use case

Future work that may be required in bullet points

Documentation and more comprehensive testihng

For Maintainers:

Check that any new parameters added are updated in docs.nav2.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

Signed-off-by: Alexander Yuen <[email protected]>

…e in score method for all existing edge scorers Signed-off-by: Alexander Yuen <[email protected]>

Signed-off-by: Alexander Yuen <[email protected]>

…, added tf_buffer as a protected variable Signed-off-by: Alexander Yuen <[email protected]>

… added isStart method to identify initial node for route_planner.hpp Signed-off-by: Alexander Yuen <[email protected]>

…ions to take a start edge bool, added test for start_pose_orientation_scorer Signed-off-by: Alexander Yuen <[email protected]>

Signed-off-by: Alexander Yuen <[email protected]>

…ns, bool for start edge also passed down to plugins Signed-off-by: Alexander Yuen <[email protected]>

mergify · 2025-02-25T18:17:32Z

@alexanderjyuen, all pull requests must be targeted towards the main development branch.
Once merged into main, it is possible to backport to @nav2_route_server, but it must be in main
to have these changes reflected into new distributions.

SteveMacenski

Some medium sized items, but all should be technically easy (just touches a few files). Overall, looks good in concept

SteveMacenski · 2025-03-07T21:03:31Z

nav2_route/include/nav2_route/edge_scorer.hpp

@@ -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 geometry_msgs::msg::PoseStamped & goal_pose, bool start_edge,


If we're going to add things like this, I think we should create an enum so more than be added without breaking API in the future. We can have an enum for SpecialEdgeType with start, final as current ones. If neither, then the default can be NONE.

SteveMacenski · 2025-03-07T21:03:55Z

nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp

    const std::string & name) = 0;

  /**
   * @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
   */
-  virtual bool score(const EdgePtr edge, const geometry_msgs::msg::PoseStamped & goal_pose, bool final_edge, float & cost) = 0;
+  virtual bool score(


Then propagated here as well

SteveMacenski · 2025-03-07T21:05:37Z

nav2_route/src/edge_scorer.cpp

@@ -58,11 +60,13 @@ EdgeScorer::EdgeScorer(nav2_util::LifecycleNode::SharedPtr node)
      throw ex;
    }
  }
+
+  tf_buffer_ = tf_buffer;


Does the edge scorer manager need the tf buffer? I think this can be removed (and in the hpp file)

SteveMacenski · 2025-03-07T21:06:59Z

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

@@ -0,0 +1,91 @@
+// Copyright (c) 2024, Polymath Robotics Inc.


Update to 2025 (with cpp file too)

SteveMacenski · 2025-03-07T21:11:49Z

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

+
+/**
+ * @class StartPoseOrientationScorer
+ * @brief Scores initial edge edge by comparing the orientation of the robot's current pose and the orientation of the edge


Suggested change

* @brief Scores initial edge edge by comparing the orientation of the robot's current pose and the orientation of the edge

* @brief Scores initial edge by comparing the orientation of the robot's current pose and the orientation of the edge

Please also provide additional information about how it scores. I don't think it actually scores as much as it rejects if > than a tolerance. (also the goal orientation one needs to be populated as well

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

Line 36 in d02bb47

* @brief Scores final edge by comparing the

)

SteveMacenski · 2025-03-07T21:12:46Z

nav2_route/src/plugins/edge_cost_functions/start_pose_orientation_scorer.cpp

+namespace nav2_route
+{
+
+void StartPoseOrientationScorer::configure(


I wanted to mention that this could cause you some real headaches with non-liberal values of the tolerance. If localization updates mess with your orientation, initial orientation is very wrong, initial orientation is not aligned with the graph when the robot starts up, or robot drives off the route for some reason, your orientation could not align well with any of the edges and cause them all to fail.

I wonder if you really want to apply a continuous cost function proportionate to the orientation instead of a straight-up rejection. It would then allow for much shorter paths to be able to use the reverse direction, which maybe isn't something you want. I think there should be modes (or separate plugins?) for both options. I could see both being useful and I think you would also have applications of this work that you would want to use either/or (such as for omni, differential, or small ackermann platforms)

SteveMacenski · 2025-03-07T21:13:04Z

nav2_route/src/plugins/edge_cost_functions/start_pose_orientation_scorer.cpp

+  robot_frame_ = node->get_parameter(getName() + ".robot_frame").as_string();
+
+  tf_buffer_ = tf_buffer;
+


Remove empty line

SteveMacenski · 2025-03-07T21:14:16Z

nav2_route/src/plugins/edge_cost_functions/start_pose_orientation_scorer.cpp

+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    getName() + ".orientation_tolerance", rclcpp::ParameterValue(M_PI / 2.0));
+  nav2_util::declare_parameter_if_not_declared(


Shouldnt thins and the robot frame exist for the server itself (rather than the plugin)? If so, you don't need to duplicate parameters, you can simply get the server's values itself from get_parameter without declaring

alexanderjyuen added 13 commits February 25, 2025 18:00

adding flag to identify start node, passing tf_buffer to edge scorer

b7230b7

Signed-off-by: Alexander Yuen <[email protected]>

passing tf to route planner

cb665ef

Signed-off-by: Alexander Yuen <[email protected]>

added null buffer to tests

178cc21

Signed-off-by: Alexander Yuen <[email protected]>

added null buffer to planner configure in performance bench marking test

eb6e17e

Signed-off-by: Alexander Yuen <[email protected]>

changed arguments of all old edge scorers to also take tf_buffer

8707ef2

Signed-off-by: Alexander Yuen <[email protected]>

changed configure to take tf_buffer, added bool to identify start_edg…

9021151

…e in score method for all existing edge scorers Signed-off-by: Alexander Yuen <[email protected]>

added start_pose_orientation_scorer to CMake

f6db192

Signed-off-by: Alexander Yuen <[email protected]>

added StartPoseOrientationScorer as and edge scroer

d507f40

Signed-off-by: Alexander Yuen <[email protected]>

added tf_buffer to constructor, added start edge bool on score method…

bedf5fc

…, added tf_buffer as a protected variable Signed-off-by: Alexander Yuen <[email protected]>

added tf_buffer to configure method, added start_id_ member variable,…

80f1189

… added isStart method to identify initial node for route_planner.hpp Signed-off-by: Alexander Yuen <[email protected]>

modified all configures to take a tf_buffer, modified all score funct…

7f7e84e

…ions to take a start edge bool, added test for start_pose_orientation_scorer Signed-off-by: Alexander Yuen <[email protected]>

adding start_pose_orientation_scorer.cpp

63d02ac

Signed-off-by: Alexander Yuen <[email protected]>

edge scorer modified to take in tf_buffer and pass it to scorer plugi…

d02bb47

…ns, bool for start edge also passed down to plugins Signed-off-by: Alexander Yuen <[email protected]>

SteveMacenski requested changes Mar 7, 2025

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Nav2 route server start pose orientation scorer #4950

Nav2 route server start pose orientation scorer #4950

alexanderjyuen commented Feb 25, 2025

mergify bot commented Feb 25, 2025

SteveMacenski left a comment

SteveMacenski Mar 7, 2025

SteveMacenski Mar 7, 2025

SteveMacenski Mar 7, 2025

SteveMacenski Mar 7, 2025

SteveMacenski Mar 7, 2025

SteveMacenski Mar 7, 2025

SteveMacenski Mar 7, 2025

SteveMacenski Mar 7, 2025

		@@ -0,0 +1,91 @@
		// Copyright (c) 2024, Polymath Robotics Inc.

	* @brief Scores initial edge edge by comparing the orientation of the robot's current pose and the orientation of the edge
	* @brief Scores initial edge by comparing the orientation of the robot's current pose and the orientation of the edge

		robot_frame_ = node->get_parameter(getName() + ".robot_frame").as_string();

		tf_buffer_ = tf_buffer;

Nav2 route server start pose orientation scorer #4950

Are you sure you want to change the base?

Nav2 route server start pose orientation scorer #4950

Conversation

alexanderjyuen commented Feb 25, 2025

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

For Maintainers:

mergify bot commented Feb 25, 2025

SteveMacenski left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment