[FEATURE]: Implement real-time position-based safety verification for missions

[Evan Palmer]

Is your feature request related to a problem? Please describe

In it's current state, there is ample information available in pymavswarm to perform real-time safety verification using reachability analysis, as described here. Supporting this safety verification would ensure that the system prevents collisions between agents and opens the door for implementing safe trajectory calculation in the future.

Describe the solution you'd like

Implement real-time safety verification using the safety algorithm proposed by Decentralized Real-Time Safety Verification for Distributed Cyber-Physical Systems. This should be implemented for agents that are currently executing missions or goto commands. Future work for this may include takeoff commands; however, this will be defined as out-of-scope for this issue. If a collision is detected, a stop mission command will be immediately sent to attempt collision prevention.

Describe alternatives you've considered

Alternatively, state prediction could be performed up to n seconds using the previous state of agents and if a collision will occur using the prediction, send a command to cancel the mission.

Implementation Ideas

• The reachable space of each agent should be computed as a callback for the ATTITUDE and GLOBAL_POSITION_INT messages. These callbacks could update the reachable space property stored on-board agents. When a potential collision is detected, the MavSwarm interface should handle commanding each agent to perform a collision prevention maneuver.

Additional context

Information that may contribute toward completing the implementation of this issue is as follows:

• Decentralized Real-Time Safety Verification for Distributed Cyber-Physical Systems
• Results of the identified paper
• StarL
• rtreach
• Collision prevention discussion

Edit: Added rtreach link