Merge commit '77886ac037f585465533271a8a5cc1642b456b80' into ros_drones_ws

This commit is contained in:
dave 2018-10-29 13:55:23 -04:00
commit 70cabdb4fa
8 changed files with 119 additions and 46 deletions

View File

@ -1,10 +1,10 @@
# Lightweight collision avoidance
function LCA( vel_vec ) {
var safety_radius = 3.0
var default_velocity = 2.0
collide = 0
var k_v = 5 # velocity gain
var k_w = 10 # angular velocity gain
var k_v = 5 # x axis gain
var k_w = 5 # y axis gain
cart = neighbors.map(
function(rid, data) {
@ -28,18 +28,24 @@ function LCA(vel_vec) {
return accum
}, {.distance= safety_radius * 2.0, .angle= 0.0})
d_i = result.distance
data_alpha_i = result.angle
penalty = math.exp(d_i - safety_radius)
if( math.cos(math.vec2.angle(vel_vec)) < 0.0){
penalty = math.exp(-(d_i - safety_radius))
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 = math.vec2.length(vel_vec) - math.cos(math.vec2.angle(vel_vec)) * penalty * k_v
W = -k_w * penalty * math.sin( math.vec2.angle(vel_vec) )
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 )
return math.vec2.newp(V, W)
} else
return vel_vec
}

View File

@ -10,7 +10,6 @@ function goto_gps(transf) {
if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )")
else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination
if(transf!=nil)
transf()
} else {
m_navigation = LimitSpeed(m_navigation, 1.0)

View File

@ -110,20 +110,3 @@ function nei_cmd_listen() {
#}
})
}
# broadcast GPS goals
function bd_goal() {
neighbors.broadcast("goal", {.id=rc_goto.id, .la=rc_goto.latitude, .lo=rc_goto.longitude, .al=rc_goto.altitude})
}
# listens to neighbors broadcasting gps goals
function nei_goal_listen() {
neighbors.listen("goal",
function(vid, value, rid) {
print("Got (", vid, ",", value.id, value.la, value.lo, ") #", rid)
if(value.id==id) {
print("Got (", vid, ",", value, ") #", rid)
storegoal(value.la, value.lo, pose.position.altitude)
}
})
}

View File

@ -95,25 +95,46 @@ function stop() {
}
}
# State function for individual waypoint control
# State functions for individual waypoint control
firsttimeinwp = 1
wpreached = 1
function indiWP() {
if(firsttimeinwp) {
nei_goal_listen()
v_wp = stigmergy.create(8)
storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
firsttimeinwp = 0
}
BVMSTATE = "INDIWP"
if(rc_goto.id != -1) {
log(rc_goto.id)
if(rc_goto.id == id) {
wpreached = 0
storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude)
} else
bd_goal()
return
} else {
v_wp.put(rc_goto.id,{.lat=rc_goto.latitude, .lon=rc_goto.longitude, .pro=0})
reset_rc()
}
}
goto_gps(nil)
wp = v_wp.get(id)
if(wp!=nil) {
#log(wp.pro,wpreached)
if(wp.pro == 0) {
wpreached = 0
storegoal(wp.lat, wp.lon, pose.position.altitude)
v_wp.put(id,{.lat=wp.lat, .lon=wp.lon, .pro=1})
return
}
}
if(wpreached!=1)
goto_gps(indiWP_done)
}
function indiWP_done() {
BVMSTATE = "INDIWP"
wpreached = 1
}
# State function to take a picture from the camera server (developed by HS)
@ -213,14 +234,62 @@ function formation() {
# Custom state function for the developer to play with
function cusfun(){
BVMSTATE="CUSFUN"
log("cusfun: yay!!!")
LOCAL_TARGET = math.vec2.new(5 ,0 )
local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location
if(math.vec2.length(local_set_point) > GOTODIST_TOL){
local_set_point = LimitSpeed(local_set_point, 1.0)
goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0)
#log("cusfun: yay!!!")
#LOCAL_TARGET = math.vec2.new(5 ,0 )
#local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location
#if(math.vec2.length(local_set_point) > GOTODIST_TOL){
# local_set_point = LimitSpeed(local_set_point, 1.0)
# goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0)
#}
#else{
# log("TARGET REACHED")
#}
initialPoint = 0
# Get waypoint list
if (id == 1) {
waypoints = {
.0 = math.vec2.new(-5.0, -5.0),
.1 = math.vec2.new(5.0, 5.0)
}
} else if (id == 2 ) {
waypoints = {
.0 = math.vec2.new(5.0, 5.0),
.1 = math.vec2.new(-5.0, -5.0)
}
}
# Current waypoint
target_point = waypoints[point]
# Get drone position and transform to global frame - depends on lauch file
offset_x = 0.0
offset_y = 0.0
if (id == 1) {
offset_x = 5.0
offset_y = 5.0
} else if (id == 2 ) {
offset_x = -5.0
offset_y = -5.0
}
pos_x = pose.position.x + offset_x;
pos_y = pose.position.y + offset_y;
# Get a vector pointing to target and target distance
vector_to_target = math.vec2.sub(target_point, math.vec2.new(pos_x, pos_y))
distance_to_target = math.vec2.length(vector_to_target)
# Run LCA and increment waypoint
if(distance_to_target > 0.2){
vector_to_target = LCA(vector_to_target)
vector_to_target = LimitSpeed(vector_to_target, 0.5)
goto_abs(vector_to_target.x, vector_to_target.y, 0.0, 0.0)
} else {
if(point < size(waypoints) - 1){
point = point + 1
} else {
point = 0
}
else{
log("TARGET REACHED")
}
}

View File

@ -11,13 +11,13 @@ include "utils/takeoff_heights.bzz"
#State launched after takeoff
AUTO_LAUNCH_STATE = "BIDDING"
AUTO_LAUNCH_STATE = "INDIWP"
TARGET = 9.0
EPSILON = 30.0
ROOT_ID = 3
graph_id = 3
graph_loop = 0
LAND_AFTER_BARRIER_EXPIRE = 0 # if set to be 1 , the robots will land after barrier expire; if set to be 0, the robots will carry on to AUTO_LAUNCH_STATE.
LAND_AFTER_BARRIER_EXPIRE = 1 # if set to be 1 , the robots will land after barrier expire; if set to be 0, the robots will carry on to AUTO_LAUNCH_STATE.
#####
# Vehicule type:

View File

@ -46,6 +46,10 @@ int buzz_exportmap(buzzvm_t vm);
* closure to take a picture
*/
int buzzuav_takepicture(buzzvm_t vm);
/*
* closure to reset RC input
*/
int buzzuav_resetrc(buzzvm_t vm);
/*
* Returns the current command from local variable
*/

View File

@ -237,6 +237,9 @@ static int buzz_register_hooks()
buzzvm_pushs(VM, buzzvm_string_register(VM, "export_map", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzz_exportmap));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "reset_rc", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_resetrc));
buzzvm_gstore(VM);
return VM->state;
}

View File

@ -333,6 +333,15 @@ mavros_msgs::Mavlink get_status()
return payload_out;
}
int buzzuav_resetrc(buzzvm_t vm)
/*
/ Buzz closure to reset the RC input.
/----------------------------------------*/
{
rc_id = -1;
return buzzvm_ret0(vm);
}
int buzzuav_takepicture(buzzvm_t vm)
/*
/ Buzz closure to take a picture here.