This software implements an online coordination method for multiple robots. Its main features are:
- Goals can be posted and paths computed online
- Precedences are inferred online, accounting for robot dynamics via provided dynamic models
- Very few assumptions are made on robot controllers
- The coordination method is not specific to a particular motion planning technique
The software includes a basic 2D robot simulation and a simple built-in motion planner (which depends on the OMPL and MRPT libraries). A separate interface package is provided to enable the use of this software in conjunction with ROS and the navigation_oru stack to obtain a fully implemented stack for multi-robot coordination and motion planning.
The algorithm provided by this implementation is detailed in
- Federico Pecora, Henrik Andreasson, Masoumeh Mansouri, and Vilian Petkov, A loosely-coupled approach for multi-robot coordination, motion planning and control. In Proc. of the International Conference on Automated Planning and Scheduling (ICAPS), 2018.
The approach makes very few assumptions on robot controllers, and can be used with any motion planning method for computing kinematically-feasible paths. Coordination is seen as a high-level control scheme for the entire fleet. Heuristics are used to update precedences of robots through critical sections while the fleet is in motion, and the dynamic feasibility of precedences is guaranteed via the inclusion of user-definable models of robot dynamics.
The coordination method is based on the trajectory envelope representation provided by the Meta-CSP framework. This representation is detailed in
- Federico Pecora, Marcello Cirillo, Dimitar Dimitrov, On Mission-Dependent Coordination of Multiple Vehicles under Spatial and Temporal Constraints, IEEE/RSJ International Conference on Intelligent Robots and Systems (2012), pp. 5262-5269.
In short, a trajectory envelope is a set of spatio-temporal constraints on a robot's trajectory. A trajectory envelope spans over a path, which is a sequence of poses <p1, ... pn>. In the current implementation, the spatial constraints defining a trajectory envelope are computed as the sweep of the robot's footprint over the path.
The approach is discussed in detail in the tutorial on Integrated Motion Planning, Coordination and Control for Fleets of Mobile Robots, given at the 2018 International Conference on Automated Planning and Scheduling (ICAPS) by F. Pecora and M. Mansouri. Slides and source code of the tutorial are available here.
To install, clone this repository:
$ git clone https://github.com/FedericoPecora/coordination_oru.git
$ cd coordination_oru
Install ROS if not already installed:
./install_ros.sh
Install dependencies and the coordination_framework:
./install.sh
The API provides three visualization methods:
BrowserVisualization: a browser-based visualization.JTSDrawingPanelVisualization: a Swing-based visualization.RVizVisualization: a visualization based on the ROS visualization tool RViz.
All three visualizations implement the abstract FleetVisualization class, which can be used as a basis to create your own visualization.
Most examples use the BrowserVisualization. The state of the fleet can be viewed from a browser at http://localhost:8080. The image below shows this visualization for the TestTrajectoryEnvelopeCoordinatorThreeRobots example:
An arrow between two robots indicates that the source robot will yield to the target robot. Priorities are computed based on a heuristic (which can be provided by the user) and a forward model of robot dynamics (which can also be provided, and is assumed to be conservative - see the ICAPS 2018 paper mentioned above). The specific poses at which robots yield are also updated online, based on the current positions of robots and the intersecting areas of their trajectory envelopes (critical sections). This makes it possible to achieve "following" behavior, that is, the yielding pose of a robot is updated online while the "leading" robot drives.
The a Swing-based GUI provided by class JTSDrawingPanelVisualization looks like this:
This GUI allows to take screenshots in SVG, EPS and PDF formats by pressing the s, e and p keys, respectively (while focus is on the GUI window). Screenshots are saved in files named with a timestamp, e.g., 2017-08-13-11:13:17:528.svg. Note that saving PDF and EPS files is computationally demanding and will temporarily interrupt the rendering of robot movements; SVG screenshots are saved much quicker.
The RVizVisualization visualization publishes visualization markers that can be visualized in RViz. The class also provides the static method writeRVizConfigFile(int ... robotIDs) for writing an appropriate RViz confiuration file for a given set of robots. An example of the visualization is shown below.
The visualization with least computational overhead is the RVizVisualization, and is recommended for fleets of many robots. The BrowserVisualization class serves an HTML page with a Javascript which communicates with the coordinator via websockets. Although rendering in this solution is less efficient than in RViz, the rendering occurs on the client platform (where the browser is running), so its computational overhead does not necessarily affect the coordination algorithm. The JTSDrawingPanelVisualization is rather slow and not recommended for fleets of more than a handful of robots, however it is practical (not requiring to start another process/program for visualization) and relatively well-tested.
A simple motion planner is provided for testing the coordination framework without the need for pre-computed path files. The planner can be used to obtain paths for robots with Reeds-Shepp kinematics (Dubin's car-like robots that can move both forwards and backwards), and is used in several of the included demos.
The provided motion planner depends on the Open Motion Planning Library (OMPL), and the Mobile Robot Programming Toolkit (MRPT). The motion planner and its Java interface are purposefully kept very simple. It performs rather poorly in terms of the quality of paths it returns, and is not suited for anything beyond simple examples. Please consider developing a more performing and principled integration with your motion planning software of choice, as done in the coordination_oru_ros package.
Please install the OMPL and MRPT libraries. Both are present in the official Ubuntu repositories (tested on Ubuntu 16.04):
$ sudo apt-get install libompl-dev
$ sudo apt-get install mrpt-apps libmrpt-dev
Then, compile and install the simplereedssheppcarplanner shared library as follows:
$ cd coordination_oru/SimpleReedsSheppCarPlanner
$ cmake .
$ make
$ sudo make install
$ sudo ldconfig
This will install libsimplereedssheppcarplanner.so in your /usr/local/lib directory. A simple JNA-based Java interface to the library is provided in package se.oru.coordination.coordination_oru.motionplanning. The Java class ReedsSheppCarPlanner in the same package can be instantiated and used to obtain motions for robots with Reeds-Shepp kinematics.
The Experience based planning update to the coordination framework allows for storing the previous planning experiences into a database and reuse them during future planning.
- Currently the Lightning and Thunder frameworks provided by OMPL have been implemented in the
OmplPlannerpackage. - Many new test cases have been added to test the frameworks in the BRSU and Agaplesion hospital scenarios
- The RViz visualization has been fixed to show the map published on the /map topic
- A graph visualization tool has been developed to display the experiences stored in the databases
- A script has been added to automatically generate training dataset to bootstrap the planning frameworks with prior experiences
- A script has been added to train the frameworks using the training datasets
Use the GenerateTrainingDataset.py script to generate training dataset. Example:
python3 generators/dataset/GenerateTrainingDataset.py BRSU_Floor0.png --nProblems 25 --dbg_image=True --robot_radius=25 --use_hotspots=True
Use the GenerateTestingDataset.py script to generate testing dataset. Example:
python3 generators/dataset/GenerateTestingDataset.py BRSU_Floor0.png --nRobots 5 --dbg_image=True --robot_radius=25
Use the GenerateOptimalityData.py script to generate optimality path length data for the testing problems. Example:
python3 generators/dataset/GenerateOptimalityData.py BRSU_Floor0.png --count=5
Use the GenerateExperiences.py script to generate experience database using the training problems. For example, to generate the experiences for Lightning framework use:
python3 generators/dataset/GenerateExperiences.py BRSU_Floor0.png --count=25 --planner_type=1
Use the GenerateExperiences.py script to visualize the generated experience database
python3 graphml_generator/PlotDatabase.py --map_image_filename=BRSU_Floor0.png --count=25
Use the launchTests.py script to iteratively execute the testing use-case. Example:
python3 launchTests.py --map=BRSU_Floor0 --nRobots=5 --planner=1 --nExperiences=25 --nIterations=50 --timeout=120 --sleep=10
Use the LogParser.py script to parse the log file and generate two CSV files to extract the planning and execution statistics. Example:
python3 generators/logging/LogParser.py BRSU_Floor0 1 --nRobots=5 --nExperiences=25
Use the AnalyzeLogs.py script to generate plots from the planning and execution statistics of one experiement. Example:
python3 generators/logging/AnalyzeLogs.py BRSU_Floor0 1 --nRobots=5 --nExperiences=25
Use the AnalyzeMultipleLogs.py script to generate plots that compare multiple test-cases of all the experiements. Example:
python3 generators/logging/AnalyzeMultipleLogs.py
This project is supported by
- The Semantic Robots Research Profile, funded by the Swedish Knowledge Foundation
- The ILIAD Project, funded by the EC H2020 Program
- The iQMobility Project, funded by Vinnova
coordination_oru - Online coordination for multiple robots
Copyright © 2017-2018 Federico Pecora
This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
You should have received a copy of the GNU General Public License along with this program. If not, see http://www.gnu.org/licenses/.