Topology-Driven Interactive Robot Swarm

0. Overview

Code in this repo is about the Topology-Driven interactive trajectory optimization framework for vehicle swarm.

Related Paper:

Topology-Driven Trajectory Optimization for Modelling Controllable Interactions within Multi-Vehicle Scenarios(IEEE, arxiv). Changjia Ma, Yi Zhao, Zhongxue Gan, Bingzhao Gao, Wenchao Ding, Accepted by IROS 2025.

Project page: link. Videos about this project can be found there.

1. Setup

All the Simulations are conducted in the Linux environment on a PC equipped with an Intel Core i9-13900K CPU and a GeForce RTX 4090 GPU(Actually, a good GPU is not necessary for this project).

Our software is developed and tested in Ubuntu 20.04 with ROS noetic installed.

Attention! Map generator in this source code requires a higher version of python3, hence we do not recommend using Ubuntu 18 or lower versions since the default version of python is python2. For higer version of ubuntu such as 22.04 and 24.04, this project is probobly not compatible since ROS1 is not supported on these systems.

2. Build on ROS

1. Install an dependency(ompl library):

sudo apt-get install libompl-dev

2. Create an empty new workspace and clone this repository to your workspace:

git clone https://github.com/Fudan-MAGIC-Lab/Interactive-Traj-Opt.git
cd Interactive-Traj-Opt

2. Compile it.

catkin_make -DCMAKE_BUILD_TYPE=Release

Attention! When compiling, be sure to use release mode!(Just follow the above command)

3. Run

source devel/setup.bash

There are 3 launch files with different number of vehicles: swarm_8cars.launch, swarm_12cars.launch, swarm_24cars.launch. You can choose anyone of them to execute. Here we execute the 8 vehicle launch as an example:

roslaunch traj_planner swarm_8cars.launch

(Very important!)Wait for several seconds until "wait for goal or trigger." comes out in the terminal.

Then, you can click the 2D Nav Goal at anywhere in RVIZ to trigger the planning.

Here is an example:

4. Parameter tuning tips

Multi-vehicle trajectory optimization is a complex non-convex problem, the performance of the optimization process is sensitive to the parameters. Here we list some of the parameters that may have an impact on the optimization results:

(1) optimizing/wei_surround_stage1, wei_surround_stage2, wei_topo_stage1, wei_topo_stage2: These parameters can be found in the launch file, meaning the weight of dynamic obstacle avoidance constraint, and topology constraint in the 1st and 2nd stage, respectively. As we mentioned in the paper, at the first stage, we don't account for the dynamic obstacle avoidance. The weight of the topology constraint can be set as a relative small value. At the second stage, both paramters should be set much larger.

(2) planning/traj_piece_duration: This parameter can also be found in the launch file. The MINCO trajectory is a multi-piece polynomial. Before the optimization process, the initial path is uniformly divided into multiple pieces. The number of the pieces is decided by this parameter. The number of pieces = Total time duration of the initial path / traj_piece_duration. When traj_piece_duration is small, there are more pieces and optimization variables, making the trajectory more flexible but also introducing more computation burden. When traj_piece_duration is large, there are less pieces, making the trajectory more smooth but the freedom is limited. Here I recommend keeping the number of pieces to around 5-6. Therefore, you need to estimate the total time duration of the initial path and then decide the traj_piece_duration. If you change the max_vel and max_acc of the vehicle, this parameter probably should also be changed.

(3) arraygradt_container[id].setZero(); and total_timecost = 0; : These two lines can be found in traj_optimizer.cpp. They mean that time is not optimized in that stage. Since the first stage is optimized to a good initial value for the second stage, I recommend not optimizing time in the first stage. If you want to optimize time in the first stage, just comment these two lines.

(4) viola_topo < 2.0, cost_topo = -viola_topo + 2.0, viola_topo > -2.0, cost_topo = viola_topo + 2.0 : These lines can also be found in traj_optimizer.cpp. The parameter "2.0" is added to enhance the topological metric boundary. This parameter can also be tuned, depending on the scale of the vehicle swarm. If there are more vehicles, this parameter can be larger. If the size of the vehicle is smaller, this parameter can be smaller.

(5) planning/time_budget: This parameter can be found in the launch file, it is not very important since the value of it has nothing to do with the final optimization result. This parameter is only used for visualization. It is set to be a little bit larger than the duration of the optimization process. For example, if the computation of the optimzation takes around 2.5 seconds, this parameter can be set as a little bit larger than 2.5, such as 3.0.

Finally, if you want to change the number of the vehicles and configure the setup by yourself, just follow the instructions in the launch file. Hope you enjoy it and have fun!

5. Licence

The source code is released under GPLv3 license.

6. Acknowledgement

The trajectory representation is modified from MINCO.

This project is modified from our previous work Car-Like-Robotic-Swarm.

7. Maintaince

For any technical issue, feel free to contact Changjia Ma([email protected]).