Introduction

This package contains the code to build the risk-aware spatio-temporal (RAST) safety corridors and plan a minimum snap trajectory for MAV obstacle avoidance in dynamic uncertain environments. In the src folder, map_sim_example.cpp is the node that builds the DSP map and publishes risks in point cloud form. planning_node.cpp is the node that receives the calculated risks, generates RAST safety corridors, and plans minimum snap trajectory. The interfaces are designed for a PX4 MAV.

Compile

Tested environment: Ubuntu 18.04 + ROS Melodic and Ubuntu 20.04 + ROS Noetic

To compile the source code, you need:

PCL, Mavros, Eigen. PCL and Eigen are included in the desktop-full version of ROS. Mavros is only used for ROS message subscriptions in the example node. Check mavros for installation guidance.

Install munkers-cpp with the following steps.

git clone https://github.com/saebyn/munkres-cpp.git
cd munkres-cpp
mkdir build && cd build
cmake ..
make
sudo make install

Install OSQP, which is a lightweight QP solver. You can follow these installation guidelines.

git clone --recursive https://github.com/osqp/osqp
cd osqp
mkdir build && cd build
cmake -G "Unix Makefiles" ..
cmake --build .
sudo cmake --build . --target install

Clone the code in a ROS workspace, update the submodule, and compile.

mkdir -p rast_ws/src
cd rast_ws/src
git clone https://github.com/g-ch/RAST_corridor_planning.git
cd RAST_corridor_planning
git submodule init & git submodule update
cd ../..
catkin_make

Basic Usage

Input and output Details

The pipeline is Environment -> Mapping (include risk calculation) -> Planning (include RAST corridor building) -> Trajectory tracking.

Mapping

Necessary input to build the DSP map and calculate risk with the map_sim_example mapping node are:

Point cloud from a depth camera in topic "/camera_front/depth/points" with msg type sensor_msgs::PointCloud2.
MAV position data from Mavros in topic "/mavros/local_position/pose" with msg type geometry_msgs::PoseStamped.

Note: Building the DSP map requires the FOV parameter of the depth camera. See DSP Map.

Planning

The planning_node uses the output risk from map_sim_example and MAV position and velocity data

MAV position data from Mavros in topic "/mavros/local_position/pose" with msg type geometry_msgs::PoseStamped.
MAV velocity data from Mavros in topic "/mavros/local_position/velocity_local" with msg type geometry_msgs::TwistStamped.

The output trajectory commands include two forms:

Topic "/pva_setpoint" with msg type trajectory_msgs::JointTrajectoryPoint, which contains one position (x,y,z,yaw), velocity and acceleration target. In our experiments, we use this output form and our pva_tracker to control a PX4 drone.
Topic "/command/trajectory" with msg type trajectory_msgs::MultiDOFJointTrajectory, which contains the position (x,y,z), velocity and acceleration command of 20 steps, where the first step is the current state of the MAV. This output form is designed for mpc-based trackers. If the planned trajectory is less than 20 steps, we use a constant velocity model to predict the rest steps. One step is 0.05s in our code.

Test in Simulation

Quick Test with a ROS Bag

Download a bag file named street.bag containing the point cloud and pose data collected with a MAV in Gazebo. Download. Save the bag file in the data folder and launch a test by

roslaunch rast_corridor_planning quick_test.launch

Test in Gazebo Simulation Environment

We use the PX4 + Gazebo simulation environment. Details of the simulation environment can be found at PX4+Gazebo. After you have installed the simulation environment, you should modify the variable sdf in posix_sitl.launch to use a MAV with a depth camera. In our tests, we disabled the left and the right camera in iris_triple_depth_camera.sdf and use it in posix_sitl.launch.

Launch a simulation test.

Start the simulation environment by opening a command window in your PX4 source code main folder and run:

 DONT_RUN=1 make px4_sitl_default gazebo
 source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default
 export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
 export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo
 roslaunch px4 posix_sitl.launch

Start Mavros:

roslaunch mavros px4.launch fcu_url:="udp://:14540@192.168.1.36:14557"

Hover the drone and start the tracker. You can do this step by using our
pva_tracker.
```
rosrun pva_tracker tracker_sim_auto_arm_takeoff
```

Start mapping and planning by

roslaunch rast_corridor_planning planning.launch

The goal position and parameters related to the planning can be changed in file cfg/cfg.yaml.

Test with a Physical MAV

It is recommended to use a PX4 MAV with a realsense depth camera and NUC or Xavier NX computing board. The pipeline is the same as in the simulation.

Liciense

MIT Liciense.

Additional Information

For more Information abou the DSP map, please refer to DSP Map and preprint.

siyuanwu99/RAST_corridor_planning