Implemented Algorithms: Cocktailparty (Bug2 & TangentBug)
!!!!!!!!!!!WIP!!!!!!!!!!!!
Welcome to multi-robot path planning and collision avoidance with the Cocktailparty algorithm!
Imagine a cocktail party with tons of people. A guest decides to talk to someone. He or she accomplishes this by maneuvering between tables, chairs, and other guests, planning his path “on the fly” and not consulting with other people about his or their intended motion. He assumes that other people mean well, and so as long as he somehow takes into account their movements, it is safe to move at a minimal distance from them.
The goal of this project was to implement exactly this behavior on multiple robots. It was archived by implementing a simple, for this purpose adapted, Bug2 algorithm which was later transformed to a TangentBug algorithm.
All Bug type algorithms are based on the fact that the shortest distance between two points is a straight line.
Therefore, the efforts to find the path for a mobile robot are such that to make it as close as possible to the straight line that
crosses the start and the end point. When this cannot be obtained as, for example, an environment with obstacles, the
algorithms tries first to contour the obstacles and then it resumes the path by following the origin destiny line.
Bug2
In the Bug2 algorithm, the robot starts moving directly toward the destination. When an obstacle is found, the obstacle-hit-point is memorized and the robot begins contouring it. If the robot finds the origin destiny line also called M-line during following this obstacle the robot will resume moving to the destiny point but only if this point on the M-Line is closer to the goal than the memorized hit-point.
General algorithm:
- Move along the M-line (line from the start S to the goal G) until one of the following happens:
(a) The goal is reached. The procedure stops
(b) An obstacle is encountered. Go to step 2. - Using the accepted local direction, follow the obstacle boundary until one of these occurs:
(a) The robot meets the M-line at a point between the wall-hit-point H and G that satisfies the leave condition. Go to Step 1.
(b) The condition of target non-reach-ability is satisfied. The procedure terminates.
Multi-robot Adaptions:
-
Safety distance: Since the Bug2 algorithm assumes tactile sensing and we don't want the robot to collide with the wall nor other robots before it changes its direction, a safety distance had to be implemented.
If the safety circle is free of obstacles, the robot can theoretically move in any direction without fear of collision independent from the movement of other robots. For this to work, the safety circle has to be bigger or equal to the sum of the robot size, the step size of the robot and maximal possible step size of the other robots. -
Leaving condition for infinity cycles: If the robot can not distinguish between static and dynamic obstacles it is important to implement an extra leaving condition for infinity loops during the wall following mode. The following pictures show an situation where this leaving condition is necessary.
A robot starts with wall following at the wall W1 until a bunch of robots are in its way. Since the robot can not distinguish between wall and other robots he treats them as a wall and gets to W2. After that the collection of robots is driving away. There is no "bridge" between W1 and W2 anymore. For the robot its not possible to get again to the M-Line and to leave the wall-following-mode...
TangentBug
The TangentBug iterates also between two behaviors: motion-to-goal and boundary-following.
These behaviors are nevertheless different than in the Bug2 approach.
In the motion-to-goal mode, the robot moves in a straight line towards the goal or towards a vertex of an obstacle for which the path through that vertex is the shortest known path. If the robot has to move away from the goal, the boundary- following mode is initiated and the shortest distance between the sensed boundary and the goal is memorized. While moving around the obstacle’s boundary it is checked if the memorized distance d is still the shortest distance to the goal. The robot stops moving along an obstacle’s boundary once it finds a point with a shorter distance to the goal than d. If such a point is found the robot resumes the motion-to-goal mode.
General algorithm:
- Repeat
a) Compute continuous range segments in view.
b) Move toward n∈{T,O} that minimize the heuristic h(x,n) = d(x,n) + d(n,q_goal)
With: T ≙ Target, O ≙ discontinuity point, x ≙ robot position, n ≙ waypoint, q_goal ≙ goal position, d(u,v) ≙ distance function
until:
a) Goal is encountered.
b) The value of h(x,n) begins to increase.
- Follow Boundary continuing in same direction as before repeating and update d_reach (distance current position to goal) and d_followed (shortest distance from sensed boundary to goal)
until:
a) Goal is reached.
b) a complete cycle is performed (goal is unreachable).
c) d_reach < d_followed. Go to step 1.
The picture shows the TangentBug algorithm for one robot: H ≙ hit point, D ≙ depart point, sw ≙ switch point (switch from motion-to-goal to wall-following), L ≙ leaving point (d_reach < d_followed)
Discontinuity Points:
If the way to the goal is blocked from an obstacle, the TangentBug algorithm is calculating discontinuity points which operate as waypoint for the robots. Discontinuity in this context describes the transition from no obstacle detected and there is an obstacle or the other way round (see picture).
Multi-robot Adaptions:
-
(Safety) distances: Two distances are important for navigating without collisions. First of all a safety distance like in the Bug2 algorithm which gives the robots time to react before they may collide. Second, a prediscribed distance to other obstacles which the robots will be consider during caculating the next wapoint during the motion-to-goal mode. Because of the second distance only dynamic obstacles which will enter the safety circel of the robot will force the it to start collision avoidance. This helps a little bit to distinguish between dynamic and static obstacles.
-
Detecting infinity cycles: Similar to the Bug2 algorithm it could happen that the robots end up in infinity cycle. If the robots start with the boundary-following-mode they measure the shortest distance from the sensed boundary to the goal d_followed. They will only stop cycling around the obstacle if d_reach (distance from robots position to goal) falls under d_followed. If the robot get distracted from an obstacle boundary to another and did not leave the boundary-following-mode during that it could happen that d_reach never will be smaller than d_follow . To leave infinity cycles the algorithm has to detect them and update d_followed accordingly.
-
Priority rules: Since the robots will not, like in Bug2, turn in one prescribed direction to avoid collision with obstacles, some priority rules have to be implemented. These are depending on in which direction they will turn to avoid the collision. Furthermore it is important to distinguish as good as possible between other robots and static obstacles (see No.1 Safety distances).
Independenly on the boundary-following or motion-to-goal mode, robots which would turn left if they detect an obstacle, have to wait until the area in which they want to drive is clear. All other robots will porceed with collision avoidance after the boundary-following method. If the robots could not distinguish between dynamic and static obstacle this procedure would cause that some robots will wait for ever.
Because it sometimes happens that the distinction doesn't work, it isalso important to implement a time out after which the robot will proceed with the algorithm.
The cocktailparty algorithms are implemented in one simple node which interacts with gazebo and the benchmark.
Integration with benchmark
Both cocktailparty algorithms can't be run without starting the benchmark because messages of the benchmarks waypoint-controller are needed. On the one hand the node is publishing to the waypoint-topic the targets to which the robots have to drive. On the other hand it is publishing a boolean flag to a finished-topic which gives the information whether the benchmark is completed or not. The cocktailparty-node is subscribing these two topics. Besides the waypoint-controller, two parameters which are loaded to the parameter server during the benchmark launch, are also implemented in the cocktailparty-nodes. These are the end_procedure parameter which defines how the robots will continue after they found all given targets and the wp_threshold parameter which defines how close the robots have to get to a target before it is considered as reached.
Namespaces, Nodes and Topics
Namespaces:
To navigate multiple robots independently, both, common and individual nodes are needed. Namespaces make it possible to create individual nodes. Here, one namespace belongs to one robot using the naming convention of "tb3_x/..." where x = 0,1,2,... .
Nodes and topics:
Since one goal of the cocktailparty algorithms was to keep it simple only a few nodes and topics are needed.
- odom: For robot localization within the simulation. (publisher: gazebo, subscriber: TangentBug/Bug2)
- scan: For obstacle detection and collision avoidance (publisher: gazebo, subscriber: TangentBug/Bug2)
- cmd_vel: To move the robots (publisher: TangenBug/Bug2, subscriber: gazebo)
- benchmark/finished: Information if the benchmark was finished (publisher: waypoints_controller, subscriber: TangentBug/Bug2)
- benchmark/waypoints: Target transmission (publisher: waypoint_controller, subscriber: TangentBug/Bug2)
Parameters
All parameter can be found in cocktailparty_algorithm/config/settings_Bug2.yml and cocktailparty_alogrithm/config/settings_TangentBug.yml
- robot_size --float: Size of the robot (in meter)
- max_vel --float: Maximal velocity the robot can drive (in meter per second)
- rate --integer: Frequency in which the node will publish to the cmd_vel topic.
- vision_radius --float: How far the robot can sense. In Bug2 just for eliminate infinity values of the scanner. In TangentBug mainly for calculating the next waypoints. (in meters)
- collision_scanner_size --int: The angle of the scanners front area, which has the purpose to detect obstacles (in degree)
- safety_distance --float: Minimal distance between two robots before collision avoidance will start (in meter)
- wall_distance --float: Just for TangentBug! To calculate next waypoint with a minimal distance to the wall during motion_to_goal mode. (in meter)
All experiments on the cocktailparty algorithms were run 10 times (except for the scenario with 8 robots which was run only once due to the overall duration).
Overview
With 4 robots the DWA-Local-Planner and the TangentBug clearly outperform the other two algorithms in the Two-Room-Scenario. The Bug2 and the Collvoid-Local-Planner show an almost similar outcome. In the TB3-World on the other hand the Bug2 and TangentBug show a bit better result than the DWA-Local-Planner and any of the three outperform the Collvoid-Local-Planner.
Taking a look at the Two-Room-Scenario with 8 robots, shows that both of the bug algorithms work significantly better than the other two algorithms. The TangentBug algorithm works best. But since this is the result of just 1 benchmark run we should not make too rash conclusions.
For complete log data and data processing, please, refer to the respective csv and excel files
Details
Parameters
Bug2:
robot_size: 0.2
max_vel: 0.22
rate: 20
vision_radius: 3.5
safety_distance: 0.4
collision_scanner_size: 50
TangentBug:
robot_size: 0.2
max_vel: 0.22
rate: 20
vision_radius: 1.5
collision_scanner_size: 50
safety_distance: 0.4
wall_distance: 0.6
TB3 World
World & Waypoints:
Benchmark Settings:
({ "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": "start", "include_start_time": false })
Bug2
TangentBug
Observations:
Both algorithms show almost the same outcome. Because in this scenario it doesn't really matter if the robot circulates right or left around the obstacles and the Bug2 algorithm will be most of the time on the MLine (shortest distance to the goal) since the obstacles are small and don't lead to big detours on the way to the goal.
Two-Rooms World (4 robots)
World & Waypoints:
Benchmark Settings:
({ "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 })
Bug2
TangentBug
Observations: You easily can see that the TangentBug algorithm works much better in the two-room-scenario than the Bug2 algorithm. That's mainly because there is no predefined moving direction in the TangentBug algorithm. The robots can choose there moving direction depending on which would be more efficient in the current situation. This will mostly avoid that the robots take detours to get to their goal.
Another thing you can see in the diagrams is, that the difference between the robots regarding the time they need to finish the benchmark, is not that significant with the TangentBug than the Bug2. That's because with the Bug2 it depends on the starting position and the order of the goals, on how fast the robot can finish the benchmark since the world is not symmetric.
For example tb3_3: This robot starts in the lower left. The first waypoint is in the lower right. Since the turning direction is defined as right, the robot has to drive along almost the whole outer wall of the world to get to its goal. In the diagram you can see that it needs much longer for the first goal than for example tb3_0. After reaching its goal tb3_3 has to drive to the upper left waypoint. Because it may hit the edge of the wall before reaching the goal and the turning direction is defined as right, the robot again has to drive along almost the whole outer wall to reach its goal. Tb3_3 needs again much more time for its second goal than all of the others (see diagram). But after that tb3_3 reaches the next two goals much faster because the turning direction fits to the travel direction.
Two-Rooms World (8 robots)
Benchmark Settings:
({ "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 })
Observations: The TangentBug algorithm is still better than the Bug2 algorithm. But the performance gap between the two algorithm didn't increase.