Tree-based trajectory search PointXYori isntead of PointXY

cone_detection_lidar/include/cone_detection_lidar/trajectory.hpp

@@ -6,6 +6,7 @@
 #include <algorithm>
 #include <functional>
 #include <iostream>
+// #include "rclcpp/rclcpp.hpp"
 
 class SearchConfig
 {
@@ -90,7 +91,7 @@ class GridMapTrajectory : public GridMap
         return true;
     }
 
-    std::vector<PointXYori> angualar_search(SearchConfig s, std::vector<signed char> &hpoints, double &max_true_angle)
+    std::vector<PointXYori> angular_search(SearchConfig s, std::vector<signed char> &hpoints, double &max_true_angle)
     {
         double search_range = s.search_range_deg * M_PI / 180;
         double search_resolution = s.search_resolution_deg * M_PI / 180;
@@ -159,10 +160,17 @@ class GridMapTrajectory : public GridMap
 
         double x_end = s.x_start + s.search_length * cos(max_true_angle);
         double y_end = s.y_start + s.search_length * sin(max_true_angle);
-        PointXYori p_end = PointXYori(x_end, y_end, max_true_angle);
+        PointXYori p_end = PointXYori(x_end, y_end, max_true_angle * 180 / M_PI);
         std::vector<PointXYori> p_end_vector;
         p_end_vector.push_back(p_end);
         // TODO: return the second longest segment center as well
+        // TODO: remove this test:
+        // if (max_true_end - max_true_start < 90){
+        //     double r_x_end = s.search_length * cos(max_true_angle);
+        //     double r_y_end = s.search_length * sin(max_true_angle);
+        //     PointXYori r_end = PointXYori(r_x_end, r_y_end, max_true_start * 180 / M_PI);
+        //     p_end_vector.push_back(r_end);
+        // }
         return p_end_vector;
     }
 };

cone_detection_lidar/include/cone_detection_lidar/tree.hpp

@@ -9,47 +9,47 @@
 #include <stack>
 #include "pointcloud_to_grid_core.hpp"
 
-class Node
+class TreeNode
 {
 public:
-    PointXY pos;
+    PointXYori pos;
     float prob = 0.5;
-    std::vector<Node *> children;
-    Node(PointXY pos) : pos(pos) {}
+    std::vector<TreeNode *> children;
+    TreeNode(PointXYori pos) : pos(pos) {}
 };
 
 class Tree
 {
 
 public:
     // create a root node and reserve memory for it
-    Node *root = new Node(PointXY(0.0, 0.0));
+    TreeNode *root = new TreeNode(PointXYori(0.0, 0.0, 0.0));
 
-    Node *getRoot()
+    TreeNode *getRoot()
     {
         return root;
     }
 
-    Node *modifyRoot(PointXY pos)
+    TreeNode *modifyRoot(PointXYori pos)
     {
         root->pos = pos;
         return root;
     }
 
-    Node *modifyNode(Node *node, PointXY pos)
+    TreeNode *modifyNode(TreeNode *node, PointXYori pos)
     {
         node->pos = pos;
         return node;
     }
 
-    Node *addNode(Node *parent, PointXY pos)
+    TreeNode *addNode(TreeNode *parent, PointXYori pos)
     {
-        Node *newNode = new Node(pos);
+        TreeNode *newNode = new TreeNode(pos);
         parent->children.push_back(newNode);
         return newNode;
     }
     // recursive function to visit all nodes in the tree
-    void printTree(Node *node, std::string indent = "")
+    void printTree(TreeNode *node, std::string indent = "")
     {
         if (node != nullptr)
         {
@@ -61,36 +61,60 @@ class Tree
             {
                 std::cout << indent << "(" << node->pos.x << ", " << node->pos.y << ")" << std::endl;
             }
-            for (Node *child : node->children)
+            for (TreeNode *child : node->children)
             {
-                Node *father = node;
+                TreeNode *father = node;
                 std::cout << "[" << father->pos.x << ", " << father->pos.y << "] ";
                 printTree(child, indent + "->");
             }
         }
     }
+    // a recursive function to return all the leaves of the tree as a node
+    std::vector<TreeNode*> getLeaves(TreeNode* node)
+    {
+        std::vector<TreeNode*> leaves;
+
+        if (node != nullptr)
+        {
+            if (node->children.empty())
+            {
+                leaves.push_back(node);
+            }
+            else
+            {
+                for (TreeNode* child : node->children)
+                {
+                    std::vector<TreeNode*> childLeaves = getLeaves(child);
+                    leaves.insert(leaves.end(), childLeaves.begin(), childLeaves.end());
+                }
+            }
+        }
+
+        return leaves;
+    }
+
     // alternative way to print (and walk thru) the whole tree (non-recursive)
-    void printTreeAlt(Node *root)
+    void printTreeAlt(TreeNode *root)
     {
         if (root == nullptr)
         {
             return;
         }
 
-        std::stack<Node *> stack;
+        std::stack<TreeNode *> stack;
         stack.push(root);
 
         while (!stack.empty())
         {
-            Node *node = stack.top();
+            TreeNode *node = stack.top();
             stack.pop();
 
             if (node != root)
             {
                 std::cout << "(" << node->pos.x << ", " << node->pos.y << ")" << std::endl;
             }
 
-            for (Node *child : node->children)
+            for (TreeNode *child : node->children)
             {
                 std::cout << "[" << node->pos.x << ", " << node->pos.y << "] ";
                 stack.push(child);
@@ -99,7 +123,7 @@ class Tree
     }
 
     // recursive function to visualize the tree in rviz
-    void markerTree(Node *node, visualization_msgs::msg::Marker &line1_marker, std::string indent = "")
+    void markerTree(TreeNode *node, visualization_msgs::msg::Marker &line1_marker, std::string indent = "")
     {
         if (node != nullptr)
         {
@@ -115,9 +139,9 @@ class Tree
                 p_node.z = 0.0;
                 line1_marker.points.push_back(p_node);
             }
-            for (Node *child : node->children)
+            for (TreeNode *child : node->children)
             {
-                Node *father = node;
+                TreeNode *father = node;
                 geometry_msgs::msg::Point p_father;
                 p_father.x = father->pos.x;
                 p_father.y = father->pos.y;

cone_detection_lidar/src/grid_trajectory.cpp

@@ -229,48 +229,42 @@ class PointCloudToGrid : public rclcpp::Node
         }
       }
     }
+    // search_iter is how deep the tree will be (e.g. 6 = 6 levels deep, max 6^2 = 36 nodes in the tree, if every node has 2 children)
     if (search_iter > 6)
     {
       search_iter = 6;
     }
-    std::vector<PointXYori> traj_end_points(1);
+    std::vector<TreeNode *> traj_end_points(1);
     double traj_search_angle = 0.0;
     Tree tree;
-
-    traj_end_points[0] = PointXYori(0.0, 0.0, 0.0);
-    tree.modifyRoot(PointXY(0.0, 0.0));
+    tree.modifyRoot(PointXYori(0.0, 0.0, search_start_mid_deg));
     auto node_actual = tree.getRoot();
-    double traj_search_start_mid = search_start_mid_deg;
     SearchConfig config;
+    // iterate through search_iter -> deepness of the tree 
     for (int i = 1; i <= search_iter; i++)
     {
-      for (size_t i = 0; i < traj_end_points.size(); ++i)
+      // traj_end_points are the leaves of the tree
+      traj_end_points = tree.getLeaves(node_actual);
+      // for every leaf, search for new end points
+      for (size_t t = 0; t < traj_end_points.size(); t++)
       {
-        config.x_start = traj_end_points[i].x;
-        config.y_start = traj_end_points[i].y;
+        config.x_start = traj_end_points[t]->pos.x;
+        config.y_start = traj_end_points[t]->pos.y;
+        config.search_start_mid_deg = traj_end_points[t]->pos.ori;
         config.search_range_deg = search_range_deg;
         config.search_resolution_deg = search_resolution_deg;
-        config.search_start_mid_deg = traj_search_start_mid;
         config.search_length = search_length;
-        std::vector<PointXYori> actual_end_points = grid_map.angualar_search(config, hpoints, traj_search_angle);
-        node_actual = tree.addNode(node_actual, traj_end_points[i]);
-        traj_search_start_mid = traj_search_angle * 180 / M_PI;
-        // TODO: test this logic with multiple actual end points
-        for(size_t j = 0; j < actual_end_points.size(); ++j)
+        std::vector<PointXYori> new_end_points = grid_map.angular_search(config, hpoints, traj_search_angle);
+        // new end points are added to the tree (typically 1 or 2)
+        for (size_t j = 0; j < new_end_points.size(); j++)
         {
-          if (j == 0){
-            traj_end_points[i] = actual_end_points[j]; // replace the current end point with the first actual end point
-          }
-          else{
-            traj_end_points.insert(traj_end_points.begin() + i, actual_end_points[j]); // insert if there are more than one actual end points
-          }
+          tree.addNode(traj_end_points[t], new_end_points[j]);
         }
       }
     }
-
     visualization_msgs::msg::MarkerArray mark_array;
     visualization_msgs::msg::Marker debug1_marker, line1_marker;
-    init_debug_marker(debug1_marker, traj_end_points[0].x, traj_end_points[0].y, 1);
+    init_debug_marker(debug1_marker, traj_end_points[0]->pos.x, traj_end_points[0]->pos.y, 1);
     init_line_marker(line1_marker, 2);
     debug1_marker.header.frame_id = input_msg->header.frame_id;
     debug1_marker.header.stamp = this->now();