diff --git a/cfg/GroundSegmentation.cfg b/cfg/GroundSegmentation.cfg
index 3046ecd74bad018534327d76525fdc4cddf092ab..bacd6f6f8d625bf8ee8c8b8416c8b4ae3156d68d 100644
--- a/cfg/GroundSegmentation.cfg
+++ b/cfg/GroundSegmentation.cfg
@@ -52,20 +52,15 @@ gen.add("elevation_grid_resolution", double_t, 0, "define the coarseness of the
gen.add("angle_reduction_factor", double_t, 0, "To make the search area dependent to the data sparseness", 2.0, -100.0, 100.0);
gen.add("search_limit_in_shadow_area", double_t, 0, "minimum ROI area = 4*search_limit_in_shadow_area squared", 3.0, 0.0, 1000.0);
-gen.add("distance_to_virtual_references_in_shadow_area", double_t, 0, "distance from origin to the virtual ground reference points created under the shadow area", 2.75, 0.0, 1000.0);
gen.add("propagation_z_additive_noise_per_meter", double_t, 0, "Noise added to new ground references scaled by the range", 0.1, 0.0, 1000.0);
gen.add("propagation_additive_noise_deg_per_meter", double_t, 0, "Noise added to new ground references scaled by the range", 0.1, 0.0, 1000.0);
gen.add("z_observation_std_dev", double_t, 0, "Measurement std_dev", 0.1, 0.0, 1000.0);
-gen.add("robot_width", double_t, 0, "used to discard edges", 1.05, 0.0, 3.0);
gen.add("robot_height", double_t, 0, "used to discard edges", 1.20, 0.0, 3.0);
-gen.add("safety_margin", double_t, 0, "used to discard edges", 0.15, 0.0, 1.0);
gen.add("max_traversable_obstacle_height", double_t, 0, "used to discard edges", 0.15, 0.01, 0.99);
gen.add("max_slope_abs", double_t, 0, "used to discard edges", 0.3, 0.01, 0.99);
-gen.add("flag_generate_discontinuity_obstacles_for_real_obstacles", bool_t, 0, "used to display obstacles perceived", True);
-gen.add("flag_connect_vertices_even_if_there_are_obstacles", bool_t, 0, "used to avoid discarding edges", False);
gen.add("measure_performance", bool_t, 0, "to measure times of inner functions", False);
gen.add("show_dense_reconstruction", bool_t, 0, "creates dense pointcloud with ground predictions", False);
diff --git a/include/ground_segmentation_alg.h b/include/ground_segmentation_alg.h
index da73383be2be29b15bb7a78a1f301183257e79f5..332add5fefd4ce87f91a212c4c9dca66d0c5a70d 100644
--- a/include/ground_segmentation_alg.h
+++ b/include/ground_segmentation_alg.h
@@ -139,7 +139,7 @@ public:
*
* \param level level in which the update is taken place
*/
- void config_update(Config& config, uint32_t level = 0);
+ void config_update(Config &config, uint32_t level = 0);
// here define all two_segmented_inputs_vehicle_detector_alg interface methods to retrieve and set
// the driver parameters
@@ -154,10 +154,9 @@ public:
void showPerformanceStatistics(void);
- void segmentGround(pcl::PointCloud<pcl::PointXYZRGBNormal>& velodyne_pcl_cloud,
- pcl::PointCloud<pcl::PointXYZRGBNormal>& ground_references_pcl_cloud,
- visualization_msgs::MarkerArray& local_goals, iri_ground_segmentation::edges_array& edges_array,
- std_msgs::Header common_header);
+ void segmentGround(const std_msgs::Header common_header, pcl::PointCloud<pcl::PointXYZRGBNormal> &lidar_pcl_cloud,
+ pcl::PointCloud<pcl::PointXYZRGBNormal> &roadmap_vertices_pcl_cloud, iri_ground_segmentation::edges_array &edges_array,
+ pcl::PointCloud<pcl::PointXYZRGBNormal> &ground_dense_reconstruction_pcl_cloud, visualization_msgs::MarkerArray &roadmap_markers);
};
#endif
diff --git a/include/ground_segmentation_alg_node.h b/include/ground_segmentation_alg_node.h
index 365966ebd2fb028afa9ec2855a1a4bf0da3931b9..66ce27be4db47475aa0348c4e5107aaaf3fc2983 100644
--- a/include/ground_segmentation_alg_node.h
+++ b/include/ground_segmentation_alg_node.h
@@ -50,9 +50,11 @@ private:
ros::Publisher roadmap_markers_publisher_;
visualization_msgs::MarkerArray roadmap_MarkerArray_msg_;
- ros::Publisher roadmap_nodes_publisher_;
- sensor_msgs::PointCloud2 roadmap_nodes_ros_cloud_;
- pcl::PointCloud<pcl::PointXYZRGBNormal> roadmap_nodes_pcl_cloud_;
+ ros::Publisher ground_dense_reconstruction_publisher_;
+ sensor_msgs::PointCloud2 ground_dense_reconstruction_ros_cloud_;
+ pcl::PointCloud<pcl::PointXYZRGBNormal> ground_dense_reconstruction_pcl_cloud_;
+
+ pcl::PointCloud<pcl::PointXYZRGBNormal> roadmap_vertices_pcl_cloud_;
ros::Publisher roadmap_edges_publisher_;
iri_ground_segmentation::edges_array roadmap_edges_array_;
@@ -60,7 +62,6 @@ private:
ros::Publisher roadmap_vertices_publisher_;
iri_ground_segmentation::vertices_array roadmap_vertices_array_;
- // Publishers for integration with navigation stack
ros::Publisher obstacle_points_publisher_;
sensor_msgs::PointCloud2 obstacle_points_ros_cloud_;
pcl::PointCloud<pcl::PointXYZ> obstacle_points_pcl_cloud_;
@@ -69,21 +70,11 @@ private:
sensor_msgs::PointCloud2 ground_points_ros_cloud_;
pcl::PointCloud<pcl::PointXYZ> ground_points_pcl_cloud_;
- ros::Publisher dicontinuity_obstacles_publisher_;
- sensor_msgs::PointCloud2 dicontinuity_obstacles_ros_cloud_;
- pcl::PointCloud<pcl::PointXYZ> dicontinuity_obstacles_pcl_cloud_;
-
- ros::Publisher exploration_frontier_points_publisher_;
- sensor_msgs::PointCloud2 exploration_frontier_points_ros_cloud_;
- pcl::PointCloud<pcl::PointXYZ> exploration_frontier_points_pcl_cloud_;
-
// [subscriber attributes]
// Variables for copying the callbacks inputs
sensor_msgs::PointCloud2 lidar_ros_cloud_;
pcl::PointCloud<pcl::PointXYZRGBNormal> lidar_pcl_cloud_;
-
-
ros::Subscriber lidar_subscriber_;
void cb_lidar(const sensor_msgs::PointCloud2::ConstPtr &msg);
diff --git a/src/ground_segmentation_alg.cpp b/src/ground_segmentation_alg.cpp
index e8edffc24ac36e95786ef03d121d00b3cd58befc..7db306481f4678567b125e8bd86d6a92c3df07ef 100644
--- a/src/ground_segmentation_alg.cpp
+++ b/src/ground_segmentation_alg.cpp
@@ -29,11 +29,12 @@ void GroundSegmentationAlgorithm::showPerformanceStatistics(void)
performance_supervisor_ptr_->printStatistics();
}
-void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGBNormal> &velodyne_pcl_cloud,
- pcl::PointCloud<pcl::PointXYZRGBNormal> &ground_references_pcl_cloud,
- visualization_msgs::MarkerArray &local_goals,
- iri_ground_segmentation::edges_array &edges_array,
- std_msgs::Header common_header)
+void GroundSegmentationAlgorithm::segmentGround(
+ const std_msgs::Header common_header, pcl::PointCloud<pcl::PointXYZRGBNormal> &lidar_pcl_cloud,
+ pcl::PointCloud<pcl::PointXYZRGBNormal> &roadmap_vertices_pcl_cloud,
+ iri_ground_segmentation::edges_array &edges_array,
+ pcl::PointCloud<pcl::PointXYZRGBNormal> &ground_dense_reconstruction_pcl_cloud,
+ visualization_msgs::MarkerArray &roadmap_markers)
{
CFunctionMonitor performance_monitor("segmentGround", performance_supervisor_ptr_);
std::vector<std::vector<int>> edges(100000);
@@ -41,19 +42,23 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
//std::cout << "Calling to processor_.groundSegmentation!" << std::endl;
- processor_.groundSegmentation(lidar_configuration_, filtering_configuration_, velodyne_pcl_cloud,
- ground_references_pcl_cloud, edges, discarded_edges);
+ processor_.groundSegmentation(lidar_configuration_, filtering_configuration_, lidar_pcl_cloud,
+ roadmap_vertices_pcl_cloud, edges, discarded_edges,
+ ground_dense_reconstruction_pcl_cloud);
- //std::cout << "Number of vertices = " << ground_references_pcl_cloud.points.size();
+ //std::cout << "Number of vertices = " << roadmap_vertices_pcl_cloud.points.size();
//std::cout << "Number of edge vectors = " << edges.size();
+ // Now we create the markers for visualization and change the format of edges to implement the interface
+ // between the library and the node
+
int vertex_id = 0;
int discarded_vertex_id = 0;
int edge_id = 0;
int discarded_edge_id = 0;
- for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = ground_references_pcl_cloud.begin();
- i != ground_references_pcl_cloud.end(); ++i)
+ for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = roadmap_vertices_pcl_cloud.begin();
+ i != roadmap_vertices_pcl_cloud.end(); ++i)
{
pcl::PointXYZRGBNormal ground_ref = *i;
@@ -87,7 +92,7 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
vertex.pose.position.x = ground_ref.x;
vertex.pose.position.y = ground_ref.y;
- vertex.pose.position.z = ground_ref.z;
+ vertex.pose.position.z = ground_ref.z + 0.15 / 2.0;
vertex.pose.orientation.x = 0.0;
vertex.pose.orientation.y = 0.0;
@@ -96,14 +101,14 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
vertex.scale.x = 0.15;
vertex.scale.y = 0.15;
- vertex.scale.z = 0.30;
+ vertex.scale.z = 0.15;
vertex.color.a = 1.0; // Don't forget to set the alpha!
- if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] < VERTEX_NOT_CONNECTED_TO_ROOT)
+ if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
{
- vertex.color.r = 1.0;
- vertex.color.g = 0.66;
- vertex.color.b = 0.0;
+ vertex.color.r = 1.0;
+ vertex.color.g = 0.66;
+ vertex.color.b = 0.0;
}
else
{
@@ -112,9 +117,8 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
vertex.color.b = 0.0;
}
vertex.lifetime = ros::Duration(0.1);
- //vertex.frame_locked = true;
- local_goals.markers.push_back(vertex);
+ roadmap_markers.markers.push_back(vertex);
iri_ground_segmentation::edges edges_temp;
for (std::vector<int>::iterator e = edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].begin();
@@ -126,7 +130,7 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
edge.action = visualization_msgs::Marker::ADD;
edge.header = common_header;
- if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] < VERTEX_NOT_CONNECTED_TO_ROOT)
+ if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
{
edge.ns = "edges";
edge.id = edge_id++;
@@ -145,8 +149,7 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
p.z = ground_ref.z;
edge.points.push_back(p);
- //pcl::PointXYZRGBNormal parent_ground_ref = ground_references_pcl_cloud.points[ground_ref.data_n[3]]; //GRND_REF_DATA_N_3_PARENT_INDEX
- pcl::PointXYZRGBNormal child_ground_ref = ground_references_pcl_cloud.points[*e];
+ pcl::PointXYZRGBNormal child_ground_ref = roadmap_vertices_pcl_cloud.points[*e];
p.x = child_ground_ref.x;
p.y = child_ground_ref.y;
p.z = child_ground_ref.z;
@@ -160,7 +163,7 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
edge.pose.orientation.w = 1.0;
edge.color.a = 0.7; // Don't forget to set the alpha!
- if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] < VERTEX_NOT_CONNECTED_TO_ROOT)
+ if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
{
edge.color.r = 0.8;
edge.color.g = 0.53;
@@ -168,15 +171,15 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
}
else
{
- edge.color.r = R_CLASS_VIRTUAL_OBSTACLE_NOT_CONNECTED_TO_ROOT / 255.0;
- edge.color.g = G_CLASS_VIRTUAL_OBSTACLE_NOT_CONNECTED_TO_ROOT / 255.0;
- edge.color.b = B_CLASS_VIRTUAL_OBSTACLE_NOT_CONNECTED_TO_ROOT / 255.0;
+ edge.color.r = R_VERTEX_NOT_CONNECTED_TO_ROOT / 255.0;
+ edge.color.g = G_VERTEX_NOT_CONNECTED_TO_ROOT / 255.0;
+ edge.color.b = B_VERTEX_NOT_CONNECTED_TO_ROOT / 255.0;
}
edge.lifetime = ros::Duration(0.1);
- //edge.frame_locked = true;
- local_goals.markers.push_back(edge);
+ roadmap_markers.markers.push_back(edge);
}
+
edges_array.edges_array.push_back(edges_temp);
// We create markers for the discarded edges
@@ -199,7 +202,7 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
discarded_edge.points.push_back(p);
pcl::PointXYZRGBNormal child_ground_ref =
- ground_references_pcl_cloud.points[discarded_edge_index_and_class.vertex_index];
+ roadmap_vertices_pcl_cloud.points[discarded_edge_index_and_class.vertex_index];
p.x = child_ground_ref.x;
p.y = child_ground_ref.y;
p.z = child_ground_ref.z;
@@ -215,26 +218,26 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
discarded_edge.color.a = 0.7;
switch (discarded_edge_index_and_class.edge_class)
{
- case CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_UNCERTAINTY:
+ case CLASS_EDGE_DISCARDED_TOO_MUCH_UNCERTAINTY:
//std::cout << "Coloring edge too much uncertainty! class = " << discarded_edge_index_and_class.edge_class
// << std::endl;
- discarded_edge.color.r = (float)R_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_UNCERTAINTY / 255.0;
- discarded_edge.color.g = (float)G_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_UNCERTAINTY / 255.0;
- discarded_edge.color.b = (float)B_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_UNCERTAINTY / 255.0;
+ discarded_edge.color.r = (float)R_EDGE_DISCARDED_TOO_MUCH_UNCERTAINTY / 255.0;
+ discarded_edge.color.g = (float)G_EDGE_DISCARDED_TOO_MUCH_UNCERTAINTY / 255.0;
+ discarded_edge.color.b = (float)B_EDGE_DISCARDED_TOO_MUCH_UNCERTAINTY / 255.0;
break;
- case CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_MAHALANOBIS_DISTANCE:
+ case CLASS_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE:
//std::cout << "Coloring edge too much Mahalanobis distance! class = "
// << discarded_edge_index_and_class.edge_class << std::endl;
- discarded_edge.color.r = (float)R_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
- discarded_edge.color.g = (float)G_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
- discarded_edge.color.b = (float)B_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
+ discarded_edge.color.r = (float)R_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
+ discarded_edge.color.g = (float)G_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
+ discarded_edge.color.b = (float)B_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
break;
- case CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_SLOPE:
+ case CLASS_EDGE_DISCARDED_TOO_MUCH_SLOPE:
//std::cout << "Coloring edge too much slope! class = " << discarded_edge_index_and_class.edge_class
// << std::endl;
- discarded_edge.color.r = (float)R_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_SLOPE / 255.0;
- discarded_edge.color.g = (float)G_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_SLOPE / 255.0;
- discarded_edge.color.b = (float)B_CLASS_VIRTUAL_OBSTACLE_TOO_MUCH_SLOPE / 255.0;
+ discarded_edge.color.r = (float)R_EDGE_DISCARDED_TOO_MUCH_SLOPE / 255.0;
+ discarded_edge.color.g = (float)G_EDGE_DISCARDED_TOO_MUCH_SLOPE / 255.0;
+ discarded_edge.color.b = (float)B_EDGE_DISCARDED_TOO_MUCH_SLOPE / 255.0;
break;
default:
//std::cout << "WARNING! Discarded edge class not known!! class = " << discarded_edge_index_and_class.edge_class
@@ -245,7 +248,7 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
break;
}
discarded_edge.lifetime = ros::Duration(0.1);
- local_goals.markers.push_back(discarded_edge);
+ roadmap_markers.markers.push_back(discarded_edge);
}
}
diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp
index 05bd58e32bfab5d2f7a5662e25969e8f89a00f70..c649e31743ab214ce6f9ca1962106771268902aa 100644
--- a/src/ground_segmentation_alg_node.cpp
+++ b/src/ground_segmentation_alg_node.cpp
@@ -42,13 +42,15 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
this->alg_.filtering_configuration_.pitch_initial_std_dev = tan(config_.initial_angular_std_dev_deg * M_PI / 180.0);
}
- if (!this->private_node_handle_.getParam("ground_reference_search_resolution_deg", this->config_.ground_reference_search_resolution_deg))
+ if (!this->private_node_handle_.getParam("ground_reference_search_resolution_deg",
+ this->config_.ground_reference_search_resolution_deg))
{
- ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'ground_reference_search_resolution_deg' not found");
+ ROS_WARN(
+ "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'ground_reference_search_resolution_deg' not found");
}
else
- this->alg_.filtering_configuration_.ground_reference_search_resolution = config_.ground_reference_search_resolution_deg
- * M_PI / 180.0;
+ this->alg_.filtering_configuration_.ground_reference_search_resolution =
+ config_.ground_reference_search_resolution_deg * M_PI / 180.0;
if (!this->private_node_handle_.getParam("elevation_grid_resolution", this->config_.elevation_grid_resolution))
{
@@ -71,24 +73,20 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
else
this->alg_.filtering_configuration_.search_limit_in_shadow_area = config_.search_limit_in_shadow_area;
- if (!this->private_node_handle_.getParam("distance_to_virtual_references_in_shadow_area", this->config_.distance_to_virtual_references_in_shadow_area))
- {
- ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'distance_to_virtual_references_in_shadow_area' not found");
- }
- else
- this->alg_.filtering_configuration_.distance_to_virtual_references_in_shadow_area =
- config_.distance_to_virtual_references_in_shadow_area;
-
- if (!this->private_node_handle_.getParam("propagation_z_additive_noise_per_meter", this->config_.propagation_z_additive_noise_per_meter))
+ if (!this->private_node_handle_.getParam("propagation_z_additive_noise_per_meter",
+ this->config_.propagation_z_additive_noise_per_meter))
{
- ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'propagation_z_additive_noise_per_meter' not found");
+ ROS_WARN(
+ "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'propagation_z_additive_noise_per_meter' not found");
}
else
this->alg_.filtering_configuration_.propagation_z_additive_noise = config_.propagation_z_additive_noise_per_meter;
- if (!this->private_node_handle_.getParam("propagation_additive_noise_deg_per_meter", this->config_.propagation_additive_noise_deg_per_meter))
+ if (!this->private_node_handle_.getParam("propagation_additive_noise_deg_per_meter",
+ this->config_.propagation_additive_noise_deg_per_meter))
{
- ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'propagation_additive_noise_deg_per_meter' not found");
+ ROS_WARN(
+ "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'propagation_additive_noise_deg_per_meter' not found");
}
else
this->alg_.filtering_configuration_.propagation_additive_noise = config_.propagation_additive_noise_deg_per_meter
@@ -101,12 +99,6 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
else
this->alg_.filtering_configuration_.z_observation_std_dev = config_.z_observation_std_dev;
- if (!this->private_node_handle_.getParam("robot_width", this->config_.robot_width))
- {
- ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'robot_width' not found");
- }
- else
- this->alg_.filtering_configuration_.robot_width = config_.robot_width;
if (!this->private_node_handle_.getParam("robot_height", this->config_.robot_height))
{
@@ -115,13 +107,6 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
else
this->alg_.filtering_configuration_.robot_height = config_.robot_height;
- if (!this->private_node_handle_.getParam("safety_margin", this->config_.safety_margin))
- {
- ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'safety_margin' not found");
- }
- else
- this->alg_.filtering_configuration_.safety_margin = config_.safety_margin;
-
if (!this->private_node_handle_.getParam("max_slope_abs", this->config_.max_slope_abs))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'max_slope_abs' not found");
@@ -129,27 +114,14 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
else
this->alg_.filtering_configuration_.max_slope_abs = config_.max_slope_abs;
- if (!this->private_node_handle_.getParam("max_traversable_obstacle_height", this->config_.max_traversable_obstacle_height))
+ if (!this->private_node_handle_.getParam("max_traversable_obstacle_height",
+ this->config_.max_traversable_obstacle_height))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'max_traversable_obstacle_height' not found");
}
else
this->alg_.filtering_configuration_.max_traversable_obstacle_height = config_.max_traversable_obstacle_height;
- if (!this->private_node_handle_.getParam("flag_generate_discontinuity_obstacles_for_real_obstacles", this->config_.flag_generate_discontinuity_obstacles_for_real_obstacles))
- {
- ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'flag_generate_discontinuity_obstacles_for_real_obstacles' not found");
- }
- else
- this->alg_.filtering_configuration_.flag_generate_discontinuity_obstacles_for_real_obstacles = config_.flag_generate_discontinuity_obstacles_for_real_obstacles;
-
- if (!this->private_node_handle_.getParam("flag_connect_vertices_even_if_there_are_obstacles", this->config_.flag_connect_vertices_even_if_there_are_obstacles))
- {
- ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'flag_connect_vertices_even_if_there_are_obstacles' not found");
- }
- else
- this->alg_.filtering_configuration_.flag_connect_vertices_even_if_there_are_obstacles = config_.flag_connect_vertices_even_if_there_are_obstacles;
-
if (!this->private_node_handle_.getParam("measure_performance", this->config_.measure_performance))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'measure_performance' not found");
@@ -164,41 +136,38 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
else
this->alg_.filtering_configuration_.show_dense_reconstruction = config_.show_dense_reconstruction;
- flag_new_lidar_data_ = false;
+ // [init subscribers]
+ this->lidar_subscriber_ = this->public_node_handle_.subscribe("/lidar_points_extended_structure", 1,
+ &GroundSegmentationAlgNode::cb_lidar, this);
// [init publishers]
+
+ //////// Segmentation outputs /////////////////////
+ // This pointcloud has several point classes represented with different colors
this->lidar_points_ground_segmented_publisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
> ("lidar_points_ground_segmented", 1);
- this->roadmap_markers_publisher_ = this->private_node_handle_.advertise < visualization_msgs::MarkerArray
- > ("roadmap_markers", 1);
+ this->obstacle_points_publisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
+ > ("obstacle_points", 1);
- this->roadmap_nodes_publisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
- > ("roadmap_nodes", 1);
+ this->ground_points_publisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
+ > ("ground_points", 1);
+ //////// Roadmap outputs //////////////////////////
this->roadmap_edges_publisher_ = this->private_node_handle_.advertise < iri_ground_segmentation::edges_array
> ("roadmap_edges", 1);
this->roadmap_vertices_publisher_ = this->private_node_handle_.advertise < iri_ground_segmentation::vertices_array
> ("roadmap_vertices", 1);
- // Outputs for integration with navigation stack
- this->obstacle_points_publisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
- > ("obstacle_points", 1);
-
- this->ground_points_publisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
- > ("ground_points", 1);
-
- this->dicontinuity_obstacles_publisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
- > ("discontinuity_obstacle_points", 1);
-
- this->exploration_frontier_points_publisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
- > ("exploration_frontier_points", 1);
+ //////// Visualization outputs ///////////////////
+ this->roadmap_markers_publisher_ = this->private_node_handle_.advertise < visualization_msgs::MarkerArray
+ > ("roadmap_markers", 1);
- // [init subscribers]
- this->lidar_subscriber_ = this->public_node_handle_.subscribe("/lidar_points_extended_structure", 1,
- &GroundSegmentationAlgNode::cb_lidar, this);
+ this->ground_dense_reconstruction_publisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
+ > ("ground_dense_reconstruction", 1);
+ flag_new_lidar_data_ = false;
pthread_mutex_init(&this->mutex_, NULL);
// [init services]
}
@@ -294,7 +263,6 @@ void GroundSegmentationAlgNode::storeHeaderForSync(void)
void GroundSegmentationAlgNode::convertInputsToSuitableFormats(void)
{
//std::cout << "GroundSegmentationAlgNode::convertInputsToSuitableFormats..." << std::endl;
- //std::cout << " 1) rosPointCloudtoPCLPointCloud with lidar_ros_cloud_..." << std::endl;
this->rosPointCloudtoPCLPointCloud(lidar_ros_cloud_, lidar_pcl_cloud_);
flag_new_lidar_data_ = false;
}
@@ -318,14 +286,7 @@ void GroundSegmentationAlgNode::splitSegmentedCloudInOnePerClass(void)
case CLASS_OBSTACLE:
obstacle_points_pcl_cloud_.push_back(regular_point);
break;
- case CLASS_DISCONTINUITY:
- dicontinuity_obstacles_pcl_cloud_.push_back(regular_point);
- break;
- case CLASS_FRONTIER:
- exploration_frontier_points_pcl_cloud_.push_back(regular_point);
- break;
default:
- //if ((int)std::floor(classified_point.data_c[DATA_C_1_ID_CLASS]) != CLASS_UNKNOWN) //TODO: See if it is needed to add more outputs
// std::cout << "GroundSegmentationAlgNode::splitSegmentedCloudInOnePerClass -> unexpected class value of: "
// << classified_point.data_c[DATA_C_1_ID_CLASS] << std::endl;
break;
@@ -340,51 +301,52 @@ void GroundSegmentationAlgNode::convertOutputsToSuitableFormats(void)
this->PCLPointCloudToRosPointCloud(lidar_pcl_cloud_, lidar_points_ground_segmented_ros_cloud_);
- this->PCLPointCloudToRosPointCloud(roadmap_nodes_pcl_cloud_, roadmap_nodes_ros_cloud_);
+ this->PCLPointCloudToRosPointCloud(ground_dense_reconstruction_pcl_cloud_, ground_dense_reconstruction_ros_cloud_);
- this->nodesPCLPointCloudToVerticesArray(roadmap_nodes_pcl_cloud_, roadmap_vertices_array_);
+ this->nodesPCLPointCloudToVerticesArray(roadmap_vertices_pcl_cloud_, roadmap_vertices_array_);
- // Outputs for integration with navigation stack
this->splitSegmentedCloudInOnePerClass();
this->PCLPointCloudToRosPointCloud(obstacle_points_pcl_cloud_, obstacle_points_ros_cloud_);
this->PCLPointCloudToRosPointCloud(ground_points_pcl_cloud_, ground_points_ros_cloud_);
-
- this->PCLPointCloudToRosPointCloud(dicontinuity_obstacles_pcl_cloud_, dicontinuity_obstacles_ros_cloud_);
-
- this->PCLPointCloudToRosPointCloud(exploration_frontier_points_pcl_cloud_, exploration_frontier_points_ros_cloud_);
}
void GroundSegmentationAlgNode::publishAll(void)
{
//std::cout << "Starting GroundSegmentationAlgNode::publishAll.." << std::endl;
+
+ // Segmentation topics
this->lidar_points_ground_segmented_publisher_.publish(this->lidar_points_ground_segmented_ros_cloud_);
- this->roadmap_nodes_publisher_.publish(this->roadmap_nodes_ros_cloud_);
- this->roadmap_markers_publisher_.publish(this->roadmap_MarkerArray_msg_);
+ this->obstacle_points_publisher_.publish(this->obstacle_points_ros_cloud_);
+ this->ground_points_publisher_.publish(this->ground_points_ros_cloud_);
+
+ // Roadmap topics
this->roadmap_edges_publisher_.publish(this->roadmap_edges_array_);
this->roadmap_vertices_publisher_.publish(this->roadmap_vertices_array_);
- // Publishers for integration with navigation stack
- this->obstacle_points_publisher_.publish(this->obstacle_points_ros_cloud_);
- this->ground_points_publisher_.publish(this->ground_points_ros_cloud_);
- this->dicontinuity_obstacles_publisher_.publish(this->dicontinuity_obstacles_ros_cloud_);
- this->exploration_frontier_points_publisher_.publish(this->exploration_frontier_points_ros_cloud_);
+ // Visualization topics
+ this->ground_dense_reconstruction_publisher_.publish(this->ground_dense_reconstruction_ros_cloud_);
+ this->roadmap_markers_publisher_.publish(this->roadmap_MarkerArray_msg_);
}
void GroundSegmentationAlgNode::prepareForNextIteration(void)
{
+ // Segmentation
lidar_pcl_cloud_.clear();
- roadmap_nodes_pcl_cloud_.clear();
- roadmap_MarkerArray_msg_.markers.clear();
+ obstacle_points_pcl_cloud_.clear();
+ ground_points_pcl_cloud_.clear();
+
+ // Roadmap
roadmap_edges_array_.edges_array.clear();
roadmap_vertices_array_.vertices_array.clear();
- // Pointclouds for integration with navigation stack
- obstacle_points_pcl_cloud_.clear();
- ground_points_pcl_cloud_.clear();
- dicontinuity_obstacles_pcl_cloud_.clear();
- exploration_frontier_points_pcl_cloud_.clear();
+ // Visualization
+ ground_dense_reconstruction_pcl_cloud_.clear();
+ roadmap_MarkerArray_msg_.markers.clear();
+
+ // Aux pointcloud to store vertices information (internally vertices are organized in a pointcloud)
+ roadmap_vertices_pcl_cloud_.clear();
}
void GroundSegmentationAlgNode::showPerformanceStatistics(void)
@@ -409,8 +371,8 @@ void GroundSegmentationAlgNode::mainNodeThread(void)
this->mutex_exit(); // Then we can free the callback
- this->alg_.segmentGround(lidar_pcl_cloud_, roadmap_nodes_pcl_cloud_, roadmap_MarkerArray_msg_, roadmap_edges_array_,
- common_header_);
+ this->alg_.segmentGround(common_header_, lidar_pcl_cloud_, roadmap_vertices_pcl_cloud_, roadmap_edges_array_,
+ ground_dense_reconstruction_pcl_cloud_, roadmap_MarkerArray_msg_);
this->convertOutputsToSuitableFormats();
this->publishAll();
@@ -427,8 +389,8 @@ void GroundSegmentationAlgNode::cb_lidar(const sensor_msgs::PointCloud2::ConstPt
assert(msg != NULL && "Null pointer!!! in function cb_lidar!");
this->mutex_enter();
- flag_new_lidar_data_ = true; // This flag is shared with the main node thread, as the lidar_ros_cloud, but the latter is protected
- // by the flag, so it is not required to use the mutex
+ flag_new_lidar_data_ = true; // This flag is shared with the main node thread, as the lidar_ros_cloud, but the latter is protected
+ // by the flag, so it is not required to use the mutex
this->mutex_exit();
}
@@ -470,22 +432,16 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
this->alg_.filtering_configuration_.angle_reduction_factor = config.angle_reduction_factor;
this->alg_.filtering_configuration_.search_limit_in_shadow_area = config.search_limit_in_shadow_area;
- this->alg_.filtering_configuration_.distance_to_virtual_references_in_shadow_area =
- config.distance_to_virtual_references_in_shadow_area;
this->alg_.filtering_configuration_.propagation_z_additive_noise = config.propagation_z_additive_noise_per_meter;
this->alg_.filtering_configuration_.propagation_additive_noise = config.propagation_additive_noise_deg_per_meter
* M_PI / 180.0;
this->alg_.filtering_configuration_.z_observation_std_dev = config.z_observation_std_dev;
- this->alg_.filtering_configuration_.robot_width = config.robot_width;
this->alg_.filtering_configuration_.robot_height = config.robot_height;
- this->alg_.filtering_configuration_.safety_margin = config.safety_margin;
this->alg_.filtering_configuration_.max_traversable_obstacle_height = config_.max_traversable_obstacle_height;
this->alg_.filtering_configuration_.max_slope_abs = config.max_slope_abs;
- this->alg_.filtering_configuration_.flag_generate_discontinuity_obstacles_for_real_obstacles = config.flag_generate_discontinuity_obstacles_for_real_obstacles;
- this->alg_.filtering_configuration_.flag_connect_vertices_even_if_there_are_obstacles = config.flag_connect_vertices_even_if_there_are_obstacles;
this->alg_.filtering_configuration_.measure_performance = config.measure_performance;
this->alg_.filtering_configuration_.show_dense_reconstruction = config.show_dense_reconstruction;
@@ -517,9 +473,6 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
std::cout << "search_limit_in_shadow_area = "
<< this->alg_.filtering_configuration_.search_limit_in_shadow_area << std::endl;
- std::cout << "distance_to_virtual_references_in_shadow_area = "
- << this->alg_.filtering_configuration_.distance_to_virtual_references_in_shadow_area << std::endl;
-
std::cout << "propagation_z_additive_noise in m/m = "
<< this->alg_.filtering_configuration_.propagation_z_additive_noise << std::endl;
std::cout << "propagation_additive_noise in deg/m = "
@@ -527,18 +480,11 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
std::cout << "z_observation_std_dev = "
<< this->alg_.filtering_configuration_.z_observation_std_dev << std::endl;
- std::cout << "robot_width = " << this->alg_.filtering_configuration_.robot_width << std::endl;
std::cout << "robot_height = " << this->alg_.filtering_configuration_.robot_height << std::endl;
- std::cout << "safety_margin = " << this->alg_.filtering_configuration_.safety_margin << std::endl;
std::cout << "max_traversable_obstacle_height = "
<< this->alg_.filtering_configuration_.max_traversable_obstacle_height << std::endl;
- std::cout << "max_slope_abs (adimensional) = "
- << this->alg_.filtering_configuration_.max_slope_abs << std::endl;
- std::cout << "flag_generate_discontinuity_obstacles_for_real_obstacles = "
- << this->alg_.filtering_configuration_.flag_generate_discontinuity_obstacles_for_real_obstacles << std::endl;
- std::cout << "flag_connect_vertices_even_if_there_are_obstacles = "
- << this->alg_.filtering_configuration_.flag_connect_vertices_even_if_there_are_obstacles << std::endl;
+ std::cout << "max_slope_abs (adimensional) = " << this->alg_.filtering_configuration_.max_slope_abs << std::endl;
std::cout << "measure_performance = " << this->alg_.filtering_configuration_.measure_performance << std::endl;
std::cout << "show_dense_reconstruction = " << this->alg_.filtering_configuration_.show_dense_reconstruction