This repository contains the code that is used to perform the aerial docking as part of the project
Autonomous Docking of Multi-Rotor UAVs on Blimps under the Influence of Wind Gusts
by Pascal Goldschmid and Aamir Ahmad.
The main project repository can be found here.
If you have any questions, comments or suggestions please contact pascal.goldschmid@ifr.uni-stuttgart.de.
This repository contains the core components of the project. These comprise mainly a ROS workspace where the required nodes for perfoming the autonomous docking procedure are implemented. Furthermore, nodes for recording gust response data of an airship as well as scripts for the generation of meshes of the no fly zone and hashable functions encoding information for collision avoidance are included in this repository. In addition, scripts for evaluating the performance of the control framework are presented.
This readme file is structured as follows. First, the most important commands for running the simulation environment and control framework are given. Second, the procedure for generating the mesh of the no-fly zone and the calculation of the hashable data structures containing data for obstacle avoidance are presented. And third, the scripts used for the experiment evaluation are explained.
The simulation framework best runs on two distinct computers which should be prepared as described in the following section.
Download the zip file called hashables from here and copy its contents in the folder src/docking_sphinx/hashables.
In the file config file update the values with the information related to your TCN.
Execute the command catkin_make followed by source other_files/setup.bash. This script sets some required environment variables required by the framework.
Prepare your computer to host a distributed ROS system as described here.
The first computer hosts the majority of the system. To this end, you can launch the most important nodes by executing the shell script ./src/docking_sphinx/launch/launch_frg06_sphinx_environment_in_virtual_screens.sh
This hosts several ROS nodes as well as the Sphinx Simulator for the simulation of the Anafi drone in virtual screens.
In order to launch a simulated blimp trajectory, execute the command roslaunch docking_sphinx sphinx_simulate_docking_port_fc_data.launch. In the associate node script located here src/docking_sphinx/scripts/sphinx_simulate_docking_port_fc_data.py adapt the file path to the blimp trajectory you wish to load.
You can always reset the blimp trajectory by publishing on the topic /blimp_trajectory_sim/reset_topic a Bool True message. If the Bool False message is sent, the trajectory is started again if it has been reset before.
In a new terminal window with sourced workspace (see above), execute the command roslaunch anafi_control anafi_control_waypoint_mpc.launch
In another terminal window with sourced workspace, execute the command
rostopic pub /anafi/drone/location_spawn_point_enu geometry_msgs/PointStamped "header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
point:
x: -3.3
y: 0.0
z: 0.0" This is needed to shift the simulated anafi drones state estimate to match the simulated environment.
In a new terminal window with sourced workspace launch the trajectory MPC by running the command roslaunch docking_sphinx sphinx_mpc_controller_osqp.launch
In the associated script located here src/docking_sphinx/scripts/sphinx_mpc_osqp.py you will find a class method called set_mpc_parameters(self). In this function you can define the parameters of the MPC, such as the activating the collision avoidance method or selcting a velocity model.
In a new terminal window with sourced workspace launch the bias EKF by running the command roslaunch docking_sphinx bias_ekf.launch
In a new terminal window with sourced workspace execute the command rosrun docking_sphinx sphinx_anafi_camera_tf_broadcaster.py and
again in a new terminal window with sourced workspace execute the command rosrun docking_sphinx sphinx_detected_tag_tf_broadcaster.py
In a new terminal window with sourced workspace execute the command
rostopic pub /anafi/gimbal/cmd olympe_bridge/GimbalCommand "header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
roll: 0.0
pitch: -88.0
mode: 0" This will make the camera look upwards.
In a new terminal window with sourced workspace launch the system monitor by running the command roslaunch docking_sphinx system_monitor.launch
This node provides messages about the status of the framework which can be visualized with rviz (see below).
In a new terminal window with sourced workspace launch the docking analyzer by running the command roslaunch docking_sphinx docking_analyzer.launch
This node checks if the docking condition has been met. Furthermore, information about the docking procedure such as the blimp and UAV position is stored in a .csv file.
Prepare computer 2 in the same way than computer 1 (see above).
The second computer hosts the blimp simulation and the nodes dealing with the AprilTag detection used for the relative localization. To this end, you can launch the most important nodes by executing the shell script ./src/docking_sphinx/launch/launch_frg05_sphinx_environment_in_virtual_screens.sh
In a new terminal window with sourced workspace launch the AprilTag detection by running the command roslaunch apriltag_ros continuous_detection.launch
In order to synchronize the timestamps between the Sphinx simulator and the Gazebo simulator running the blimp simulation, in a new terminal window with sourced workspace run roslaunch docking_sphinx sphinx_correct_timestamp_of_gazebo.launch
In a new terminal window with sourced workspace, launch rviz by running the command rviz.
Select the configuration file located here other_files/sphinx_simulation.rviz
For the data recording this airship simulation is used. It relies on a SITL control of the airship. This is implemented by means of the open source project LibrePilot. Instructions for the installation and usage of the LibrePilot Ground Control Station (LibrePilot GCS) can be found here.
Once the LibrePilot GCS is installed,launch it and then click on Options --> IP Network Telemetry. Enter localhost in the field Host Name/Number and specify a port of 9000. Select UDP connection and save by clicking Apply and then Ok.
You can use this framework in order to record log data of the responses of an airship to different customizable wind gusts. A wind gust follows a 1-cos shape in a predefined direction. The following parameters are defined in the file params_gust_response_dataset_generation.yaml file and characterize the wind gust and other parameters required for the data logging.
| Category | Parameter | Value (example) | Unit | Notes |
|---|---|---|---|---|
| general | seed | null |
— | Seed used for the random number generator. |
| general | reset_time | 1 |
s | Time between episodes (1 episode = wind gust + time to next episode). |
| max_abs_gust_velocity | min | 4 |
m/s | Will be used for clipping. |
| max_abs_gust_velocity | max | 2 |
m/s | Will be used for clipping. |
| max_abs_gust_velocity | distribution | uniform |
— | Probability distribution. Available are uniform and normal distribution. |
| max_abs_gust_velocity | prob_params | [4, 4] |
— | For uniform distribution:[min, max]; for normal distribution:[mean, std]. |
| gust_direction | x.min | -1 |
m | Will be used for clipping. |
| gust_direction | x.max | 1 |
m | Will be used for clipping. |
| gust_direction | x.distribution | list |
— | Probability distribution. Available are uniform and normal distribution. |
| gust_direction | x.prob_params | [-1, 1] |
— | For uniform:[min, max]; for normal:[mean, std]. |
| gust_direction | y.min | -1 |
m | Will be used for clipping. |
| gust_direction | y.max | 1 |
m | Will be used for clipping. |
| gust_direction | y.distribution | list |
— | Probability distribution. Available are uniform and normal distribution. |
| gust_direction | y.prob_params | [-1, 1] |
— | For uniform:[min, max]; for normal:[mean, std]. |
| gust_direction | z.min | -1 |
m | Will be used for clipping. |
| gust_direction | z.max | 1 |
m | Will be used for clipping. |
| gust_direction | z.distribution | list |
— | Probability distribution. Available are uniform and normal distribution. |
| gust_direction | z.prob_params | [-1, 1] |
— | For uniform:[min, max]; for normal:[mean, std]. |
| — | pre_gust_duration | 10 |
s | Time before wind gust starts. |
| gust_duration | min | 1 |
s | Will be used for clipping. |
| gust_duration | max | 4 |
s | Will be used for clipping. |
| gust_duration | distribution | uniform |
— | Probability distribution. Available are uniform and normal distribution. |
| gust_duration | prob_params | [4, 4] |
s | For uniform:[min, max]; for normal:[mean, std]. |
| gust_break_duration | mode | fixed_episode_length |
— | If fixed_episode_length: pre-gust + gust + break sum to logging.episode_duration. If random: break drawn from distribution regardless of max episode length. |
| gust_break_duration | min | 5 |
s | Will be used for clipping. |
| gust_break_duration | max | 5 |
s | Will be used for clipping. |
| gust_break_duration | distribution | uniform |
— | Probability distribution. Available are uniform and normal distribution. |
| gust_break_duration | prob_params | [30, 30] |
m | For uniform:[min, max]; for normal:[mean, std]. Time after a wind gust. |
| blimp_init_state | px | 0 |
m | Initial position along the x-axis. |
| blimp_init_state | py | 0 |
m | Initial position along the y-axis. |
| blimp_init_state | pz | 30 |
m | Initial position along the z-axis. |
| blimp_init_state | vx | 2 |
m/s | Initial velocity along the x-axis. |
| blimp_init_state | vy | 0 |
m/s | Initial velocity along the y-axis. |
| blimp_init_state | vz | 0 |
m/s | Initial velocity along the z-axis. |
| blimp_init_state | roll | 0 |
m/s | Initial pitch angle. |
| blimp_init_state | pitch | 0 |
rad | Initial roll angle. |
| blimp_init_state | yaw | 0 |
rad | Initial yaw angle. |
| logging | log_id | "evaulation" |
rad | Root name of the recorded .csv files containing the logged data. This name will be suffixed with the integer index of the currently recorded episode. |
| logging | log_dir | "/mnt/data_3/gust_response_data" |
— | Path to where the data shall be stored. |
| logging | episode_duration | 200 |
s | Total length of a recorded episode. |
| logging | max_number_episodes | 30 |
- | Number of recorded episodes. |
| logging | cast_float_type | "float64" |
- | Data format in which the values shall be stored. |
| logging | freq | 10 |
hz | Frequency with which the data shall be logged. |
| logging | verbose | False |
- | Flag for producing a more verbose output to the CLI. |
| logging | show_progress_bar | True |
- | Shows a progress bar in the CLI to indicate the progress made. |
In a new terminal window change directory to this folder and run the following commands
source other_files/setup.bash.`
roslaunch blimp_description blimp_gcs_wind.launchThis will launch the airship simulation in Gazebo.
In a new terminal window, run the commands
source other_files/setup.bash
roslaunch docking_sphinx wind_gust_generator.launch config_file:=params_gust_response_dataset_generation.yamlIn a new terminal window launch the commands
source other_files/setup.bash
rostopic pub /gazebo/ActivateWindGustGenerator std_msgs/Bool "data:true"This will launch the wind gusts acting on the airship.
In a new terminal window, launch the commands
source other_files/setup.bash
roslaunch docking_sphinx gust_response_recorder.launchIn a new terminal window, launch the simposix controller which emulates the real flight controller of the airship by changing directory to the LibrePilot folder first before launching the simposix controller. This can be achieved by using the following commands.
cd .. # go to the root folder of the repository.
cd other_files
./LibrePilot/build/firmware/fw_simposix/fw_simposix.elf 0It is important to launch the simposix controller from the folder other_files because here important configuration files are stored.
In the bottom right corner of the GCS chose the entry UDP:localhost from the drop-down menu and click on Connect. In the tab SITL select ROS HITL from the dropdown menu. In the controls panel on the right side of the screen tick the checkboxes GCS Control and Arm switch (Accessory0). In the drop-down menu labeled Flight Mode select Sw pos 4.
Go back to the main panel and click on the load waypoints button on the right side of the UAV map. Select this file. Upload it to the FC by clicking on the arrow pointing upward. Important: Please do so directly after the airship position has been reseted in Gazebo by the ROS node gust_response_recorder.
Relaunch the gust_response_recorder_node. Data recording is now active.Creation of a hashable data structure for close range maneuvering
The framework requires a hashable data structure to perform the maneuvering of the multi-rotor UAV in close range to the blimp during the docking procedure. To this end, a predefined space around a predefined obstacle defined by a mesh is discretized using a 3D grid. For each grid point certain information is calculated such as the shortest distance to the obstacle mesh. This information is stored in a hashable data structure (a python dict variable) which can be quickly queried.
The script hashable.py can be used for this purpose. When you execute the script you can choose between 3 different options. Proceed a the generation of a hashable data structure if it was previously interrupted, start the generation of a new datastructure or visualize an existing datastructure by plotting some of the generated grid values. Important script parameters can be defined at the top of the file hashable.py.