added representation for vertices and edges discarded for planning because are...

added representation for vertices and edges discarded for planning because are not connected with the root vertex

src/ground_segmentation_alg.cpp

@@ -48,8 +48,10 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
   //std::cout << "Number of edge vectors = " << edges.size();
 
   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)
   {
@@ -69,8 +71,18 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
     visualization_msgs::Marker vertex;
     vertex.action = visualization_msgs::Marker::ADD;
     vertex.header = common_header;
-    vertex.ns = "vertices";
-    vertex.id = vertex_id++;
+
+    if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] < VERTEX_NOT_CONNECTED_TO_ROOT)
+    {
+      vertex.ns = "vertices_connected_to_root";
+      vertex.id = vertex_id++;
+    }
+    else
+    {
+      vertex.ns = "vertices_NOT_connected_to_root";
+      vertex.id = discarded_vertex_id++;
+    }
+
     vertex.type = visualization_msgs::Marker::CUBE;
 
     vertex.pose.position.x = ground_ref.x;
@@ -87,10 +99,18 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
     vertex.scale.z = 0.30;
 
     vertex.color.a = 1.0; // Don't forget to set the alpha!
-    vertex.color.r = 1.0;
-    vertex.color.g = 0.66;
-    vertex.color.b = 0.0;
-
+    if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] < VERTEX_NOT_CONNECTED_TO_ROOT)
+    {
+      vertex.color.r = 1.0;
+      vertex.color.g = 0.66;
+      vertex.color.b = 0.0;
+    }
+    else
+    {
+      vertex.color.r = 1.0;
+      vertex.color.g = 0.0;
+      vertex.color.b = 0.0;
+    }
     vertex.lifetime = ros::Duration(0.1);
     //vertex.frame_locked = true;
 
@@ -105,8 +125,18 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
       visualization_msgs::Marker edge;
       edge.action = visualization_msgs::Marker::ADD;
       edge.header = common_header;
-      edge.ns = "edges";
-      edge.id = edge_id++;
+
+      if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] < VERTEX_NOT_CONNECTED_TO_ROOT)
+      {
+        edge.ns = "edges";
+        edge.id = edge_id++;
+      }
+      else
+      {
+        edge.ns = "discarded_edges";
+        edge.id = discarded_edge_id++;
+      }
+
       edge.type = visualization_msgs::Marker::LINE_LIST;
 
       geometry_msgs::Point p;
@@ -130,10 +160,18 @@ 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!
-      edge.color.r = 0.8;
-      edge.color.g = 0.53;
-      edge.color.b = 0.0;
-
+      if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] < VERTEX_NOT_CONNECTED_TO_ROOT)
+      {
+        edge.color.r = 0.8;
+        edge.color.g = 0.53;
+        edge.color.b = 0.0;
+      }
+      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.lifetime = ros::Duration(0.1);
       //edge.frame_locked = true;
 
@@ -178,25 +216,29 @@ void GroundSegmentationAlgorithm::segmentGround(pcl::PointCloud<pcl::PointXYZRGB
       switch (discarded_edge_index_and_class.edge_class)
       {
         case CLASS_VIRTUAL_OBSTACLE_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;
+          //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;
           break;
         case CLASS_VIRTUAL_OBSTACLE_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;
+          //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;
           break;
         case CLASS_VIRTUAL_OBSTACLE_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;
+          //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;
           break;
         default:
-          std::cout << "WARNING! Discarded edge class not known!! class = " << discarded_edge_index_and_class.edge_class << std::endl;
+          //std::cout << "WARNING! Discarded edge class not known!! class = " << discarded_edge_index_and_class.edge_class
+          //    << std::endl;
           discarded_edge.color.r = 1.0;
           discarded_edge.color.g = 1.0;
           discarded_edge.color.b = 1.0;