visualization tool for planner is implemented and works on simple test

src/gnc/planner/Astar.cpp

@@ -169,8 +169,8 @@ Path Astar::operator()(const Waypoint& start, const Waypoint& goal, const std::s
     while (!openNodes.empty())
     {
         // assert cost of the beginning node is less than the last node
-        assert(*openNodes.begin() < *openNodes.rbegin() ||  
-            openNodes.begin()->cost() == openNodes.rbegin()->cost());
+        // assert(*openNodes.begin() < *openNodes.rbegin() ||  
+        //     openNodes.begin()->cost() == openNodes.rbegin()->cost());
         shared_ptr<Node> n = *openNodes.begin();
         openNodes.erase(openNodes.begin());
         // check for the coordinate in the correct tolerance to be

tools/planner/CMakeLists.txt

@@ -20,24 +20,16 @@ include_directories(
     ${Octomap_INCLUDE_DIRS}
 )
 
-add_executable(tool-octomap-creation createOctomap.cpp)
+add_executable(tool-planner-visualization visualizePlanner.cpp)
 
-# add_executable(tool-planner-visualization visualizePlanner.cpp)
-
-target_link_libraries(tool-octomap-creation
-    maav-utils
-    maav-msg
-    VisionUtils
-    ${ZCM_LIBRARIES}
-    ${YAMLCPP_LIBRARY}
-    ${Octomap_LIBRARIES}
-)
-
-# target_link_libraries(tool-planner-visualization
-#     maav-utils
-#     maav-msg
-#     VisionUtils
-#     ${ZCM_LIBRARIES}
-#     ${YAMLCPP_LIBRARY}
-#     ${Octomap_LIBRARIES}
-# )
+target_link_libraries(tool-planner-visualization
+     maav-state
+     maav-utils
+     maav-msg
+     maav-guidance
+     maav-path-planner
+     VisionUtils
+     ${ZCM_LIBRARIES}
+     ${YAMLCPP_LIBRARY}
+     ${Octomap_LIBRARIES}
+ )

tools/planner/visualizePlanner.cpp

@@ -1,21 +1,25 @@
-#include <opencv2/core/core.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc.hpp>
-
-#include <rapidjson/document.h>
-
-#include <zcm/zcm-cpp.hpp>
-
 #include <common/utils/GetOpt.hpp>
-
 #include <Eigen/Core>
-
 #include <fstream>
 #include <iostream>
+#include <memory>
+#include <octomap/octomap.h>
+#include <octomap/OcTree.h>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include <rapidjson/document.h>
 #include <string>
 #include <streambuf>
-#include <vector>
 #include <utility>
+#include <vector>
+#include <zcm/zcm-cpp.hpp>
+
+#include "gnc/planner/Astar.hpp"
+#include "gnc/planner/Path.hpp"
+#include "gnc/Planner.hpp"
+#include "gnc/measurements/Waypoint.hpp"
+#include "gnc/State.hpp"
 
 using std::string;
 using std::ifstream;
@@ -24,6 +28,11 @@ using std::endl;
 using std::cerr;
 using std::vector;
 using std::pair;
+using std::shared_ptr;
+using std::make_shared;
+
+using octomap::OcTree;
+using octomap::point3d;
 
 using rapidjson::Document;
 using rapidjson::GenericArray;
@@ -37,50 +46,20 @@ using cv::Point;
 using cv::Scalar;
 using cv::arrowedLine;
 
+using maav::gnc::Path;
+using maav::gnc::Waypoint;
+using maav::gnc::State;
+using maav::gnc::Planner;
+
 // The number of columns and rows in the Mat used for rendering
 constexpr int MAT_WIDTH = 1920;
 constexpr int MAT_HEIGHT = 1080;
 
-// Used to extract the pairs of 3d points that represent the walls from the JSON dom
-vector<pair<Vector3d, Vector3d> > extractWalls(Document& doc)
-{
-    GenericArray walls = doc["walls"].GetArray();
-    vector<pair<Vector3d, Vector3d> > eigen_walls;
-    eigen_walls.reserve(walls.Size());
-    for (auto& wall : walls)
-    {
-        GenericArray point1 = wall[0].GetArray();
-        GenericArray point2 = wall[1].GetArray();
-        eigen_walls.emplace_back(
-            Vector3d(point1[0].GetDouble(), point1[1].GetDouble(), point1[2].GetDouble()),
-            Vector3d(point2[0].GetDouble(), point2[1].GetDouble(), point2[2].GetDouble())
-        );
-    }
-    return eigen_walls;
-}
-
-<<<<<<< 01c184ca8945e647efa6c842e54f320e2303bbdd
-// TODO Just put all of rywunder's code here
-=======
-// Used for testing, extracts the path from the map spec
-vector<Vector3d> extractPath(Document& doc)
-{
-    GenericArray path = doc["path"].GetArray();
-    vector<Vector3d> extracted;
-    extracted.reserve(path.Size());
-    for (auto& point : path) 
-    {
-        extracted.emplace_back(point[0].GetDouble(), point[1].GetDouble(), point[2].GetDouble());
-    }
-    return extracted;
-}
->>>>>>> Wrote planner path and obstacle visualizer.
+shared_ptr<OcTree> createOctomap(Document& blueprint);
+vector<Vector3d> extractPath(Document& doc);
+vector<pair<Vector3d, Vector3d> > extractWalls(Document& doc);
+void renderWall(Mat& arena, pair<Vector3d, Vector3d>& wall);
 
-void renderWall(Mat& arena, pair<Vector3d, Vector3d>& wall)
-{
-    rectangle(arena, cv::Point2d(wall.first[1], wall.first[0]), cv::Point2d(wall.second[1], 
-        wall.second[0]), cv::Scalar(250), CV_FILLED);
-}
 
 int main(int argc, char** argv) {
     GetOpt gopt;
@@ -109,9 +88,30 @@ int main(int argc, char** argv) {
     Document doc;
     doc.Parse(map_spec.c_str());
 
-    vector<pair<Vector3d, Vector3d>> eigen_walls = extractWalls(doc);
+    // create an octomap from the config specifications
+    shared_ptr<OcTree> tree = createOctomap(doc);
+    cout << "octree created" << endl;
+    // generate a path with astar
+    Planner planner(""); //TODO add config for planner
+    planner.update_target(Waypoint(Vector3d(4,4,-1), Vector3d(0,0,0), 0));
+    State initial = State::zero(0);
+    initial.position() = Vector3d(-4,-4,-1);
+    planner.update_state(initial);
+    planner.update_map(tree);
+    // run A*
+    Path p = planner.get_path();
+    planner.print_path(p);
 
-    cout << eigen_walls.size() << endl;
+    vector<Vector3d> path;
+    std::transform(p.waypoints.cbegin(), p.waypoints.cend(), std::back_inserter(path), 
+        [](const Waypoint& w) {
+        Vector3d pos = w.position;
+        return pos;
+    });
+
+
+    // create renderings
+    vector<pair<Vector3d, Vector3d>> eigen_walls = extractWalls(doc);
 
     // Every point is multiplied by this scaling such that the space of the
     // rendering is most effectively used while not allowing the arena to bleed outside the frame
@@ -156,8 +156,9 @@ int main(int argc, char** argv) {
         renderWall(arena, wall);
     }
 
-    // For testing purposes read in a path from the config
-    vector<Vector3d> path = extractPath(doc);
+    // // For testing purposes read in a path from the config
+    // vector<Vector3d> path = extractPath(doc);
+
     // Adjust the path astar spit out with the scaling and offset
     for (auto& point : path)
     {
@@ -187,9 +188,94 @@ int main(int argc, char** argv) {
         Vector3d& pt2 = path[i];
         arrowedLine(arena, Point(pt1[1], pt1[0]), Point(pt2[1], pt2[0]), Scalar(200, 100, 200), 3);
     }
+
     // Display the end result
     cv::namedWindow("A* Test Rendering", cv::WINDOW_AUTOSIZE);
     imshow("A* Test Rendering", arena);
     cv::waitKey(0);
 
+}
+
+// helper function for vector to point3d
+point3d VecToPoint3d(const Vector3d& v)
+{
+    return point3d(v[0], v[1], v[2]);
+}
+
+shared_ptr<OcTree> createOctomap(Document& blueprint)
+{   
+    double resolution = blueprint["resolution"].GetDouble();
+    GenericArray arena_size = blueprint["arena-size"].GetArray();
+    auto tree = make_shared<OcTree>(OcTree(resolution));
+    std::vector<double> dims = {arena_size[0].GetDouble(), arena_size[1].GetDouble(), 
+                                arena_size[2].GetDouble()};
+    // insert measurements of 'free' environmenet
+    for (float x=-dims[0]/2; x < dims[0]/2; x+=resolution ) {
+        for (float y=-dims[1]/2; y < dims[1]/2; y+=resolution) {
+            for (float z=0; z > dims[2]; z-=resolution) {
+                point3d endpoint ((float) x, (float) y, (float) z);
+                tree->updateNode(endpoint, false);  
+            }
+        }
+    }
+
+    // adds the object to the tree using a bbx_iterator
+    auto addObject = [&tree](const point3d& min, const point3d& max)
+    {
+        for(OcTree::leaf_bbx_iterator it = tree->begin_leafs_bbx(min,max),
+           end=tree->end_leafs_bbx(); it!= end; ++it)
+        {
+          tree->updateNode(it.getCoordinate(), true);
+        }
+    };
+
+    // create the floor
+    addObject(point3d(-dims[0]/2, -dims[1]/2, 0), 
+              point3d(-dims[0]/2, -dims[1]/2, dims[2]));
+
+    // create the walls
+    vector<pair<Vector3d, Vector3d>> walls = extractWalls(blueprint);
+    for(auto &wall: walls)
+    {
+        addObject(VecToPoint3d(wall.first), VecToPoint3d(wall.second));
+    }
+
+    return tree;
+}
+
+// Used for testing, extracts the path from the map spec
+vector<Vector3d> extractPath(Document& doc)
+{
+    GenericArray path = doc["path"].GetArray();
+    vector<Vector3d> extracted;
+    extracted.reserve(path.Size());
+    for (auto& point : path) 
+    {
+        extracted.emplace_back(point[0].GetDouble(), point[1].GetDouble(), point[2].GetDouble());
+    }
+    return extracted;
+}
+
+// Used to extract the pairs of 3d points that represent the walls from the JSON dom
+vector<pair<Vector3d, Vector3d> > extractWalls(Document& doc)
+{
+    GenericArray walls = doc["walls"].GetArray();
+    vector<pair<Vector3d, Vector3d> > eigen_walls;
+    eigen_walls.reserve(walls.Size());
+    for (auto& wall : walls)
+    {
+        GenericArray point1 = wall[0].GetArray();
+        GenericArray point2 = wall[1].GetArray();
+        eigen_walls.emplace_back(
+            Vector3d(point1[0].GetDouble(), point1[1].GetDouble(), point1[2].GetDouble()),
+            Vector3d(point2[0].GetDouble(), point2[1].GetDouble(), point2[2].GetDouble())
+        );
+    }
+    return eigen_walls;
+}
+
+void renderWall(Mat& arena, pair<Vector3d, Vector3d>& wall)
+{
+    rectangle(arena, cv::Point2d(wall.first[1], wall.first[0]), cv::Point2d(wall.second[1], 
+        wall.second[0]), cv::Scalar(250), CV_FILLED);
 }