diff --git a/cfg/GroundSegmentation.cfg b/cfg/GroundSegmentation.cfg
index c9eceaa10be223ce6b6abf9f44cf7a8eab3ecb96..a65ac071587891c1cfb2fbe74ebb8098f88c4735 100644
--- a/cfg/GroundSegmentation.cfg
+++ b/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);
diff --git a/include/ground_segmentation_alg_node.h b/include/ground_segmentation_alg_node.h
index 9c1c12e27168c565de94cb477ed3d3f9bfa7014e..b18dd8cdf96953152bb123aabe29c9aac280185e 100644
--- a/include/ground_segmentation_alg_node.h
+++ b/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_;
diff --git a/params/iri_ground_segmentation.yaml b/params/iri_ground_segmentation.yaml
index 083bfd61689222c7f0d61fa6068848c73817ad23..b74d51e44f588bbf6e57aab5c877d109e2dbb162 100644
--- a/params/iri_ground_segmentation.yaml
+++ b/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,
diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp
index 403b9c9235247318f6e1c51de0dbfcf86a52f757..514aaa0366a8fbe643305197a647e376880995f2 100644
--- a/src/ground_segmentation_alg_node.cpp
+++ b/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 = "