SIMP: Safe Interval Motion Planning for Quadrotors

About

SIMP is an efficient, safe interval motion planning framework for navigation in dynamic environments.

Authors: Songhao Huang *, Yuwei Wu *, Yuezhan Tao, and Vijay Kumar

Related Paper: Songhao Huang*, Yuwei Wu*, Yuezhan Tao, and Vijay Kumar. "Safe Interval Motion Planning for Quadrotors in Dynamic Environments," 2025 IEEE International Conference on Robotics and Automation (ICRA), Atlanta, GA, USA, 2025, pp. 2780-2786, doi: 10.1109/ICRA55743.2025.11127946. Paper, Video , Preprint

If this repo helps your research, please cite our paper at:

@INPROCEEDINGS{11127946,
  author={Huang, Songhao and Wu, Yuwei and Tao, Yuezhan and Kumar, Vijay},
  booktitle={2025 IEEE International Conference on Robotics and Automation (ICRA)}, 
  title={Safe Interval Motion Planning for Quadrotors in Dynamic Environments}, 
  year={2025},
  volume={},
  number={},
  pages={2780-2786},
  keywords={Navigation;Search methods;Dynamics;Trajectory;Planning;Windows;Safety;Splines (mathematics);Optimization;Quadrotors},
  doi={10.1109/ICRA55743.2025.11127946}}

Requirements

• Ubuntu 20.04
• ROS Noetic

sudo apt update
sudo apt install \
  python3-catkin-tools \
  python3-wstool \
  libsdl1.2-dev \
  libsdl-image1.2-dev

Workspace Setup

1. Clone This Repository

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
git clone git@github.com:KumarRobotics/SIMP.git

2. Import External Repositories

From the workspace src directory:

cd ..
cd src
wstool init . utils.rosinstall
wstool update

3. Build

cd ~/catkin_ws
catkin build
source devel/setup.bash

Run

The main top-level launch is simp.launch. It starts:

• the quadrotor control stack
• the simulator or hardware comms path through kr_mav_launch
• structure_map
• the planner node configured through advanced_param_dyn.xml

Run it with:

roslaunch plan_core simp.launch

and

rosrun rqt_mav_manager rqt_mav_manager

It will apear a GUI, click motors on and take off, and use "Waypoint Nav" to set goals.

By default, the topics:

• odometry from /<mav_name>/odom
• map cloud from /structure_map/global_cloud
• dynamic obstacle trajectories from /structure_map/obsinfo_topic

Acknowledgements

• ego planner the codebase is built on ego planner
• kr_param_map for parameterized dynamic map evaluations
• OpenAI API for code refactoring

Maintaince

For any technical issues, please contact Yuwei Wu (yuweiwu@seas.upenn.edu, yuweiwu20001@outlook.com).