Controller FollowPath feedback: Fix wrong frame in distance calculation, add more info to feedback #4931
Conversation
adivardi
changed the title
Signed-off-by: Adi Vardi <[email protected]>
Signed-off-by: Adi Vardi <[email protected]>
also remove debug prints Signed-off-by: Adi Vardi <[email protected]>
This reverts commit 1ee9ec1. Signed-off-by: Adi Vardi <[email protected]>
Signed-off-by: Adi Vardi <[email protected]>
adivardi
force-pushed
the
av/controller_feedback_rolling
branch
from
f6734a5 to
227bec6
Compare
Signed-off-by: Adi Vardi <[email protected]>
Codecov ReportAttention: Patch coverage is
|
| @@ -24,4 +24,6 @@ string error_msg | |||
| --- | |||
| #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
| costmap_ros_->getTfBuffer(), current_path_.header.frame_id, pose, | ||
| robot_pose_in_path_frame, tolerance)) | ||
| { | ||
| RCLCPP_ERROR(get_logger(), "Robot pose is not available."); |
There was a problem hiding this comment.
I think an exception should be thrown, this is a true error condition if TF transformations are not available
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: