removed floor, add PY rosprint, hard-coded swarm size, add states bzzcmd
This commit is contained in:
dave
parent
b119737740
commit
98c0dbc62f
|
|
@ -8,9 +8,11 @@ include "update.bzz"
|
|||
#include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header.
|
||||
include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header.
|
||||
include "uavstates.bzz" # require an 'action' function to be defined here.
|
||||
include "rrtstar.bzz"
|
||||
|
||||
include "graphs/shapes_P.bzz"
|
||||
include "graphs/shapes_O.bzz"
|
||||
include "graphs/shapes_L.bzz"
|
||||
include "graphs/shapes_Y.bzz"
|
||||
|
||||
ROBOT_RADIUS = 50
|
||||
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
|
||||
|
|
@ -680,7 +682,8 @@ function DoJoined(){
|
|||
#if(v_tag.size()==ROBOTS){
|
||||
# TransitionToLock()
|
||||
#}
|
||||
barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1)
|
||||
#barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1)
|
||||
barrier_wait(6, TransitionToLock, DoJoined, -1)
|
||||
}
|
||||
|
||||
#
|
||||
|
|
@ -701,6 +704,9 @@ function DoLock(){
|
|||
}
|
||||
#move
|
||||
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
|
||||
|
||||
# prepare to restart a new shape
|
||||
statef=action
|
||||
}
|
||||
|
||||
function action(){
|
||||
|
|
@ -745,7 +751,7 @@ function step() {
|
|||
# get the swarm commands
|
||||
uav_neicmd()
|
||||
# update the vstig (status/net/batt)
|
||||
#uav_updatestig()
|
||||
#uav_updatestig()
|
||||
|
||||
#update the graph
|
||||
UpdateNodeInfo()
|
||||
|
|
@ -797,8 +803,16 @@ function step() {
|
|||
# Executed when reset
|
||||
#
|
||||
function Reset(){
|
||||
Read_Graph()
|
||||
m_nLabel=-1
|
||||
if(rc_State==0){
|
||||
Read_GraphP()
|
||||
} else if(rc_State==1) {
|
||||
Read_GraphO()
|
||||
} else if(rc_State==2) {
|
||||
Read_GraphL()
|
||||
} else if(rc_State==3) {
|
||||
Read_GraphY()
|
||||
}
|
||||
m_nLabel=-1
|
||||
|
||||
#start listening
|
||||
start_listen()
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#Table of the nodes in the graph
|
||||
m_vecNodes={}
|
||||
|
||||
function Read_Graph(){
|
||||
function Read_GraphL(){
|
||||
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
|
||||
.Label = 0, # Label of the point
|
||||
.Pred = -1, # Label of its predecessor
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#Table of the nodes in the graph
|
||||
m_vecNodes={}
|
||||
m_vecNodes_fixed={}
|
||||
function Read_Graph(){
|
||||
function Read_GraphO(){
|
||||
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
|
||||
.Label = 0, # Label of the point
|
||||
.Pred = -1, # Label of its predecessor
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#Table of the nodes in the graph
|
||||
m_vecNodes={}
|
||||
m_vecNodes_fixed={}
|
||||
function Read_Graph(){
|
||||
function Read_GraphP(){
|
||||
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
|
||||
.Label = 0, # Label of the point
|
||||
.Pred = -1, # Label of its predecessor
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#Table of the nodes in the graph
|
||||
m_vecNodes={}
|
||||
m_vecNodes_fixed={}
|
||||
function Read_Graph(){
|
||||
function Read_GraphY(){
|
||||
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
|
||||
.Label = 0, # Label of the point
|
||||
.Pred = -1, # Label of its predecessor
|
||||
|
|
|
|||
|
|
@ -1,5 +1,7 @@
|
|||
|
||||
# Write to matrix
|
||||
robot_marker = "X"
|
||||
|
||||
function wmat(mat, row, col, val) {
|
||||
var index = (row-1)*mat.nb_col + (col - 1)
|
||||
if( row <= mat.nb_row) { # update val
|
||||
|
|
@ -13,8 +15,9 @@ function wmat(mat, row, col, val) {
|
|||
# Read from matrix
|
||||
function rmat(mat, row, col) {
|
||||
#log("rmat ", mat, row, col)
|
||||
var index = (row-1)*mat.nb_col + (col - 1)
|
||||
index = (row-1)*mat.nb_col + (col - 1)
|
||||
if (mat.mat[index] == nil) {
|
||||
log("Wrong matrix read index: ", row, " ", col)
|
||||
return -1
|
||||
} else {
|
||||
return mat.mat[index]
|
||||
|
|
@ -36,46 +39,56 @@ function getvec(t,row){
|
|||
return math.vec2.new(rmat(t,row,1),rmat(t,row,2))
|
||||
}
|
||||
|
||||
function init_test_map(){
|
||||
# creates a 10x10 map
|
||||
map = {.nb_col=10, .nb_row=10, .mat={}}
|
||||
cur_cell = math.vec2.new(1,1)
|
||||
function init_test_map(len){
|
||||
map = {.nb_col=len, .nb_row=len, .mat={}}
|
||||
index = 0
|
||||
while(index<10*10){
|
||||
map.mat[index]=1
|
||||
while(index<len*len){
|
||||
map.mat[index]=1.0
|
||||
index = index + 1
|
||||
}
|
||||
# puts an obstacle right in the middle
|
||||
wmat(map,5,5,0)
|
||||
wmat(map,6,5,0)
|
||||
wmat(map,4,5,0)
|
||||
wmat(map,5,5,0.0)
|
||||
wmat(map,6,5,0.0)
|
||||
wmat(map,4,5,0.0)
|
||||
|
||||
log(rmat(map,1,1),rmat(map,3,3))
|
||||
log("Occupancy grid initialized (",len,"x",len,") with obstacles.")
|
||||
}
|
||||
|
||||
function init_map(len){
|
||||
map = {.nb_col=len, .nb_row=len, .mat={}}
|
||||
# center the robot on the grid
|
||||
cur_cell = math.vec2.new(floor(len/2.0),floor(len/2.0))
|
||||
index = 0
|
||||
while(index<len*len){
|
||||
map.mat[index]=1
|
||||
map.mat[index]=1.0
|
||||
index = index + 1
|
||||
}
|
||||
log("Occupancy grid initialized (",len,"x",len,").")
|
||||
}
|
||||
|
||||
function add_obstacle(pos) {
|
||||
xi = floor(pos.x)+cur_cell.x
|
||||
yi = floor(pos.y)+cur_cell.y
|
||||
log("Obstacle in cell: ", xi, yi)
|
||||
|
||||
function add_obstacle(pos, off, inc_trust) {
|
||||
xi = math.round(pos.x)
|
||||
yi = math.round(pos.y)
|
||||
|
||||
if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) {
|
||||
#log("Add obstacle in cell: ", xi, " ", yi)
|
||||
old=rmat(map,xi,yi)
|
||||
if(old-inc_trust > 0.0)
|
||||
wmat(map,xi,yi,old-inc_trust)
|
||||
else
|
||||
wmat(map,xi,yi,0.0)
|
||||
}
|
||||
}
|
||||
|
||||
function remove_obstacle(pos, off, dec_trust) {
|
||||
xi = math.round(pos.x)
|
||||
yi = math.round(pos.y)
|
||||
|
||||
if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0){
|
||||
wmat(map,xi,yi,0)
|
||||
wmat(map,xi+1,yi,0)
|
||||
wmat(map,xi-1,yi,0)
|
||||
wmat(map,xi,yi+1,0)
|
||||
wmat(map,xi,yi-1,0)
|
||||
#log("Remove obstacle in cell: ", xi, " ", yi)
|
||||
old=rmat(map, xi, yi)
|
||||
if(old + dec_trust < 1.0) #x,y
|
||||
wmat(map, xi, yi, old+dec_trust)
|
||||
else
|
||||
wmat(map, xi, yi, 1.0)
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -84,10 +97,55 @@ function table_print(t) {
|
|||
log(key, " -> ", value)
|
||||
})
|
||||
}
|
||||
|
||||
function table_copy(t) {
|
||||
var t2 = {}
|
||||
foreach(t, function(key, value) {
|
||||
t2[key] = value
|
||||
})
|
||||
return t2
|
||||
}
|
||||
|
||||
function print_pos(t) {
|
||||
ir=1
|
||||
while(ir<=t.nb_row){
|
||||
log(ir, ": ", rmat(t,ir,1), rmat(t,ir,2))
|
||||
log(ir, ": ", rmat(t,ir,1), " ", rmat(t,ir,2))
|
||||
ir = ir + 1
|
||||
}
|
||||
}
|
||||
}
|
||||
function print_map(t) {
|
||||
ir=t.nb_row
|
||||
log("Printing a ", t.nb_row, " by ", t.nb_col, " map")
|
||||
while(ir>0){
|
||||
logst=string.concat("\t", string.tostring(ir), "\t:")
|
||||
ic=t.nb_col
|
||||
while(ic>0){
|
||||
if(ir==cur_cell.x and ic==cur_cell.y)
|
||||
logst = string.concat(logst, " XXXXXXXX")
|
||||
else
|
||||
logst = string.concat(logst, " ", string.tostring(rmat(t,ir,ic)))
|
||||
ic = ic - 1
|
||||
}
|
||||
log(logst)
|
||||
ir = ir - 1
|
||||
}
|
||||
}
|
||||
|
||||
function print_map_argos(t){
|
||||
ir=t.nb_row
|
||||
msg = string.tostring(ir)
|
||||
while(ir>0){
|
||||
ic=t.nb_col
|
||||
while(ic>0){
|
||||
if(ir==cur_cell.x and ic==cur_cell.y){
|
||||
msg = string.concat(msg, ":", robot_marker)
|
||||
}
|
||||
else {
|
||||
msg = string.concat(msg, ":", string.tostring(rmat(t,ir,ic)))
|
||||
}
|
||||
ic = ic - 1
|
||||
}
|
||||
ir = ir - 1
|
||||
}
|
||||
set_argos_map(msg)
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,45 +5,187 @@
|
|||
#####
|
||||
include "mapmatrix.bzz"
|
||||
|
||||
map = {}
|
||||
RRT_TIMEOUT = 200
|
||||
map = nil
|
||||
cur_cell = {}
|
||||
nei_cell = {}
|
||||
|
||||
function pathPlanner(m_navigation) {
|
||||
function UAVpathPlanner(m_navigation) {
|
||||
# create a map big enough for the goal
|
||||
init_map(2*floor(math.vec2.length(m_navigation))+2)
|
||||
init_map(2*math.round(math.vec2.length(m_navigation))+2)
|
||||
# center the robot on the grid
|
||||
cur_cell = math.vec2.new(math.round(len/2.0),math.round(len/2.0))
|
||||
# create the goal in the map grid
|
||||
mapgoal = math.vec2.add(m_navigation,cur_cell)
|
||||
# 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))
|
||||
add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0)
|
||||
})
|
||||
# TODO: add proximity sensor obstacles to the grid
|
||||
# search for a path
|
||||
return RRTSTAR(mapgoal,math.vec2.new(map.nb_col/20.0,map.nb_row/20.0))
|
||||
return RRTSTAR(mapgoal,math.vec2.new(5,5)) #map.nb_col/20.0,map.nb_row/20.0))
|
||||
}
|
||||
|
||||
function Kh4pathPlanner(m_goal, m_pos) {
|
||||
# move 0,0 to a corner instead of the center
|
||||
m_goal = math.vec2.sub(m_goal,math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
|
||||
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)
|
||||
log("Starting from cell: ", cur_cell.x, " ", cur_cell.y)
|
||||
log("Going to cell: ", m_goal.x, " ", m_goal.y)
|
||||
|
||||
# search for a path
|
||||
print_map(map)
|
||||
# print_map_argos(map)
|
||||
return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y))
|
||||
}
|
||||
|
||||
function update_curcell(m_curpos) {
|
||||
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))
|
||||
if(cur_cell.x>map.nb_row)
|
||||
cur_cell.x=map.nb_row
|
||||
if(cur_cell.y>map.nb_col)
|
||||
cur_cell.y=map.nb_col
|
||||
if(cur_cell.x<1)
|
||||
cur_cell.x=1
|
||||
if(cur_cell.y<1)
|
||||
cur_cell.y=1
|
||||
}
|
||||
|
||||
function getneiobs (m_curpos) {
|
||||
foundobstacle = 0
|
||||
update_curcell(m_curpos)
|
||||
old_nei = table_copy(nei_cell)
|
||||
#log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3])
|
||||
|
||||
neighbors.foreach(function(rid, data) {
|
||||
#log("nei rel pos: ", data.distance, "m ", data.azimuth*180.0/math.pi, "deg")
|
||||
Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos)
|
||||
#log("nei abs pos: ", Ncell.x, "cm ", Ncell.y, "cm")
|
||||
Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
|
||||
Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y))
|
||||
nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y}
|
||||
#log("Neighbor in : ", Ncell.x, " ", Ncell.y)
|
||||
if(old_nei[rid]!=nil) {
|
||||
if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) {
|
||||
#log("Neighbor moved ! ", nei_cell[rid].x, " ", nei_cell[rid].y)
|
||||
remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1)
|
||||
remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1)
|
||||
remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1)
|
||||
remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1)
|
||||
remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1)
|
||||
add_obstacle(Ncell, 0, 1.0)
|
||||
add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
|
||||
add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
|
||||
add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
|
||||
add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
|
||||
foundobstacle = 1
|
||||
}
|
||||
} else {
|
||||
add_obstacle(Ncell, 0, 1.0)
|
||||
add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
|
||||
add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
|
||||
add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
|
||||
add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
|
||||
foundobstacle = 1
|
||||
}
|
||||
})
|
||||
|
||||
#if(foundobstacle) {
|
||||
#print_map(map)
|
||||
#}
|
||||
|
||||
return foundobstacle
|
||||
}
|
||||
|
||||
function getproxobs (m_curpos) {
|
||||
foundobstacle = 0
|
||||
update_curcell(m_curpos)
|
||||
|
||||
reduce(proximity,
|
||||
function(key, value, acc) {
|
||||
obs = math.vec2.add(math.vec2.newp(1.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell)
|
||||
obs2 = math.vec2.add(math.vec2.newp(2.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell)
|
||||
per = math.vec2.sub(obs,cur_cell)
|
||||
obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs)
|
||||
obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2)
|
||||
obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs)
|
||||
obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2)
|
||||
if(value.value > IR_SENSOR_THRESHOLD) {
|
||||
if(rmat(map,math.round(obs.x),math.round(obs.y))!=0) {
|
||||
add_obstacle(obs, 0, 0.25)
|
||||
add_obstacle(obs2, 0, 0.25)
|
||||
add_obstacle(obsr, 0, 0.25)
|
||||
add_obstacle(obsr2, 0, 0.25)
|
||||
add_obstacle(obsl, 0, 0.25)
|
||||
add_obstacle(obsl2, 0, 0.25)
|
||||
foundobstacle = 1
|
||||
}
|
||||
} else if(rmat(map,math.round(obs.x),math.round(obs.y))!=1) {
|
||||
remove_obstacle(obs, 0, 0.05)
|
||||
foundobstacle = 1
|
||||
}
|
||||
return acc
|
||||
}, math.vec2.new(0, 0))
|
||||
|
||||
#if(foundobstacle) {
|
||||
# reduce(proximity,
|
||||
# function(key, value, acc){
|
||||
# log(value.value, ", ", value.angle)
|
||||
# return acc
|
||||
# }, math.vec2.new(0, 0))
|
||||
# print_map(map)
|
||||
#}
|
||||
|
||||
return foundobstacle
|
||||
}
|
||||
|
||||
function check_path(m_path, goal_l, m_curpos) {
|
||||
pathisblocked = 0
|
||||
nb=goal_l
|
||||
update_curcell(m_curpos)
|
||||
#m_curpos = math.vec2.sub(m_curpos,math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
|
||||
#Cvec = math.vec2.new(math.round(m_curpos.x*CM2KH4.x), math.round(m_curpos.y*CM2KH4.y))
|
||||
Cvec = cur_cell
|
||||
while(nb < m_path.nb_row and nb <= goal_l+1){
|
||||
Nvec = getvec(m_path, nb)
|
||||
if(doesItIntersect(Cvec, Nvec)){
|
||||
log("Obstacle in the way ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")")
|
||||
pathisblocked = 1
|
||||
}
|
||||
Cvec=Nvec
|
||||
nb = nb + 1
|
||||
}
|
||||
|
||||
return pathisblocked
|
||||
}
|
||||
|
||||
function RRTSTAR(GOAL,TOL) {
|
||||
HEIGHT = map.nb_col
|
||||
WIDTH = map.nb_row
|
||||
RADIUS = map.nb_col/10.0 # to consider 2 points consecutive
|
||||
|
||||
rng.setseed(11)
|
||||
HEIGHT = map.nb_col-1
|
||||
WIDTH = map.nb_row-1
|
||||
RADIUS = 1.25 #TOL.x #map.nb_col/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}
|
||||
table_print(goalBoundary)
|
||||
#table_print(goalBoundary)
|
||||
arrayOfPoints = {.nb_col=2, .nb_row=1, .mat={.0=cur_cell.x,.1=cur_cell.y}}
|
||||
Path = {.nb_col=2, .nb_row=1, .mat={.0=cur_cell.x,.1=cur_cell.y}}
|
||||
numberOfPoints = 1
|
||||
Q = {.nb_col=5,.nb_row=1,.mat={.0=cur_cell.x,.1=cur_cell.y,.2=0,.3=1,.4=0}}
|
||||
|
||||
|
||||
goalReached = 0;
|
||||
timeout = 0
|
||||
##
|
||||
# main search loop
|
||||
##
|
||||
while(goalReached == 0 and timeout < 200) {
|
||||
# Point generation
|
||||
pt = math.vec2.new(HEIGHT*rng.uniform(1.0)+1,WIDTH*rng.uniform(1.0)+1)
|
||||
while(goalReached == 0 and timeout < RRT_TIMEOUT) {
|
||||
|
||||
# Point generation only as grid cell centers
|
||||
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);
|
||||
|
||||
|
|
@ -51,14 +193,13 @@ function RRTSTAR(GOAL,TOL) {
|
|||
nbCount = 0;
|
||||
minCounted = 999;
|
||||
minCounter = 0;
|
||||
|
||||
|
||||
if(pointList.nb_row!=0) {
|
||||
#log("Found ", pointList.nb_row, " close point:", pointList.mat)
|
||||
#log("Found ", pointList.nb_row, " close point:", pointList.mat)
|
||||
ipt=1
|
||||
while(ipt<=pointList.nb_row){
|
||||
pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}}
|
||||
mat_copyrow(pointNumber,1,pointList,ipt)
|
||||
#log("pointnumber: ", pointNumber.mat)
|
||||
|
||||
# Follow the line to see if it intersects anything
|
||||
intersects = doesItIntersect(pt,getvec(pointNumber,1));
|
||||
|
|
@ -88,7 +229,7 @@ function RRTSTAR(GOAL,TOL) {
|
|||
wmat(Q,numberOfPoints,4, numberOfPoints)
|
||||
wmat(Q,numberOfPoints,5, minCounted)
|
||||
|
||||
log("added point to Q(", Q.nb_row, "): ", pt.x, pt.y)
|
||||
#log("added point to Q(", Q.nb_row, "): ", pt.x, " ", pt.y)
|
||||
|
||||
# Now check to see if any of the other points can be redirected
|
||||
nbCount = 0;
|
||||
|
|
@ -138,7 +279,7 @@ function RRTSTAR(GOAL,TOL) {
|
|||
wmat(Q,numberOfPoints,4, numberOfPoints)
|
||||
wmat(Q,numberOfPoints,5, rmat(Q,pointNum,5)+math.vec2.length(math.vec2.sub(getvec(Q,pointNum),pt)))
|
||||
|
||||
log("added point to Q(", Q.nb_row, "): ", pt.x, pt.y)
|
||||
#log("added point to Q(", Q.nb_row, "): ", pt.x, " ", pt.y)
|
||||
|
||||
# Check to see if this new point is within the goal
|
||||
if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax)
|
||||
|
|
@ -154,6 +295,9 @@ function RRTSTAR(GOAL,TOL) {
|
|||
log("Goal found(",numberOfPoints,")!")
|
||||
Path = getPath(Q,numberOfPoints)
|
||||
print_pos(Path)
|
||||
} else {
|
||||
log("FAILED TO FIND A PATH!!!")
|
||||
Path = nil
|
||||
}
|
||||
return Path
|
||||
}
|
||||
|
|
@ -166,7 +310,7 @@ function findClosestPoint(point,aPt) {
|
|||
ifcp=1
|
||||
while(ifcp<=aPt.nb_row) {
|
||||
range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp)))
|
||||
|
||||
|
||||
if(range < distance) {
|
||||
distance = range;
|
||||
pointNumber = ifcp;
|
||||
|
|
@ -176,13 +320,16 @@ function findClosestPoint(point,aPt) {
|
|||
return pointNumber
|
||||
}
|
||||
|
||||
# Find the closest point in the tree
|
||||
function findPointsInRadius(point,q,r) {
|
||||
counted = 0;
|
||||
pointList = {.nb_col=q.nb_col, .nb_row=counted, .mat={}}
|
||||
iir=1
|
||||
while(iir <= q.nb_row) {
|
||||
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))
|
||||
|
||||
|
||||
if(distance < r) {
|
||||
counted = counted+1;
|
||||
pointList.nb_row=counted
|
||||
|
|
@ -194,37 +341,54 @@ function findPointsInRadius(point,q,r) {
|
|||
return pointList
|
||||
}
|
||||
|
||||
# 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)
|
||||
vec = math.vec2.scale(dif,1/distance)
|
||||
|
||||
idii = 1
|
||||
while(idii<=100) {
|
||||
range = distance/100.0*idii
|
||||
pos_chk = math.vec2.sub(pt,math.vec2.scale(vec,range));
|
||||
|
||||
if(distance==0.0){
|
||||
# Find what block we're in right now
|
||||
xi = floor(pos_chk.x)+1
|
||||
yi = floor(pos_chk.y)+1
|
||||
#log("Grid :", pos_chk.x, pos_chk.y, xi, yi)
|
||||
|
||||
if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) {
|
||||
if(rmat(map,xi,yi) == 0) {
|
||||
#log("Obstacle (", rmat(map,xi,yi), ")!")
|
||||
return 1;
|
||||
}
|
||||
} else {
|
||||
#log("Outside the map(", x, y, ")!")
|
||||
return 1;
|
||||
xi = math.round(point.x) #+1
|
||||
yi = math.round(point.y) #+1
|
||||
if(xi!=cur_cell.x and yi!=cur_cell.y){
|
||||
if(rmat(map,xi,yi) > 0.5)
|
||||
return 1
|
||||
else
|
||||
return 0
|
||||
} else
|
||||
return 0
|
||||
}
|
||||
vec = math.vec2.scale(dif,1.0/distance)
|
||||
pathstep = map.nb_col - 2
|
||||
|
||||
idii = 1.0
|
||||
while(idii <= pathstep) {
|
||||
range = distance*idii/pathstep
|
||||
pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range));
|
||||
|
||||
# Find what block we're in right now
|
||||
xi = math.round(pos_chk.x) #+1
|
||||
yi = math.round(pos_chk.y) #+1
|
||||
#log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range)
|
||||
|
||||
if(xi!=cur_cell.x and yi!=cur_cell.y){
|
||||
if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) {
|
||||
if(rmat(map,xi,yi) < 0.5) { # because obstacle use trust values
|
||||
#log("Obstacle in the way(", xi, " ", yi, ": ", rmat(map,xi,yi), ")!")
|
||||
return 1;
|
||||
}
|
||||
} else {
|
||||
#log("Outside the map(", xi, " ", yi, ")! ", range, "/", distance, " : ", pos_chk.x, " ", pos_chk.y, " : ", vec.x, " ", vec.y)
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
idii = idii + 1
|
||||
idii = idii + 1.0
|
||||
}
|
||||
#log("No intersect!")
|
||||
return 0
|
||||
}
|
||||
|
||||
function getPath(Q,nb){
|
||||
function getPathGPS(Q,nb){
|
||||
path={.nb_col=2, .nb_row=0, .mat={}}
|
||||
npt=0
|
||||
# get the path from the point list
|
||||
|
|
@ -245,4 +409,31 @@ function getPath(Q,nb){
|
|||
npt = npt - 1
|
||||
}
|
||||
return pathR
|
||||
}
|
||||
}
|
||||
|
||||
# create a table with only the path's points in order
|
||||
function getPath(Q,nb){
|
||||
path={.nb_col=2, .nb_row=0, .mat={}}
|
||||
npt=0
|
||||
|
||||
# log("get the path from the point list")
|
||||
while(nb!=1){
|
||||
npt=npt+1
|
||||
path.nb_row=npt
|
||||
path.mat[(npt-1)*2]=rmat(Q,nb,1)
|
||||
path.mat[(npt-1)*2+1]=rmat(Q,nb,2)
|
||||
nb = rmat(Q,nb,3);
|
||||
}
|
||||
|
||||
# log("re-order the list")
|
||||
# table_print(path.mat)
|
||||
pathR={.nb_col=2, .nb_row=path.nb_row, .mat={}}
|
||||
while(npt>0){
|
||||
tmpgoal = getvec(path,npt)
|
||||
pathR.mat[(path.nb_row-npt)*2]=tmpgoal.x
|
||||
pathR.mat[(path.nb_row-npt)*2+1]=tmpgoal.y
|
||||
npt = npt - 1
|
||||
}
|
||||
log("Double-check path: ", check_path(pathR, 1, cur_cell))
|
||||
return pathR
|
||||
}
|
||||
|
|
|
|||
|
|
@ -3,6 +3,8 @@
|
|||
# FLIGHT-RELATED FUNCTIONS
|
||||
#
|
||||
########################################
|
||||
include "rrtstar.bzz"
|
||||
|
||||
TARGET_ALTITUDE = 5.0 # m.
|
||||
UAVSTATE = "TURNEDOFF"
|
||||
PICTURE_WAIT = 20 # steps
|
||||
|
|
@ -11,6 +13,7 @@ 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)
|
||||
|
|
@ -32,7 +35,8 @@ function takeoff() {
|
|||
statef=takeoff
|
||||
|
||||
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
||||
barrier_set(ROBOTS, action, land, -1)
|
||||
#barrier_set(ROBOTS, action, land, -1)
|
||||
barrier_set(6, action, land, -1)
|
||||
barrier_ready()
|
||||
} else {
|
||||
log("Altitude: ", position.altitude)
|
||||
|
|
@ -49,7 +53,8 @@ function land() {
|
|||
uav_land()
|
||||
|
||||
if(flight.status != 2 and flight.status != 3) {
|
||||
barrier_set(ROBOTS,turnedoff,land, 21)
|
||||
#barrier_set(ROBOTS,turnedoff,land, 21)
|
||||
barrier_set(6,turnedoff,land, 21)
|
||||
barrier_ready()
|
||||
}
|
||||
}
|
||||
|
|
@ -85,7 +90,7 @@ function picture() {
|
|||
ptime=ptime+1
|
||||
}
|
||||
|
||||
function gotoWP(transf) {
|
||||
function gotoWPRRT(transf) {
|
||||
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
|
||||
print(" has to move ", math.vec2.length(rc_goal))
|
||||
|
||||
|
|
@ -93,7 +98,7 @@ function gotoWP(transf) {
|
|||
log("Sorry this is too far.")
|
||||
else {
|
||||
if(Path==nil){
|
||||
Path = pathPlanner(rc_goal)
|
||||
Path = UAVpathPlanner(rc_goal)
|
||||
cur_goal_l = 1
|
||||
} else if(cur_goal_l<=Path.nb_row) {
|
||||
cur_gps=getvec(Path,cur_goal_l) #x=latitude, y=longitude
|
||||
|
|
@ -110,16 +115,21 @@ function gotoWP(transf) {
|
|||
transf()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
# 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-position.altitude)
|
||||
# } else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination
|
||||
# transf()
|
||||
# else
|
||||
# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
|
||||
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-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-position.altitude)
|
||||
}
|
||||
|
||||
function follow() {
|
||||
|
|
@ -167,6 +177,18 @@ function uav_rccmd() {
|
|||
} 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
|
||||
} else if (flight.rc_cmd==901){
|
||||
flight.rc_cmd=0
|
||||
rc_State = 1
|
||||
} else if (flight.rc_cmd==902){
|
||||
flight.rc_cmd=0
|
||||
rc_State = 2
|
||||
} else if (flight.rc_cmd==903){
|
||||
flight.rc_cmd=0
|
||||
rc_State = 3
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -176,14 +198,22 @@ function uav_neicmd() {
|
|||
print("Got (", vid, ",", value, ") #", rid, "(", UAVSTATE, ")")
|
||||
if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") {
|
||||
statef=takeoff
|
||||
UAVSTATE = "TAKEOFF"
|
||||
UAVSTATE = "TAKEOFF"
|
||||
} else if(value==21 and UAVSTATE!="LAND" and UAVSTATE!="BARRIERWAIT") {
|
||||
statef=land
|
||||
UAVSTATE = "LAND"
|
||||
UAVSTATE = "LAND"
|
||||
} else if(value==400 and UAVSTATE=="TURNEDOFF") {
|
||||
uav_arm()
|
||||
} else if(value==401 and UAVSTATE=="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 UAVSTATE=="IDLE"){
|
||||
# neighbors.listen("gt",function(vid, value, rid) {
|
||||
# print("Got (", vid, ",", value, ") from robot #", rid)
|
||||
|
|
|
|||
|
|
@ -297,9 +297,6 @@ void in_message_process(){
|
|||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "floor", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzz_floor));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
||||
|
|
@ -470,9 +467,11 @@ int create_stig_tables() {
|
|||
if(VM) buzzvm_destroy(&VM);
|
||||
Robot_id = robot_id;
|
||||
VM = buzzvm_new((int)robot_id);
|
||||
ROS_INFO(" Robot ID -1: %i" , robot_id);
|
||||
/* Get rid of debug info */
|
||||
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
|
||||
DBG_INFO = buzzdebug_new();
|
||||
ROS_INFO(" Robot ID -2: %i" , robot_id);
|
||||
/* Read bytecode and fill in data structure */
|
||||
FILE* fd = fopen(bo_filename, "rb");
|
||||
if(!fd) {
|
||||
|
|
@ -491,6 +490,7 @@ int create_stig_tables() {
|
|||
return 0;
|
||||
}
|
||||
fclose(fd);
|
||||
ROS_INFO(" Robot ID -3: %i" , robot_id);
|
||||
/* Read debug information */
|
||||
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
|
||||
buzzvm_destroy(&VM);
|
||||
|
|
@ -498,6 +498,7 @@ int create_stig_tables() {
|
|||
perror(bdbg_filename);
|
||||
return 0;
|
||||
}
|
||||
ROS_INFO(" Robot ID -4: %i" , robot_id);
|
||||
/* Set byte code */
|
||||
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
|
|
@ -505,6 +506,7 @@ int create_stig_tables() {
|
|||
ROS_ERROR("[%i] %s: Error loading Buzz script", Robot_id, bo_filename);
|
||||
return 0;
|
||||
}
|
||||
ROS_INFO(" Robot ID -5: %i" , robot_id);
|
||||
/* Register hook functions */
|
||||
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
|
|
@ -512,6 +514,7 @@ int create_stig_tables() {
|
|||
ROS_ERROR("[%i] Error registering hooks", Robot_id);
|
||||
return 0;
|
||||
}
|
||||
ROS_INFO(" Robot ID -6: %i" , robot_id);
|
||||
|
||||
/* Create vstig tables
|
||||
if(create_stig_tables() != BUZZVM_STATE_READY) {
|
||||
|
|
|
|||
|
|
@ -41,48 +41,52 @@ namespace buzzuav_closures{
|
|||
/****************************************/
|
||||
/****************************************/
|
||||
|
||||
int buzzros_print(buzzvm_t vm) {
|
||||
int i;
|
||||
char buffer [100] = "";
|
||||
sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid());
|
||||
for(uint32_t i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
|
||||
buzzvm_lload(vm, i);
|
||||
buzzobj_t o = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_pop(vm);
|
||||
switch(o->o.type) {
|
||||
case BUZZTYPE_NIL:
|
||||
sprintf(buffer,"%s BUZZ - [nil]", buffer);
|
||||
break;
|
||||
case BUZZTYPE_INT:
|
||||
sprintf(buffer,"%s %d", buffer, o->i.value);
|
||||
//fprintf(stdout, "%d", o->i.value);
|
||||
break;
|
||||
case BUZZTYPE_FLOAT:
|
||||
sprintf(buffer,"%s %f", buffer, o->f.value);
|
||||
break;
|
||||
case BUZZTYPE_TABLE:
|
||||
sprintf(buffer,"%s [table with %d elems]", buffer, (buzzdict_size(o->t.value)));
|
||||
break;
|
||||
case BUZZTYPE_CLOSURE:
|
||||
if(o->c.value.isnative)
|
||||
sprintf(buffer,"%s [n-closure @%d]", buffer, o->c.value.ref);
|
||||
else
|
||||
sprintf(buffer,"%s [c-closure @%d]", buffer, o->c.value.ref);
|
||||
break;
|
||||
case BUZZTYPE_STRING:
|
||||
sprintf(buffer,"%s %s", buffer, o->s.value.str);
|
||||
break;
|
||||
case BUZZTYPE_USERDATA:
|
||||
sprintf(buffer,"%s [userdata @%p]", buffer, o->u.value);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
ROS_INFO(buffer);
|
||||
//fprintf(stdout, "\n");
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
int buzzros_print(buzzvm_t vm)
|
||||
{
|
||||
std::ostringstream buffer (std::ostringstream::ate);
|
||||
buffer << buzz_utility::get_robotid();
|
||||
for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index)
|
||||
{
|
||||
buzzvm_lload(vm, index);
|
||||
buzzobj_t o = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_pop(vm);
|
||||
switch (o->o.type)
|
||||
{
|
||||
case BUZZTYPE_NIL:
|
||||
buffer << " BUZZ - [nil]";
|
||||
break;
|
||||
case BUZZTYPE_INT:
|
||||
buffer << " " << o->i.value;
|
||||
break;
|
||||
case BUZZTYPE_FLOAT:
|
||||
buffer << " " << o->f.value;
|
||||
break;
|
||||
case BUZZTYPE_TABLE:
|
||||
buffer << " [table with " << buzzdict_size(o->t.value) << " elems]";
|
||||
break;
|
||||
case BUZZTYPE_CLOSURE:
|
||||
if (o->c.value.isnative)
|
||||
{
|
||||
buffer << " [n-closure @" << o->c.value.ref << "]";
|
||||
}
|
||||
else
|
||||
{
|
||||
buffer << " [c-closure @" << o->c.value.ref << "]";
|
||||
}
|
||||
break;
|
||||
case BUZZTYPE_STRING:
|
||||
buffer << " " << o->s.value.str;
|
||||
break;
|
||||
case BUZZTYPE_USERDATA:
|
||||
buffer << " [userdata @" << o->u.value << "]";
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
ROS_INFO("%s",buffer.str().c_str());
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
void setWPlist(string path){
|
||||
WPlistname = path + "include/graphs/waypointlist.csv";
|
||||
|
|
@ -171,14 +175,6 @@ namespace buzzuav_closures{
|
|||
}
|
||||
|
||||
|
||||
int buzz_floor(buzzvm_t vm) {
|
||||
buzzvm_lnum_assert(vm, 1);
|
||||
buzzvm_lload(vm, 1);
|
||||
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
|
||||
float fval = buzzvm_stack_at(vm, 1)->f.value;
|
||||
buzzvm_pushi(vm, floor(fval));
|
||||
return buzzvm_ret1(vm);
|
||||
}
|
||||
/*----------------------------------------/
|
||||
/ Buzz closure to move following a 2D vector
|
||||
/----------------------------------------*/
|
||||
|
|
|
|||
Loading…
Reference in New Issue