Added inital tree-based trajectory planning

cone_detection_lidar/README.md

@@ -15,6 +15,19 @@ cd ~/ros2_ws/ && colcon build --symlink-install --packages-select cone_detection
 </details>
 
 
+## Run 
+<details>
+<summary> don't forget to source </summary>
+
+``` bash
+source ~/ros2_ws/install/setup.bash
+```
+</details>
+
+``` bash
+ros2 launch cone_detection_lidar grid_detection.launch.py
+```
+
 ## Remarks
 
 In VS code it is advised to add the following to include path:

cone_detection_lidar/include/cone_detection_lidar/tree.hpp

@@ -0,0 +1,132 @@
+#ifndef TREE_HPP_
+#define TREE_HPP_
+
+#include <cmath>
+#include <vector>
+#include <algorithm>
+#include <functional>
+#include <iostream>
+#include <stack>
+#include "pointcloud_to_grid_core.hpp"
+
+class Node
+{
+public:
+    PointXY pos;
+    float prob = 0.5;
+    std::vector<Node *> children;
+    Node(PointXY pos) : pos(pos) {}
+};
+
+class Tree
+{
+
+public:
+    // create a root node and reserve memory for it
+    Node *root = new Node(PointXY(0.0, 0.0));
+
+    Node *getRoot()
+    {
+        return root;
+    }
+
+    Node *modifyRoot(PointXY pos)
+    {
+        root->pos = pos;
+        return root;
+    }
+
+    Node *modifyNode(Node *node, PointXY pos)
+    {
+        node->pos = pos;
+        return node;
+    }
+
+    Node *addNode(Node *parent, PointXY pos)
+    {
+        Node *newNode = new Node(pos);
+        parent->children.push_back(newNode);
+        return newNode;
+    }
+    // recursive function to visit all nodes in the tree
+    void printTree(Node *node, std::string indent = "")
+    {
+        if (node != nullptr)
+        {
+            if (root == node)
+            {
+                // skip printing root node
+            }
+            else
+            {
+                std::cout << indent << "(" << node->pos.x << ", " << node->pos.y << ")" << std::endl;
+            }
+            for (Node *child : node->children)
+            {
+                Node *father = node;
+                std::cout << "[" << father->pos.x << ", " << father->pos.y << "] ";
+                printTree(child, indent + "->");
+            }
+        }
+    }
+    // alternative way to print (and walk thru) the whole tree (non-recursive)
+    void printTreeAlt(Node *root)
+    {
+        if (root == nullptr)
+        {
+            return;
+        }
+
+        std::stack<Node *> stack;
+        stack.push(root);
+
+        while (!stack.empty())
+        {
+            Node *node = stack.top();
+            stack.pop();
+
+            if (node != root)
+            {
+                std::cout << "(" << node->pos.x << ", " << node->pos.y << ")" << std::endl;
+            }
+
+            for (Node *child : node->children)
+            {
+                std::cout << "[" << node->pos.x << ", " << node->pos.y << "] ";
+                stack.push(child);
+            }
+        }
+    }
+
+    // recursive function to visualize the tree in rviz
+    void markerTree(Node *node, visualization_msgs::msg::Marker &line1_marker, std::string indent = "")
+    {
+        if (node != nullptr)
+        {
+            if (root == node)
+            {
+                // skip root node
+            }
+            else
+            {
+                geometry_msgs::msg::Point p_node;
+                p_node.x = node->pos.x;
+                p_node.y = node->pos.y;
+                p_node.z = 0.0;
+                line1_marker.points.push_back(p_node);
+            }
+            for (Node *child : node->children)
+            {
+                Node *father = node;
+                geometry_msgs::msg::Point p_father;
+                p_father.x = father->pos.x;
+                p_father.y = father->pos.y;
+                p_father.z = 0.0;
+                line1_marker.points.push_back(p_father);
+                markerTree(child, line1_marker, indent + "->");
+            }
+        }
+    }
+};
+
+#endif // TREE_HPP_

cone_detection_lidar/src/grid_trajectory.cpp

@@ -14,6 +14,7 @@
 #include "cone_detection_lidar/pointcloud_to_grid_core.hpp"
 #include "cone_detection_lidar/trajectory.hpp"
 #include "cone_detection_lidar/marker.hpp"
+#include "cone_detection_lidar/tree.hpp"
 // c++
 #include <chrono>
 #include <functional>
@@ -231,14 +232,19 @@ class PointCloudToGrid : public rclcpp::Node
     if (search_iter > 6){
       search_iter = 6;
     }
-    std::vector<PointXY> traj(search_iter + 1);
+    std::vector<PointXY> traj(search_iter + 1); // TODO: remove and use only tree
     double max_true_angle = 0.0;
+    Tree tree;
 
     traj[0] = PointXY(0.0, 0.0);
+    tree.modifyRoot(PointXY(0.0, 0.0));
+    auto node_actual = tree.getRoot();
     double traj_search_start_mid = search_start_mid_deg;
     for(int i = 1; i <= search_iter; i++)
     {
       traj[i] = grid_map.angualar_search(traj[i-1].x, traj[i-1].y, search_range_deg, search_resolution_deg, traj_search_start_mid, search_length, hpoints, max_true_angle);
+      node_actual = tree.addNode(node_actual, traj[i]);
+      // TODO: add alternative paths to tree
       traj_search_start_mid = max_true_angle * 180 / M_PI;
     }
 
@@ -250,11 +256,7 @@ class PointCloudToGrid : public rclcpp::Node
     debug1_marker.header.stamp = this->now();
     line1_marker.header.frame_id = input_msg->header.frame_id;
     line1_marker.header.stamp = this->now();
-    for(int i = 1; i <= search_iter; i++)
-    {
-      add_point_to_line_marker(line1_marker, traj[i-1].x, traj[i-1].y);
-      add_point_to_line_marker(line1_marker, traj[i-0].x, traj[i-0].y);
-    }    
+    tree.markerTree(tree.root, line1_marker); // add tree to line marker, which can be visualized in rviz
     mark_array.markers.push_back(debug1_marker);
     mark_array.markers.push_back(line1_marker);
     intensity_grid->header.stamp = this->now();