Grid trajectory search fixed longest continous segment

Author: horverno

launch/grid_trajectory.launch.py

@@ -16,16 +16,20 @@ def generate_launch_description():
             output='screen',
             parameters=[
                 {'cloud_in_topic': LaunchConfiguration("topic")},
-                {'position_x': -20.0},
-                {'position_y': 0.0},
+                {'position_x': -20.0},  # meters
+                {'position_y': 0.0},  # meters
                 {'verbose1': False},
                 {'verbose2': False},
-                {'cell_size': 0.8},
-                {'length_x': 40.0},
-                {'length_y': 60.0},
+                {'cell_size': 0.2},  # meters
+                {'length_x': 40.0},  # meters
+                {'length_y': 60.0},  # meters
                 #{'frame_out': 'os1_sensor'},
                 {'mapi_topic_name': 'intensity_grid'},
                 {'maph_topic_name': 'height_grid'},
+                {'search_length': 4.0}, # meters
+                {'search_range_deg': 80.0}, # degrees
+                {'search_resolution_deg': 0.5}, # degrees
+                {'search_start_mid_deg': -180.0}, # degrees
             ]
         )
 

src/grid_trajectory.cpp

@@ -99,6 +99,10 @@ class PointCloudToGrid : public rclcpp::Node
       {
         search_start_mid_deg = param.as_double();
       }
+      if (param.get_name() == "search_length")
+      {
+        search_length = param.as_double();
+      }
       // grid_map.frame_out = config.frame_out;
       grid_map.paramRefresh();
     }
@@ -123,6 +127,7 @@ class PointCloudToGrid : public rclcpp::Node
     this->declare_parameter<double>("search_range_deg", search_range_deg);
     this->declare_parameter<double>("search_resolution_deg", search_resolution_deg);
     this->declare_parameter<double>("search_start_mid_deg", search_start_mid_deg);
+    this->declare_parameter<double>("search_length", search_length);
 
     this->get_parameter("mapi_topic_name", grid_map.mapi_topic_name);
     this->get_parameter("maph_topic_name", grid_map.maph_topic_name);
@@ -139,6 +144,7 @@ class PointCloudToGrid : public rclcpp::Node
     this->get_parameter("search_range_deg", search_range_deg);
     this->get_parameter("search_resolution_deg", search_resolution_deg);
     this->get_parameter("search_start_mid_deg", search_start_mid_deg);
+    this->get_parameter("search_length", search_length);
 
     grid_map.paramRefresh();
 
@@ -182,7 +188,7 @@ class PointCloudToGrid : public rclcpp::Node
     // initialize grid vectors: -128
     for (auto &p : hpoints)
     {
-      p = -128;
+      p = -1;
     }
     for (auto &p : ipoints)
     {
@@ -200,7 +206,7 @@ class PointCloudToGrid : public rclcpp::Node
             if (cell.x < grid_map.cell_num_x && cell.y < grid_map.cell_num_y)
             {
               ipoints[cell.y * grid_map.cell_num_x + cell.x] = char(p.intensity * grid_map.intensity_factor);
-              hpoints[cell.y * grid_map.cell_num_x + cell.x] = +99; //char(p.z * grid_map.height_factor);
+              hpoints[cell.y * grid_map.cell_num_x + cell.x] = +99; // char(p.z * grid_map.height_factor);
             }
             else
             {
@@ -218,42 +224,65 @@ class PointCloudToGrid : public rclcpp::Node
     std::vector<bool> search_results(loop_increment);
     double x_start = -0.6;
     double y_start = 0.0;
-    double length = 10.0;
     for (int loop = 0; loop < loop_increment; loop++)
     {
       double search_ang = -0.5 * search_range + double(loop) * search_resolution;
-      search_results[loop] = grid_map.drawline(hpoints, x_start, y_start, search_start_mid + search_ang, length);
+      search_results[loop] = grid_map.drawline(hpoints, x_start, y_start, search_start_mid + search_ang, search_length);
     }
     // find the longest continous true (free) segment in search_results
     int max_true = 0;
     int max_true_start = 0;
     int max_true_end = 0;
-    int true_start = 0;
+    int true_count = 0;
     int true_end = 0;
     for (int loop = 0; loop < loop_increment; loop++)
     {
       if (search_results[loop])
       {
-        true_end = loop;
-        if (true_end - true_start > max_true)
+        true_count += 1;
+      }
+      else
+      {
+        if (loop >= 1 and search_results[loop - 1])
+        {
+          true_end = loop - 1;
+        }
+        if (true_count > max_true)
         {
-          max_true = true_end - true_start;
-          max_true_start = true_start;
+
+          max_true = true_count;
           max_true_end = true_end;
         }
+        true_count = 0;
+      }
+    }
+    // if everything is true (false else never evaluated)
+    if (true_count > max_true){
+        max_true = true_count;
+        max_true_end = loop_increment - 1;
+    }
+    max_true_start = max_true_end - max_true + 1;
+
+    // this for loop is only fro visualization
+    // true and false segments (green and red)
+    for (int i = 0; i < loop_increment; i++)
+    {
+      if (search_results[i])
+      {
+        hpoints[(grid_map.cell_num_y/2 + i - loop_increment / 2) * grid_map.cell_num_x] = 110;
       }
       else
       {
-        true_start = loop;
+        hpoints[(grid_map.cell_num_y/2 + i - loop_increment / 2) * grid_map.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_start)/2;
+    int max_true_center = (max_true_start + max_true_end) / 2;
     double max_true_angle = -0.5 * search_range + search_start_mid + double(max_true_center) * search_resolution;
 
-    double x_end = x_start + length * cos(max_true_angle);
-    double y_end = y_start + length * sin(max_true_angle);
+    double x_end = x_start + search_length * cos(max_true_angle);
+    double y_end = y_start + search_length * sin(max_true_angle);
 
     visualization_msgs::msg::MarkerArray mark_array;
     visualization_msgs::msg::Marker debug1_marker;
@@ -286,7 +315,8 @@ class PointCloudToGrid : public rclcpp::Node
   OnSetParametersCallbackHandle::SharedPtr callback_handle_;
   std::string cloud_in_topic = "nonground";
   bool verbose1 = true, verbose2 = false;
-  double search_range_deg = 120, search_resolution_deg = 10, search_start_mid_deg = -180; 
+  double search_range_deg = 120, search_resolution_deg = 10, search_start_mid_deg = -180;
+  double search_length = 10.0;
   // nav_msgs::msg::OccupancyGrid::Ptr intensity_grid = std::make_shared<nav_msgs::msg::OccupancyGrid>();
   // nav_msgs::msg::OccupancyGrid::Ptr intensity_grid(new nav_msgs::msg::OccupancyGrid);
   // nav_msgs::msg::OccupancyGrid::Ptr height_grid = std::make_shared<nav_msgs::msg::OccupancyGrid>();
@@ -300,4 +330,4 @@ int main(int argc, char **argv)
   rclcpp::spin(std::make_shared<PointCloudToGrid>());
   rclcpp::shutdown();
   return 0;
-}
+}