From 6d53409aab47de8fc8d0bba089ff28c60bb85f78 Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 18 Oct 2018 22:18:12 -0400 Subject: [PATCH 1/2] 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") From 4dec1ca100675d22bc48896fb5da518a6eb3cd97 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 19 Oct 2018 02:34:18 -0400 Subject: [PATCH 2/2] individual waypoint ctl, map-based waypoint in webctl in formation --- buzz_scripts/include/act/naviguation.bzz | 16 +++-- buzz_scripts/include/act/neighborcomm.bzz | 23 ++++++- buzz_scripts/include/act/states.bzz | 19 ++++-- .../include/utils/takeoff_heights.bzz | 3 +- include/buzzuav_closures.h | 4 ++ src/buzzuav_closures.cpp | 67 ++++++++++++++----- src/roscontroller.cpp | 11 +-- 7 files changed, 108 insertions(+), 35 deletions(-) diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz index 300d3b6..7cd6879 100644 --- a/buzz_scripts/include/act/naviguation.bzz +++ b/buzz_scripts/include/act/naviguation.bzz @@ -1,15 +1,21 @@ +GOTO_MAXVEL = 1.5 # m/steps +GOTO_MAXDIST = 150 # m. +GOTODIST_TOL = 0.4 # m. +GOTOANG_TOL = 0.1 # rad. + # 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) + m_navigation = vec_from_gps(cur_goal.latitude, cur_goal.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 { + 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) #m_navigation = LCA(m_navigation) - goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) + goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0) } } diff --git a/buzz_scripts/include/act/neighborcomm.bzz b/buzz_scripts/include/act/neighborcomm.bzz index 5c3211d..1761039 100644 --- a/buzz_scripts/include/act/neighborcomm.bzz +++ b/buzz_scripts/include/act/neighborcomm.bzz @@ -18,9 +18,9 @@ function rc_cmd_listen() { 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==16) { +# flight.rc_cmd=0 +# BVMSTATE = "PATHPLAN" } else if(flight.rc_cmd==400) { flight.rc_cmd=0 arm() @@ -110,3 +110,20 @@ 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 dce6374..01fda7f 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -14,10 +14,6 @@ TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 1.5 # m/steps -GOTO_MAXDIST = 150 # m. -GOTODIST_TOL = 0.4 # m. -GOTOANG_TOL = 0.1 # rad. path_it = 0 pic_time = 0 g_it = 0 @@ -100,8 +96,23 @@ function stop() { } # State function for individual waypoint control +firsttimeinwp = 1 function indiWP() { + if(firsttimeinwp) { + nei_goal_listen() + 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) { + storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude) + } else + bd_goal() + } + + goto_gps(nil) } diff --git a/buzz_scripts/include/utils/takeoff_heights.bzz b/buzz_scripts/include/utils/takeoff_heights.bzz index 588afb4..81081aa 100644 --- a/buzz_scripts/include/utils/takeoff_heights.bzz +++ b/buzz_scripts/include/utils/takeoff_heights.bzz @@ -2,10 +2,11 @@ takeoff_heights ={ .1 = 21.0, .2 = 18.0, .3 = 12.0, - .5 = 24.0, + .5 = 12.0, .6 = 3.0, .9 = 15.0, .11 = 6.0, + .14 = 18.0, .16 = 9.0, .17 = 3.0, .18 = 6.0, diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 71b99f1..46ef816 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -50,6 +50,10 @@ int buzzuav_takepicture(buzzvm_t vm); * Returns the current command from local variable */ int getcmd(); +/* + * update GPS goal value + */ +void set_gpsgoal(double goal[3]); /* * Sets goto position from rc client */ diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 4c8b2a5..b76aa21 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -14,23 +14,29 @@ namespace buzzuav_closures // TODO: Minimize the required global variables and put them in the header // static const rosbzz_node::roscontroller* roscontroller_ptr; static double goto_pos[4]; -static double rc_goto_pos[3]; -static float rc_gimbal[4]; -static float batt[3]; -static float obst[5] = { 0, 0, 0, 0, 0 }; +static double goto_gpsgoal[3]; static double cur_pos[4]; static double cur_NEDpos[2]; -static uint8_t status; -static int cur_cmd = 0; -static int rc_cmd = 0; + static int rc_id = -1; +static int rc_cmd = 0; +static double rc_gpsgoal[3]; +static float rc_gimbal[4]; + +static float batt[3]; +static float obst[5] = { 0, 0, 0, 0, 0 }; +static uint8_t status; + +static int cur_cmd = 0; static int buzz_cmd = 0; static float height = 0; + static bool deque_full = false; static int rssi = 0; static float raw_packet_loss = 0.0; static int filtered_packet_loss = 0; static float api_rssi = 0.0; + string WPlistname = ""; std::map targets_map; @@ -231,7 +237,7 @@ int buzzuav_moveto(buzzvm_t vm) // DEBUG // ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], // goto_pos[1], goto_pos[2]); - buzz_cmd = NAV_SPLINE_WAYPOINT; // TODO: standard mavros? + buzz_cmd = NAV_SPLINE_WAYPOINT; return buzzvm_ret0(vm); } @@ -386,14 +392,24 @@ int buzzuav_storegoal(buzzvm_t vm) wplist_map.erase(wplist_map.begin()->first); } - double rb[3]; + set_gpsgoal(goal); + // prevent an overwrite + rc_id = -1; + return buzzvm_ret0(vm); +} + +void set_gpsgoal(double goal[3]) +/* +/ update GPS goal value +-----------------------------------*/ +{ + double rb[3]; rb_from_gps(goal, rb, cur_pos); if (fabs(rb[0]) < 150.0) { - rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]); - ROS_INFO("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); + goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = goal[2]; + ROS_INFO("Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); } - return buzzvm_ret0(vm); } int buzzuav_arm(buzzvm_t vm) @@ -504,9 +520,9 @@ void rc_set_goto(int id, double latitude, double longitude, double altitude) -----------------------------------*/ { rc_id = id; - rc_goto_pos[0] = latitude; - rc_goto_pos[1] = longitude; - rc_goto_pos[2] = altitude; + rc_gpsgoal[0] = latitude; + rc_gpsgoal[1] = longitude; + rc_gpsgoal[2] = altitude; } void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t) @@ -773,15 +789,30 @@ int buzzuav_update_flight_status(buzzvm_t vm) buzzvm_tput(vm); buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); - buzzvm_pushf(vm, rc_goto_pos[0]); + buzzvm_pushf(vm, rc_gpsgoal[0]); buzzvm_tput(vm); buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); - buzzvm_pushf(vm, rc_goto_pos[1]); + buzzvm_pushf(vm, rc_gpsgoal[1]); buzzvm_tput(vm); buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); - buzzvm_pushf(vm, rc_goto_pos[2]); + buzzvm_pushf(vm, rc_gpsgoal[2]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "cur_goal", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); + buzzvm_pushf(vm, goto_gpsgoal[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); + buzzvm_pushf(vm, goto_gpsgoal[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); + buzzvm_pushf(vm, goto_gpsgoal[2]); buzzvm_tput(vm); buzzvm_gstore(vm); return vm->state; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index fbcc069..ca27a9d 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -762,7 +762,10 @@ script Arm(); // Registering HOME POINT. home = cur_pos; - BClpose = true; + // Initialize GPS goal for safety. + double gpspgoal[3] = {cur_pos.latitude,cur_pos.longitude,cur_pos.altitude}; + buzzuav_closures::set_gpsgoal(gpspgoal); + BClpose = true; } if (current_mode != "GUIDED" && setmode) SetMode("GUIDED", 2000); // added for compatibility with 3DR Solo @@ -792,7 +795,7 @@ script { armstate = 0; Arm(); - BClpose = false; + BClpose = false; } if (mav_client.call(cmd_srv)) { @@ -944,7 +947,7 @@ void roscontroller::gps_rb(POSE nei_pos, double out[]) void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, POSE t) /* -/ Get GPS from NED and a reference GPS point (struct input) +/ Get NED coordinates from a reference GPS point (struct input) ----------------------------------------------------------- */ { gps_convert_ned(ned_x, ned_y, t.longitude, t.latitude, cur_pos.longitude, cur_pos.latitude); @@ -953,7 +956,7 @@ void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, POSE t) void roscontroller::gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon, double gps_r_lat) /* -/ Get GPS from NED and a reference GPS point +/ Get NED coordinates from a reference GPS point and current GPS location ----------------------------------------------------------- */ { double d_lon = gps_t_lon - gps_r_lon;