Implemented Algorithms: Collvoid
Welcome to multi-robot path planning and collision avoidance with the Collvoid algorithm!
During the research, the multi_robot_collision_avoidance ROS packages has been found (see Research Results). Since this package was developed based on ROS Indigo distribution, it had to be migrated to Melodic and afterwards, adapted to the project's needs. In the original code, there were two planner plugins implemented. The one augmenting the DWALocalPlanner and one self-written planner called CollvoidLocalPlanner. The main difference is: The DWA planner samples several velocities and scores them afterwards based on predefined criteria, while the CollvoidLocalPlanner directly calculates one velocity only (based on the spatial information of the neighbors). Both will be described and analyzed separately.
The core of Collvoid is the utilization of so called velocity obstacles. That is why the following sections will first give a short overview of theoretical-background, while the remainder is explaining the actual code. Lastly, the evaluation results of the benchmark executed with Collvoid are listed.
Due to the scope and complexity of the original ROS package its exact functioning could not be retraced entirely. That's why, the implemented program is not completely validated. This means, the actual functionality of the code can differ from the theory aimed to achieve (described below). Wherever possible, remarks were made.
Velocity Obstacles
A velocity obstacle (VO) [6] is the set of all velocities of one robot that will result in a collision with a second robot (assuming that the other robot maintains its current velocity).
Velocities outside the VO are collision-free.
In velocity space, VOs are geometrically represented as cones.
To identify safe movements in multi-agent scenarios, the VOs of all surrounding agents have to be determined - for each agent.
However, "[i]n a multi-mobile robot scenario, robots can not merely be regarded as dynamic obstacles.
Each robot is a pro-active agent, taking actions to avoid collisions. [...] Van den Berg et al. [5] introduced the reciprocal velocity obstacle to address reciprocity: for each pair of robots, one takes half of the responsibility to avoid the other.
RVO has since been extended to the hybrid reciprocal velocity obstacle [4] to overcome ”reciprocal dances” that occur when robots cannot reach independent agreement on which side to pass each other." [1]
Nevertheless, the velocity obstacle is the representation only.
The collision free velocities have to be computed by an algorithm.
Therefore, the optimal reciprocal collision avoidance (ORCA, [2]) was developed.
Its objective is to find the collision-free velocity that is closest to preferred, i.e. current, velocity.
Based on that, the Non-holonomic optimal reciprocal collision avoidance (NH-ORCA, [3]) allows adding kinematic and dynamic constraints.
This is necessary because "differential drive robots with only two motorized wheels are much more common [...] and can only accelerate and decelerate within certain dynamic constraints." [1]
Collvoid Naming and Approach
The ROS package implements the algorithm described in this paper referred to as COCALU (Convex outline collision avoidance under localization uncertainty) [1]. This is because the code initially was the outcome of the Master thesis of Daniel Claes where the algorithm was called Collvoid (collision avoidance). With each paper published a new name was introduced in order to distinguish the algorithms (CALU, COCALU etc.). However in the code, the namespace "Collvoid" was maintained which is the reason for the same naming for this project.
In contrast to other approaches using global positioning (e.g. overhead tracking camera) or assuming perfect sensing, the Collvoid program combines the velocity obstacle approach with individual on-board localization (AMCL) and navigation. It is based on:
- decentralized independent computation of actions,
- on-board localization, and
- local inter-robot communication to share information about shape (footprint), position and velocity.
With this data the velocity obstacle representation is build, namely HRVOs. In order to approximate the real robot's footprint the shape of the particle cloud created by AMCL is used (instead of simplifying it to a circumscribed circle that is inflated depending on the localization uncertainty). For further details on how it is determined, please refer to the paper.
To compute new collision-free velocities, instead of NH-ORCA (see section Velocity Obstacles), another algorithm called ClearPath [7] is employed.
Here, the velocity chosen is either "on the intersection of two line segments of any two velocity obstacle,
or the projection of the preferred velocity onto the closest leg of each velocity obstacle.
[1]
References
### References * [`1`] [Collision Avoidance under Bounded Localization Uncertainty](http://www.willowgarage.com/sites/default/files/2012%20IROS%20Collision%20avoidance%20under%20bounded%20localization%20uncertainty.pdf) * [`2`] [Reciprocal n-body Collision Avoidance](http://gamma.cs.unc.edu/ORCA/publications/ORCA.pdf) * [`3`] [Optimal Reciprocal Collision Avoidance for Multiple Non-Holonomic Robots](https://s3-us-west-1.amazonaws.com/disneyresearch/wp-content/uploads/20140726232515/alonsomora10dars_paper1.pdf) * [`4`] [The Hybrid Reciprocal Velocity Obstacle](http://gamma.cs.unc.edu/HRVO/HRVO-T-RO.pdf) * [`5`] [Reciprocal velocity obstacles for real-time multi-agent navigation](http://gamma.cs.unc.edu/RVO/icra2008.pdf) * [`6`] [Motion Planning in Dynamic Environments using Velocity Obstacles](https://static.aminer.org/pdf/PDF/000/351/930/motion_planning_in_dynamic_environments_using_the_relative_velocity_paradigm.pdf) * [`7`] [Clearpath: Highly Parallel Collision Avoidance for Multi-Agent Simulation](http://gamma.cs.unc.edu/CA/ClearPath.pdf)Integration with ROS
Integration with ROS nav stack
Since the Collvoid algorithm is designed to avoid collisions with other robots, its major issue is the lack of handling static and temporary obstacles of the environment. That is why, it is integrated into the ROS move_base. In the original code, there were two local planner plugins implemented:
-
local planner 1:
DWALocalPlanner= Original DWA augmented with collvoid scoring function , -
local planner 2:
CollvoidLocalPlanner= self coded VO planner augmented with useful scoring functions
The main difference is:
The DWALocalPlanner samples several velocities and scores them afterwards based on predefined criteria (including collvoid costs evaluating in terms of VOs),
while the CollvoidLocalPlanner calculates one optimal velocity (based on the spatial information of the neighbors).
However, in order to consider also static obstacles, samples are created close to the optimal velocity and subsequently, scored similarly to the DWALocalPlanner.
Apart from the local planner both implementations share the same components:
As global planner Navfn is chosen.
Localization is done via AMCL.
By integrating it into the nav stack, the map representation is already incorporated (costmap_2d).
An image of the world serves as the basis of the static layer, whereas the obstacle layer is created by using laser scanner information.
This includes static as well as dynamic obstacles.
Select one of the planners for further details on the implementation (params and insights into the move_base):
Changes to the Original
The original ROS package multi_robot_collision_avoidance is implemented with the ROS distribution Indigo, mainly written in C++.
In order to use it for this project, the code was migrated to the ROS distribution Melodic.
Mainly, this included replacing deprecated methods, packages and libraries (e.g. tf to tf2, amcl and dwa code of melodic distro) as well as adjusting build files.
At this point, already some of the original components were discarded because they were not necessary for the project's purpose (e.g. experiments),
or their features were already covered in another package (e.g. Gazebo simulation instead of Stage, recovery behavior).
After the code has been successfully compiled, a bunch of adaptions were necessary to run the code eventually (e.g. launch and shell scripts, topic names, parameters, tf-tree).
Useful hint: If catkin build doesn't work, use catkin_make_isolated instead. When the code successfully compiles that way, delete the "devel" folder and compile again with catkin build.
Moreover, the original map layers were modified such that a static layer is created, and laser scanner data is used to add obstacle information in the map.
Additionally, a map layer called "stationary_robots" was inserted. In the CollvoidLocalPlanner, this layer is used like in the original code.
In the DWALocalPlanner, on the other hand, the "stationary_robots" layer was removed (commented out).
If you wish to use it anyways, please remove the comment signs in costmap_common_params.yaml and add the layer to the recovery behaviors in move_base_params.yaml.
(More on stationary robots layer: The purpose is to overcome the detection problem in a multi-robot scenario where all robots have the same hight (homogeneous). Basically, they can only detect the others laser scanner which is significantly smaller than the robot in most cases. That's why, each robot's footprint (determined based on the AMCL point cloud) is written into in the costmap as an obstacle. Assuming such a scenario where robots cannot detect each other, and using VOs for collision avoidance, especially marking robots that are standing still (stationary) is important. They don't create a VO which would eventually lead to a collision without the extra map layer. Anyways, this is not the case in this project: The laser scanner of the turtlebot3s are insignificantly smaller thant the robots footprint, so that, with the inflation layer, the robot is well represented in the map. The determined footprint is still used for calculating the velocity obstacles. The reason, why it is used in the CollvoidLocalPlanner is simply, that it doesn't seem to work without it. This lack of clarity should be addressed in future work.)
Packages
Note: As mentioned before, the original ROS package was used for different publications in research which is why the code includes different algorithms and further experimental parts (e.g. for human tracking). Thus, there is still some legacy code left that could not be cleaned entirely. The following sections help understanding what is important for this very project.
-
collvoid_amcl: is the original amcl package modified such that weighted particle clouds are published (for the purpose of calculating the robots' footprint)
-
collvoid_benchmark: contains the goal_controller linking the benchmark with the collvoid program
-
collvoid_controller: contains the position_share_controller publishing to the stationary_robots topic and clearing_scan topic
-
collvoid_msgs: contains special message formats, e.g. PoseTwistWithCovariance for inter-robot data exchange
-
collvoid_srvs: declares special services called getMe, getNeighbors (and getObstacles) to get the position, speed etc. of the robot itself and all neighbors
-
collvoid_turtlebot: contains all script, param, and launch files
-
collvoid_local_planner in CollvoidLocalPlanner: contains the algorithms for VO calculation and further scoring_functions adding costs to the trajectory
-
collvoid_local_planner in DWALocalPlanner: contains the algorithms for VO calculation and the collvoid_scoring_function determining the collvoid_costs
Nodes, Namespaces, Transformations, Topics
For a multi-robot scenarios, it is necessary to have common and individual nodes, topics and tf-frames. To create individual nodes, namespaces are used. Here, one namespace belongs to one robot using the naming convention of "tb3_x/..." where x = 0,1,2,... .
- initial_pose_publisher: publishes tf-frames and transformation belonging to one robot ("tb3_x/base_footprint" etc.)
- gazebo: publishes transformation between robot's footprint ("tb3_x/base_footprint") and the odometry frame ("tb3_x/odom")
- amcl: publishes transformation between common frame "map" (without namespace) and "tb3_x/odom"
Common nodes:
- map_server: publishes the map of the world, the robots are moving in
- when running the benchmark, goal_controller: transforms received waypoints (message type: Point) into goals (message type: PoseStamped) and sends them to the move_base (via service API), initializes end_procedure
(corresponds to the movement_controller in the benchmark when enabling the default_movement)
Every robot has the same nodes and topics:
- amcl: for localizing the robot within the map based on odometry data (which is published by gazebo)
- me_publisher: publishes footprint, position and velocity information to common topic "/position_share", and provides service "getMe"
- position_share_controller: subscribes to "/position_share", provides service "getNeighbors", and publishes to topic "/stationary_robots"
- move_base: calculates and publishes the resulting velocity (translation, rotation) to topic "/tb3_x/cmd_vel" (which is subscribed by gazebo), further details below
The dwa local planner seems to beat the collvoid local planner ... /collvoid_res/comparison4.png
... but the dwa local planner is worse with increased number of robots. More experiments are necessary (more robots, other worlds).
- For complete log data and data processing, please, refer to the respective csv and excel files
Params
- Most important parameter for the benchmark is the maximum velocity = 0.22
- For complete parameter set see dwa planner's param files
- For complete parameter set see collvoid planner's param files
TB3 World
({ "model_name": "turtlebot3", "model_type": "burger", "namespace": "tb3_", "number_of_robots": 5, "formation": "dense_block", "distance": 0.3, "position": [1.5, 0.5, 0.5], "orientation": [0.0, 0.0, 0.0], "world": "turtlebot3.world", "wp_threshold": 0.2, "wp_map": "tb3_edge", "rounds": 1, "end_procedure": "idle", "include_start_time": false })
Two-Rooms World (4 robots)
({ "model_name": "turtlebot3", "model_type": "burger", "namespace": "tb3_", "number_of_robots": 4, "formation": "two_rooms", "distance": 0.3, "position": [1.5, 0.5, 0.5], "orientation": [0.0, 0.0, 0.0], "world": "tworooms.world", "wp_threshold": 0.2, "wp_map": "two_rooms", "rounds": 1, "end_procedure": "despawn", "include_start_time": false })
- Complete log data can be found here
- For complete log data please refer to the respective csv and excel files
Two-Rooms World (8 robots)
({ "model_name": "turtlebot3", "model_type": "burger", "namespace": "tb3_", "number_of_robots": 8, "formation": "two_rooms", "distance": 0.3, "position": [1.5, 0.5, 0.5], "orientation": [0.0, 0.0, 0.0], "world": "tworooms.world", "wp_threshold": 0.2, "wp_map": "two_rooms", "rounds": 1, "end_procedure": "despawn", "include_start_time": false })
- Complete log data can be found here
- For complete log data please refer to the respective csv and excel files