From df11060f84b269a57db13b359e853b46c969aa94 Mon Sep 17 00:00:00 2001
From: dave <david.st-onge@polymtl.ca>
Date: Wed, 20 Dec 2017 14:51:00 -0500
Subject: [PATCH] add comments and TODOs

---
 buzz_scripts/include/rrtstar.bzz | 43 +++++++++++++++++++-------------
 1 file changed, 26 insertions(+), 17 deletions(-)

diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz
index 86d1ce0..89fb106 100644
--- a/buzz_scripts/include/rrtstar.bzz
+++ b/buzz_scripts/include/rrtstar.bzz
@@ -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){