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

add comments and TODOs

This commit is contained in:
dave 2017-12-20 14:51:00 -05:00
parent 2751230d6f
commit 081269cdab
1 changed files with 26 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
buzz_scripts/include/rrtstar.bzz
View File

@ -15,6 +15,7 @@ nei_cell = {}
mapgoal = {} mapgoal = {}
# create a map with its length to fit the goal (rel. pos.) and the current position as the center # create a map with its length to fit the goal (rel. pos.) and the current position as the center
# TODO: test with khepera/argos
function dyn_init_map(m_navigation) { function dyn_init_map(m_navigation) {
# create a map big enough for the goal # create a map big enough for the goal
init_map(math.round(2*math.vec2.length(m_navigation))+10) init_map(math.round(2*math.vec2.length(m_navigation))+10)
@ -23,6 +24,7 @@ function dyn_init_map(m_navigation) {
} }
# start the RRT* to reach the goal (rel. pos.) # start the RRT* to reach the goal (rel. pos.)
# TODO: test with khepera/argos
function pathPlanner(m_goal, m_pos, kh4) { function pathPlanner(m_goal, m_pos, kh4) {
# place the robot on the grid # place the robot on the grid
cur_cell = getcell(m_pos) cur_cell = getcell(m_pos)
@ -53,6 +55,7 @@ function pathPlanner(m_goal, m_pos, kh4) {
rrtstar() rrtstar()
} }
# get the grid cell position (x,y) equivalent to the input position
function getcell(m_curpos) { function getcell(m_curpos) {
var cell = {} var cell = {}
# relative to center (start position) # relative to center (start position)
@ -78,7 +81,7 @@ function populateGrid(m_pos) {
getneiobs (m_pos) getneiobs (m_pos)
} }
# TODO: populate the map with a blob instead? # TODO: populate the map with neighors as blobs instead ?
function getneiobs (m_curpos) { function getneiobs (m_curpos) {
cur_cell = getcell(m_curpos) cur_cell = getcell(m_curpos)
# add all neighbors as obstacles in the grid # add all neighbors as obstacles in the grid
@ -126,6 +129,7 @@ function getneiobs (m_curpos) {
# } # }
# populate a line in front of the sensor using plateform independant proximity sensing # populate a line in front of the sensor using plateform independant proximity sensing
# TODO: adapt to M100 stereo
function getproxobs (m_curpos) { function getproxobs (m_curpos) {
var foundobstacle = 0 var foundobstacle = 0
cur_cell = getcell(m_curpos) cur_cell = getcell(m_curpos)
@ -159,6 +163,8 @@ function getproxobs (m_curpos) {
return foundobstacle return foundobstacle
} }
# check if any obstacle blocks the way
# TODO: remove the kh4 bool for retro-compatilibty
function check_path(m_path, goal_l, m_curpos, kh4) { function check_path(m_path, goal_l, m_curpos, kh4) {
var pathisblocked = 0 var pathisblocked = 0
var nb = goal_l var nb = goal_l
@ -181,14 +187,15 @@ function check_path(m_path, goal_l, m_curpos, kh4) {
return pathisblocked return pathisblocked
} }
##
# main search loop as a state
##
function rrtstar() { function rrtstar() {
# update state machine variables # update state machine variables
statef = rrtstar statef = rrtstar
BVMSTATE = "RRTSTAR" BVMSTATE = "RRTSTAR"
##
# main search loop
##
step_timeout = 0 step_timeout = 0
while(goalReached == 0 and timeout < RRT_TIMEOUT and step_timeout < RRT_RUNSTEP) { while(goalReached == 0 and timeout < RRT_TIMEOUT and step_timeout < RRT_RUNSTEP) {
@ -309,20 +316,23 @@ function rrtstar() {
} }
if(goalReached){ if(goalReached){
log("Goal found(",numberOfPoints,")!") log("Goal found(",numberOfPoints,")!")
Path = getPath(Q,numberOfPoints, 1) Path = getPath(Q,numberOfPoints, 1)
# done. resume the state machine # done. resume the state machine
BVMSTATE = "GOTOGPS" BVMSTATE = "GOTOGPS"
statef = old_statef statef = old_statef
} else { } else if(timeout >= RRT_TIMEOUT) {
#log("FAILED TO FIND A PATH!!!") log("FAILED TO FIND A PATH!!!")
Path = nil Path = nil
# done. resume the state machine
BVMSTATE = "GOTOGPS"
statef = old_statef
} }
} }
# Go through each points and find the distances between them and the
# target point
function findClosestPoint(point,aPt) { function findClosestPoint(point,aPt) {
# Go through each points and find the distances between them and the
# target point
var distance = 999 var distance = 999
var pointNb = -1 var pointNb = -1
var ifcp=1 var ifcp=1
@ -388,6 +398,7 @@ function doesItIntersect(point,vector) {
var 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, "(Map size: ", size(map), ")") #log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range, "(Map size: ", size(map), ")")
# TODO: this check if the pos_chk is under the robot, but we need to check the whole line for a cross
if(is_in(get_occupied_cells(cur_cell), xi, 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!=cur_cell.x and yi!=cur_cell.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) {
@ -408,7 +419,6 @@ function doesItIntersect(point,vector) {
# create a table with only the path's points in order # create a table with only the path's points in order
function getPath(Q,nb,gps){ function getPath(Q,nb,gps){
#log("-----------CONVERTING PATH!!!")
var pathL={} var pathL={}
var npt=0 var npt=0
# get the path from the point list # get the path from the point list
@ -419,14 +429,13 @@ function getPath(Q,nb,gps){
pathL[npt][2]=Q[nb][2] pathL[npt][2]=Q[nb][2]
if(nb != Q[Q[nb][3]][3]) if(nb != Q[Q[nb][3]][3])
nb = Q[nb][3]; nb = Q[nb][3];
else { else { # TODO: why is this happening?
log("ERROR - Recursive path !!!") log("ERROR - Recursive path !!!")
return nil return nil
} }
} }
# log("re-order the list") # Re-Order the list.
# table_print(path.mat)
var pathR={} var pathR={}
var totpt = npt + 1 var totpt = npt + 1
while(npt > 0){ while(npt > 0){