Tree-based trajectory search PointXYori isntead of PointXY

horverno · horverno · commit 39eb13e6fe73 · 2024-03-12T17:18:39.000+01:00
diff --git a/cone_detection_lidar/include/cone_detection_lidar/trajectory.hpp b/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;
     }
 };
diff --git a/cone_detection_lidar/include/cone_detection_lidar/tree.hpp b/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;
diff --git a/cone_detection_lidar/src/grid_trajectory.cpp b/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();

Original file line number	Diff line number	Diff line change
`@@ -9,47 +9,47 @@`
`9`	`9`	`#include <stack>`
`10`	`10`	`#include "pointcloud_to_grid_core.hpp"`
`11`	`11`
`12`		`-class Node`
	`12`	`+class TreeNode`
`13`	`13`	`{`
`14`	`14`	`public:`
`15`		`- PointXY pos;`
	`15`	`+ PointXYori pos;`
`16`	`16`	`float prob = 0.5;`
`17`		`- std::vector<Node *> children;`
`18`		`- Node(PointXY pos) : pos(pos) {}`
	`17`	`+ std::vector<TreeNode *> children;`
	`18`	`+ TreeNode(PointXYori pos) : pos(pos) {}`
`19`	`19`	`};`
`20`	`20`
`21`	`21`	`class Tree`
`22`	`22`	`{`
`23`	`23`
`24`	`24`	`public:`
`25`	`25`	`// create a root node and reserve memory for it`
`26`		`- Node *root = new Node(PointXY(0.0, 0.0));`
	`26`	`+ TreeNode *root = new TreeNode(PointXYori(0.0, 0.0, 0.0));`
`27`	`27`
`28`		`- Node *getRoot()`
	`28`	`+ TreeNode *getRoot()`
`29`	`29`	`{`
`30`	`30`	`return root;`
`31`	`31`	`}`
`32`	`32`
`33`		`- Node *modifyRoot(PointXY pos)`
	`33`	`+ TreeNode *modifyRoot(PointXYori pos)`
`34`	`34`	`{`
`35`	`35`	`root->pos = pos;`
`36`	`36`	`return root;`
`37`	`37`	`}`
`38`	`38`
`39`		`- Node modifyNode(Node node, PointXY pos)`
	`39`	`+ TreeNode modifyNode(TreeNode node, PointXYori pos)`
`40`	`40`	`{`
`41`	`41`	`node->pos = pos;`
`42`	`42`	`return node;`
`43`	`43`	`}`
`44`	`44`
`45`		`- Node addNode(Node parent, PointXY pos)`
	`45`	`+ TreeNode addNode(TreeNode parent, PointXYori pos)`
`46`	`46`	`{`
`47`		`- Node *newNode = new Node(pos);`
	`47`	`+ TreeNode *newNode = new TreeNode(pos);`
`48`	`48`	`parent->children.push_back(newNode);`
`49`	`49`	`return newNode;`
`50`	`50`	`}`
`51`	`51`	`// recursive function to visit all nodes in the tree`
`52`		`- void printTree(Node *node, std::string indent = "")`
	`52`	`+ void printTree(TreeNode *node, std::string indent = "")`
`53`	`53`	`{`
`54`	`54`	`if (node != nullptr)`
`55`	`55`	`{`
`@@ -61,36 +61,60 @@ class Tree`
`61`	`61`	`{`
`62`	`62`	`std::cout << indent << "(" << node->pos.x << ", " << node->pos.y << ")" << std::endl;`
`63`	`63`	`}`
`64`		`- for (Node *child : node->children)`
	`64`	`+ for (TreeNode *child : node->children)`
`65`	`65`	`{`
`66`		`- Node *father = node;`
	`66`	`+ TreeNode *father = node;`
`67`	`67`	`std::cout << "[" << father->pos.x << ", " << father->pos.y << "] ";`
`68`	`68`	`printTree(child, indent + "->");`
`69`	`69`	`}`
`70`	`70`	`}`
`71`	`71`	`}`
	`72`	`+ // a recursive function to return all the leaves of the tree as a node`
	`73`	`+ std::vector<TreeNode> getLeaves(TreeNode node)`
	`74`	`+ {`
	`75`	`+ std::vector<TreeNode*> leaves;`
	`76`	`+`
	`77`	`+ if (node != nullptr)`
	`78`	`+ {`
	`79`	`+ if (node->children.empty())`
	`80`	`+ {`
	`81`	`+ leaves.push_back(node);`
	`82`	`+ }`
	`83`	`+ else`
	`84`	`+ {`
	`85`	`+ for (TreeNode* child : node->children)`
	`86`	`+ {`
	`87`	`+ std::vector<TreeNode*> childLeaves = getLeaves(child);`
	`88`	`+ leaves.insert(leaves.end(), childLeaves.begin(), childLeaves.end());`
	`89`	`+ }`
	`90`	`+ }`
	`91`	`+ }`
	`92`	`+`
	`93`	`+ return leaves;`
	`94`	`+ }`
	`95`	`+`
`72`	`96`	`// alternative way to print (and walk thru) the whole tree (non-recursive)`
`73`		`- void printTreeAlt(Node *root)`
	`97`	`+ void printTreeAlt(TreeNode *root)`
`74`	`98`	`{`
`75`	`99`	`if (root == nullptr)`
`76`	`100`	`{`
`77`	`101`	`return;`
`78`	`102`	`}`
`79`	`103`
`80`		`- std::stack<Node *> stack;`
	`104`	`+ std::stack<TreeNode *> stack;`
`81`	`105`	`stack.push(root);`
`82`	`106`
`83`	`107`	`while (!stack.empty())`
`84`	`108`	`{`
`85`		`- Node *node = stack.top();`
	`109`	`+ TreeNode *node = stack.top();`
`86`	`110`	`stack.pop();`
`87`	`111`
`88`	`112`	`if (node != root)`
`89`	`113`	`{`
`90`	`114`	`std::cout << "(" << node->pos.x << ", " << node->pos.y << ")" << std::endl;`
`91`	`115`	`}`
`92`	`116`
`93`		`- for (Node *child : node->children)`
	`117`	`+ for (TreeNode *child : node->children)`
`94`	`118`	`{`
`95`	`119`	`std::cout << "[" << node->pos.x << ", " << node->pos.y << "] ";`
`96`	`120`	`stack.push(child);`
`@@ -99,7 +123,7 @@ class Tree`
`99`	`123`	`}`
`100`	`124`
`101`	`125`	`// recursive function to visualize the tree in rviz`
`102`		`- void markerTree(Node *node, visualization_msgs::msg::Marker &line1_marker, std::string indent = "")`
	`126`	`+ void markerTree(TreeNode *node, visualization_msgs::msg::Marker &line1_marker, std::string indent = "")`
`103`	`127`	`{`
`104`	`128`	`if (node != nullptr)`
`105`	`129`	`{`
`@@ -115,9 +139,9 @@ class Tree`
`115`	`139`	`p_node.z = 0.0;`
`116`	`140`	`line1_marker.points.push_back(p_node);`
`117`	`141`	`}`
`118`		`- for (Node *child : node->children)`
	`142`	`+ for (TreeNode *child : node->children)`
`119`	`143`	`{`
`120`		`- Node *father = node;`
	`144`	`+ TreeNode *father = node;`
`121`	`145`	`geometry_msgs::msg::Point p_father;`
`122`	`146`	`p_father.x = father->pos.x;`
`123`	`147`	`p_father.y = father->pos.y;`