From 6cd162bff4be2c7aca47e2e692437c89082ff0eb Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 22 Dec 2017 16:48:39 -0500 Subject: [PATCH] restructured bzz for path planning --- buzz_scripts/include/{ => act}/barrier.bzz | 28 +- buzz_scripts/include/act/states.bzz | 234 ++++++++++++++ buzz_scripts/include/{ => plan}/mapmatrix.bzz | 0 buzz_scripts/include/{ => plan}/rrtstar.bzz | 29 +- buzz_scripts/include/shapes.bzz | 111 ------- .../taskallocate}/graphformGPS.bzz | 0 .../graphs/images}/Graph_droneL.graph | 0 .../graphs/images}/Graph_droneO.graph | 0 .../graphs/images}/Graph_droneP.graph | 0 .../graphs/images}/Graph_droneY.graph | 0 .../graphs/images}/frame_01186.png | Bin .../graphs/images}/frame_01207.png | Bin .../graphs/images}/frame_01394.png | Bin .../graphs/images}/frame_01424.png | Bin .../{ => taskallocate}/graphs/shapes_L.bzz | 0 .../{ => taskallocate}/graphs/shapes_O.bzz | 0 .../{ => taskallocate}/graphs/shapes_P.bzz | 0 .../{ => taskallocate}/graphs/shapes_Y.bzz | 0 .../graphs/shapes_square.bzz | 0 .../{graphs => taskallocate}/waypointlist.csv | 0 buzz_scripts/include/uavstates.bzz | 293 ------------------ buzz_scripts/include/utilities.bzz | 152 --------- buzz_scripts/include/utils/conversions.bzz | 63 ++++ buzz_scripts/include/{ => utils}/string.bzz | 0 buzz_scripts/include/{ => utils}/vec2.bzz | 0 buzz_scripts/include/vstigenv.bzz | 8 +- buzz_scripts/main.bzz | 81 +++++ buzz_scripts/testRRT.bzz | 45 --- src/buzz_utility.cpp | 4 +- 29 files changed, 402 insertions(+), 646 deletions(-) rename buzz_scripts/include/{ => act}/barrier.bzz (68%) create mode 100644 buzz_scripts/include/act/states.bzz rename buzz_scripts/include/{ => plan}/mapmatrix.bzz (100%) rename buzz_scripts/include/{ => plan}/rrtstar.bzz (96%) delete mode 100644 buzz_scripts/include/shapes.bzz rename buzz_scripts/{ => include/taskallocate}/graphformGPS.bzz (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/Graph_droneL.graph (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/Graph_droneO.graph (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/Graph_droneP.graph (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/Graph_droneY.graph (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/frame_01186.png (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/frame_01207.png (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/frame_01394.png (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/frame_01424.png (100%) rename buzz_scripts/include/{ => taskallocate}/graphs/shapes_L.bzz (100%) rename buzz_scripts/include/{ => taskallocate}/graphs/shapes_O.bzz (100%) rename buzz_scripts/include/{ => taskallocate}/graphs/shapes_P.bzz (100%) rename buzz_scripts/include/{ => taskallocate}/graphs/shapes_Y.bzz (100%) rename buzz_scripts/include/{ => taskallocate}/graphs/shapes_square.bzz (100%) rename buzz_scripts/include/{graphs => taskallocate}/waypointlist.csv (100%) delete mode 100644 buzz_scripts/include/uavstates.bzz delete mode 100644 buzz_scripts/include/utilities.bzz create mode 100644 buzz_scripts/include/utils/conversions.bzz rename buzz_scripts/include/{ => utils}/string.bzz (100%) rename buzz_scripts/include/{ => utils}/vec2.bzz (100%) create mode 100644 buzz_scripts/main.bzz delete mode 100644 buzz_scripts/testRRT.bzz diff --git a/buzz_scripts/include/barrier.bzz b/buzz_scripts/include/act/barrier.bzz similarity index 68% rename from buzz_scripts/include/barrier.bzz rename to buzz_scripts/include/act/barrier.bzz index 31a452e..8b56f3c 100644 --- a/buzz_scripts/include/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -28,9 +28,9 @@ function barrier_create() { barrier = stigmergy.create(BARRIER_VSTIG) } -function barrier_set(threshold, transf, resumef, bdt) { +function barrier_set(threshold, transf, resumef) { statef = function() { - barrier_wait(threshold, transf, resumef, bdt); + barrier_wait(threshold, transf, resumef); } BVMSTATE = "BARRIERWAIT" barrier_create() @@ -48,7 +48,7 @@ function barrier_ready() { # # Executes the barrier # -function barrier_wait(threshold, transf, resumef, bdt) { +function barrier_wait(threshold, transf, resumef) { barrier.put(id, 1) barrier.get(id) @@ -56,31 +56,13 @@ function barrier_wait(threshold, transf, resumef, bdt) { if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { barrier.put("d", 1) timeW = 0 - transf() + BVMSTATE = transf } else if(timeW >= BARRIER_TIMEOUT) { log("------> Barrier Timeout !!!!") barrier=nil timeW = 0 - resumef() + BVMSTATE = resumef } - if(bdt!=-1) - neighbors.broadcast("cmd", bdt) - timeW = timeW+1 } - -# get the lowest id of the fleet, but requires too much bandwidth... -function getlowest(){ - Lid = 15; - u=15 - while(u>=0){ - tab = barrier.get(u) - if(tab!=nil){ - if(tab LOWEST ID:",Lid) -} diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz new file mode 100644 index 0000000..d8e3790 --- /dev/null +++ b/buzz_scripts/include/act/states.bzz @@ -0,0 +1,234 @@ +######################################## +# +# FLIGHT-RELATED FUNCTIONS +# +######################################## +include "utils/vec2.bzz" +include "plan/rrtstar.bzz" +include "act/barrier.bzz" +include "utils/conversions.bzz" + +TARGET_ALTITUDE = 15.0 # m. +BVMSTATE = "TURNEDOFF" +PICTURE_WAIT = 20 # steps +GOTO_MAXVEL = 1.5 # m/steps +GOTO_MAXDIST = 150 # m. +GOTODIST_TOL = 0.5 # m. +GOTOANG_TOL = 0.1 # rad. +path_it = 0 +rc_State = 0 +pic_time = 0 +g_it = 0 + +function turnedoff() { + BVMSTATE = "TURNEDOFF" +} + +function idle() { + BVMSTATE = "IDLE" +} + +function launch() { + BVMSTATE = "LAUNCH" + if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND + homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} + if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS, "NAVIGATE", "LAUNCH") + barrier_ready() + } else { + log("Altitude: ", pose.position.altitude) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } + } else { + barrier_set(ROBOTS, "NAVIGATE", "LAUNCH") + barrier_ready() + } +} + +function stop() { + BVMSTATE = "STOP" + if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND + neighbors.broadcast("cmd", 21) + uav_land() + if(flight.status != 2 and flight.status != 3) { + barrier_set(ROBOTS,"TURNEDOFF","STOP") + barrier_ready() + } + } else { + barrier_set(ROBOTS,"TURNEDOFF","STOP") + barrier_ready() + } +} + +function take_picture() { + BVMSTATE="PICTURE" + uav_setgimbal(0.0, 0.0, -90.0, 20.0) + if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize + uav_takepicture() + } else if(pic_time>=PICTURE_WAIT) { # wait for the picture + BVMSTATE="IDLE" + pic_time=0 + } + pic_time=pic_time+1 +} + +# +# Work in progress.... +function navigate() { + BVMSTATE="NAVIGATE" + if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz + uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude) + cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) + m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) + } + + print(" has to move ", math.vec2.length(cur_goal), "from ", m_pos.x, " ", m_pos.y) + if(math.vec2.length(cur_goal)>GOTO_MAXDIST*5) { + log("Sorry this is too far.") + return + } + + # create the map + if(map==nil) { + dyn_init_map(cur_goal) + if(V_TYPE == 0) { + homegps.lat = pose.position.latitude + homegps.long = pose.position.longitude + } + } + + if(Path==nil) { + # add proximity sensor and neighbor obstacles to the map + populateGrid(m_pos) + # start the planner + path_it = 1 + pathPlanner(cur_goal, m_pos) + } else if(path_it <= size(Path)) { + var cur_path_pt = convert_pt(getvec(Path, path_it)) + print(" heading to [", path_it, "/", size(Path), "]", cur_path_pt.x, cur_path_pt.y) + if(math.vec2.length(cur_path_pt) > GOTODIST_TOL) { + populateGrid(m_pos) + if(check_path(Path, path_it, m_pos, 0)) { + # stop + goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude) + Path = nil + if(V_TYPE == 0) + cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) + # re-start the planner + path_it = 1 + pathPlanner(cur_goal, m_pos) + } else { + cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt)) + goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude) + } + } else + path_it = path_it + 1 + } else { + Path = nil + BVMSTATE="IDLE" + } +} + +# function goto_direct(transf) { +# m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude) +# 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.") +# else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity +# m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) +# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) +# } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination +# transf() +# else +# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) +# } + +function follow() { + if(size(targets)>0) { + BVMSTATE = "FOLLOW" + attractor=math.vec2.newp(0,0) + foreach(targets, function(id, tab) { + force=(0.05)*(tab.range)^4 + attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) + }) + uav_moveto(attractor.x, attractor.y, 0.0) + } else { + log("No target in local table!") + BVMSTATE = "IDLE" + } +} + +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) { + log("cmd 21") + flight.rc_cmd=0 + BVMSTATE = "STOP" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + BVMSTATE = "PATHPLAN" + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } else if (flight.rc_cmd==666){ + flight.rc_cmd=0 + stattab_send() + } else if (flight.rc_cmd==900){ + flight.rc_cmd=0 + rc_State = 0 + neighbors.broadcast("cmd", 900) + } else if (flight.rc_cmd==901){ + flight.rc_cmd=0 + rc_State = 1 + neighbors.broadcast("cmd", 901) + } else if (flight.rc_cmd==902){ + flight.rc_cmd=0 + rc_State = 2 + neighbors.broadcast("cmd", 902) + } else if (flight.rc_cmd==903){ + flight.rc_cmd=0 + rc_State = 3 + neighbors.broadcast("cmd", 903) + } +} + +function nei_cmd_listen() { + neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") + if(value==22 and BVMSTATE!="LAUNCH" and BVMSTATE!="BARRIERWAIT") { + BVMSTATE = "LAUNCH" + } else if(value==21 and BVMSTATE!="STOP" and BVMSTATE!="BARRIERWAIT") { + BVMSTATE = "STOP" + } else if(value==400 and BVMSTATE=="TURNEDOFF") { + uav_arm() + } else if(value==401 and BVMSTATE=="TURNEDOFF"){ + uav_disarm() + } else if(value==900){ + rc_State = 0 + } else if(value==901){ + rc_State = 1 + } else if(value==902){ + rc_State = 2 + } else if(value==903){ + rc_State = 3 + } 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/mapmatrix.bzz b/buzz_scripts/include/plan/mapmatrix.bzz similarity index 100% rename from buzz_scripts/include/mapmatrix.bzz rename to buzz_scripts/include/plan/mapmatrix.bzz diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/plan/rrtstar.bzz similarity index 96% rename from buzz_scripts/include/rrtstar.bzz rename to buzz_scripts/include/plan/rrtstar.bzz index 3fa686c..6d40626 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/plan/rrtstar.bzz @@ -3,7 +3,7 @@ # # map table-based matrix ##### -include "mapmatrix.bzz" +include "plan/mapmatrix.bzz" RRT_TIMEOUT = 500 RRT_RUNSTEP = 3 @@ -78,7 +78,7 @@ function getcell(m_curpos) { } function populateGrid(m_pos) { - getproxobs(m_pos) + # getproxobs(m_pos) getneiobs (m_pos) export_map(map) } @@ -211,8 +211,7 @@ function check_path(m_path, goal_l, m_curpos, kh4) { ## function rrtstar() { # update state machine variables - statef = rrtstar - BVMSTATE = "RRTSTAR" + BVMSTATE = "PATHPLAN" step_timeout = 0 @@ -336,16 +335,14 @@ function rrtstar() { if(goalReached){ log("Goal found(",numberOfPoints,")!") - Path = getPath(Q,numberOfPoints, 1) + Path = convert_path(getPath(Q,numberOfPoints)) # done. resume the state machine - BVMSTATE = "GOTOGPS" - statef = old_statef + BVMSTATE = "NAVIGATE" } else if(timeout >= RRT_TIMEOUT) { log("FAILED TO FIND A PATH!!!") Path = nil # done. resume the state machine - BVMSTATE = "GOTOGPS" - statef = old_statef + BVMSTATE = "NAVIGATE" } } @@ -437,7 +434,7 @@ function doesItIntersect(point,vector) { } # create a table with only the path's points in order -function getPath(Q,nb,gps){ +function getPath(Q,nb){ var pathL={} var npt=0 # get the path from the point list @@ -460,15 +457,9 @@ function getPath(Q,nb,gps){ var totpt = npt + 1 while(npt > 0){ pathR[totpt-npt] = {} - if(gps) { - tmpgoal = gps_from_vec(math.vec2.sub(getvec(pathL,npt),cur_cell)) - pathR[totpt-npt][1]=tmpgoal.latitude - pathR[totpt-npt][2]=tmpgoal.longitude - } else { - tmpgoal = getvec(path,npt) - pathR[totpt-npt][1]=tmpgoal.x - pathR[totpt-npt][2]=tmpgoal.y - } + var tmpgoal = getvec(pathL,npt) + pathR[totpt-npt][1]=tmpgoal.x + pathR[totpt-npt][2]=tmpgoal.y npt = npt - 1 } #log("Double-check path: ", check_path(pathR, 1, cur_cell, 1)) diff --git a/buzz_scripts/include/shapes.bzz b/buzz_scripts/include/shapes.bzz deleted file mode 100644 index 53cdd3b..0000000 --- a/buzz_scripts/include/shapes.bzz +++ /dev/null @@ -1,111 +0,0 @@ -#Table of the nodes in the graph -m_vecNodes={} -m_vecNodes_fixed={} -m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing - .Lable = 0, # Lable of the point - .Pred = -1, # Lable of its predecessor - .distance = -1, # distance to the predecessor - .bearing = -1, # bearing form the predecessor to this dot - .height = 3000, # height of this dot - .State="UNASSIGNED", - .StateAge=0 -} -m_vecNodes[1] = { - .Lable = 1, - .Pred = 0, - .distance = 1000, - .bearing = 0.0, - .height = 5000, - .State="UNASSIGNED", - .StateAge=0 -} -m_vecNodes[2] = { - .Lable = 2, - .Pred = 0, - .distance = 1000, - .bearing = 1.57, - .height = 7000, - .State="UNASSIGNED", - .StateAge=0 -} -m_vecNodes[3] = { - .Lable = 3, - .Pred = 0, - .distance = 1000, - .bearing = 3.14, - .height = 9000, - .State="UNASSIGNED", - .StateAge=0 -} -m_vecNodes[4] = { - .Lable = 4, - .Pred = 0, - .distance = 1000, - .bearing = 4.71, - .height = 11000, - .State="UNASSIGNED", - .StateAge=0 -} - -# -# Graph parsing -# -function parse_graph(fname) { - # Graph data - var gd = {} - # Open the file - var fd = io.fopen(fname, "r") - if(not fd) { - log("Can't open '", fname, "'") - return nil - } - # Parse the file line by line - var rrec # Record read from line - var grec # Record parsed into graph - io.fforeach(fd, function(line) { - # Parse file line - rrec = string.split(line, "\t ") - # Make record - gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing - .Lable = string.toint(rrec[0]), # Lable of the point - .Pred = string.toint(rrec[1]), # Lable of its predecessor - .distance = string.tofloat(rrec[2]), # distance to the predecessor - .bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot - .height = string.tofloat(rrec[4]), # height of this dot - .State="UNASSIGNED", - .StateAge=0 - }}) - # All done - io.fclose(fd) - return gd -} - -function parse_graph_fixed(fname) { - # Graph data - var gd = {} - # Open the file - var fd = io.fopen(fname, "r") - if(not fd) { - log("Can't open '", fname, "'") - return nil - } - # Parse the file line by line - var rrec # Record read from line - var grec # Record parsed into graph - io.fforeach(fd, function(line) { - # Parse file line - rrec = string.split(line, "\t ") - # Make record - gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2 - .Pred1 = string.toint(rrec[1]), # Pred 1 lable - .Pred2 = string.toint(rrec[3]), # Pred 2 lable - .d1 = string.tofloat(rrec[2]), # Pred 1 distance - .d2 = string.tofloat(rrec[4]), # Pred 2 distance - .Lable=string.toint(rrec[0]), - .State="UNASSIGNED", - .StateAge=0 - }}) - # All done - io.fclose(fd) - return gd -} \ No newline at end of file diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz similarity index 100% rename from buzz_scripts/graphformGPS.bzz rename to buzz_scripts/include/taskallocate/graphformGPS.bzz diff --git a/buzz_scripts/include/graphs/Graph_droneL.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneL.graph similarity index 100% rename from buzz_scripts/include/graphs/Graph_droneL.graph rename to buzz_scripts/include/taskallocate/graphs/images/Graph_droneL.graph diff --git a/buzz_scripts/include/graphs/Graph_droneO.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneO.graph similarity index 100% rename from buzz_scripts/include/graphs/Graph_droneO.graph rename to buzz_scripts/include/taskallocate/graphs/images/Graph_droneO.graph diff --git a/buzz_scripts/include/graphs/Graph_droneP.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneP.graph similarity index 100% rename from buzz_scripts/include/graphs/Graph_droneP.graph rename to buzz_scripts/include/taskallocate/graphs/images/Graph_droneP.graph diff --git a/buzz_scripts/include/graphs/Graph_droneY.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneY.graph similarity index 100% rename from buzz_scripts/include/graphs/Graph_droneY.graph rename to buzz_scripts/include/taskallocate/graphs/images/Graph_droneY.graph diff --git a/buzz_scripts/include/graphs/frame_01186.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01186.png similarity index 100% rename from buzz_scripts/include/graphs/frame_01186.png rename to buzz_scripts/include/taskallocate/graphs/images/frame_01186.png diff --git a/buzz_scripts/include/graphs/frame_01207.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01207.png similarity index 100% rename from buzz_scripts/include/graphs/frame_01207.png rename to buzz_scripts/include/taskallocate/graphs/images/frame_01207.png diff --git a/buzz_scripts/include/graphs/frame_01394.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01394.png similarity index 100% rename from buzz_scripts/include/graphs/frame_01394.png rename to buzz_scripts/include/taskallocate/graphs/images/frame_01394.png diff --git a/buzz_scripts/include/graphs/frame_01424.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01424.png similarity index 100% rename from buzz_scripts/include/graphs/frame_01424.png rename to buzz_scripts/include/taskallocate/graphs/images/frame_01424.png diff --git a/buzz_scripts/include/graphs/shapes_L.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_L.bzz similarity index 100% rename from buzz_scripts/include/graphs/shapes_L.bzz rename to buzz_scripts/include/taskallocate/graphs/shapes_L.bzz diff --git a/buzz_scripts/include/graphs/shapes_O.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_O.bzz similarity index 100% rename from buzz_scripts/include/graphs/shapes_O.bzz rename to buzz_scripts/include/taskallocate/graphs/shapes_O.bzz diff --git a/buzz_scripts/include/graphs/shapes_P.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_P.bzz similarity index 100% rename from buzz_scripts/include/graphs/shapes_P.bzz rename to buzz_scripts/include/taskallocate/graphs/shapes_P.bzz diff --git a/buzz_scripts/include/graphs/shapes_Y.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_Y.bzz similarity index 100% rename from buzz_scripts/include/graphs/shapes_Y.bzz rename to buzz_scripts/include/taskallocate/graphs/shapes_Y.bzz diff --git a/buzz_scripts/include/graphs/shapes_square.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_square.bzz similarity index 100% rename from buzz_scripts/include/graphs/shapes_square.bzz rename to buzz_scripts/include/taskallocate/graphs/shapes_square.bzz diff --git a/buzz_scripts/include/graphs/waypointlist.csv b/buzz_scripts/include/taskallocate/waypointlist.csv similarity index 100% rename from buzz_scripts/include/graphs/waypointlist.csv rename to buzz_scripts/include/taskallocate/waypointlist.csv diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz deleted file mode 100644 index 8a27c38..0000000 --- a/buzz_scripts/include/uavstates.bzz +++ /dev/null @@ -1,293 +0,0 @@ -######################################## -# -# FLIGHT-RELATED FUNCTIONS -# -######################################## -include "vec2.bzz" -include "rrtstar.bzz" - -TARGET_ALTITUDE = 15.0 # m. -BVMSTATE = "TURNEDOFF" -PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 1.5 # m/steps -GOTO_MAXDIST = 150 # m. -GOTODIST_TOL = 0.5 # m. -GOTOANG_TOL = 0.1 # rad. -cur_goal_l = 0 -rc_State = 0 - -function uav_initswarm() { - s = swarm.create(1) - s.join() -} - -function turnedoff() { - statef=turnedoff - BVMSTATE = "TURNEDOFF" -} - -function idle() { - statef=idle - BVMSTATE = "IDLE" -} - -function takeoff() { - BVMSTATE = "TAKEOFF" - statef=takeoff - homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} - - if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS, action, land, -1) - barrier_ready() - } else { - log("Altitude: ", pose.position.altitude) - neighbors.broadcast("cmd", 22) - uav_takeoff(TARGET_ALTITUDE) - } -} - -function land() { - BVMSTATE = "LAND" - statef=land - - neighbors.broadcast("cmd", 21) - uav_land() - - if(flight.status != 2 and flight.status != 3) { - barrier_set(ROBOTS,turnedoff,land, 21) - barrier_ready() - } -} - -function set_goto(transf) { - BVMSTATE = "GOTOGPS" - statef=function() { - gotoWPRRT(transf) - } - - if(rc_goto.id==id){ - if(s!=nil){ - if(s.in()) - s.leave() - } - } else { - neighbors.broadcast("cmd", 16) - neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) - } -} - -ptime=0 -function picture() { - statef=picture - BVMSTATE="PICTURE" - uav_setgimbal(0.0, 0.0, -90.0, 20.0) - if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize - uav_takepicture() - } else if(ptime>=PICTURE_WAIT) { # wait for the picture - statef=action - ptime=0 - } - ptime=ptime+1 -} - -# -# still requires to be tuned, replaning takes too much time.. -# DS 23/11/2017 -function gotoWPRRT(transf) { - rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) - m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) - print(" has to move ", math.vec2.length(rc_goal), "from ", m_pos.x, " ", m_pos.y) - - if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5) { - log("Sorry this is too far.") - return - } - # create the map - if(map==nil) { - dyn_init_map(rc_goal) - homegps.lat = pose.position.latitude - homegps.long = pose.position.longitude - } - - if(Path==nil) { - # add proximity sensor and neighbor obstacles to the map - populateGrid(m_pos) - # start the planner - pathPlanner(rc_goal, m_pos) - cur_goal_l = 1 - } else if(cur_goal_l <= size(Path)) { - var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude - var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0) - print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y) - if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { - populateGrid(m_pos) - if(check_path(Path, cur_goal_l, m_pos, 0)) { - uav_moveto(0.0, 0.0, rc_goto.altitude - pose.position.altitude) - Path = nil - rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) - # re-start the planner - pathPlanner(rc_goal, m_pos) - cur_goal_l = 1 - } else { - cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) - uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude - pose.position.altitude) - } - } else - cur_goal_l = cur_goal_l + 1 - } else { - Path = nil - transf() - } -} - -function gotoWP(transf) { - m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude) - 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.") - else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity - m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) - } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination - transf() - else - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) -} - -function follow() { - if(size(targets)>0) { - BVMSTATE = "FOLLOW" - statef=follow - attractor=math.vec2.newp(0,0) - foreach(targets, function(id, tab) { - force=(0.05)*(tab.range)^4 - attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) - }) - uav_moveto(attractor.x, attractor.y, 0.0) - } else { - log("No target in local table!") - #statef=idle - } -} - -function uav_rccmd() { - if(flight.rc_cmd==22) { - log("cmd 22") - flight.rc_cmd=0 - statef = takeoff - BVMSTATE = "TAKEOFF" - neighbors.broadcast("cmd", 22) - } else if(flight.rc_cmd==21) { - log("cmd 21") - log("To land") - flight.rc_cmd=0 - statef = land - BVMSTATE = "LAND" - neighbors.broadcast("cmd", 21) - } else if(flight.rc_cmd==16) { - flight.rc_cmd=0 - BVMSTATE = "GOTOGPS" - statef = goto - } else if(flight.rc_cmd==400) { - flight.rc_cmd=0 - uav_arm() - neighbors.broadcast("cmd", 400) - } else if (flight.rc_cmd==401){ - flight.rc_cmd=0 - uav_disarm() - neighbors.broadcast("cmd", 401) - } else if (flight.rc_cmd==666){ - flight.rc_cmd=0 - stattab_send() - } else if (flight.rc_cmd==900){ - flight.rc_cmd=0 - rc_State = 0 - neighbors.broadcast("cmd", 900) - } else if (flight.rc_cmd==901){ - flight.rc_cmd=0 - rc_State = 1 - neighbors.broadcast("cmd", 901) - } else if (flight.rc_cmd==902){ - flight.rc_cmd=0 - rc_State = 2 - neighbors.broadcast("cmd", 902) - } else if (flight.rc_cmd==903){ - flight.rc_cmd=0 - rc_State = 3 - neighbors.broadcast("cmd", 903) - } -} - -function uav_neicmd() { - neighbors.listen("cmd", - function(vid, value, rid) { - print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") - if(value==22 and BVMSTATE!="TAKEOFF" and BVMSTATE!="BARRIERWAIT") { - statef=takeoff - BVMSTATE = "TAKEOFF" - } else if(value==21 and BVMSTATE!="LAND" and BVMSTATE!="BARRIERWAIT") { - statef=land - BVMSTATE = "LAND" - } else if(value==400 and BVMSTATE=="TURNEDOFF") { - uav_arm() - } else if(value==401 and BVMSTATE=="TURNEDOFF"){ - uav_disarm() - } else if(value==900){ - rc_State = 0 - } else if(value==901){ - rc_State = 1 - } else if(value==902){ - rc_State = 2 - } else if(value==903){ - rc_State = 3 - } 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 - # }) - } - }) -} - -function LimitAngle(angle){ - if(angle>2*math.pi) - return angle-2*math.pi - else if (angle<0) - return angle+2*math.pi - else - return angle -} - -function vec_from_gps(lat, lon, home_ref) { - d_lon = lon - pose.position.longitude - d_lat = lat - pose.position.latitude - if(home_ref == 1) { - d_lon = lon - homegps.long - d_lat = lat - homegps.lat - } - ned_x = d_lat/180*math.pi * 6371000.0; - ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); - #Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); - #Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x)); - return math.vec2.new(ned_x,ned_y) -} - -function gps_from_vec(vec) { - Lgoal = {.latitude=0, .longitude=0} - Vrange = math.vec2.length(vec) - Vbearing = LimitAngle(math.atan(vec.y, vec.x)) -# print("rb2gps: ",Vrange,Vbearing, pose.position.latitude, pose.position.longitude) - latR = pose.position.latitude*math.pi/180.0; - lonR = pose.position.longitude*math.pi/180.0; - target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing)); - target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat)); - Lgoal.latitude = target_lat*180.0/math.pi; - Lgoal.longitude = target_lon*180.0/math.pi; - #d_lat = (vec.x / 6371000.0)*180.0/math.pi; - #goal.latitude = d_lat + pose.position.latitude; - #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; - #goal.longitude = d_lon + pose.position.longitude; - - return Lgoal -} diff --git a/buzz_scripts/include/utilities.bzz b/buzz_scripts/include/utilities.bzz deleted file mode 100644 index 6c4dab6..0000000 --- a/buzz_scripts/include/utilities.bzz +++ /dev/null @@ -1,152 +0,0 @@ -# Utilities - -# Rads to degrees -function rtod(r) { - return (r*(180.0/math.pi)) -} - -# Copy a table -function table_deep_copy(new_t, old_t, depth) { - depth = depth - 1 - if (old_t != nil) { - foreach(old_t, function(key, value) { - new_t[key] = value - if(depth != 0) { - new_t[key] = {} - table_deep_copy(new_t[key], value, depth) - } - }) - } -} - -function table_copy(old_t, depth) { - var t = {}; - table_deep_copy(t, old_t, depth); - return t; -} - -# Print the contents of a table -function table_print(t, depth) { - depth = depth - 1 - if (t != nil) { - foreach(t, function(key, value) { - log(key, " -> ", value) - if(depth != 0) { - table_print(t[key], depth) - } - }) - } -} - -# Write a table as if it was a vector -function write_vector(k, index, val) { - var key = string.tostring(index) - k[key] = val -} - -# Read a table as if it was a vector -function read_vector(k, index) { - var key = string.tostring(index) - if (k[key] == nil) { - return -1 - } else { - return k[key] - } -} - -# Write a table as if it was a matrix -function write_matrix(k, row, col, val) { - var key = string.concat(string.tostring(row),"-",string.tostring(col)) - k[key] = val -} - -# Read a table as if it was a matrix -function read_matrix(k, row, col) { - var key = string.concat(string.tostring(row),"-",string.tostring(col)) - if (k[key] == nil) { - return -1 - } else { - return k[key] - } -} - -# Int to String -function itos(i) { - - log("Use 'string.tostring(OJB)' instead") - - if (i==0) { return "0" } - if (i==1) { return "1" } - if (i==2) { return "2" } - if (i==3) { return "3" } - if (i==4) { return "4" } - if (i==5) { return "5" } - if (i==6) { return "6" } - if (i==7) { return "7" } - if (i==8) { return "8" } - if (i==9) { return "9" } - - log("Function 'itos' out of bounds, returning the answer (42)") - return "42" -} - -# String to Int -function stoi(s) { - if (s=='0') { return 0 } - if (s=='1') { return 1 } - if (s=='2') { return 2 } - if (s=='3') { return 3 } - if (s=='4') { return 4 } - if (s=='5') { return 5 } - if (s=='6') { return 6 } - if (s=='7') { return 7 } - if (s=='8') { return 8 } - if (s=='9') { return 9 } - - log("Function 'stoi' out of bounds, returning the answer (42)") - return 42 - -} - -# Force angles in the (-pi,pi) interval -function radians_interval(a) { - var temp = a - while ((temp>2.0*math.pi) or (temp<0.0)) { - if (temp > 2.0*math.pi) { - temp = temp - 2.0*math.pi - } else if (temp < 0.0){ - temp = temp + 2.0*math.pi - } - } - if (temp > math.pi) { - temp = temp - 2.0*math.pi - } - return temp -} - -############################################ - -#base = {} - -#base.create = function() { -# return { -# .method = function(a,b) { -# return a + b -# } -# } -# } - -#x = base.create() -#x.method(3,4) # returns 7 - -#derived = {} - -#derived.create = function() { -# b = base.create() -# b.method = function(a,b) { -# return a * b -# } -#} - -#x = derived.create() -#x.method(3,4) # returns 12 diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz new file mode 100644 index 0000000..144cabf --- /dev/null +++ b/buzz_scripts/include/utils/conversions.bzz @@ -0,0 +1,63 @@ +function LimitAngle(angle){ + if(angle>2*math.pi) + return angle-2*math.pi + else if (angle<0) + return angle+2*math.pi + else + return angle +} + +# TODO: add other conversions.... +function convert_path(P) { + var pathR={} + if(V_TYPE == 0) { + var n = 1 + while(n <= size(P)){ + pathR[n] = {} + var tmpgoal = gps_from_vec(math.vec2.sub(getvec(P,n),cur_cell)) + pathR[n][1]=tmpgoal.latitude + pathR[n][2]=tmpgoal.longitude + n = n + 1 + } + } + return pathR +} + +# TODO: add other conversions.... +function convert_pt(in) { + if(V_TYPE == 0) + return vec_from_gps(in.x, in.y, 0) +} + +function vec_from_gps(lat, lon, home_ref) { + d_lon = lon - pose.position.longitude + d_lat = lat - pose.position.latitude + if(home_ref == 1) { + d_lon = lon - homegps.long + d_lat = lat - homegps.lat + } + ned_x = d_lat/180*math.pi * 6371000.0; + ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); + #Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); + #Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x)); + return math.vec2.new(ned_x,ned_y) +} + +function gps_from_vec(vec) { + Lgoal = {.latitude=0, .longitude=0} + Vrange = math.vec2.length(vec) + Vbearing = LimitAngle(math.atan(vec.y, vec.x)) +# print("rb2gps: ",Vrange,Vbearing, pose.position.latitude, pose.position.longitude) + latR = pose.position.latitude*math.pi/180.0; + lonR = pose.position.longitude*math.pi/180.0; + target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing)); + target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat)); + Lgoal.latitude = target_lat*180.0/math.pi; + Lgoal.longitude = target_lon*180.0/math.pi; + #d_lat = (vec.x / 6371000.0)*180.0/math.pi; + #goal.latitude = d_lat + pose.position.latitude; + #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; + #goal.longitude = d_lon + pose.position.longitude; + + return Lgoal +} \ No newline at end of file diff --git a/buzz_scripts/include/string.bzz b/buzz_scripts/include/utils/string.bzz similarity index 100% rename from buzz_scripts/include/string.bzz rename to buzz_scripts/include/utils/string.bzz diff --git a/buzz_scripts/include/vec2.bzz b/buzz_scripts/include/utils/vec2.bzz similarity index 100% rename from buzz_scripts/include/vec2.bzz rename to buzz_scripts/include/utils/vec2.bzz diff --git a/buzz_scripts/include/vstigenv.bzz b/buzz_scripts/include/vstigenv.bzz index 0a7b01f..4c0e06b 100644 --- a/buzz_scripts/include/vstigenv.bzz +++ b/buzz_scripts/include/vstigenv.bzz @@ -19,7 +19,13 @@ var v_ground = {} b_updating = 0 vstig_counter = WAIT4STEP -function uav_initstig() { + +function init_swarm() { + s = swarm.create(1) + s.join() +} + +function init_stig() { v_status = stigmergy.create(STATUS_VSTIG) v_ground = stigmergy.create(GROUND_VSTIG) } diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz new file mode 100644 index 0000000..6bf2856 --- /dev/null +++ b/buzz_scripts/main.bzz @@ -0,0 +1,81 @@ +include "update.bzz" +# don't use a stigmergy id=11 with this header, for barrier +# it requires an 'action' function to be defined here. +include "act/states.bzz" +include "vstigenv.bzz" + +##### +# Vehicule type: +# 0 -> outdoor flying vehicle +# 1 -> indoor flying vehicle +# 2 -> outdoor wheeled vehicle +# 3 -> indoor wheeled vehicle +V_TYPE = 0 + +goal_list = { + .0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5} +} + +function action() { + statef=action + set_goto(idle) +} + +# Executed once at init time. +function init() { + init_stig() + init_swarm() + + TARGET_ALTITUDE = 25.0 # m + + # start the swarm command listener + # nei_cmd_listen() + + # Starting state + # BVMSTATE = "TURNEDOFF" + BVMSTATE = "LAUNCHED" +} + +# Executed at each time step. +function step() { + # listen to Remote Controller + rc_cmd_listen() + + # update the vstig (status/net/batt/...) + # uav_updatestig() + + # + # graph state machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCHED") # ends on navigate + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="TASK_ALLOCATE") #TODO: not tested in new structure + statef=makegraph # or bidding + else if(BVMSTATE=="PATHPLAN") # ends on navigate + statef=rrtstar + else if(BVMSTATE=="NAVIGATE") # ends on idle + statef=navigate + else if(BVMSTATE == "FOLLOW") #TODO: not tested in new structure + statef=follow + else if(BVMSTATE == "PICTURE") #TODO: not tested in new structure + statef=take_picture + + statef() + + log("Current state: ", BVMSTATE) + +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/buzz_scripts/testRRT.bzz b/buzz_scripts/testRRT.bzz deleted file mode 100644 index 3ad4cc6..0000000 --- a/buzz_scripts/testRRT.bzz +++ /dev/null @@ -1,45 +0,0 @@ -include "update.bzz" -include "barrier.bzz" # don't use a stigmergy id=11 with this header. -include "uavstates.bzz" # require an 'action' function to be defined here. -include "vstigenv.bzz" - -function action() { - statef=action - uav_storegoal(45.5088103899,-73.1540826153,2.5) - set_goto(idle) -} - -# Executed once at init time. -function init() { - uav_initstig() - uav_initswarm() - - TARGET_ALTITUDE = 2.5 # m - # statef=turnedoff - # BVMSTATE = "TURNEDOFF" - statef = takeoff - BVMSTATE = "TAKEOFF" -} - -# Executed at each time step. -function step() { - uav_rccmd() - - statef() - - log("Current state: ", BVMSTATE) - # log("Obstacles: ") - # reduce(proximity, - # function(key, value, acc) { - # log(key, " - ", value.angle, value.value) - # return acc - # }, math.vec2.new(0, 0)) -} - -# Executed once when the robot (or the simulator) is reset. -function reset() { -} - -# Executed once at the end of experiment. -function destroy() { -} diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index db43d6f..507e70c 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -201,7 +201,7 @@ static int buzz_register_hooks() buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); @@ -255,7 +255,7 @@ static int testing_buzz_register_hooks() buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));