Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AC_Avoid: Added new OA type (Dijkstra + BendyRuler fusion) #14934

Merged
merged 1 commit into from
Aug 26, 2020
Merged

AC_Avoid: Added new OA type (Dijkstra + BendyRuler fusion) #14934

merged 1 commit into from
Aug 26, 2020

Conversation

rishabsingh3003
Copy link
Contributor

@rishabsingh3003 rishabsingh3003 commented Jul 29, 2020 •
edited
Loading

This PR creates a new OA Type which attempts to fuse Dijkstra and BendyRuler to be used together.
The basic idea is:

  1. For WP Navigation through fence, Dijkstra's calculates the shortest path
  2. BendyRuler is meanwhile set to only look for Proximity Sensor found obstacles.
  3. The mission is run as if it was Normal Dijkstra's, UNTIL a physical obstacle is detected.
  4. After that we switch to (Dijkstra's is turned off) BendyRuler type avoidance again (for both fence + obstacles)
  5. After the BendyRuler turns off (I,.e path is clear towards destination), Dijkstras is triggered to calculate new path based on deviated current position and Dijkstra's becomes active again (BendyRuler goes back to only looking for physical obstacles).

Testing:
This has been tested on Morse + SITL with fence's and obstacle in between a guided mission
https://www.youtube.com/watch?v=s0z0b2U2fAk

@rmackay9
Copy link
Contributor

very exciting to see this

@rishabsingh3003 rishabsingh3003 mentioned this pull request Jul 30, 2020
Closed
7 tasks
@rishabsingh3003
Copy link
Contributor Author

This PR has been rebased to work with the new BendyRuler type (Vertical Avoidance)
Here is a demonstration of the same: (You can see which type of avoidance is active on the console and map)
https://youtu.be/EGuz5NeAZAA (watch in 2x please)

rishabsingh3003
rishabsingh3003 commented Aug 24, 2020
View reviewed changes
libraries/AC_Avoidance/AP_OABendyRuler.cpp
@@ -649,6 +655,9 @@ bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, co
if (!start.get_vector_from_origin_NEU(start_NEU) || !end.get_vector_from_origin_NEU(end_NEU)) {
return false;
}
if (start_NEU == end_NEU) {
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I have inlcluded this bit as an extra safety check. I found this as a potential bug. Its not really needed in this PR.
What happens is, if the two points are the same, Vector3f::closest_distance_between_line_and_point returns 0
therefore the line
Vector3f::closest_distance_between_line_and_point(start_NEU, end_NEU, point_cm) * 0.01f - item.radius; would falsely return a negative margin.. Something that can be scary mid flight..

@rishabsingh3003
Copy link
Contributor Author

I must also add that this new feature, when in use with Vertical BendyRuler can get potentially dangerous if obstacle + fence approach together.. Vertical BendyRuler does not have the capabilities to dodge any fence except alt fence. It only exists to climb over proximity objects . So incase a user enables Vertical BendyRuler, and an obstacle were to force the vehicle go near a polygon fence.. Copter would start climbing till Fence alt is near and be stuck there.
This is not really a problem with this PR but a problem with Vertical BendyRuler itself

rmackay9
rmackay9 reviewed Aug 26, 2020
View reviewed changes
libraries/AC_Avoidance/AP_OAPathPlanner.cpp
@@ -136,6 +145,12 @@ bool AP_OAPathPlanner::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
return false;
}
break;
case OA_PATHPLAN_DJIKSTRA_BENDYRULER:
if(_oadijkstra == nullptr || _oabendyruler == nullptr) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

tiny thing but we should have a space after the "if".

@rmackay9 rmackay9 merged commit caf5bfe into ArduPilot:master Aug 26, 2020
@rmackay9
Copy link
Contributor

Merged, thanks!!

@rishabsingh3003 rishabsingh3003 mentioned this pull request Mar 19, 2021
Open
11 tasks
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@rmackay9 rmackay9 rmackay9 left review comments

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
2 participants
@rishabsingh3003 @rmackay9