Added reconfigurable parameter to activate / deactivate the low obstacle detection algorithm

cfg/GroundSegmentation.cfg

@@ -79,6 +79,7 @@ gen.add("classify_not_labeled_points_as_obstacles", bool_t, 0, "when a point has
 gen.add("ground_threshold_in_not_analyzed_areas", double_t, 0, "when it is not possible to make a local analysis of the data, we will use the lowest point (the one in elevation_cloud) as ground height, and all the points above it and below this threshold will be classified as ground", 0.3, 0.01, 1.0);
 
 # Low obstacle detection parameters
+gen.add("use_low_obstacle_detection", bool_t, 0, "adds a low obstacle detection algorithm", False);
 gen.add("low_obstacle_min_height", double_t, 0, "obstacle below this height are not considered", 0.05, 0.001, 0.3);
 gen.add("sensor_best_resolution_deg", double_t, 0, "Either vertical or horizontal, used to calculate distances at which low obstacles can be detected", 0.333, 0.01, 10.0);
 gen.add("max_navigation_slope", double_t, 0, "slopes greater than this threshold will be considered obstacles", 0.2, 0.01, 1.0);

include/ground_segmentation_alg_node.h

@@ -65,8 +65,11 @@ class GroundSegmentationAlgNode : public algorithm_base::IriBaseAlgorithm<Ground
 {
 private:
   std_msgs::Header common_header_;
+
   bool flag_new_lidar_data_;
   bool flag_new_ground_truth_lidar_data_;
+  bool flag_use_low_obstacle_detection_;
+
   ros::Publisher lidar_points_ground_segmented_publisher_;
   sensor_msgs::PointCloud2 lidar_points_ground_segmented_ros_cloud_;
 

params/iri_ground_segmentation.yaml

@@ -36,7 +36,7 @@ iri_ground_segmentation: {
                                     ## and a big value will increase the number of false ground points)
 
   # Neural Network related parameters
-  use_neural_network: true,
+  use_neural_network: false,
   extract_data_to_external_training_of_the_network: false,
   dataset_filename_with_global_path: '/home/idelpino/Documentos/dataset.csv', # This is the filename with path where the data for neural net training is dumped
   neural_net_filename_with_global_path: '/home/idelpino/iri-lab/ground_segmentation_ws/src/iri_ground_segmentation/shallow_neural_nets/veg_terrain_and_obs_13_features_39_neurons.csv',
@@ -52,6 +52,7 @@ iri_ground_segmentation: {
                                                          ## all the points above it and below this threshold will be classified as ground 
                            
   # low obstacle detection parameters
+  use_low_obstacle_detection: false,
   low_obstacle_min_height: 0.05,
   sensor_best_resolution_deg: 0.2, # Best resolution, either horizontal or vertical
   max_navigation_slope: 0.5,                           

src/ground_segmentation_alg_node.cpp

@@ -229,6 +229,13 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
   else
     this->alg_.filtering_configuration_.show_dense_reconstruction = config_.show_dense_reconstruction;
 
+  if (!this->private_node_handle_.getParam("use_low_obstacle_detection", this->config_.use_low_obstacle_detection))
+  {
+    ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'use_low_obstacle_detection' not found");
+  }
+  else
+    this->flag_use_low_obstacle_detection_ = config_.use_low_obstacle_detection;
+
   if (!this->private_node_handle_.getParam("low_obstacle_min_height", this->config_.low_obstacle_min_height))
   {
     ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'low_obstacle_min_height' not found");
@@ -751,9 +758,12 @@ void GroundSegmentationAlgNode::mainNodeThread(void)
     this->alg_.segmentGround(common_header_, lidar_pcl_cloud_, roadmap_vertices_pcl_cloud_, roadmap_edges_array_,
                              ground_dense_reconstruction_pcl_cloud_, roadmap_MarkerArray_msg_);
 
-    this->detectLowObstacles(this->alg_.filtering_configuration_.low_obstacle_min_height,
-                             this->alg_.filtering_configuration_.sensor_best_resolution_deg,
-                             this->alg_.filtering_configuration_.max_navigation_slope);
+    if (this->flag_use_low_obstacle_detection_)
+    {
+      this->detectLowObstacles(this->alg_.filtering_configuration_.low_obstacle_min_height,
+                               this->alg_.filtering_configuration_.sensor_best_resolution_deg,
+                               this->alg_.filtering_configuration_.max_navigation_slope);
+    }
 
     //std::cout << "Ground segmentation finished!" << std::endl;
     this->alg_.unlock();
@@ -839,11 +849,6 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
   // threshold for outlier rejection and classification
   this->alg_.filtering_configuration_.mahalanobis_threshold = config.mahalanobis_threshold;
 
-  // Neural Network parameters
-  this->alg_.filtering_configuration_.use_neural_network = config.use_neural_network;
-  this->alg_.filtering_configuration_.extract_data_to_external_training_of_the_network =
-      config.extract_data_to_external_training_of_the_network;
-
   // labeling parameters
   this->alg_.filtering_configuration_.score_threshold = config_.score_threshold;
   this->alg_.filtering_configuration_.max_pred_std_dev_for_labelling = config.max_pred_std_dev_for_labelling;
@@ -851,6 +856,7 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
       config_.classify_not_labeled_points_as_obstacles;
 
   // Low obstacles detection parameters
+  this->flag_use_low_obstacle_detection_ = config.use_low_obstacle_detection;
   this->alg_.filtering_configuration_.low_obstacle_min_height = config_.low_obstacle_min_height;
   this->alg_.filtering_configuration_.sensor_best_resolution_deg = config_.sensor_best_resolution_deg;
   this->alg_.filtering_configuration_.max_navigation_slope = config_.max_navigation_slope;
@@ -894,15 +900,12 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
   std::cout << "mahalanobis_threshold       =                   "
       << this->alg_.filtering_configuration_.mahalanobis_threshold << std::endl;
 
-  std::cout << "score_threshold       =                   " << this->alg_.filtering_configuration_.score_threshold
+  std::cout << "score_threshold             =                   " << this->alg_.filtering_configuration_.score_threshold
       << std::endl;
 
-  std::cout << "use_neural_network       =                   " << this->alg_.filtering_configuration_.use_neural_network
+  std::cout << "use_low_obstacle_detection  =                   " << this->flag_use_low_obstacle_detection_
       << std::endl;
 
-  std::cout << "extract_data_to_external_training_of_the_network       =                   "
-      << this->alg_.filtering_configuration_.extract_data_to_external_training_of_the_network << std::endl;
-
   std::cout << "max_pred_std_dev_for_labelling      =                   "
       << this->alg_.filtering_configuration_.max_pred_std_dev_for_labelling << std::endl;
   std::cout << "classify_not_labeled_points_as_obstacles =  "