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

View File

@ -10,7 +10,6 @@ function goto_gps(transf) {
if(math.vec2.length(m_navigation)>GOTO_MAXDIST) if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
log("Sorry this is too far (", 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 else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination
if(transf!=nil)
transf() transf()
} else { } else {
m_navigation = LimitSpeed(m_navigation, 1.0) 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 firsttimeinwp = 1
wpreached = 1
function indiWP() { function indiWP() {
if(firsttimeinwp) { if(firsttimeinwp) {
nei_goal_listen() v_wp = stigmergy.create(8)
storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude) storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
firsttimeinwp = 0 firsttimeinwp = 0
} }
BVMSTATE = "INDIWP" BVMSTATE = "INDIWP"
if(rc_goto.id != -1) { if(rc_goto.id != -1) {
log(rc_goto.id)
if(rc_goto.id == id) { if(rc_goto.id == id) {
wpreached = 0
storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude) storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude)
} else return
bd_goal() } 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) # 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 # Custom state function for the developer to play with
function cusfun(){ function cusfun(){
BVMSTATE="CUSFUN" BVMSTATE="CUSFUN"
log("cusfun: yay!!!") #log("cusfun: yay!!!")
LOCAL_TARGET = math.vec2.new(5 ,0 ) #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 #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){ #if(math.vec2.length(local_set_point) > GOTODIST_TOL){
local_set_point = LimitSpeed(local_set_point, 1.0) # local_set_point = LimitSpeed(local_set_point, 1.0)
goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.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 #State launched after takeoff
AUTO_LAUNCH_STATE = "BIDDING" AUTO_LAUNCH_STATE = "INDIWP"
TARGET = 9.0 TARGET = 9.0
EPSILON = 30.0 EPSILON = 30.0
ROOT_ID = 3 ROOT_ID = 3
graph_id = 3 graph_id = 3
graph_loop = 0 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: # Vehicule type:

View File

@ -46,6 +46,10 @@ int buzz_exportmap(buzzvm_t vm);
* closure to take a picture * closure to take a picture
*/ */
int buzzuav_takepicture(buzzvm_t vm); int buzzuav_takepicture(buzzvm_t vm);
/*
* closure to reset RC input
*/
int buzzuav_resetrc(buzzvm_t vm);
/* /*
* Returns the current command from local variable * 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_pushs(VM, buzzvm_string_register(VM, "export_map", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzz_exportmap)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzz_exportmap));
buzzvm_gstore(VM); 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; return VM->state;
} }

View File

@ -333,6 +333,15 @@ mavros_msgs::Mavlink get_status()
return payload_out; 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) int buzzuav_takepicture(buzzvm_t vm)
/* /*
/ Buzz closure to take a picture here. / Buzz closure to take a picture here.