add comments and TODOs
This commit is contained in:
parent
2751230d6f
commit
081269cdab
|
@ -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){
|
||||
|
|
Loading…
Reference in New Issue