debug new matrix form for RRT and add blob check for intersection
This commit is contained in:
dave
vivek-shankar
parent
177baf5d15
commit
6371581b66
|
|
@ -5,8 +5,8 @@ robot_marker = "X"
|
|||
# copy a full matrix row
|
||||
function mat_copyrow(out,ro,in,ri){
|
||||
out[ro] = {}
|
||||
var icr = 0
|
||||
while(icr < size(in[ri])) {
|
||||
var icr = 1
|
||||
while(icr <= size(in[ri])) {
|
||||
out[ro][icr]=in[ri][icr]
|
||||
icr = icr + 1
|
||||
}
|
||||
|
|
@ -18,11 +18,11 @@ function getvec(t,row){
|
|||
|
||||
function init_test_map(len){
|
||||
map = {}
|
||||
var indexR = 0
|
||||
while(indexR < len) {
|
||||
var indexR = 1
|
||||
while(indexR <= len) {
|
||||
map[indexR] = {}
|
||||
var indexC = 0
|
||||
while(indexC < len) {
|
||||
var indexC = 1
|
||||
while(indexC <= len) {
|
||||
map[indexR][indexC] = 1.0
|
||||
indexC = indexC + 1
|
||||
}
|
||||
|
|
@ -33,22 +33,22 @@ function init_test_map(len){
|
|||
map[6][5] = 0.0
|
||||
map[4][5] = 0.0
|
||||
|
||||
log("Occupancy grid initialized (",len,"x",len,") with obstacles.")
|
||||
log("Occupancy grid initialized (",size(map),"x",size(map[1]),") with obstacles.")
|
||||
}
|
||||
|
||||
function init_map(len){
|
||||
map = {}
|
||||
var indexR = 0
|
||||
while(indexR < len) {
|
||||
var indexR = 1
|
||||
while(indexR <= len) {
|
||||
map[indexR] = {}
|
||||
var indexC = 0
|
||||
while(indexC < len) {
|
||||
var indexC = 1
|
||||
while(indexC <= len) {
|
||||
map[indexR][indexC] = 1.0
|
||||
indexC = indexC + 1
|
||||
}
|
||||
indexR = indexR + 1
|
||||
}
|
||||
log("Occupancy grid initialized (",len,"x",len,").")
|
||||
log("Occupancy grid initialized (",size(map),"x",size(map[1]),").")
|
||||
}
|
||||
|
||||
function add_obstacle(pos, off, inc_trust) {
|
||||
|
|
@ -57,7 +57,7 @@ function add_obstacle(pos, off, inc_trust) {
|
|||
|
||||
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
|
||||
#log("Add obstacle in cell: ", xi, " ", yi)
|
||||
old=map[xi][yi]
|
||||
var old=map[xi][yi]
|
||||
if(old-inc_trust > 0.0)
|
||||
map[xi][yi] = old-inc_trust
|
||||
else
|
||||
|
|
@ -71,7 +71,7 @@ function remove_obstacle(pos, off, dec_trust) {
|
|||
|
||||
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0){
|
||||
#log("Remove obstacle in cell: ", xi, " ", yi)
|
||||
old=map[xi][yi]
|
||||
var old=map[xi][yi]
|
||||
if(old + dec_trust < 1.0) #x,y
|
||||
map[xi][yi] = old+dec_trust
|
||||
else
|
||||
|
|
@ -79,6 +79,30 @@ function remove_obstacle(pos, off, dec_trust) {
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
function get_occupied_cells(cur_cell){
|
||||
var i = 1
|
||||
var occupied_cells = {}
|
||||
occupied_cells[0] = cur_cell
|
||||
occupied_cells[1] = math.vec2.new(cur_cell.x + 1, cur_cell.y)
|
||||
occupied_cells[2] = math.vec2.new(cur_cell.x - 1, cur_cell.y)
|
||||
occupied_cells[3] = math.vec2.new(cur_cell.x, cur_cell.y + 1)
|
||||
occupied_cells[4] = math.vec2.new(cur_cell.x, cur_cell.y - 1)
|
||||
return occupied_cells
|
||||
}
|
||||
|
||||
function is_in(dictionary, x, y){
|
||||
var i = 0
|
||||
while(i < size(dictionary)){
|
||||
if(dictionary[i].x == x and dictionary[i].y == y){
|
||||
return 1
|
||||
}
|
||||
i = i + 1
|
||||
}
|
||||
return 0
|
||||
}
|
||||
|
||||
|
||||
function table_print(t) {
|
||||
foreach(t, function(key, value) {
|
||||
log(key, " -> ", value)
|
||||
|
|
|
|||
|
|
@ -14,12 +14,12 @@ function UAVinit_map(m_navigation) {
|
|||
# create a map big enough for the goal
|
||||
init_map(2*math.round(math.vec2.length(m_navigation))+10)
|
||||
# center the robot on the grid
|
||||
cur_cell = math.vec2.new(math.round(size(map[1])/2.0),math.round(size(map)/2.0))
|
||||
cur_cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0))
|
||||
}
|
||||
|
||||
function UAVpathPlanner(m_navigation, m_pos) {
|
||||
# place the robot on the grid
|
||||
update_curcell(m_pos,0)
|
||||
cur_cell = getcell(m_pos,0)
|
||||
# create the goal in the map grid
|
||||
mapgoal = math.vec2.add(m_navigation,cur_cell)
|
||||
mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y))
|
||||
|
|
@ -34,7 +34,7 @@ function Kh4pathPlanner(m_goal, m_pos) {
|
|||
m_goal = math.vec2.new(math.round(m_goal.x*CM2KH4.x), math.round(m_goal.y*CM2KH4.y))
|
||||
|
||||
# place the robot on the grid
|
||||
update_curcell(m_pos,1)
|
||||
cur_cell = getcell(m_pos,1)
|
||||
log("Starting from cell: ", cur_cell.x, " ", cur_cell.y)
|
||||
log("Going to cell: ", m_goal.x, " ", m_goal.y)
|
||||
|
||||
|
|
@ -44,26 +44,35 @@ function Kh4pathPlanner(m_goal, m_pos) {
|
|||
return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y))
|
||||
}
|
||||
|
||||
function update_curcell(m_curpos, kh4) {
|
||||
function getcell(m_curpos, kh4) {
|
||||
var cell = {}
|
||||
if(kh4) { # for khepera playground
|
||||
cur_cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
|
||||
cur_cell = math.vec2.new(math.round(cur_cell.x*CM2KH4.x), math.round(cur_cell.y*CM2KH4.y))
|
||||
cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
|
||||
cell = math.vec2.new(math.round(cell.x*CM2KH4.x), math.round(cell.y*CM2KH4.y))
|
||||
} else { # for uav map relative to home
|
||||
cur_cell = math.vec2.add(cur_cell, m_curpos)
|
||||
cur_cell = math.vec2.new(math.round(m_curpos.x), math.round(m_curpos.y))
|
||||
cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0))
|
||||
cell = math.vec2.add(cell, m_curpos)
|
||||
cell = math.vec2.new(math.round(cell.x), math.round(cell.y))
|
||||
}
|
||||
if(cur_cell.x > size(map))
|
||||
cur_cell.x = size(map)
|
||||
if(cur_cell.y > size(map[1]))
|
||||
cur_cell.y = size(map[1])
|
||||
if(cur_cell.x < 1)
|
||||
cur_cell.x = 1
|
||||
if(cur_cell.y < 1)
|
||||
cur_cell.y = 1
|
||||
if(cell.x > size(map))
|
||||
cell.x = size(map)
|
||||
if(cell.y > size(map[1]))
|
||||
cell.y = size(map[1])
|
||||
if(cell.x < 1)
|
||||
cell.x = 1
|
||||
if(cell.y < 1)
|
||||
cell.y = 1
|
||||
|
||||
return cell
|
||||
}
|
||||
|
||||
function updateMap(m_pos) {
|
||||
#getproxobs(m_pos)
|
||||
UAVgetneiobs (m_pos)
|
||||
}
|
||||
|
||||
function UAVgetneiobs (m_curpos) {
|
||||
update_curcell(m_curpos,0)
|
||||
cur_cell = getcell(m_curpos,0)
|
||||
# add all neighbors as obstacles in the grid
|
||||
neighbors.foreach(function(rid, data) {
|
||||
add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0)
|
||||
|
|
@ -72,7 +81,7 @@ function UAVgetneiobs (m_curpos) {
|
|||
|
||||
function getneiobs (m_curpos) {
|
||||
var foundobstacle = 0
|
||||
update_curcell(m_curpos,1)
|
||||
cur_cell = getcell(m_curpos,1)
|
||||
var old_nei = table_copy(nei_cell)
|
||||
#log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3])
|
||||
|
||||
|
|
@ -118,7 +127,7 @@ function getneiobs (m_curpos) {
|
|||
|
||||
function getproxobs (m_curpos) {
|
||||
foundobstacle = 0
|
||||
update_curcell(m_curpos,1)
|
||||
cur_cell = getcell(m_curpos,1)
|
||||
|
||||
reduce(proximity,
|
||||
function(key, value, acc) {
|
||||
|
|
@ -159,19 +168,21 @@ function getproxobs (m_curpos) {
|
|||
}
|
||||
|
||||
function check_path(m_path, goal_l, m_curpos, kh4) {
|
||||
pathisblocked = 0
|
||||
nb=goal_l
|
||||
update_curcell(m_curpos,kh4)
|
||||
Cvec = cur_cell
|
||||
while(nb < size(m_path) and nb <= goal_l+1) {
|
||||
Nvec = getvec(m_path, nb)
|
||||
if(kh4==0)
|
||||
Nvec=vec_from_gps(Nvec.x,Nvec.y)
|
||||
if(doesItIntersect(Cvec, Nvec)){
|
||||
log("Obstacle in the way ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")")
|
||||
var pathisblocked = 0
|
||||
var nb = goal_l
|
||||
cur_cell = getcell(m_curpos,kh4)
|
||||
var Cvec = cur_cell
|
||||
while(nb <= size(m_path) and nb <= goal_l+1) {
|
||||
var Nvec = getvec(m_path, nb)
|
||||
if(kh4 == 0) {
|
||||
Nvec = vec_from_gps(Nvec.x,Nvec.y,1)
|
||||
Nvec = getcell(Nvec,kh4)
|
||||
}
|
||||
if(doesItIntersect(Cvec, Nvec)) {
|
||||
log("Path blocked ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")")
|
||||
pathisblocked = 1
|
||||
}
|
||||
Cvec=Nvec
|
||||
Cvec = Nvec
|
||||
nb = nb + 1
|
||||
}
|
||||
|
||||
|
|
@ -184,12 +195,16 @@ function RRTSTAR(GOAL,TOL) {
|
|||
RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive
|
||||
|
||||
|
||||
goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y}
|
||||
var goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y}
|
||||
#table_print(goalBoundary)
|
||||
numberOfPoints = 1
|
||||
arrayOfPoints = {}
|
||||
var numberOfPoints = 1
|
||||
var arrayOfPoints = {}
|
||||
arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y}
|
||||
Path = {}
|
||||
mat_copyrow(Path,1,arrayOfPoints,1)
|
||||
# RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance
|
||||
Q = {}
|
||||
Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0}
|
||||
|
||||
goalReached = 0;
|
||||
timeout = 0
|
||||
|
|
@ -202,7 +217,7 @@ function RRTSTAR(GOAL,TOL) {
|
|||
pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1))
|
||||
#log("Point generated: ", pt.x, " ", pt.y)
|
||||
|
||||
pointList = findPointsInRadius(pt,Q,RADIUS);
|
||||
var pointList = findPointsInRadius(pt,Q,RADIUS);
|
||||
|
||||
# Find connection that provides the least cost to come
|
||||
nbCount = 0;
|
||||
|
|
@ -210,7 +225,7 @@ function RRTSTAR(GOAL,TOL) {
|
|||
minCounter = 0;
|
||||
|
||||
if(size(pointList) != 0) {
|
||||
#log("Found ", size(pointList), " close point:", pointList.mat)
|
||||
log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3])
|
||||
ipt=1
|
||||
while(ipt <= size(pointList)) {
|
||||
pointNumber = {}
|
||||
|
|
@ -224,7 +239,7 @@ function RRTSTAR(GOAL,TOL) {
|
|||
nbCount = nbCount + 1;
|
||||
if(intersects != 1) {
|
||||
#log(pointNumber, "do not intersect (",pointNumber.mat[3],")")
|
||||
distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][3]][5]
|
||||
var distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][3]][5]
|
||||
|
||||
if(distance < minCounted) {
|
||||
minCounted = distance;
|
||||
|
|
@ -235,9 +250,11 @@ function RRTSTAR(GOAL,TOL) {
|
|||
}
|
||||
if(minCounter > 0) {
|
||||
numberOfPoints = numberOfPoints + 1;
|
||||
arrayOfPoints[numberOfPoints] = {}
|
||||
arrayOfPoints[numberOfPoints][1]=pt.x
|
||||
arrayOfPoints[numberOfPoints][2]=pt.y
|
||||
|
||||
Q[numberOfPoints] = {}
|
||||
Q[numberOfPoints][1] = pt.x
|
||||
Q[numberOfPoints][2] = pt.y
|
||||
Q[numberOfPoints][3] = pointList[minCounter][4]
|
||||
|
|
@ -249,9 +266,10 @@ function RRTSTAR(GOAL,TOL) {
|
|||
# Now check to see if any of the other points can be redirected
|
||||
nbCount = 0;
|
||||
ipt = 1
|
||||
while(ipt < size(pointList)) {
|
||||
while(ipt <= size(pointList)) {
|
||||
pointNumber = {}
|
||||
mat_copyrow(pointNumber,1,pointList,ipt)
|
||||
#log("pN ", size(pointNumber), ", ipt: ", ipt, "prec: ", pointNumber[1][4])
|
||||
|
||||
# Follow the line to see if it intersects anything
|
||||
intersects = doesItIntersect(pt,getvec(pointNumber,1));
|
||||
|
|
@ -262,6 +280,7 @@ function RRTSTAR(GOAL,TOL) {
|
|||
if(intersects != 1) {
|
||||
# If the alternative path is shorter than change it
|
||||
tmpdistance = Q[numberOfPoints][5]+math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))
|
||||
#log("Q: ", size(Q), "x", size(Q[1]))
|
||||
if(tmpdistance < Q[pointNumber[1][4]][5]) {
|
||||
Q[pointNumber[1][4]][3] = numberOfPoints
|
||||
Q[pointNumber[1][4]][5] = tmpdistance
|
||||
|
|
@ -285,9 +304,11 @@ function RRTSTAR(GOAL,TOL) {
|
|||
# If there is no intersection we need to add to the tree
|
||||
if(intersects != 1) {
|
||||
numberOfPoints = numberOfPoints + 1;
|
||||
arrayOfPoints[numberOfPoints] = {}
|
||||
arrayOfPoints[numberOfPoints][1]=pt.x
|
||||
arrayOfPoints[numberOfPoints][2]=pt.y
|
||||
|
||||
Q[numberOfPoints] = {}
|
||||
Q[numberOfPoints][1] = pt.x
|
||||
Q[numberOfPoints][2] = pt.y
|
||||
Q[numberOfPoints][3] = pointNum
|
||||
|
|
@ -320,51 +341,51 @@ function RRTSTAR(GOAL,TOL) {
|
|||
function findClosestPoint(point,aPt) {
|
||||
# Go through each points and find the distances between them and the
|
||||
# target point
|
||||
distance = 999
|
||||
pointNumber = -1
|
||||
ifcp=1
|
||||
var distance = 999
|
||||
var pointNb = -1
|
||||
var ifcp=1
|
||||
while(ifcp <= size(aPt)) {
|
||||
range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp)))
|
||||
var range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp)))
|
||||
|
||||
if(range < distance) {
|
||||
distance = range;
|
||||
pointNumber = ifcp;
|
||||
pointNb = ifcp;
|
||||
}
|
||||
ifcp = ifcp + 1
|
||||
}
|
||||
return pointNumber
|
||||
return pointNb
|
||||
}
|
||||
|
||||
# Find the closest point in the tree
|
||||
function findPointsInRadius(point,q,r) {
|
||||
pointList = {}
|
||||
var ptList = {}
|
||||
var counted = 0;
|
||||
var iir = 1
|
||||
while(iir <= size(q)) {
|
||||
tmpvv = getvec(q,iir)
|
||||
var tmpvv = getvec(q,iir)
|
||||
#log("FPiR :", point.x, " ", point.y," ", tmpvv.x," ", tmpvv.y)
|
||||
distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point))
|
||||
var distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point))
|
||||
|
||||
if(distance < r) {
|
||||
counted = counted+1;
|
||||
mat_copyrow(pointList,counted,q,iir)
|
||||
mat_copyrow(ptList,counted,q,iir)
|
||||
}
|
||||
|
||||
iir = iir + 1
|
||||
}
|
||||
return pointList
|
||||
return ptList
|
||||
}
|
||||
|
||||
# check if the line between 2 point intersect an obstacle
|
||||
function doesItIntersect(point,vector) {
|
||||
#log("DiI :", point.x, " ", point.y," ", vector.x," ", vector.y)
|
||||
dif = math.vec2.sub(point,vector)
|
||||
distance = math.vec2.length(dif)
|
||||
if(distance==0.0){
|
||||
var dif = math.vec2.sub(point,vector)
|
||||
var distance = math.vec2.length(dif)
|
||||
if(distance == 0.0){
|
||||
# Find what block we're in right now
|
||||
var xi = math.round(point.x) #+1
|
||||
var yi = math.round(point.y) #+1
|
||||
if(xi!=cur_cell.x and yi!=cur_cell.y){
|
||||
if(xi >= size(map) and yi >= size(map[1]) and xi != cur_cell.x and yi != cur_cell.y) {
|
||||
if(map[xi][yi] > 0.5)
|
||||
return 1
|
||||
else
|
||||
|
|
@ -372,21 +393,22 @@ function doesItIntersect(point,vector) {
|
|||
} else
|
||||
return 0
|
||||
}
|
||||
vec = math.vec2.scale(dif,1.0/distance)
|
||||
pathstep = size(map[1]) - 2
|
||||
var vec = math.vec2.scale(dif,1.0/distance)
|
||||
var pathstep = size(map) - 2
|
||||
|
||||
idii = 1.0
|
||||
while(idii <= pathstep) {
|
||||
range = distance*idii/pathstep
|
||||
pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range));
|
||||
var range = distance*idii/pathstep
|
||||
var pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range));
|
||||
|
||||
# Find what block we're in right now
|
||||
var xi = math.round(pos_chk.x) #+1
|
||||
var yi = math.round(pos_chk.y) #+1
|
||||
#log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range)
|
||||
#log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range, "(Map size: ", size(map), ")")
|
||||
|
||||
if(xi!=cur_cell.x and yi!=cur_cell.y){
|
||||
if(xi <= size(map[1]) and yi <= size(map) and xi > 0 and yi > 0) {
|
||||
if(is_in(get_occupied_cells(cur_cell), xi, yi) == 0){
|
||||
#if(xi!=cur_cell.x and yi!=cur_cell.y) {
|
||||
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
|
||||
if(map[xi][yi] < 0.5) { # because obstacle use trust values
|
||||
#log("Obstacle in the way(", xi, " ", yi, ": ", map[xi][yi], ")!")
|
||||
return 1;
|
||||
|
|
@ -403,20 +425,28 @@ function doesItIntersect(point,vector) {
|
|||
}
|
||||
|
||||
function getPathGPS(Q,nb){
|
||||
#log("-----------CONVERTING PATH!!!")
|
||||
path={}
|
||||
var npt=0
|
||||
var isrecursive = 0
|
||||
# get the path from the point list
|
||||
while(nb!=1){
|
||||
while(nb > 1 and isrecursive!=1){
|
||||
npt=npt+1
|
||||
path[npt] = {}
|
||||
path[npt][1]=Q[nb][1]
|
||||
path[npt][2]=Q[nb][2]
|
||||
nb = Q[nb][3];
|
||||
if(nb != Q[Q[nb][3]][3])
|
||||
nb = Q[nb][3];
|
||||
else {
|
||||
isrecursive = 1
|
||||
path={}
|
||||
log("ERROR - Recursive path !!!")
|
||||
}
|
||||
}
|
||||
|
||||
# re-order the list and make the path points absolute
|
||||
pathR={}
|
||||
var totpt = npt
|
||||
var totpt = npt + 1
|
||||
while(npt > 0){
|
||||
tmpgoal = gps_from_vec(math.vec2.sub(getvec(path,npt),cur_cell))
|
||||
pathR[totpt-npt] = {}
|
||||
|
|
@ -433,7 +463,7 @@ function getPath(Q,nb){
|
|||
var npt=0
|
||||
|
||||
# log("get the path from the point list")
|
||||
while(nb!=1){
|
||||
while(nb > 1){
|
||||
npt=npt+1
|
||||
path[npt] = {}
|
||||
path[npt][1]=Q[nb][1]
|
||||
|
|
@ -444,7 +474,7 @@ function getPath(Q,nb){
|
|||
# log("re-order the list")
|
||||
# table_print(path.mat)
|
||||
pathR={}
|
||||
var totpt = npt
|
||||
var totpt = npt + 1
|
||||
while(npt > 0){
|
||||
tmpgoal = getvec(path,npt)
|
||||
pathR[totpt-npt] = {}
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@
|
|||
# FLIGHT-RELATED FUNCTIONS
|
||||
#
|
||||
########################################
|
||||
include "vec2.bzz"
|
||||
include "rrtstar.bzz"
|
||||
|
||||
TARGET_ALTITUDE = 15.0 # m.
|
||||
|
|
@ -14,7 +15,6 @@ GOTODIST_TOL = 0.5 # m.
|
|||
GOTOANG_TOL = 0.1 # rad.
|
||||
cur_goal_l = 0
|
||||
rc_State = 0
|
||||
homegps = {.lat=0, .long=0}
|
||||
|
||||
function uav_initswarm() {
|
||||
s = swarm.create(1)
|
||||
|
|
@ -34,6 +34,7 @@ function idle() {
|
|||
function takeoff() {
|
||||
UAVSTATE = "TAKEOFF"
|
||||
statef=takeoff
|
||||
homegps = {.lat=position.latitude, .long=position.longitude}
|
||||
|
||||
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
||||
barrier_set(ROBOTS, action, land, -1)
|
||||
|
|
@ -61,7 +62,7 @@ function land() {
|
|||
function set_goto(transf) {
|
||||
UAVSTATE = "GOTOGPS"
|
||||
statef=function() {
|
||||
gotoWP(transf)
|
||||
gotoWPRRT(transf)
|
||||
}
|
||||
|
||||
if(rc_goto.id==id){
|
||||
|
|
@ -93,10 +94,8 @@ function picture() {
|
|||
# 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)
|
||||
homegps.lat = position.latitude
|
||||
homegps.long = position.longitude
|
||||
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1)
|
||||
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)
|
||||
|
|
@ -104,35 +103,36 @@ function gotoWPRRT(transf) {
|
|||
else {
|
||||
if(Path==nil){
|
||||
# create the map
|
||||
if(map==nil)
|
||||
if(map==nil) {
|
||||
UAVinit_map(rc_goal)
|
||||
homegps.lat = position.latitude
|
||||
homegps.long = position.longitude
|
||||
}
|
||||
# add proximity sensor and neighbor obstacles to the map
|
||||
while(Path==nil) {
|
||||
#getproxobs(m_pos)
|
||||
UAVgetneiobs (m_pos)
|
||||
updateMap(m_pos)
|
||||
Path = UAVpathPlanner(rc_goal, m_pos)
|
||||
}
|
||||
cur_goal_l = 1
|
||||
} else if(cur_goal_l <= size(Path)) {
|
||||
cur_gps=getvec(Path,cur_goal_l) #x=latitude, y=longitude
|
||||
cur_pt=vec_from_gps(cur_gps.x,cur_gps.y)
|
||||
print(" heading to ", cur_pt.x,cur_pt.y)
|
||||
if(math.vec2.length(cur_pt)>GOTODIST_TOL) {
|
||||
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1)
|
||||
UAVgetneiobs(m_pos)
|
||||
if(check_path(Path,cur_goal_l,m_pos,0)) {
|
||||
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) {
|
||||
updateMap(m_pos)
|
||||
if(check_path(Path, cur_goal_l, m_pos, 0)) {
|
||||
uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude)
|
||||
Path = nil
|
||||
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
|
||||
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
|
||||
while(Path == nil) {
|
||||
#getproxobs(m_pos)
|
||||
UAVgetneiobs (m_pos)
|
||||
updateMap(m_pos)
|
||||
Path = UAVpathPlanner(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-position.altitude)
|
||||
}
|
||||
cur_pt = math.vec2.scale(cur_pt, GOTO_MAXVEL/math.vec2.length(cur_pt))
|
||||
uav_moveto(cur_pt.x, cur_pt.y, rc_goto.altitude-position.altitude)
|
||||
}
|
||||
else
|
||||
cur_goal_l = cur_goal_l + 1
|
||||
|
|
@ -262,14 +262,18 @@ function LimitAngle(angle){
|
|||
return angle
|
||||
}
|
||||
|
||||
function vec_from_gps(lat, lon) {
|
||||
d_lon = lon - position.longitude
|
||||
function vec_from_gps(lat, lon, home_ref) {
|
||||
d_lon = lon - position.longitude
|
||||
d_lat = lat - 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)
|
||||
return math.vec2.new(ned_x,ned_y)
|
||||
}
|
||||
|
||||
function gps_from_vec(vec) {
|
||||
|
|
|
|||
|
|
@ -1,4 +1,3 @@
|
|||
include "vec2.bzz"
|
||||
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.
|
||||
|
|
@ -6,15 +5,18 @@ include "vstigenv.bzz"
|
|||
|
||||
function action() {
|
||||
statef=action
|
||||
uav_storegoal(45.5085,-73.1531935979886,5.0)
|
||||
set_goto(picture)
|
||||
uav_storegoal(45.5088103899,-73.1540826153,25.0)
|
||||
set_goto(idle)
|
||||
}
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
statef=turnedoff
|
||||
UAVSTATE = "TURNEDOFF"
|
||||
uav_initstig()
|
||||
uav_initswarm()
|
||||
#statef=turnedoff
|
||||
#UAVSTATE = "TURNEDOFF"
|
||||
statef = takeoff
|
||||
UAVSTATE = "TAKEOFF"
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
|
|
@ -2,7 +2,6 @@ include "vec2.bzz"
|
|||
include "update.bzz"
|
||||
include "barrier.bzz" # don't use a stigmergy id=80 (auto-increment up) with this header.
|
||||
include "uavstates.bzz" # require an 'action' function to be defined here.
|
||||
include "vstigenv.bzz"
|
||||
|
||||
function action() {
|
||||
uav_storegoal(-1.0,-1.0,-1.0)
|
||||
|
|
@ -13,7 +12,6 @@ function action() {
|
|||
function init() {
|
||||
statef=turnedoff
|
||||
UAVSTATE = "TURNEDOFF"
|
||||
uav_initstig()
|
||||
TARGET_ALTITUDE = 15.0
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue