maintain traversibility info in pointcloud with new point type

Author: MaverickPeter

elevation_mapping/elevation_mapping/.vscode/settings.json

@@ -1,5 +1,6 @@
 {
     "files.associations": {
+        "*.cpp": "cpp",
         "limits": "cpp",
         "vector": "cpp",
         "array": "cpp",
@@ -54,6 +55,10 @@
         "mutex": "cpp",
         "algorithm": "cpp",
         "cfenv": "cpp",
-        "string": "cpp"
+        "string": "cpp",
+        "unordered_set": "cpp",
+        "hash_map": "cpp",
+        "hash_set": "cpp",
+        "string_view": "cpp"
     }
 }

elevation_mapping/elevation_mapping/include/elevation_mapping/GridUtilHash.hpp

@@ -27,13 +27,14 @@ struct GridPoint{
 };
 
 struct GridPointData{
-    float elevation;
     float var;
+    float obstacle;
+    float elevation;
+    float intensity;
     int r;
     int g;
     int b;
-    float intensity;
-    GridPointData(float ele_, float var_, int r_, int g_, int b_, float intensity_):elevation(ele_), var(var_),r(r_),g(g_),b(b_),intensity(intensity_){}
+    GridPointData(float ele_, float var_, int r_, int g_, int b_, float intensity_, float obstacle_):elevation(ele_), var(var_),r(r_),g(g_),b(b_),intensity(intensity_),obstacle(obstacle_){}
 };
 
 struct GridPointHashFunc{

elevation_mapping/elevation_mapping/include/elevation_mapping/PointXYZRGBICO.hpp

@@ -0,0 +1,64 @@
+/*
+ * PointXYZRGBICO.hpp
+ *
+ *  Created on: July 1, 2021
+ *      Author: Peter XU
+ *	 Institute: ZJU, Robotics 104
+ */
+
+#define PCL_NO_PRECOMPILE
+
+#include <pcl/pcl_macros.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/impl/instantiate.hpp>
+
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/impl/voxel_grid.hpp>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/impl/passthrough.hpp>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/kdtree/impl/kdtree_flann.hpp>
+#include <pcl/search/impl/kdtree.hpp>
+#include <boost/shared_ptr.hpp>
+
+struct PointXYZRGBICO
+{
+    PCL_ADD_POINT4D;
+    PCL_ADD_RGB;
+
+    float covariance;
+    float intensity;
+    float obstacle;
+
+    PointXYZRGBICO() {}
+    PointXYZRGBICO(const PointXYZRGBICO& input)
+    {
+        this->x = input.x;
+        this->y = input.y;
+        this->z = input.z;
+        this->rgb = input.rgb;
+        this->intensity = input.intensity;
+        this->covariance = input.covariance;
+        this->obstacle = input.obstacle;
+    }
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+} EIGEN_ALIGN16;
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZRGBICO,
+                                   (float, x, x)
+                                   (float, y, y)
+                                   (float, z, z)
+                                   (float, rgb, rgb)
+                                   (float, intensity, intensity)
+                                   (float, covariance, covariance)
+                                   (float, obstacle, obstacle)
+);
+
+PCL_INSTANTIATE(VoxelGrid, PointXYZRGBICO);
+PCL_INSTANTIATE(KdTree, PointXYZRGBICO);
+PCL_INSTANTIATE(KdTreeFLANN, PointXYZRGBICO);
+
+typedef pcl::PointCloud<PointXYZRGBICO> PointCloudXYZRGBICO;

elevation_mapping/elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp

@@ -11,6 +11,7 @@
 #include "elevation_mapping/PointXYZRGBCE.hpp"
 #include "elevation_mapping/PointXYZRGBC.hpp"
 #include "elevation_mapping/PointXYZRGBIC.hpp"
+#include "elevation_mapping/PointXYZRGBICO.hpp"
 
 // ROS
 #include <ros/ros.h>
@@ -32,7 +33,7 @@
 #include <memory>
 
 namespace elevation_mapping {
-typedef PointXYZRGBIC Anypoint;
+typedef PointXYZRGBICO Anypoint;
 typedef pcl::PointCloud<Anypoint> pointCloud;
 
 /*!

elevation_mapping/elevation_mapping/src/ElevationMapping.cpp

@@ -427,6 +427,7 @@ void ElevationMapping::savingMap()
     pt.r = visualCloud_.points[i].r;
     pt.g = visualCloud_.points[i].g;
     pt.b = visualCloud_.points[i].b;
+    pt.obstacle = visualCloud_.points[i].obstacle;
     pt.intensity = visualCloud_.points[i].intensity;
     pt.covariance = visualCloud_.points[i].covariance;
     out_color.push_back(pt);
@@ -685,7 +686,7 @@ void ElevationMapping::updateLocalMap(const sensor_msgs::PointCloud2ConstPtr& ra
 
             GridPoint save_pos(position.x(), position.y());
             GridPointData save_data(prevMap_.at("elevation", *iterator), prevMap_.at("variance", *iterator), prevMap_.at("color_r", *iterator), 
-                                    prevMap_.at("color_g", *iterator), prevMap_.at("color_b", *iterator), prevMap_.at("intensity", *iterator));
+                                    prevMap_.at("color_g", *iterator), prevMap_.at("color_b", *iterator), prevMap_.at("intensity", *iterator), prevMap_.at("traver", *iterator));
 
             // Update the grid information within local map
             auto got = localMap_.find(save_pos);
@@ -705,6 +706,7 @@ void ElevationMapping::updateLocalMap(const sensor_msgs::PointCloud2ConstPtr& ra
             pt.r = prevMap_.at("color_r", *iterator);
             pt.g = prevMap_.at("color_g", *iterator);
             pt.b = prevMap_.at("color_b", *iterator);
+            pt.obstacle = prevMap_.at("traver", *iterator);
             pt.intensity = prevMap_.at("intensity", *iterator);
             pt.covariance = prevMap_.at("variance", *iterator);
             visualCloud_.push_back(pt);
@@ -1020,7 +1022,7 @@ void ElevationMapping::pointCloudtoHash(pointCloud localPointCloud, umap& out)
     round_z = localPointCloud.points[i].z;
 
     GridPoint save_pos(round_x, round_y);
-    GridPointData save_data(round_z, localPointCloud.points[i].covariance, localPointCloud.points[i].r, localPointCloud.points[i].g, localPointCloud.points[i].b, localPointCloud.points[i].intensity);
+    GridPointData save_data(round_z, localPointCloud.points[i].covariance, localPointCloud.points[i].r, localPointCloud.points[i].g, localPointCloud.points[i].b, localPointCloud.points[i].intensity, localPointCloud.points[i].obstacle);
     out.insert(make_pair(save_pos, save_data));
   }
 }