Multi-Agent Coordination Planner for Multi-Goal Tasks

Plan and control multiple MuSHR cars to different goals without collisions

Intermediate Tutorial | Expected duration is 30 minutes

By: Podshara Chanrungmaneekul, Adit Jha

Introduction

Goal

This tutorial will teach you how to setup the Enhanced Conflict-Based Search with Optimal Task Assignment (ECBS-TA) planning algorithm on a set of MuSHR cars. ECBS-TA produces a set of collision free trajectories, one for each car, from start to goal. By the end of the tutorial, a set of cars will be able to generate paths that avoid each other.

Requirements

If in sim, complete the quickstart tutorial
If on real car, complete the first steps tutorial
MuSHR Navigation System mushr_navigation_system

Coordination Planner Problem Statement

There is a team of num_agents MuSHR cars and a set of num_goals tasks. Each task requires one car to follow all of the num_waypoints points and stop at the last one. The environment is a grid world where the movement of the car is restricted to orthongonal movement. This approximate approach can work well when combined with the nhttc controller. The coordination planner has the ability to create an initial path for each car that is avoiding all possible collisions. This path is leveraged with the nhttc controller to adapt a car’s path and avoid collisions in real-time if need be. The combination of these two services is more powerful than just the nhttc controller by itself as potential conflicts are sorted out prior to declaring each car’s path. It is possible that some cars will not have any tasks and others will have multiple to complete sequentially.

Install

Clone repo and install the package:

$ cd ~/catkin_ws/src/
$ git clone https://github.com/prl-mushr/mushr_coordination.git
$ cd mushr_coordination
$ git clone https://github.com/whoenig/libMultiRobotPlanning.git
$ cd ~/catkin_ws/
$ catkin_make

API

For adjusting params see launch/mushr_coordination.launch it has the environment params for the planner. Change the number of waypoints for each goal by setting parameter num_waypoint. Change the car team setting by editing or creating new config file.

Running the coordination planner

Run the planner that will set up the environment publishers and subscribers.

$ roslaunch mushr_coordination mushr_coordination.launch cars_file:={car's team config file}

For simulation, first launch the mushr_sim. Download the multi_teleop.launch to the mushr_sim repository. Run rviz then and subscribe to planner’s topics. Download the rviz config file for an out-of-box rviz setup. This setup subscribes to all the important rostopics and organizes them in categories.

Launch the mushr_sim.

$ roslaunch mushr_sim multi_teleop.launch

$ rviz

Below is the format for setting the initial position for a team of cars and a set of tasks to complete. For out-of-box examples, look at the Examples of Initializing the Planner section which utilizes files already part of the repository.

$ roslaunch mushr_coordination init_planner.launch cars_file:={car's team config file} tasks_file:={task benchmark file}

Configuration File Example for Car Information

This 4cars1.yaml file shows how to edit and create a config file. The file will have the number of agents (num_agent) following with car{n}’s name and color in hex.

Note that, It would be advised to use the same config file for both mushr_coordination and init_planner.

num_agent: 4
car1:
    name: "car1"
    color: "FF0000" #red
car2:
    name: "car2"
    color: "00FF00" #green
car3:
    name: "car3"
    color: "0000FF" #blue
car4:
    name: "car4"
    color: "DB921D" #orange

Examples of Initializing the Planner

Example #1

In this environment, there are 4 cars and have been given 4 tasks to complete within a 5 by 3 space.

$ roslaunch mushr_coordination mushr_coordination.launch cars_file:=4cars.yaml
$ rviz
$ roslaunch mushr_coordination init_planner.launch cars_file:=4cars.yaml tasks_file:=4cars4tasks5x3.yaml

Each car get assigned to their respective task, in which their accumulated distance would be minimal.

Example #2

In this environment, there are 4 cars and have been given 6 tasks to complete within a 6 by 6 space.

$ roslaunch mushr_coordination mushr_coordination.launch cars_file:=4cars.yaml
$ rviz
$ roslaunch mushr_coordination init_planner.launch cars_file:=4cars.yaml tasks_file:=4cars6tasks6x6.yaml

Number of tasks is more than number of cars, so the blue car and the green car have been given one task more than the others to complete.

Example #3

In this environment, there are 4 cars and have been given 2 tasks to complete within a 5 by 3 space.

$ roslaunch mushr_coordination mushr_coordination.launch cars_file:=4cars.yaml
$ rviz
$ roslaunch mushr_coordination init_planner.launch cars_file:=4cars.yaml tasks_file:=4cars2tasks5x3.yaml

There is not enough task to assign to every cars. In this case, the red car and green car are idle and stay in place.