adding the fixes for navigate through poses terminating pre-maturely #5203
Conversation
Signed-off-by: Pradheep <[email protected]>
Codecov ReportAttention: Patch coverage is
... and 5 files with indirect coverage changes 🚀 New features to boost your workflow:
|
There was a problem hiding this comment.
I actually don't think this really works. What happens if the final goal is literally the same pose as an intermediary pose? There would be not difference in the angle.
I think something like what was done in #4789 is the better approach by not just looking at the pose, but also the path related to that pose. Generally speaking, I think this can be closed, this approach is not viable and the reworks would be essentially a ground-up restart. If we want to work on something like this, it should be via building on or completing #4789
| @@ -137,6 +144,15 @@ NavigateThroughPosesNavigator::goalCompleted( | |||
| result->waypoint_statuses = std::move(waypoint_statuses); | |||
| } | |||
|
|
|||
| inline double createYawFromQuat(const geometry_msgs::msg::Quaternion & orientation) | |||
There was a problem hiding this comment.
tf2 Utils has a function that does this getYaw
| if (curr_dist < curr_min_dist && | ||
| abs(orient_diff) < orient_angle_rad) | ||
| { |
There was a problem hiding this comment.
| if (curr_dist < curr_min_dist && | |
| abs(orient_diff) < orient_angle_rad) | |
| { | |
| if (curr_dist < curr_min_dist && abs(orient_diff) < orient_angle_rad) { |
| res = blackboard->get(path_blackboard_id_, current_path); | ||
| if (res && current_path.poses.size() > 0u) { | ||
| // Find the closest pose to current pose on global path | ||
| auto find_closest_pose_idx = | ||
| [¤t_pose, ¤t_path]() { | ||
| [¤t_pose, ¤t_path, &orient_angle_rad]() { | ||
| size_t closest_pose_idx = 0; | ||
| double curr_min_dist = std::numeric_limits<double>::max(); | ||
| for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) { | ||
| double curr_dist = nav2_util::geometry_utils::euclidean_distance( |
There was a problem hiding this comment.
Instead we could check for the distance not just on XY but XY,Yaw so it would be 3D.
Basic Info
Description of contribution in a few bullet points
Thanks to @anath93 for the contribution.
This still needs to be tested.
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: