Controller FollowPath feedback: Fix wrong frame in distance calculation, add more info to feedback#4931
Conversation
f6734a5 to
227bec6
Compare
Codecov ReportAttention: Patch coverage is
... and 2 files with indirect coverage changes 🚀 New features to boost your workflow:
|
nav2_msgs/action/FollowPath.action
Outdated
| --- | ||
| #feedback definition | ||
| float32 distance_to_goal | ||
| uint64 closest_path_pose_idx # index of the pose on global path closest to current robot pose |
There was a problem hiding this comment.
I think these should be removed or at least adjusted. The distance_to_path is not very accurate since it is the nearest path point, not the actual distance from the path. When using very dense paths, this might be reasonably accurate, but when the path markers are further away, then these measures are pretty poor.
How would these two values be used by client applications?
There was a problem hiding this comment.
true, but I think that's the best estimation we have without linearly interpolating the path, which introduces a different assumption. Shouldn't most paths be quite dense if they are coming from a nav2 planner?
For my use case: I am using them to monitor whether the robot has deviated the too much from the path and switch to another behavior. For example: while following a coverage path re-plan a detour path, or requesting user input if the deviation is way too big.
There was a problem hiding this comment.
Shouldn't most paths be quite dense if they are coming from a nav2 planner?
Not always and/or promised. Right now, there aren't any assumptions that I'm aware of in Nav2 about the density of the path and there are custom user planners out there that are not as dense as those provided within Nav2.
Even so, if a costmap resolution is 5cm, some folks tracking error might only be 10 cm and the error from just the binning is already bordering the value.
I am using them to monitor whether the robot has deviated the too much from the path and switch to another behavior.
I think that might be better suited for a new method that takes in the path and the robot pose and compute that (with an optional argument of the closest path point; else find itself). That would actually be valuable contribution to nav2_util and then could be later used in the controller server to provide that feedback. I think the interpolation however is necessary.
That util could then also be used as a BT condition node itself to check if the path tracking accuracy has dropped. If so, then we go down a different branch. Another reason that this should be a util (and makes it unit testable)! This provides alot of flexibility in where it is computed and used
There was a problem hiding this comment.
removed the extra feedback.
I am not sure I have time to work on the extra util right now, should I open an issue so it is not forgotten?
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
also remove debug prints Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
This reverts commit 1ee9ec1. Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
3776bdd to
0bf0e40
Compare
|
Sorry for the delayed review, I've been having more thought-required PRs filed this week than I had time for thoughts to review them. This looks perfect to me, thanks for finding this bug! Please do file file that ticket and please tag it with help wanted, and good first issue, its how alot of new contributors find projects and I think this is a good candidate for that! |
|
Opened. |
…on, add more info to feedback (ros-navigation#4931) * Add more info to feedback, fix wrong frame in distance calculation Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * Write FollowPath Feedback to blackboard Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * first publish velocity, then try to publish feedback also remove debug prints Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * Revert "Write FollowPath Feedback to blackboard" This reverts commit 1ee9ec1. Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * small fixes to na2_tree_nodes.xml Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * fix formatting issue Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * remove additional feedback fields from FollowPath action Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * throw if transform failed Signed-off-by: Adi Vardi <adi.vardi@enway.ai> --------- Signed-off-by: Adi Vardi <adi.vardi@enway.ai> Signed-off-by: stevedanomodolor <stevedan.o.omodolor@gmail.com>
…on, add more info to feedback (ros-navigation#4931) * Add more info to feedback, fix wrong frame in distance calculation Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * Write FollowPath Feedback to blackboard Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * first publish velocity, then try to publish feedback also remove debug prints Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * Revert "Write FollowPath Feedback to blackboard" This reverts commit 1ee9ec1. Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * small fixes to na2_tree_nodes.xml Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * fix formatting issue Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * remove additional feedback fields from FollowPath action Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * throw if transform failed Signed-off-by: Adi Vardi <adi.vardi@enway.ai> --------- Signed-off-by: Adi Vardi <adi.vardi@enway.ai> Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>
Basic Info
Description of contribution in a few bullet points
closest_path_pose_idxanddistance_to_pathto the feedbacknav2_behavior_tree/nav2_tree_nodes.xmlDescription of documentation updates required from your changes
none (?)
Description of how this change was tested
Tested on a Gazebo simulation of our robot, using Jazzy (same commits, just based on jazzy)
Future work that may be required in bullet points
Open question for discussion: Should the feedback from the
FollowPathaction be written into the Blackboard in the BT action node (follow_path_action)?Write FollowPath Feedback to blackboard(reverted for now)For Maintainers: