Fix and rewrite collision prevention

[AlexKlimaj]

Describe problem solved by the proposed feature

Currently collision prevention in PX4 is broken.

Describe your preferred solution

Move to vector field histogram (VFH) algorithm.

Don't hit obstacles that you can see, apply negative force to push drone away if within minimum distance, only apply allowed components of sticks vector while up against the invisible wall

Handle arbitrary obstacle_distance message formats, ie allow greater than or less than 10 degree angle step sizes, allow more or less than 36 sectors

Work in the default position mode (manual acceleration

Another nice option would be params (reboot required) to adjust number of sectors, so you can have bigger/smaller resolution if you have the memory/cycles for it. Maybe set defaults scaled to each hardware platform.

It should work with distance_sensor and obstacle_distance as uorb inputs.

Describe possible alternatives

Remove collision prevention from PX4 and expect it to be done on the companion computer in offboard mode.

Additional context

No response

[AlexKlimaj]

/bounty $500

[algora-pbc]

## 💎 $500 bounty • Ascend Engineering
## 💎 $500 bounty • PX4 Autopilot

### Steps to solve:
1. Start working: Comment /attempt #22464 with your implementation plan
2. Submit work: Create a pull request including /claim #22464 in the PR body to claim the bounty
3. Receive payment: 100% of the bounty is received 2-5 days post-reward. Make sure you are eligible for payouts

Thank you for contributing to PX4/PX4-Autopilot!

Add a bounty • Share on socials

Attempt	Started (GMT+0)	Solution
🔴 @aazam-gh	Dec 5, 2023, 11:07:50 AM	WIP
🟢 @Sambit003	May 18, 2024, 8:46:51 AM	WIP
🔴 @ydv129	Jun 23, 2024, 4:24:30 AM	WIP
🟢 @Claudio-Chies	Jul 22, 2024, 11:26:10 AM	#23507
🟢 @vishwamartur	Nov 13, 2024, 5:32:00 AM	WIP

[junwoo091400]

Currently obstacle avoidance in PX4 is broken.

As mentioned in #22312 (comment), there I think we should make a distinction between Obstacle Avoidance via Companion Computer vs Collision Prevention within PX4.

I'm quite surprised that the Collision Prevention doesn't work so well currently (excluding Manual Acceleration case, which was never supported). Will check it out!

[DronecodeBot]

This issue has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-bounties-are-here-solve-an-issue-receive-a-reward/35604/1

[Jaeyoung-Lim]

@AlexKlimaj https://github.com/PX4/PX4-Avoidance was already using VFH, but also probably broken.

Is this concerning collision prevention? or obstacle avoidance? Obstacle avoidance never really existed on the PX4 level and usually hard to achieve with reactive methods in general. There has been recent interest on reactive avoidance methods using Riemannian Motion Policies in academia(e.g. https://ieeexplore.ieee.org/abstract/document/10161365), but require a dense lidar to work reliably.

[OgnjenX]

I tested the collision prevention feature today with a setup similar to this one #22418
It seems to me that VFH works (yet only in position mode - when MPC_POS_MODE is set to 0 which is displayed as 'Direct velocity' in the QGroundControl parameter list).

@AlexKlimaj can you explain more details on missing/broken features? It does not work when in 'Manual acceleration mode', but I guess the reason why it has never been implemented is because of the speed limitation required for the algorithm to work correctly (yet I'm not completely sure).
I'm on the other hand interested in local obstacle avoidance implementation (without a companion computer) which is intended mainly for auto modes - like it is, for example, done in Ardupilot https://ardupilot.org/copter/docs/common-object-avoidance-landing-page.html

[DronecodeBot]

This issue has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/using-obstacle-distance-uorb-msgs-for-collision-prevention/37524/8

[dirksavage88]

@OgnjenX the broken part of collision prevention is that the guide angle tuning doesn't work. But also not working with the default MPC_POS_Mode probably doesn't help either.

Collision prevention should not only stop the vehicle but also give setpoints around the obstacle to "slide" past the obstacle. I believe it was intended as a middle ground between basic obstacle detection and stopping and a full fledged companion computer that incorporates a nav stack/planner.

[Sambit003]

/attempt #22464

Options

• Cancel my attempt

[ydv129]

/attempt #22464

Options

• Cancel my attempt

[Claudio-Chies]

/attempt #22464

Options

• Cancel my attempt

[vishwamartur]

/attempt #22464

Options

• Cancel my attempt

[algora-pbc]

💡 @Claudio-Chies submitted a pull request that claims the bounty. You can visit your bounty board to reward.

[AlexKlimaj]

@Claudio-Chies Great work! Let me figure out how to award the bounty.

[algora-pbc]

🎉🎈 @Claudio-Chies has been awarded $500! 🎈🎊

• Share on socials
• Give feedback

[algora-pbc]

🎉🎈 @Claudio-Chies has been awarded $500! 🎈🎊

• Share on socials
• Give feedback

[Claudio-Chies]

@AlexKlimaj Thanks alot!