cut path at first unknown node and fix up getId

include/gnc/planner/plannerUtils.hpp

@@ -25,24 +25,30 @@ double l2norm(const point3d& a, const point3d& b)
     return c.norm();
 }
 
-// gets the linear index from a 2D grid. Counts N/W/S/E
-// TODO: Fix the special way
 struct GetId
 {
-    GetId(double resolution) : resolution_(resolution) {}
+    GetId(double resolution) : resolution_(resolution)
+        {
+            size_t units = std::round((double) MAX_ARENA_SIZE / resolution_);
+            if (units % 2 == 0) { units += 1; }
+            arena_grid_row_units_ = units;
+            size_t midpt = ceil((double) units / 2.0);
+            translation_ = point3d(midpt, midpt, 0.0);
+        }
     // return linear index from the 2d grid
     size_t operator()(const point3d& a)
     {
-        // Assume the arena size is 601 x 601 gridunits at 0.05 res. 301,301 is middle
-        // scaline
         // X is row, Y is col
         point3d scaled = (a * (1. / resolution_));
-        scaled += point3d(301.0, 301.0, 0.0);
+        scaled += translation_;
         assert(scaled.x() > 0);
         assert(scaled.y() > 0);
-        return std::round((scaled.x()*601.0 + scaled.y()));
+        return std::round((scaled.x()*arena_grid_row_units_ + scaled.y()));
     }
     double resolution_;
+    point3d translation_;
+    size_t arena_grid_row_units_;
+    const unsigned int MAX_ARENA_SIZE = 60; // in meters
 };
 
 }

src/gnc/planner/Astar.cpp

@@ -35,8 +35,6 @@ namespace gnc
 namespace planner
 {
 
-const unsigned int ARENA_SIZE = 30; // in meters 
-
 /*
 * A point has no collision if it is a safe distance away from the nearest
 * obstacle.
@@ -246,23 +244,35 @@ Path Astar::operator()(const Waypoint& start, const Waypoint& goal, const std::s
         if(i == (int)node_path.size() - 1)
         {
             // handles start waypoint
-            // TODO: Calculate Rates when controller has implemented it
             waypoints.emplace_back(pos, Eigen::Vector3d(0,0,0), start.yaw);
-            //waypoints.emplace_back(pos, Eigen::Vector3d(0,0,0),0);
         }
         else
         {
-            // TODO: Calculate Rates when controller has implemented it
             waypoints.emplace_back(pos, Eigen::Vector3d(0,0,0),
                 yaw_between(waypoints.back().position, pos));
-            //waypoints.emplace_back(pos, Eigen::Vector3d(0,0,0),0);
 
         }
     }
+    // If the path contains any unknown waypoints, then resize the path to 
+    // right before the unknown waypoint. This ensures the quad doesn't 
+    // fly into unknown territory.
+    auto itUnknown = find_if(waypoints.begin(), waypoints.end(), 
+                [&tree, depth](const auto &w) { 
+                    return !tree->search(w.position.x(), 
+                                         w.position.y(), 
+                                         w.position.z(), depth);
+                });
+    if(itUnknown != waypoints.end())
+    {
+        cerr << "unknown location\n";
+        waypoints.resize(itUnknown - waypoints.begin());
+    }
+
     Path path;
     path.waypoints = waypoints;
     path.utime = std::chrono::duration_cast<std::chrono::microseconds>
                  (std::chrono::system_clock::now().time_since_epoch()).count();
+
     return path;
 }