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

add collision boxes for gazebo buildings and debugged collision detection for rrt

This commit is contained in:
dave 2017-12-21 21:41:57 -05:00
parent 11053c82dc
commit 390a02df8d
6 changed files with 58 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
buzz_scripts/include/mapmatrix.bzz
View File

@ -56,7 +56,7 @@ function add_obstacle(pos, off, inc_trust) {
var yi = math.round(pos.y) var yi = math.round(pos.y)
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
#log("Add obstacle in cell: ", xi, " ", yi) # log("Add obstacle in cell: ", xi, " ", yi)
var old=map[xi][yi] var old=map[xi][yi]
if(old-inc_trust > 0.0) if(old-inc_trust > 0.0)
map[xi][yi] = old-inc_trust map[xi][yi] = old-inc_trust
@ -69,8 +69,8 @@ function remove_obstacle(pos, off, dec_trust) {
var xi = math.round(pos.x) var xi = math.round(pos.x)
var yi = math.round(pos.y) var yi = math.round(pos.y)
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0){ if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
#log("Remove obstacle in cell: ", xi, " ", yi) # log("Remove obstacle in cell: ", xi, " ", yi)
var old=map[xi][yi] var old=map[xi][yi]
if(old + dec_trust < 1.0) #x,y if(old + dec_trust < 1.0) #x,y
map[xi][yi] = old+dec_trust map[xi][yi] = old+dec_trust

76
buzz_scripts/include/rrtstar.bzz
View File

@ -7,7 +7,7 @@ include "mapmatrix.bzz"
RRT_TIMEOUT = 500 RRT_TIMEOUT = 500
RRT_RUNSTEP = 3 RRT_RUNSTEP = 3
PROX_SENSOR_THRESHOLD_M = 10 PROX_SENSOR_THRESHOLD_M = 8.0
GSCALE = {.x=1, .y=1} GSCALE = {.x=1, .y=1}
map = nil map = nil
cur_cell = {} cur_cell = {}
@ -46,7 +46,7 @@ function pathPlanner(m_goal, m_pos, kh4) {
arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y} arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y}
# RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance # RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance
Q = {} Q = {}
Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0} Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=0,.4=1,.5=0}
goalReached = 0 goalReached = 0
timeout = 0 timeout = 0
@ -56,6 +56,7 @@ function pathPlanner(m_goal, m_pos, kh4) {
} }
# get the grid cell position (x,y) equivalent to the input position # get the grid cell position (x,y) equivalent to the input position
# input should be relative to home position (planing start point)
function getcell(m_curpos) { function getcell(m_curpos) {
var cell = {} var cell = {}
# relative to center (start position) # relative to center (start position)
@ -79,6 +80,7 @@ function getcell(m_curpos) {
function populateGrid(m_pos) { function populateGrid(m_pos) {
getproxobs(m_pos) getproxobs(m_pos)
getneiobs (m_pos) getneiobs (m_pos)
export_map(map)
} }
# TODO: populate the map with neighors as blobs instead ? # TODO: populate the map with neighors as blobs instead ?
@ -136,26 +138,43 @@ function getproxobs (m_curpos) {
reduce(proximity, reduce(proximity,
function(key, value, acc) { function(key, value, acc) {
obs = getcell(math.vec2.newp(value.value, value.angle + pose.orientation.yaw)) if (value.angle != -1) { # down sensor of M100
obs2 = getcell(math.vec2.newp(value.value + 1.0, value.angle + pose.orientation.yaw)) if(value.value < PROX_SENSOR_THRESHOLD_M) {
per = math.vec2.sub(obs,cur_cell) obs = getcell(math.vec2.add(m_curpos, math.vec2.newp(value.value, value.angle + pose.orientation.yaw)))
obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) per = math.vec2.sub(obs,cur_cell)
obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs)
obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) 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) obsr2 = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) + math.pi/2.0), obs)
if(value.value < PROX_SENSOR_THRESHOLD_M) { obsl2 = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) - math.pi/2.0), obs)
if(map[math.round(obs.x)][math.round(obs.y)]!=0) { # obs2 = getcell(math.vec2.newp(math.vec2.length(obs) + 1.0, math.vec2.angle(obs)))
add_obstacle(obs, 0, 0.25) # obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2)
add_obstacle(obs2, 0, 0.25) # obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2)
add_obstacle(obsr, 0, 0.25) if(map[math.round(obs.x)][math.round(obs.y)]!=0) {
add_obstacle(obsr2, 0, 0.25) add_obstacle(obs, 0, 0.25)
add_obstacle(obsl, 0, 0.25) add_obstacle(obsr, 0, 0.25)
add_obstacle(obsl2, 0, 0.25) add_obstacle(obsl, 0, 0.25)
updated_obstacle = 1 add_obstacle(obsr2, 0, 0.25)
add_obstacle(obsl2, 0, 0.25)
# add_obstacle(obs2, 0, 0.25)
# add_obstacle(obsr2, 0, 0.25)
# add_obstacle(obsl2, 0, 0.25)
updated_obstacle = 1
}
} else {
var line_length = PROX_SENSOR_THRESHOLD_M;
while(line_length > 0) {
obs = getcell(math.vec2.add(m_curpos, math.vec2.newp(line_length, value.angle + pose.orientation.yaw)))
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)
obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs)
obsr = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) + math.pi/2.0), obs)
obsl = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) - math.pi/2.0), obs)
remove_obstacle(obs, 0, 0.05)
remove_obstacle(obsr, 0, 0.05)
remove_obstacle(obsl, 0, 0.05)
line_length = line_length - 1;
}
} }
} else if(map[math.round(obs.x)][math.round(obs.y)]!=1) {
remove_obstacle(obs, 0, 0.05)
updated_obstacle = 1
} }
return acc return acc
}, math.vec2.new(0, 0)) }, math.vec2.new(0, 0))
@ -225,7 +244,7 @@ function rrtstar() {
nbCount = nbCount + 1; nbCount = nbCount + 1;
if(intersects != 1) { if(intersects != 1) {
#log(pointNumber, "do not intersect (",pointNumber.mat[3],")") #log(pointNumber, "do not intersect (",pointNumber.mat[3],")")
var 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][4]][5]
if(distance < minCounted) { if(distance < minCounted) {
minCounted = distance; minCounted = distance;
@ -422,17 +441,18 @@ function getPath(Q,nb,gps){
var pathL={} var pathL={}
var npt=0 var npt=0
# get the path from the point list # get the path from the point list
while(nb > 1) { while(nb > 0) {
npt=npt+1 npt=npt+1
pathL[npt] = {} pathL[npt] = {}
pathL[npt][1]=Q[nb][1] pathL[npt][1]=Q[nb][1]
pathL[npt][2]=Q[nb][2] pathL[npt][2]=Q[nb][2]
if(nb != Q[Q[nb][3]][3]) if( nb > 1) {
nb = Q[nb][3]; if(nb == Q[Q[nb][3]][3]) {# TODO: why is this happening?
else { # TODO: why is this happening? log("ERROR - Recursive path !!!")
log("ERROR - Recursive path !!!") return nil
return nil }
} }
nb = Q[nb][3];
} }
# Re-Order the list. # Re-Order the list.

2
buzz_scripts/include/uavstates.bzz
View File

@ -9,7 +9,7 @@ include "rrtstar.bzz"
TARGET_ALTITUDE = 15.0 # m. TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF" BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 2 # m/steps GOTO_MAXVEL = 1.5 # m/steps
GOTO_MAXDIST = 150 # m. GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 0.5 # m. GOTODIST_TOL = 0.5 # m.
GOTOANG_TOL = 0.1 # rad. GOTOANG_TOL = 0.1 # rad.

4
buzz_scripts/testRRT.bzz
View File

@ -5,7 +5,7 @@ include "vstigenv.bzz"
function action() { function action() {
statef=action statef=action
uav_storegoal(45.5088103899,-73.1540826153,5.0) uav_storegoal(45.5088103899,-73.1540826153,2.5)
set_goto(idle) set_goto(idle)
} }
@ -14,7 +14,7 @@ function init() {
uav_initstig() uav_initstig()
uav_initswarm() uav_initswarm()
TARGET_ALTITUDE = 5.0 # m. TARGET_ALTITUDE = 2.5 # m
# statef=turnedoff # statef=turnedoff
# BVMSTATE = "TURNEDOFF" # BVMSTATE = "TURNEDOFF"
statef = takeoff statef = takeoff

3
src/buzzuav_closures.cpp
View File

@ -179,6 +179,7 @@ int buzz_exportmap(buzzvm_t vm)
/ Buzz closure to export a 2D map / Buzz closure to export a 2D map
/----------------------------------------*/ /----------------------------------------*/
{ {
grid.clear();
buzzvm_lnum_assert(vm, 1); buzzvm_lnum_assert(vm, 1);
// Get the parameter // Get the parameter
buzzvm_lload(vm, 1); buzzvm_lload(vm, 1);
@ -193,7 +194,7 @@ int buzz_exportmap(buzzvm_t vm)
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushi(vm, j); buzzvm_pushi(vm, j);
buzzvm_tget(vm); buzzvm_tget(vm);
row.insert(std::pair<int,int>(j,round(buzzvm_stack_at(vm, 1)->f.value*100.0))); row.insert(std::pair<int,int>(j, 100.0 - round(buzzvm_stack_at(vm, 1)->f.value*100.0)));
buzzvm_pop(vm); buzzvm_pop(vm);
} }
grid.insert(std::pair<int,std::map<int, int>>(i,row)); grid.insert(std::pair<int,std::map<int, int>>(i,row));

4
src/roscontroller.cpp
View File

@ -473,8 +473,8 @@ void roscontroller::grid_publisher()
grid_msg.info.resolution = 0.01;//gridMap.getResolution(); grid_msg.info.resolution = 0.01;//gridMap.getResolution();
grid_msg.info.width = g_w; grid_msg.info.width = g_w;
grid_msg.info.height = g_h; grid_msg.info.height = g_h;
grid_msg.info.origin.position.x = 0.0; grid_msg.info.origin.position.x = round(g_w/2.0) * grid_msg.info.resolution;
grid_msg.info.origin.position.y = 0.0; grid_msg.info.origin.position.y = round(g_h/2.0) * grid_msg.info.resolution;
grid_msg.info.origin.position.z = 0.0; grid_msg.info.origin.position.z = 0.0;
grid_msg.info.origin.orientation.x = 0.0; grid_msg.info.origin.orientation.x = 0.0;
grid_msg.info.origin.orientation.y = 0.0; grid_msg.info.origin.orientation.y = 0.0;