restructured bzz for path planning

This commit is contained in:
dave 2017-12-22 16:48:39 -05:00
parent 390a02df8d
commit 80799aeaa6
29 changed files with 402 additions and 646 deletions

View File

@ -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<Lid)
Lid=tab
}
u=u-1
}
log("--> LOWEST ID:",Lid)
}

View File

@ -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
# })
}
})
}

View File

@ -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)
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))

View File

@ -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
}

View File

Before

Width:  |  Height:  |  Size: 67 KiB

After

Width:  |  Height:  |  Size: 67 KiB

View File

Before

Width:  |  Height:  |  Size: 77 KiB

After

Width:  |  Height:  |  Size: 77 KiB

View File

Before

Width:  |  Height:  |  Size: 75 KiB

After

Width:  |  Height:  |  Size: 75 KiB

View File

Before

Width:  |  Height:  |  Size: 96 KiB

After

Width:  |  Height:  |  Size: 96 KiB

View File

@ -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
}

View File

@ -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

View File

@ -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
}

View File

@ -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)
}

81
buzz_scripts/main.bzz Normal file
View File

@ -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() {
}

View File

@ -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() {
}

View File

@ -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));