Gestelt

Gestelt: A framework for accelerating the sim-to-real transition for swarm UAVs

@article{tan2024gestelt,
  title={Gestelt: A framework for accelerating the sim-to-real transition for swarm UAVs},
  author={Tan, John and Sun, Tianchen and Lin, Feng and Teo, Rodney and Khoo, Boo Cheong},
  journal={IEEE International Conference on Control & Automation},
  year={2024},
  publisher={IEEE}
}

Paper: Gestelt: A framework for accelerating the sim-to-real transition for swarm UAVs

Or see the pdf at: tan2024gestelt.pdf

For simulation and deployment on a physical drone, PX4 is the firmware of choice, although it is possible to remap the topics for use with Ardupilot or any other Mavlink-compatible system.

Multi-agent navigation with corridor constraints

Architecture

Navigation module

The navigation module comprises the front-end planner, safe flight corridor generation and back-end planner. The front-end planner utilises either a search-based or sampling-based algorithm to plan an initial collision-free path from a given starting point to the goal position. Then, the front-end path acts as a reference for safe flight corridor generation, similar to what has been done in \cite{liu2017sfc}. Finally, the front-end path and the safe flight corridor are given as an initial guess and constraint respectively to the back-end optimizer.

Perception module

The perception module provides a three-dimensional voxel grid map that is queried by the navigation module for collision checking. Map construction is performed using the probabilistic mapping method with point cloud input from either a simulated or actual sensor. The use of voxels lends itself to efficient nearest neighbour queries using KD-Trees, which is crucial for collision checking in path planning algorithms. Voxel representation utilises the Bonxai library , a lightweight and performant hierarchical data structure influenced by OpenVDB.

Controller module

The controller module is a state machine controlling the flow between different low-level flight states such as taking off, landing, emergency stop and mission mode. Additionally, it converts trajectories from the navigation module and converts it to positional/velocity/acceleration command inputs for execution

Planner adaptor

The last module, planner adaptor, acts as an abstraction layer between the controller and navigation module. This is useful because existing available planners can be integrated without any heavy modification. The only requirement is to establish a set of ROS message interfaces between the planner and planner adaptor, thereby treating the planner module as a black box.

Dependencies

1. ROS Noetic
2. Ubuntu 20.04

Installation and Setup for Simulation

1. Install dependencies

# Install ROS (if not done)
sudo apt install ros-noetic-desktop-full

# Install other dependencies
sudo apt install git build-essential tmux python3-catkin-tools python3-vcstool xmlstarlet -y
sudo apt install ros-${ROS_DISTRO}-mavlink ros-${ROS_DISTRO}-mavros ros-${ROS_DISTRO}-mavros-msgs ros-${ROS_DISTRO}-mavros-extras -y
# For vicon package
sudo apt-get install ros-noetic-vrpn-client-ros

2. Clone third party repositories

mkdir -p ~/gestelt_ws/src/
cd ~/gestelt_ws/src
git clone https://github.com/JohnTGZ/gestelt.git -b master
cd gestelt
vcs import < simulators.repos --recursive
vcs import < thirdparty.repos --recursive

3. Install PX4 firmware

# cd to PX4-Autopilot repo
cd ~/gestelt_ws/PX4-Autopilot
bash ./Tools/setup/ubuntu.sh 
# Make SITL target for Gazebo simulation
DONT_RUN=1 make px4_sitl gazebo-classic

# Copy the custom drone model over
cp -r ~/gestelt_ws/src/gestelt/gestelt_bringup/simulation/models/raynor ~/gestelt_ws/PX4-Autopilot/Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/

# [EMERGENCY USE] IF you screw up the PX4 Autopilot build at any point, clean up the build files via the following command:
make distclean

4. Building the workspace

# Assuming your workspace is named as follows
cd ~/gestelt_ws/

# Building for debugging/development
catkin build
# Building for release mode (For use on Radxa)
catkin build -DCMAKE_BUILD_TYPE=Release

Quick start

0. Configurations

1. The primary planner configuration file is navigator.yaml. We can choose between 3 different planner configurations, the EGO-planner, Spherical Safe Flight Corridor or Polyhedral Safe Flight Corridor:

• EGO-Planner [4] with modifications.

...
##### Set planner types
front_end/planner_type: ~  # IGNORED
sfc/planner_type: ~        # IGNORED
back_end/planner_type: 1   # EGO-Planner
...

• Spherical Safe Flight Corridor based on [5] with modifications and improvements.

...
##### Set planner types
front_end/planner_type: 1  # Choose between A* (0) or JPS (1)
sfc/planner_type: 0        # Spherical SFC
back_end/planner_type: 0   # Spherical SFC Back-end
...
 # IMPORTANT NOTE: IF USING JPS for front-end, "interpolate" must be set to TRUE
 # No special configuration required if using A*
 jps:
    ...
    interpolate: true           # Interpolate JPS (necessary for spherical safe flight generation)
...

• Polyhedral Safe Flight Corridor based on [1], [2] and [3].

...
##### Set planner types
front_end/planner_type: 1  # Only use JPS (1)
sfc/planner_type: 1        # Polyhedral SFC
back_end/planner_type: 2   # Polyhedral SFC Back-end
...
 s# IMPORTANT NOTE: This is mainly made use with JPS without interpolation
 jps:
    ...
    interpolate: false           # Do not interpolate JPS
...

2. Another crucial configuration file is the planner adaptor configuration minco_adaptor.yaml which determines how the executable command is sampled and fed to the trajectory server. The trajectory server which handles take-off, landing, mission-mode etc. has it's parameters at traj_server_default.yaml.

3. The configuration for the fake drone (Simulated drone with no dynamics) and the fake sensors are located at fake_drone_params.yaml and fake_sensor_params.yaml

4. The configuration for PX4 are px4_config.yaml and px4_pluginlists.yaml

1. (Simulation with dynamics) Run PX4 SITL with Gazebo.

Instructions to be updated.

2. (Simulation without dynamics) Run a drone simulation without dynamics

Inside the /home/john/gestelt_ws/src/gestelt/gestelt_bringup/scripts folder, we can run the following scenarios:

# empty_map scenario for 1 drone
./scenario.sh -s empty_map
# Vicon environment
./scenario.sh -s vicon_empty1
./scenario.sh -s vicon_empty3
./scenario.sh -s vicon_obs3
# cluttered forest for 1 drone
./scenario.sh -s forest_single
./scenario.sh -s forest1_0p8
# antipodal_swap for 8-10 drones
./scenario.sh -s antipodal_swap8_empty
./scenario.sh -s antipodal_swap10_empty
./scenario.sh -s antipodal_swap8
./scenario.sh -s antipodal_swap10
./scenario.sh -s antipodal_swap12
# Forest scene for 8 to 16 drones with obstacles
./scenario.sh -s forest8_50obs
./scenario.sh -s forest10_50obs
./scenario.sh -s forest16_50obs

Creating new scenarios

Each scenario is a pre-set configuration of drones and an environment, which is ideal for reproducibility of results during testing

1. Generate a new point cloud file (as the map environment) and name it SCENARIO_NAME.pcd in gestelt_bringup/simulation/fake_maps. The density of the obstacles can be configured in forest_generate.yaml. Further modifications will need to be done via the source code fake_map_generator.cpp.

# Generate a new PCD file using fake_map_generator node
roslaunch fake_map fake_map_generator.launch

2. Create new launch file SCENARIO_NAME.launch in gestelt_bringup/launch/scenarios/scenarios. This launch file determines the number of drones and their initial configuration.

3. Create new mission script SCENARIO_NAME.pys in gestelt_bringup/src/scenarios. This python script will automate taking off and sending goal waypoints to the drones. See the other files for an example.

Additional tips

To kill all ROS Nodes and Gazebo immediately, use the following command:

# To kill everything, use the following command
killall -9 gazebo; killall -9 gzserver; killall -9 gzclient; killall -9 rosmaster; tmux kill-server;

To speed up building of packages, try the following

1. Cache packages that have been built: catkin build --env-cache
2. Add packages to skiplist to save time

catkin config --skiplist bonxai catkin_simple ikd_tree planner_adaptor logger gestelt_test trajectory_inspector fake_drone fake_map swarm_bridge swarm_collision_checker central_benchmark  jps3d trajectory_server gestelt_debug_msgs gestelt_msgs vrpn_client_ros simple_quad_sim path_searching decomp_test_node
# Clear from skiplist
catkin config --no-skiplist

3. Change Optimization flags to influence build speed

catkin_make -DCMAKE_BUILD_TYPE=Release
# Release with optimization and debug symbols and 
catkin_make -DCMAKE_BUILD_TYPE=RelWithDebInfo 

# No optimization done
catkin_make -DCMAKE_CXX_FLAGS=-O0
catkin_make -DCMAKE_BUILD_TYPE=Debug

Contributors

1. John Tan (NUS)
2. Tianchen Sun (NUS)
3. Lin Feng (NUS)
4. Dan Dan (NUS)
5. Navin (NUS)

Acknowledgements

1. EGO-Planner-V2 repo
2. ETHZ-ASL/mav_trajectory_generation

Third party libraries

1. Bonxai
2. DecompROS
3. convex_decomp_util
4. ikd_tree
5. jps3d
6. catkin_simple

References

[1] Toumieh, C. and Lambert, A., 2022. Voxel-grid based convex decomposition of 3d space for safe corridor generation. Journal of Intelligent & Robotic Systems, 105(4), p.87.

[2] Toumieh, C. and Lambert, A., 2022. Shape-aware Safe Corridors Generation using Voxel Grids. arXiv preprint arXiv:2208.06111

[3] Liu, S., Watterson, M., Mohta, K., Sun, K., Bhattacharya, S., Taylor, C.J. and Kumar, V., 2017. Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments. IEEE Robotics and Automation Letters, 2(3), pp.1688-1695.

[4] Xin Zhou et al. ,Swarm of micro flying robots in the wild.Sci. Robot.7,eabm5954(2022).DOI:10.1126/scirobotics.abm5954

[5] Ren, Yunfan, et al. “Bubble Planner: Planning High-Speed Smooth Quadrotor Trajectories Using Receding Corridors.” 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), IEEE, 2022. Crossref, https://doi.org/10.1109/iros47612.2022.9981518.