Px4-autopilot: [Feature] Handle avoidance setpoints correctly regarding smoothing

[AuterionWrikeBot]

At the moment, the waypoints coming from obstacle avoidance override the setpoints generated by a FlightTask: https://github.com/PX4/Firmware/blob/f1d17c9003280498724211310c6e5601b1df44da/src/modules/mc_pos_control/mc_pos_control_main.cpp#L693

In order to benefit from smoothing features given by the "Smooth" FlightTasks, we need to read and use those setpoints directly in the FlightTask.
Since the avoidance is only used in the Auto FlightTasks, the setpoints should be read in the AutoMapper.

Tasks:

• [ ] Merge AutoMapper and AutoMapper2
• [ ] Get avoidance waypoints in AutoMapper and ManualPosition

@Stifael @MaEtUgR Please comment

[jkflying]

@mrivi FYI

[bresch]

Update after discussion with @mrivi :

• We need to have avoidance available in manual position and auto modes.
• We want to have the overall same flight behavior when the drone flies a regular mission (no obstacles) with and without avoidance:
  
  
  
  • Solution: Smoothing has to be done in the FlightTask or stop publishing avoidance when there are no obstacles.
  
  
• To avoid discontinuities in the setpoints when the avoidance starts and stop publishing, the FlightTask has to be aware of the avoidance setpoint
  
  
  
  • Solution: Read avoidance setpoints in the FlightTasks
  
  
• If avoidance wants to stop quickly the drone, we don't want to add delay because of the smoothing
  
  
  
  • Solution: Use a second set of jerk/acceleration constraints when the avoidance publishes a setpoint to allow fast reaction.
  
  

[MaEtUgR]

Sidenote: it's confusing when the @AuterionWrikeBot creates issues. Could at least be in the description who wrote it?

we need to read and use those setpoints directly in the FlightTask

yes ok, but we should keep it contained in a library like @mrivi started out in #11561 because if it's modular in this way it's better maintainable.

We need to have avoidance available in manual position and auto modes.

Does the avoidance always react the same way? I expected a different behavior when flying manually compared to a mission.

[mrivi]

We need to have avoidance available in manual position and auto modes.

Does the avoidance always react the same way? I expected a different behavior when flying manually compared to a mission.

PX4 avoidance can be run only in auto modes. However the current interface supports pos control modes as well (even if they are not enabled by default).

[jkflying]

We have collision prevention available in manual position control, using the laser scanner message.

[MaEtUgR]

We have collision prevention available in manual position control, using the laser scanner message.

Ok, that's what I expected, the implementation here https://github.com/PX4/Firmware/tree/6c3e79f3614f0060d5998c39be358512101252ff/src/lib/CollisionPrevention is used with distance data from the external module in manual position flight.

[mrivi]

The PX4 collision prevention doesn't uses the vehicle_trajectory_waypoint interface these changes don't have an impact on that feature.
However I am aware of other obstacle avoidance system integrated with PX4 that can be used in position control mode. Therefore the current master implementation is thought for pos control mode as well even if it's not enabled by default upstream since PX4 avoidance doesn't support it.

[stale[bot]]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

[jkflying]

This is fixed now.