From 6d53409aab47de8fc8d0bba089ff28c60bb85f78 Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 18 Oct 2018 22:18:12 -0400 Subject: [PATCH] split state file, commented out bidding: was crashing --- buzz_scripts/include/act/CA.bzz | 45 ++++ buzz_scripts/include/act/naviguation.bzz | 20 ++ buzz_scripts/include/act/neighborcomm.bzz | 112 +++++++++ buzz_scripts/include/act/states.bzz | 226 +++--------------- .../{bidding => taskallocate}/bidding.bzz | 0 buzz_scripts/include/utils/conversions.bzz | 6 - buzz_scripts/main.bzz | 10 +- 7 files changed, 216 insertions(+), 203 deletions(-) create mode 100644 buzz_scripts/include/act/CA.bzz create mode 100644 buzz_scripts/include/act/naviguation.bzz create mode 100644 buzz_scripts/include/act/neighborcomm.bzz rename buzz_scripts/include/{bidding => taskallocate}/bidding.bzz (100%) diff --git a/buzz_scripts/include/act/CA.bzz b/buzz_scripts/include/act/CA.bzz new file mode 100644 index 0000000..6d923f0 --- /dev/null +++ b/buzz_scripts/include/act/CA.bzz @@ -0,0 +1,45 @@ +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 + + 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 + + penalty = math.exp(d_i - safety_radius) + if( math.cos(math.vec2.angle(vel_vec)) < 0.0){ + penalty = 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) ) + + return math.vec2.newp(V, W) + } else + return vel_vec +} \ No newline at end of file diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz new file mode 100644 index 0000000..300d3b6 --- /dev/null +++ b/buzz_scripts/include/act/naviguation.bzz @@ -0,0 +1,20 @@ +# Core naviguation function to travel to a GPS target location. +function goto_gps(transf) { + m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) + #print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) + 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 + transf() + else { + m_navigation = LimitSpeed(m_navigation, 1.0) + #m_navigation = LCA(m_navigation) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) + } +} + +function LimitSpeed(vel_vec, factor){ + if(math.vec2.length(vel_vec)>GOTO_MAXVEL*factor) + vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec)) + return vel_vec +} \ No newline at end of file diff --git a/buzz_scripts/include/act/neighborcomm.bzz b/buzz_scripts/include/act/neighborcomm.bzz new file mode 100644 index 0000000..5c3211d --- /dev/null +++ b/buzz_scripts/include/act/neighborcomm.bzz @@ -0,0 +1,112 @@ +# listens to commands from the remote control (web, commandline, rcclient node, etc) +function rc_cmd_listen() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + BVMSTATE = "LAUNCH" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + flight.rc_cmd=0 + AUTO_LAUNCH_STATE = "TURNEDOFF" + #barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21) + #barrier_ready(21) + BVMSTATE = "STOP" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==20) { + flight.rc_cmd=0 + AUTO_LAUNCH_STATE = "IDLE" + barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20) + barrier_ready(20) + neighbors.broadcast("cmd", 20) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + BVMSTATE = "PATHPLAN" + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + disarm() + neighbors.broadcast("cmd", 401) + } else if (flight.rc_cmd==666){ + flight.rc_cmd=0 + stattab_send() + } else if (flight.rc_cmd==777){ + flight.rc_cmd=0 + reinit_time_sync() + neighbors.broadcast("cmd", 777) + }else if (flight.rc_cmd==900){ + flight.rc_cmd=0 + barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) + barrier_ready(900) + neighbors.broadcast("cmd", 900) + } else if (flight.rc_cmd==901){ + flight.rc_cmd=0 + destroyGraph() + barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) + barrier_ready(901) + neighbors.broadcast("cmd", 901) + } else if (flight.rc_cmd==902){ + flight.rc_cmd=0 + destroyGraph() + barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) + barrier_ready(902) + neighbors.broadcast("cmd", 902) + } else if (flight.rc_cmd==903){ + flight.rc_cmd=0 + destroyGraph() + barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903) + barrier_ready(903) + neighbors.broadcast("cmd", 903) + } +} + +# listens to neighbors broadcasting commands +function nei_cmd_listen() { + neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") + #if(BVMSTATE!="BARRIERWAIT") { + if(value==22 and BVMSTATE=="TURNEDOFF") { + BVMSTATE = "LAUNCH" + }else if(value==20) { + AUTO_LAUNCH_STATE = "IDLE" + BVMSTATE = "GOHOME" + } else if(value==21 and BVMSTATE!="TURNEDOFF") { + BVMSTATE = "STOP" + } else if(value==400 and BVMSTATE=="TURNEDOFF") { + arm() + } else if(value==401 and BVMSTATE=="TURNEDOFF"){ + disarm() + } else if(value==777 and BVMSTATE=="TURNEDOFF"){ + reinit_time_sync() + #neighbors.broadcast("cmd", 777) + }else if(value==900){ # Shapes + barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) + #barrier_ready(900) + neighbors.broadcast("cmd", 900) + } else if(value==901){ # Pursuit + destroyGraph() + barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) + #barrier_ready(901) + neighbors.broadcast("cmd", 901) + } else if(value==902){ # Agreggate + destroyGraph() + barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) + #barrier_ready(902) + neighbors.broadcast("cmd", 902) + } else if(value==903){ # Formation + destroyGraph() + barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903) + #barrier_ready(903) + neighbors.broadcast("cmd", 903) + } else if(value==16 and BVMSTATE=="IDLE"){ + # neighbors.listen("gt",function(vid, value, rid) { + # print("Got (", vid, ",", value, ") from robot #", rid) + # # if(gt.id == id) statef=goto + # }) + } + #} + }) +} diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index d3c1aac..dce6374 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -5,8 +5,10 @@ ######################################## include "utils/vec2.bzz" include "act/barrier.bzz" -#include "act/old_barrier.bzz" include "utils/conversions.bzz" +include "act/naviguation.bzz" +include "act/CA.bzz" +include "act/neighborcomm.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" @@ -20,30 +22,17 @@ path_it = 0 pic_time = 0 g_it = 0 +# Core state function when on the ground function turnedoff() { BVMSTATE = "TURNEDOFF" } +# Core state function when hovering function idle() { BVMSTATE = "IDLE" } - -# Custom function for the user. -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") - } -} - +# Core state function to launch the robot: takeoff and wait for others, or stop (land) function launch() { BVMSTATE = "LAUNCH" if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF @@ -62,6 +51,7 @@ function launch() { } } +# Launch function version without the timeout and stop state. function launch_switch() { BVMSTATE = "LAUNCH_SWITCH" if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF @@ -81,6 +71,7 @@ function launch_switch() { } gohomeT=100 +# State function to go back to ROSBuzz recorded home GPS position (at takeoff) function goinghome() { BVMSTATE = "GOHOME" if(gohomeT > 0) { # TODO: Make a real check if home is reached @@ -90,6 +81,7 @@ function goinghome() { BVMSTATE = AUTO_LAUNCH_STATE } +# Core state function to stop and land. function stop() { BVMSTATE = "STOP" if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND @@ -107,6 +99,13 @@ function stop() { } } +# State function for individual waypoint control +function indiWP() { + BVMSTATE = "INDIWP" + +} + +# State function to take a picture from the camera server (developed by HS) function take_picture() { BVMSTATE="PICTURE" setgimbal(0.0, 0.0, -90.0, 20.0) @@ -119,66 +118,7 @@ function take_picture() { pic_time=pic_time+1 } -function goto_gps(transf) { - m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) - #print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) - 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 - transf() - else { - m_navigation = LimitSpeed(m_navigation, 1.0) - #m_navigation = LCA(m_navigation) - goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) - } -} - -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 - - 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 - - penalty = math.exp(d_i - safety_radius) - if( math.cos(math.vec2.angle(vel_vec)) < 0.0){ - penalty = 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) ) - - return math.vec2.newp(V, W) - } else - return vel_vec -} - +# State function to follow a moving attractor (GPS sent from a user phone) function follow() { if(size(targets)>0) { BVMSTATE = "FOLLOW" @@ -194,7 +134,7 @@ function follow() { } } -# converge to centroid +# State function to converge to centroid function aggregate() { BVMSTATE="AGGREGATE" centroid = neighbors.reduce(function(rid, data, centroid) { @@ -208,7 +148,7 @@ function aggregate() { goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) } -# circle all together around centroid +# State fucntion to circle all together around centroid function pursuit() { BVMSTATE="PURSUIT" rd = 20.0 @@ -248,7 +188,7 @@ function lj_sum(rid, data, accum) { return math.vec2.add(data, accum) } -# Calculates and actuates the flocking interaction +# Sate function that calculates and actuates the flocking interaction function formation() { BVMSTATE="FORMATION" # Calculate accumulator @@ -259,115 +199,17 @@ function formation() { goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0) } -# listens to commands from the remote control (web, commandline, etc) -function rc_cmd_listen() { - if(flight.rc_cmd==22) { - log("cmd 22") - flight.rc_cmd=0 - BVMSTATE = "LAUNCH" - neighbors.broadcast("cmd", 22) - } else if(flight.rc_cmd==21) { - flight.rc_cmd=0 - AUTO_LAUNCH_STATE = "TURNEDOFF" - #barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21) - #barrier_ready(21) - BVMSTATE = "STOP" - neighbors.broadcast("cmd", 21) - } else if(flight.rc_cmd==20) { - flight.rc_cmd=0 - AUTO_LAUNCH_STATE = "IDLE" - barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20) - barrier_ready(20) - neighbors.broadcast("cmd", 20) - } else if(flight.rc_cmd==16) { - flight.rc_cmd=0 - BVMSTATE = "PATHPLAN" - } else if(flight.rc_cmd==400) { - flight.rc_cmd=0 - arm() - neighbors.broadcast("cmd", 400) - } else if (flight.rc_cmd==401){ - flight.rc_cmd=0 - disarm() - neighbors.broadcast("cmd", 401) - } else if (flight.rc_cmd==666){ - flight.rc_cmd=0 - stattab_send() - } else if (flight.rc_cmd==777){ - flight.rc_cmd=0 - reinit_time_sync() - neighbors.broadcast("cmd", 777) - }else if (flight.rc_cmd==900){ - flight.rc_cmd=0 - barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) - barrier_ready(900) - neighbors.broadcast("cmd", 900) - } else if (flight.rc_cmd==901){ - flight.rc_cmd=0 - destroyGraph() - barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) - barrier_ready(901) - neighbors.broadcast("cmd", 901) - } else if (flight.rc_cmd==902){ - flight.rc_cmd=0 - destroyGraph() - barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) - barrier_ready(902) - neighbors.broadcast("cmd", 902) - } else if (flight.rc_cmd==903){ - flight.rc_cmd=0 - destroyGraph() - barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903) - barrier_ready(903) - neighbors.broadcast("cmd", 903) - } -} - -# listens to other fleet members broadcasting commands -function nei_cmd_listen() { - neighbors.listen("cmd", - function(vid, value, rid) { - print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") - #if(BVMSTATE!="BARRIERWAIT") { - if(value==22 and BVMSTATE=="TURNEDOFF") { - BVMSTATE = "LAUNCH" - }else if(value==20) { - AUTO_LAUNCH_STATE = "IDLE" - BVMSTATE = "GOHOME" - } else if(value==21 and BVMSTATE!="TURNEDOFF") { - BVMSTATE = "STOP" - } else if(value==400 and BVMSTATE=="TURNEDOFF") { - arm() - } else if(value==401 and BVMSTATE=="TURNEDOFF"){ - disarm() - } else if(value==777 and BVMSTATE=="TURNEDOFF"){ - reinit_time_sync() - #neighbors.broadcast("cmd", 777) - }else if(value==900){ # Shapes - barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) - #barrier_ready(900) - neighbors.broadcast("cmd", 900) - } else if(value==901){ # Pursuit - destroyGraph() - barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) - #barrier_ready(901) - neighbors.broadcast("cmd", 901) - } else if(value==902){ # Agreggate - destroyGraph() - barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) - #barrier_ready(902) - neighbors.broadcast("cmd", 902) - } else if(value==903){ # Formation - destroyGraph() - barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903) - #barrier_ready(903) - neighbors.broadcast("cmd", 903) - } else if(value==16 and BVMSTATE=="IDLE"){ - # neighbors.listen("gt",function(vid, value, rid) { - # print("Got (", vid, ",", value, ") from robot #", rid) - # # if(gt.id == id) statef=goto - # }) - } - #} - }) -} +# 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") + } +} \ No newline at end of file diff --git a/buzz_scripts/include/bidding/bidding.bzz b/buzz_scripts/include/taskallocate/bidding.bzz similarity index 100% rename from buzz_scripts/include/bidding/bidding.bzz rename to buzz_scripts/include/taskallocate/bidding.bzz diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz index 8cef004..b7b2b2f 100644 --- a/buzz_scripts/include/utils/conversions.bzz +++ b/buzz_scripts/include/utils/conversions.bzz @@ -7,12 +7,6 @@ function LimitAngle(angle){ return angle } -function LimitSpeed(vel_vec, factor){ - if(math.vec2.length(vel_vec)>GOTO_MAXVEL*factor) - vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec)) - return vel_vec -} - # TODO: add other conversions.... function convert_path(P) { var pathR={} diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 4b9d54e..f499263 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -4,14 +4,14 @@ include "update.bzz" include "act/states.bzz" include "plan/rrtstar.bzz" include "taskallocate/graphformGPS.bzz" -include "bidding/bidding.bzz" +#include "taskallocate/bidding.bzz" include "vstigenv.bzz" #include "timesync.bzz" 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 @@ -35,10 +35,8 @@ goal_list = { function init() { init_stig() init_swarm() - init_bidding() + #init_bidding() -# initGraph() - TARGET_ALTITUDE = takeoff_heights[id] # start the swarm command listener @@ -73,6 +71,8 @@ function step() { statef=launch_switch else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE statef=goinghome + else if(BVMSTATE=="INDIWP") + statef=indiWP else if(BVMSTATE=="IDLE") statef=idle else if(BVMSTATE=="AGGREGATE")