Spiri
/
ROSBuzz_MISTLab
13
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'sim' of git.mistlab.ca:dasto/drones into sim

This commit is contained in:
vivek-shankar 2017-12-19 12:46:52 -05:00
parent 84eeef897e 8f80b51cf3
commit a243d5fa1a
2 changed files with 15 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
buzz_scripts/include/mapmatrix.bzz
View File

@ -15,7 +15,7 @@ function wmat(mat, row, col, val) {
# Read from matrix # Read from matrix
function rmat(mat, row, col) { function rmat(mat, row, col) {
#log("rmat ", mat, row, col) #log("rmat ", mat, row, col)
index = (row-1)*mat.nb_col + (col - 1) var index = (row-1)*mat.nb_col + (col - 1)
if (mat.mat[index] == nil) { if (mat.mat[index] == nil) {
log("Wrong matrix read index: ", row, " ", col) log("Wrong matrix read index: ", row, " ", col)
return -1 return -1
@ -41,7 +41,7 @@ function getvec(t,row){
function init_test_map(len){ function init_test_map(len){
map = {.nb_col=len, .nb_row=len, .mat={}} map = {.nb_col=len, .nb_row=len, .mat={}}
index = 0 var index = 0
while(index<len*len){ while(index<len*len){
map.mat[index]=1.0 map.mat[index]=1.0
index = index + 1 index = index + 1
@ -56,7 +56,7 @@ function init_test_map(len){
function init_map(len){ function init_map(len){
map = {.nb_col=len, .nb_row=len, .mat={}} map = {.nb_col=len, .nb_row=len, .mat={}}
index = 0 var index = 0
while(index<len*len){ while(index<len*len){
map.mat[index]=1.0 map.mat[index]=1.0
index = index + 1 index = index + 1
@ -65,8 +65,8 @@ function init_map(len){
} }
function add_obstacle(pos, off, inc_trust) { function add_obstacle(pos, off, inc_trust) {
xi = math.round(pos.x) var xi = math.round(pos.x)
yi = math.round(pos.y) var yi = math.round(pos.y)
if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) { 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) #log("Add obstacle in cell: ", xi, " ", yi)
@ -79,8 +79,8 @@ function add_obstacle(pos, off, inc_trust) {
} }
function remove_obstacle(pos, off, dec_trust) { function remove_obstacle(pos, off, dec_trust) {
xi = math.round(pos.x) var xi = math.round(pos.x)
yi = math.round(pos.y) var yi = math.round(pos.y)
if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0){ if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0){
#log("Remove obstacle in cell: ", xi, " ", yi) #log("Remove obstacle in cell: ", xi, " ", yi)
@ -107,14 +107,14 @@ function table_copy(t) {
} }
function print_pos(t) { function print_pos(t) {
ir=1 var ir=1
while(ir<=t.nb_row){ 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 ir = ir + 1
} }
} }
function print_map(t) { function print_map(t) {
ir=t.nb_row var ir=t.nb_row
log("Printing a ", t.nb_row, " by ", t.nb_col, " map") log("Printing a ", t.nb_row, " by ", t.nb_col, " map")
while(ir>0){ while(ir>0){
logst=string.concat("\t", string.tostring(ir), "\t:") logst=string.concat("\t", string.tostring(ir), "\t:")
@ -132,7 +132,7 @@ function print_map(t) {
} }
function print_map_argos(t){ function print_map_argos(t){
ir=t.nb_row var ir=t.nb_row
msg = string.tostring(ir) msg = string.tostring(ir)
while(ir>0){ while(ir>0){
ic=t.nb_col ic=t.nb_col
@ -147,5 +147,5 @@ function print_map_argos(t){
} }
ir = ir - 1 ir = ir - 1
} }
set_argos_map(msg) #set_argos_map(msg)
} }

8
buzz_scripts/include/rrtstar.bzz
View File

@ -363,8 +363,8 @@ function doesItIntersect(point,vector) {
distance = math.vec2.length(dif) distance = math.vec2.length(dif)
if(distance==0.0){ if(distance==0.0){
# Find what block we're in right now # Find what block we're in right now
xi = math.round(point.x) #+1 var xi = math.round(point.x) #+1
yi = math.round(point.y) #+1 var yi = math.round(point.y) #+1
if(xi!=cur_cell.x and yi!=cur_cell.y){ if(xi!=cur_cell.x and yi!=cur_cell.y){
if(rmat(map,xi,yi) > 0.5) if(rmat(map,xi,yi) > 0.5)
return 1 return 1
@ -382,8 +382,8 @@ function doesItIntersect(point,vector) {
pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range)); pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range));
# Find what block we're in right now # Find what block we're in right now
xi = math.round(pos_chk.x) #+1 var xi = math.round(pos_chk.x) #+1
yi = math.round(pos_chk.y) #+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)
if(xi!=cur_cell.x and yi!=cur_cell.y){ if(xi!=cur_cell.x and yi!=cur_cell.y){