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

View File

@ -15,6 +15,7 @@ nei_cell = {}
mapgoal = {}
# 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) {
# create a map big enough for the goal
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.)
# TODO: test with khepera/argos
function pathPlanner(m_goal, m_pos, kh4) {
# place the robot on the grid
cur_cell = getcell(m_pos)
@ -53,6 +55,7 @@ function pathPlanner(m_goal, m_pos, kh4) {
rrtstar()
}
# get the grid cell position (x,y) equivalent to the input position
function getcell(m_curpos) {
var cell = {}
# relative to center (start position)
@ -78,7 +81,7 @@ function populateGrid(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) {
cur_cell = getcell(m_curpos)
# 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
# TODO: adapt to M100 stereo
function getproxobs (m_curpos) {
var foundobstacle = 0
cur_cell = getcell(m_curpos)
@ -159,6 +163,8 @@ function getproxobs (m_curpos) {
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) {
var pathisblocked = 0
var nb = goal_l
@ -181,14 +187,15 @@ function check_path(m_path, goal_l, m_curpos, kh4) {
return pathisblocked
}
##
# main search loop as a state
##
function rrtstar() {
# update state machine variables
statef = rrtstar
BVMSTATE = "RRTSTAR"
##
# main search loop
##
step_timeout = 0
while(goalReached == 0 and timeout < RRT_TIMEOUT and step_timeout < RRT_RUNSTEP) {
@ -309,20 +316,23 @@ function rrtstar() {
}
if(goalReached){
log("Goal found(",numberOfPoints,")!")
Path = getPath(Q,numberOfPoints, 1)
# done. resume the state machine
BVMSTATE = "GOTOGPS"
statef = old_statef
} else {
#log("FAILED TO FIND A PATH!!!")
log("Goal found(",numberOfPoints,")!")
Path = getPath(Q,numberOfPoints, 1)
# done. resume the state machine
BVMSTATE = "GOTOGPS"
statef = old_statef
} else if(timeout >= RRT_TIMEOUT) {
log("FAILED TO FIND A PATH!!!")
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) {
# Go through each points and find the distances between them and the
# target point
var distance = 999
var pointNb = -1
var ifcp=1
@ -388,6 +398,7 @@ function doesItIntersect(point,vector) {
var yi = math.round(pos_chk.y) #+1
#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(xi!=cur_cell.x and yi!=cur_cell.y) {
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
function getPath(Q,nb,gps){
#log("-----------CONVERTING PATH!!!")
var pathL={}
var npt=0
# get the path from the point list
@ -419,14 +429,13 @@ function getPath(Q,nb,gps){
pathL[npt][2]=Q[nb][2]
if(nb != Q[Q[nb][3]][3])
nb = Q[nb][3];
else {
else { # TODO: why is this happening?
log("ERROR - Recursive path !!!")
return nil
}
}
# log("re-order the list")
# table_print(path.mat)
# Re-Order the list.
var pathR={}
var totpt = npt + 1
while(npt > 0){