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 

Package Summary
Tags  No category tags. 
Version  1.0.0 
License  Apache License 2.0 
Build type  CATKIN 
Use  RECOMMENDED 
Repository Summary
Checkout URI  https://github.com/cpswarm/swarm_functions.git 
VCS Type  git 
VCS Version  noeticdevel 
Last Updated  20220426 
Dev Status  DEVELOPED 
CI status  No Continuous Integration 
Released  UNRELEASED 
Tags  No category tags. 
Contributing 
Help Wanted (0)
Good First Issues (0) Pull Requests to Review (0) 
Package Description
Additional Links
Maintainers
 Micha Sende
Authors
 Micha Sende
collision_avoidance
This package enables cyber physical systems (CPSs) in a swarm to avoid collisions with each other.
Dependencies
This package depends on the following message definitions: * std_msgs * geometry_msgs * cpswarm_msgs
The communication between CPSs is based on the CPSwarm Communication Library.
The currently running behavior is determined from FlexBE. It requires the patch described in this pull request.
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)
Further required packages are: * roscpp * tf2
Execution
Run the launch file
roslaunch collision_avoidance collision_avoidance.launch
to launch the collision_avoidance
node.
The launch file can be configured with following parameters:
* id
(integer, default: 1
)
The identifier (ID) of the CPS used for name spacing in simulation.
* output
(string, default: screen
)
Whether to show the program output (screen
) or to write it to a log file (log
).
In the param
subdirectory there is the parameter file collision_avoidance.yaml
that allows to configure the behavior of the collision_avoidance
node.
Nodes
collision_avoidance
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 [0,1]
:
* 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 [0,1]
:
* 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 dist_critical
.
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 dist_repulse
.
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.
Subscribed Topics

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.
Published Topics

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 ifvisualize
istrue
.
Parameters

~loop_rate
(real, default:1.5
) The frequency in Hz at which to run the control loops. 
~queue_size
(integer, default:1
) The size of the message queue used for publishing and subscribing to topics. 
~visualize
(boolean, default:false
) Whether to visualize the avoidance direction by publishing on a ROS topic. 
~dist_critical
(real, default:3.0
) Distance between CPSs in meter below which the collision avoidance will work maximally, i.e., maximum repulsion, no attraction. 
~dist_attract
(real, default:6.0
) Distance between CPSs in meter below which attraction starts to decrease. Must be greater than dist_critical. 
~dist_repulse
(real, default: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.
Code API
Changelog for package collision_avoidance
1.0.0 (20210131)
 Initial release of collision_avoidance
 Contributors: Micha Sende
Wiki Tutorials
Source Tutorials
Package Dependencies
Deps  Name  

1  catkin  
1  kinematics_exchanger  
2  roscpp  
2  tf2  
2  std_msgs  
2  geometry_msgs  
1  cpswarm_msgs 
System Dependencies
Dependant Packages
Name  Repo  Deps 

swarm_functions  githubcpswarmswarm_functions 
Launch files
 launch/collision_avoidance.launch

 id [default: 1]
 output [default: log]