[Local Planner] weird low altitude trajectory

[SwiftGust]

Describe the bug
In SITL, low altitude mission (3-4m AGL) weird trajectory generated from planner during mission straight, no obstacle present path possibly affected by ground below

This behavior could be mitigated by reducing obstacle_cost_param_ but need explanation it is due to planner seeing point from ground or by other reasons.

To Reproduce
Steps to reproduce the behavior:

1. launch local_planner SITL
2. Uploaded simple takeoff - move mission attached
   SITL-SimpleLowAlt.zip
3. start mission
4. See weird trajectory generated by local planner

Expected behavior
Not straight, weird way around trajectory towards waypoint where no obstacles around except the ground

Log Files and Screenshots
SITL Log : https://logs.px4.io/plot_app?log=bdd57226-349e-4ff4-82bb-f9c5d6dbdbe9

Rviz :

Avoidance : commit c9c0952
Firmware : PX4/PX4-Autopilot@f7871ec

[mrivi]

If the mission height is lower than the distance the vehicle has to keep from the obstacle (obstacle_cost_param_), it's expected that the vehicle will try to "avoid" the ground. If you set the mission height greater than obstacle_cost_param_ you should see the vehicle fly a straight line between waypoints when there isn't any obstacle

[SwiftGust]

Thank you for explanation.
While reducing obstacle_cost_param_, small value (eg 2.0) causes collision sometimes.
How to achieve safe distance from obstacle while not over-reacting?

[mrivi]

This is an open problem. We are not doing proper trajectory tracking at the moment and this explais why we cannot guarantee the distance we keep from obstacles

[nicovanduijn]

To be honest, there is only a semantic difference between "ground" and "obstacle". After all, what about terrain changes, like a hill? It makes little sense to keep a minimum distance to obstacles that is larger than the height above ground.

The fact that the minimum distance isn't always kept properly is a separate problem. We cannot currently guarantee that it will always do so, since we are not taking dynamics into account. All that parameter really does, is to make sure the cost function explodes when an obstacle is closer than that value.

Bottom line, there are two intrinsic problems with the algorithm:

• It does not take into account vehicle dynamics, thus cannot guarantee that the planned trajectory is followed exactly. There is some work in progress (Plan dynamically feasible trajectories #449) which you are more than welcome to play with. It's not yet working so well.
• Its sense of "obstacle distance" is histogrammic. Thus, it will always fly tangentially to objects, potentially much closer than the minimum distance. The only way we remedy this is by applying some smoothing to the histogram. There is work in progress for this as well, using a KD-tree to compute nearest-neighbor distances. (KD-tree attempt #494 ). This too, is not yet working. Most dominant problem here is compute load.

Any contributions are of course more than welcome!