Tree-based trajectory update

cone_detection_lidar/include/cone_detection_lidar/pointcloud_to_grid_core.hpp

@@ -10,7 +10,17 @@ class PointXY
   PointXY() : x(0), y(0) {} // Default constructor
   // initialize with x and y
   PointXY(double x_val, double y_val) : x(x_val), y(y_val) {}
+}; 
+// X, Y and orientation
+class PointXYori : public PointXY
+{
+public:
+  double ori;
+  PointXYori() : ori(0) {} // Default constructor
+  // initialize with x and y
+  PointXYori(double x_val, double y_val, double ori_val) : PointXY(x_val, y_val), ori(ori_val) {}
 };
+
 class PointXYZI
 {
 public:

cone_detection_lidar/include/cone_detection_lidar/trajectory.hpp

@@ -7,6 +7,19 @@
 #include <functional>
 #include <iostream>
 
+class SearchConfig
+{
+public:
+    double x_start;
+    double y_start;
+    double search_range_deg;
+    double search_resolution_deg;
+    double search_start_mid_deg;
+    double search_length;
+    // default constructor
+    SearchConfig() : x_start(0), y_start(0), search_range_deg(0), search_resolution_deg(0), search_start_mid_deg(0), search_length(0) {}
+};
+
 // Extending the GridMap class
 class GridMapTrajectory : public GridMap
 {
@@ -77,21 +90,20 @@ class GridMapTrajectory : public GridMap
         return true;
     }
 
-    PointXY angualar_search(double x_start, double y_start, double search_range_deg, double search_resolution_deg, double search_start_mid_deg, double search_length, std::vector<signed char> &hpoints, double &max_true_angle)
+    std::vector<PointXYori> angualar_search(SearchConfig s, std::vector<signed char> &hpoints, double &max_true_angle)
     {
-
-        double search_range = search_range_deg * M_PI / 180;
-        double search_resolution = search_resolution_deg * M_PI / 180;
-        double search_start_mid = search_start_mid_deg * M_PI / 180;
+        double search_range = s.search_range_deg * M_PI / 180;
+        double search_resolution = s.search_resolution_deg * M_PI / 180;
+        double search_start_mid = s.search_start_mid_deg * M_PI / 180;
         int loop_increment = int(search_range / search_resolution);
         std::vector<bool> search_results(loop_increment);
         for (int loop = 0; loop < loop_increment; loop++)
         {
             double search_ang = -0.5 * search_range + double(loop) * search_resolution;
-            search_results[loop] = drawline(hpoints, x_start, y_start, search_start_mid + search_ang, search_length);
+            search_results[loop] = drawline(hpoints, s.x_start, s.y_start, search_start_mid + search_ang, s.search_length);
         }
         // find the longest continous true (free) segment in search_results
-        int max_true = 0;
+        int max_true1 = 0;
         int max_true_start = 0;
         int max_true_end = 0;
         int true_count = 0;
@@ -108,25 +120,26 @@ class GridMapTrajectory : public GridMap
                 {
                     true_end = loop - 1;
                 }
-                if (true_count > max_true)
+                if (true_count > max_true1)
                 {
 
-                    max_true = true_count;
+                    max_true1 = true_count;
                     max_true_end = true_end;
                 }
                 true_count = 0;
             }
         }
         // if everything is true (false else never evaluated)
-        if (true_count > max_true)
+        if (true_count > max_true1)
         {
-            max_true = true_count;
+            max_true1 = true_count;
             max_true_end = loop_increment - 1;
         }
-        max_true_start = max_true_end - max_true + 1;
+        max_true_start = max_true_end - max_true1 + 1;
 
-        // this for loop is only fro visualization
+        // this for loop is only for visualization
         // true and false segments (green and red)
+        /*
         for (int i = 0; i < loop_increment; i++)
         {
             if (search_results[i])
@@ -138,15 +151,19 @@ class GridMapTrajectory : public GridMap
                 hpoints[(cell_num_y / 2 + i - loop_increment / 2) * cell_num_x] = -128;
             }
         }
+        */
 
         // RCLCPP_INFO_STREAM(this->get_logger(), "max_true: " << max_true << " max_true_start: " << max_true_start << " max_true_end: " << max_true_end);
         int max_true_center = (max_true_start + max_true_end) / 2;
         max_true_angle = -0.5 * search_range + search_start_mid + double(max_true_center) * search_resolution;
 
-        double x_end = x_start + search_length * cos(max_true_angle);
-        double y_end = y_start + search_length * sin(max_true_angle);
-        PointXY p_end = PointXY(x_end, y_end);
-        return p_end;
+        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);
+        std::vector<PointXYori> p_end_vector;
+        p_end_vector.push_back(p_end);
+        // TODO: return the second longest segment center as well
+        return p_end_vector;
     }
 };
 

cone_detection_lidar/src/grid_trajectory.cpp

@@ -229,28 +229,48 @@ class PointCloudToGrid : public rclcpp::Node
         }
       }
     }
-    if (search_iter > 6){
+    if (search_iter > 6)
+    {
       search_iter = 6;
     }
-    std::vector<PointXY> traj(search_iter + 1); // TODO: remove and use only tree
-    double max_true_angle = 0.0;
+    std::vector<PointXYori> traj_end_points(1);
+    double traj_search_angle = 0.0;
     Tree tree;
 
-    traj[0] = PointXY(0.0, 0.0);
+    traj_end_points[0] = PointXYori(0.0, 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++)
+    SearchConfig config;
+    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;
+      for (size_t i = 0; i < traj_end_points.size(); ++i)
+      {
+        config.x_start = traj_end_points[i].x;
+        config.y_start = traj_end_points[i].y;
+        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)
+        {
+          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
+          }
+        }
+      }
     }
 
     visualization_msgs::msg::MarkerArray mark_array;
     visualization_msgs::msg::Marker debug1_marker, line1_marker;
-    init_debug_marker(debug1_marker, traj[0].x, traj[0].y, 1);
+    init_debug_marker(debug1_marker, traj_end_points[0].x, traj_end_points[0].y, 1);
     init_line_marker(line1_marker, 2);
     debug1_marker.header.frame_id = input_msg->header.frame_id;
     debug1_marker.header.stamp = this->now();