From b039fcfaccf78511cf22a9d8dc7e253a18be8bc1 Mon Sep 17 00:00:00 2001 From: rafaelrgb Date: Fri, 26 Oct 2018 19:26:07 -0300 Subject: [PATCH 1/3] added new lca version --- buzz_scripts/include/act/states.bzz | 120 +++++++++++++++++++++++++--- 1 file changed, 110 insertions(+), 10 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 01fda7f..7a960a8 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -210,17 +210,117 @@ function formation() { goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0) } +# Lightweight collision avoidance +function LCA_rafael( 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 +} + # 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) - } - else{ - log("TARGET REACHED") - } + #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_rafael(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 + } + } } \ No newline at end of file From a6a41309499c1022ae2cd560be3d34f8a05617ee Mon Sep 17 00:00:00 2001 From: rafaelrgb Date: Fri, 26 Oct 2018 19:32:11 -0300 Subject: [PATCH 2/3] moved the lca code to the right file --- buzz_scripts/include/act/CA.bzz | 26 ++++++++------ buzz_scripts/include/act/states.bzz | 54 +---------------------------- 2 files changed, 17 insertions(+), 63 deletions(-) diff --git a/buzz_scripts/include/act/CA.bzz b/buzz_scripts/include/act/CA.bzz index 6d923f0..ed0d055 100644 --- a/buzz_scripts/include/act/CA.bzz +++ b/buzz_scripts/include/act/CA.bzz @@ -1,10 +1,10 @@ -function LCA(vel_vec) { +# 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 } \ No newline at end of file diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 7a960a8..789381e 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -210,58 +210,6 @@ function formation() { goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0) } -# Lightweight collision avoidance -function LCA_rafael( 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 -} - # Custom state function for the developer to play with function cusfun(){ BVMSTATE="CUSFUN" @@ -313,7 +261,7 @@ function cusfun(){ # Run LCA and increment waypoint if(distance_to_target > 0.2){ - vector_to_target = LCA_rafael(vector_to_target) + 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 { From 85f2ed41c6250424cd445045b21c6815659728e1 Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 29 Oct 2018 01:47:39 -0400 Subject: [PATCH 3/3] enhanced individual waypoint control stability --- buzz_scripts/include/act/naviguation.bzz | 1 - buzz_scripts/include/act/neighborcomm.bzz | 17 ------------ buzz_scripts/include/act/states.bzz | 33 ++++++++++++++++++----- buzz_scripts/main.bzz | 4 +-- include/buzzuav_closures.h | 4 +++ src/buzz_utility.cpp | 3 +++ src/buzzuav_closures.cpp | 9 +++++++ 7 files changed, 45 insertions(+), 26 deletions(-) diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz index 7cd6879..072d312 100644 --- a/buzz_scripts/include/act/naviguation.bzz +++ b/buzz_scripts/include/act/naviguation.bzz @@ -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) diff --git a/buzz_scripts/include/act/neighborcomm.bzz b/buzz_scripts/include/act/neighborcomm.bzz index 1761039..7ed6c07 100644 --- a/buzz_scripts/include/act/neighborcomm.bzz +++ b/buzz_scripts/include/act/neighborcomm.bzz @@ -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) - } - }) -} diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 01fda7f..7245ce0 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -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) diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 787ed84..d07fc97 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -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: diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 46ef816..f8ae4d5 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -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 */ diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 4611452..d9a80ca 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -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; } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index b76aa21..a18a093 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -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.