Integrate real-time safety verification for collision avoidance (#95)

* Fixed check state example

* Saving point

* Finished initial version of single face lift

* Started implementation of face lifting iterative improvement method

* Finished initial implementation of collision checker and started implementing test cases

* Integrated bandit into dev environment for security checks

* Started implementation of hyperrectangle unit tests

* started implementing unit tests for safety checker and converted safety checker class to static class

* Added tests for collision detection

* Started integrating collision responses

* Implemented initial version of collision response

* Finished implementation of collision checking

* added acceleration measurement support and cleaned up state handlers

* Utilize force disarm for disarm methods

* started implementing functionality to expose collision detection

* Completed initial integration of collision avoidance

* Added warning to collision detection method

* Resolved bugs in collision avoidance and added examples

* Fix stale docstring

* Add check to make sure that the timestamps intersect before marking a collision

* Added check for stale states

* Added location specific config files

* Fix logging error in heartbeat handler

* Updated mavswarm collision avoidance

* Did some quick maths

* Remove unused example argument

* Document safety feature as a main feature of pymavswarm

* DO NOT TOUCH ANYTHING lol

* Cleaned up collision avoidance

* Resolved unfinished changes for PR

Author: evan-palmer

.devcontainer/Dockerfile

@@ -11,6 +11,7 @@ RUN pip3 install --upgrade pip
 # Install all required Python packages
 RUN pip3 install \
     pandas \
+    numpy \
     monotonic \
     pymavlink \
     pyserial \

.devcontainer/devcontainer.json

@@ -16,6 +16,9 @@
     "--volume=/dev:/dev",
     "--privileged"
   ],
+  "containerEnv": {
+    "DISPLAY": "${localEnv:DISPLAY}"
+  },
   "settings": {
     "terminal.integrated.profiles.linux": {
       "bash": {
@@ -35,6 +38,7 @@
     "python.linting.pylintEnabled": false,
     "python.linting.pydocstyleEnabled": true,
     "python.linting.mypyEnabled": true,
+    "python.linting.banditEnabled": true,
     "python.formatting.provider": "black",
     "python.formatting.blackPath": "/usr/local/bin/black",
     "python.testing.unittestArgs": [
@@ -91,7 +95,8 @@
     "DavidAnson.vscode-markdownlint",
     "esbenp.prettier-vscode",
     "LittleFoxTeam.vscode-python-test-adapter",
-    "tamasfe.even-better-toml"
+    "tamasfe.even-better-toml",
+    "alefragnani.Bookmarks"
   ],
   "postStartCommand": "pip3 install -e ."
 }

.github/CONTRIBUTING.md

@@ -14,15 +14,16 @@ Contributions include but are not restricted to:
 
 ## Workflow
 
-- Send all pull requests to the `main` branch (unless otherwise requested)
-- Limit each pull request to resolving a single [issue](https://github.com/unl-nimbus-lab/pymavswarm/issues)
+- Send all pull requests to the `main` branch (unless otherwise requested).
+- Limit each pull request to resolving a single
+  [issue](https://github.com/unl-nimbus-lab/pymavswarm/issues).
 - It is your responsibility to ensure that your development branch is up-to-date
-with the `main` branch. You may either rebase on `main` or merge `main` into
-your development branch.
+  with the `main` branch. You may either rebase on `main` or merge `main` into
+  your development branch.
 - Always test and document your code. We also encourage performing field tests
-for significant changes.
+  for significant changes.
 - Ensure that your changes pass our CI. We will not review your PR until the CI
-passes.
+  passes.
 
 ## Setting up a local development environment
 
@@ -34,7 +35,7 @@ passes.
 A VSCode development container has been provided to offer a fully sandboxed
 development environment. This environment includes all development Python
 packages (e.g., `pre-commit`) used by the `pymavswarm` development team and
-utilizes a variety of VSCode packages that make it easy to run tests and to
+utilizes a variety of VSCode extensions that make it easy to run tests and to
 ensure that all style conventions are followed. Follow the instructions
 [here](https://code.visualstudio.com/docs/remote/containers) to learn how to
 install and launch development containers using VSCode. Once these steps have

.github/PULL_REQUEST_TEMPLATE.md

@@ -20,7 +20,7 @@ include a high-level discussion regarding the implementation of the PR.*
 
 Fixes # (issue)
 
-## Files Changes
+## Files Changed
 
 *A list of all files changed and a summary of the changes made to the respective
 files.*

.pre-commit-config.yaml

@@ -38,13 +38,26 @@ repos:
     hooks:
       - id: pydocstyle
 
+  - repo: https://github.com/PyCQA/bandit
+    rev: 1.7.4
+    hooks:
+      - id: bandit
+
   - repo: https://github.com/pre-commit/pre-commit-hooks
     rev: v4.3.0
     hooks:
-      - id: trailing-whitespace
-      - id: end-of-file-fixer
-      - id: debug-statements
-        language_version: python3.10
+      - id: check-added-large-files
+      - id: check-case-conflict
       - id: check-toml
       - id: check-yaml
         args: ["--unsafe"]
+      - id: check-merge-conflict
+      - id: debug-statements
+      - id: detect-private-key
+      - id: trailing-whitespace
+      - id: check-docstring-first
+      - id: debug-statements
+      - id: end-of-file-fixer
+      - id: mixed-line-ending
+      - id: name-tests-test
+        args: ["--pytest-test-first"]

Pipfile

@@ -8,6 +8,7 @@ monotonic = ">=1.6"
 pymavlink = ">=2.3.3"
 pyserial = ">=3.0"
 pandas = ">=1.4.3"
+numpy = ">=1.23.1"
 PyYAML = ">=6.0"
 
 [dev-packages]

Pipfile.lock

@@ -19,6 +19,7 @@ Here are some of the main features of `pymavswarm`:
 - Implement custom swarm ground control stations
 - Log incoming MAVLink messages for future evaluation and debugging
 - Construct pre-planned missions
+- Multi-agent collision avoidance using reachability analysis
 
 ## Dependencies
 

README.md

@@ -96,7 +96,7 @@ def main() -> None:
     time.sleep(5)
 
     # Disarm each of the agents; retry on message failure
-    future = mavswarm.disarm(retry=True, verify_state=True)
+    future = mavswarm.disarm(retry=True, verify_state=True, force=True)
     future.add_done_callback(print_message_response_cb)
 
     # Wait for the disarm command to complete

examples/arming.py

@@ -0,0 +1,192 @@
+# pymavswarm is an interface for swarm control and interaction
+# Copyright (C) 2022  Evan Palmer
+
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+from __future__ import annotations
+
+import time
+from argparse import ArgumentParser
+from concurrent.futures import Future
+from typing import Any
+
+from pymavswarm import MavSwarm
+
+
+def parse_args() -> Any:
+    """
+    Parse the script arguments.
+
+    :return: argument namespace
+    :rtype: Any
+    """
+    parser = ArgumentParser()
+    parser.add_argument(
+        "port", type=str, help="port to establish a MAVLink connection over"
+    )
+    parser.add_argument("baud", type=int, help="baudrate to establish a connection at")
+    parser.add_argument("config", type=str, help="Pre-planned positions to load")
+    parser.add_argument(
+        "--takeoff_alt", type=float, default=3.0, help="altitude to takeoff to [m]"
+    )
+    parser.add_argument(
+        "--ground_speed",
+        type=float,
+        default=0.5,
+        help="ground speed that the agents should fly at when flying toward each other",
+    )
+    return parser.parse_args()
+
+
+def print_message_response_cb(future: Future) -> None:
+    """
+    Print the result of the future.
+
+    :param future: message execution future
+    :type future: Future
+    """
+    responses = future.result()
+
+    if isinstance(responses, list):
+        for response in responses:
+            print(
+                f"Result of {response.message_type} message sent to "
+                f"({response.target_agent_id}): {response.code}"
+            )
+    else:
+        print(
+            f"Result of {responses.message_type} message sent to "
+            f"({responses.target_agent_id}): {responses.code}"
+        )
+
+    return
+
+
+def main() -> None:
+    """Demonstrate how to use collision avoidance."""
+    # Parse the script arguments
+    args = parse_args()
+
+    # Create a new MavSwarm instance
+    mavswarm = MavSwarm()
+
+    # Attempt to create a new MAVLink connection
+    if not mavswarm.connect(args.port, args.baud):
+        return
+
+    # Get the target agents specified in the config file
+    target_agents = list(mavswarm.parse_yaml_mission(args.config)[0].keys())
+
+    # Wait for the swarm to register all target agents
+    while not all(agent_id in mavswarm.agent_ids for agent_id in target_agents):
+        print("Waiting for the system to recognize all target agents...")
+        time.sleep(0.5)
+
+    # Enable collision avoidance
+    mavswarm.enable_collision_avoidance(
+        3.0, 2.5, 0.1, MavSwarm.COLLISION_RESPONSE_LOITER
+    )
+
+    # Set each agent to guided mode before attempting a takeoff sequence
+    future = mavswarm.set_mode(
+        "GUIDED", agent_ids=target_agents, retry=True, verify_state=True
+    )
+    future.add_done_callback(print_message_response_cb)
+
+    while not future.done():
+        pass
+
+    responses = future.result()
+
+    # Exit if all agents didn't successfully switch into GUIDED mode
+    if isinstance(responses, list):
+        for response in responses:
+            if not response.result:
+                print(
+                    "Failed to set the flight mode of agent "
+                    f"{response.target_agent_id} to GUIDED prior to the takeoff "
+                    "sequence. Exiting."
+                )
+                return
+    else:
+        if not responses.result:
+            print(
+                "Failed to set the flight mode of agent {responses.target_agent_id} to "
+                "GUIDED prior to the takeoff sequence. Exiting."
+            )
+            return
+
+    # Perform takeoff with all agents in the swarm; retry on message failure
+    responses = mavswarm.takeoff_sequence(
+        args.takeoff_alt, agent_ids=target_agents, verify_state=True, retry=True
+    )
+
+    if isinstance(responses, list):
+        for response in responses:
+            print(
+                f"Result of {response.message_type} message sent to "
+                f"({response.target_agent_id}): {response.code}"
+            )
+    else:
+        print(
+            f"Result of {responses.message_type} message sent to "
+            f"({response.target_agent_id}): {responses.code}"
+        )
+
+    # Wait for the user to indicate that the agents should fly to their waypoints
+    input("Press the 'enter' key to command the agents to fly to their waypoints")
+
+    # Command the agent to the target location
+    # We don't need to specify the target agents because these are captured from the
+    # config file
+    future = mavswarm.goto(config_file=args.config, retry=True)
+    future.add_done_callback(print_message_response_cb)
+
+    while not future.done():
+        pass
+
+    # Set the groundspeed; make sure that this is set low when running the examples on
+    # real hardware
+    future = mavswarm.set_groundspeed(
+        args.ground_speed, agent_ids=target_agents, retry=True
+    )
+    future.add_done_callback(print_message_response_cb)
+
+    while not future.done():
+        pass
+
+    # Wait for user input
+    input("Press the 'enter' key to command the agents to land\n")
+
+    # Attempt to land the agents
+    future = mavswarm.set_mode(
+        "LAND", agent_ids=target_agents, retry=True, verify_state=True
+    )
+    future.add_done_callback(print_message_response_cb)
+
+    # Wait for the land command to complete
+    while not future.done():
+        pass
+
+    # Disable collision avoidance
+    mavswarm.disable_collision_avoidance()
+
+    # Disconnect from the swarm
+    mavswarm.disconnect()
+
+    return
+
+
+if __name__ == "__main__":
+    main()