collision_avoidance package from swarm_functions repoarea_division battery_exchanger collision_avoidance coverage_path kinematics_exchanger roi_assignment state_exchanger swarm_functions target_monitor task_allocation
|Tags||No category tags.|
|License||Apache License 2.0|
|CI status||No Continuous Integration|
|Tags||No category tags.|
Help Wanted (0)
Good First Issues (0)
Pull Requests to Review (0)
- Micha Sende
- Micha Sende
This package enables cyber physical systems (CPSs) in a swarm to avoid collisions with each other.
The communication between CPSs is based on the CPSwarm Communication Library.
The following packages of the swarm functions library are required: * kinematics_exchanger
The following packages of the sensing and actuation library are required: * *_pos_provider * *_pos_controller (if position setpoints are used by the behavior algorithms) * *_vel_controller (if velocity setpoints are used by the behavior algorithms)
Run the launch file
roslaunch collision_avoidance collision_avoidance.launch
to launch the
The launch file can be configured with following parameters:
id (integer, default:
The identifier (ID) of the CPS used for name spacing in simulation.
output (string, default:
Whether to show the program output (
screen) or to write it to a log file (
param subdirectory there is the parameter file
collision_avoidance.yaml that allows to configure the behavior of the
collision_avoidance node constantly monitors for close by CPSs using position information received through the CPSwarm Communication Library. In case there are CPSs closer than
dist_repulse, it calculates the repulsion from each CPS as a vector pointing away from the other CPS. There are five different repulsion functions to calculate the vector magnitude in the range
lin: The repulsion increases linearly, inverse to the distance between the CPSs.
li2: The repulsion increases linearly, inverse to the distance between the CPSs, twice as fast as the linear function above.
sin: The repulsion increases following the sine function with decreasing CPS distance.
log: The repulsion increases following the natural logarithm with decreasing CPS distance.
exp: The repulsion increases following the exponential function with decreasing CPS distance.
The repulsion magnitude is maximal, i.e.
1, for CPSs closer than
dist_critical. The overall repulsion is the sum of the repulsion from each CPS yielding a repulsion magnitude in the range
[0,number of neighbors closer than dist_repulse].
To calculate the resulting avoidance direction, the goal attraction of the CPS is added to the repulsion vector. It is a vector pointing to the navigation goal with the magnitude decreasing as function of the closest CPS when the distance to the closest CPS is below
dist_attract. There are five different attraction functions to calculate the vector magnitude in the range
lin: The attraction decreases linearly with the distance to the closest CPS.
li2: The attraction decreases linearly with the distance to the closest CPS twice as fast as the linear function above.
sin: The attraction decreases following the sine function with decreasing CPS distance.
log: The attraction decreases following the natural logarithm with decreasing CPS distance.
exp: The attraction decreases following the exponential function with decreasing CPS distance.
The repulsion magnitude is minimal, i.e.
0, if the closest CPS is closer than
To make CPSs move slower when close to each other, the magnitude of the avoidance vector follows a linear function that increases with the distance to the closest CPS: It is minimal with
dist_critical/2 at distance
dist_critical and maximal with
dist_repulse/2 at distance
For velocity setpoints, the generated avoidance vector represents the velocity. For position setpoints, the vector is added to the current CPS's position to get the new position. The avoidance velocity and position setpoints are published to the respective controllers of the sensing and actuation library.
A limitation of this approach is that there can be deadlocks when repulsion and attraction cancel each other.
pos_controller/goal_position(geometry_msgs/PoseStamped) The originally desired goal position of this CPS in case of no collision avoidance.
vel_controller/target_velocity(geometry_msgs/Twist) The originally desired target velocity of this CPS in case of no collision avoidance.
pos_provider/pose(geometry_msgs/PoseStamped) The current position of this CPS.
swarm_position_rel(cpswarm_msgs/ArrayOfVectors) The relative positions of the other swarm members (distance and bearing).
flexbe/behavior_update(std_msgs/String) The current behavior state of this CPS.
pos_controller/ca_goal_position(geometry_msgs/PoseStamped) The intermediate goal position where the CPS shall move to during collision avoidance. Only published if position setpoints are used by the behavior algorithm.
vel_controller/ca_target_velocity(geometry_msgs/Twist) The intermediate target velocity which the CPS shall move at during collision avoidance. Only published if velocity setpoints are used by the behavior algorithm.
collision_avoidance/direction(geometry_msgs/PoseStamped) Direction of collision avoidance. Current position with orientation into avoidance direction. Only published if
1.5) The frequency in Hz at which to run the control loops.
1) The size of the message queue used for publishing and subscribing to topics.
false) Whether to visualize the avoidance direction by publishing on a ROS topic.
3.0) Distance between CPSs in meter below which the collision avoidance will work maximally, i.e., maximum repulsion, no attraction.
6.0) Distance between CPSs in meter below which attraction starts to decrease. Must be greater than dist_critical.
12.0) Distance between CPSs in meter below which repulsion starts to increase. Must be greater than dist_critical.
~attraction_shape(string) The shape of the attraction function. Function shapes can be lin, li2, sin, log, or exp. Default is a constant value.
~repulsion_shape(string) The shape of the repulsion function. Function shapes can be lin, li2, sin, log, or exp. Default is a constant value.
~excluded(string list, default:
) No collision avoidance in these states.
Changelog for package collision_avoidance
- Initial release of collision_avoidance
- Contributors: Micha Sende
- id [default: 1]
- output [default: log]