ROSBuzz_MISTLab/buzz_scripts/include/act/CA.bzz

51 lines
1.1 KiB
Plaintext

# Lightweight collision avoidance
function LCA( vel_vec ) {
var safety_radius = 3.0
collide = 0
var k_v = 5 # x axis gain
var k_w = 5 # y axis gain
cart = neighbors.map(
function(rid, data) {
var c = {}
c.distance = data.distance
c.azimuth = data.azimuth
if (c.distance < (safety_radius * 2.0) )
collide = 1
return c
})
if (collide) {
log("")
log("------> AVOIDING NEIGHBOR! <------")
log("")
result = cart.reduce(function(rid, data, accum) {
if(data.distance < accum.distance and data.distance > 0.0){
accum.distance = data.distance
accum.angle = data.azimuth
return accum
}
return accum
}, {.distance= safety_radius * 2.0, .angle= 0.0})
d_i = result.distance
data_alpha_i = result.angle
delta = math.vec2.new( d_i * math.cos(data_alpha_i), d_i * math.sin(data_alpha_i) )
p = math.exp(-(d_i - safety_radius))
if ( math.cos( math.vec2.angle(vel_vec) - data_alpha_i ) < 0.0 ) {
p = math.exp(d_i - safety_radius)
}
V = -1 * (p / d_i) * k_v * delta.x
W = -1 * (p / d_i) * k_w * delta.y
Uavd = math.vec2.new( V, W )
return math.vec2.add( vel_vec, Uavd )
} else
return vel_vec
}