update file voxel_layer

include/costmap_2d/voxel_grid.h

@@ -0,0 +1,32 @@
+// Header header
+// uint32[] data
+// geometry_msgs/Point32 origin
+// geometry_msgs/Vector3 resolutions
+// uint32 size_x
+// uint32 size_y
+// uint32 size_z
+
+#ifndef VOXEL_GRID_COSTMAP_2D_H
+#define VOXEL_GRID_COSTMAP_2D_H
+
+#include <vector>
+#include <std_msgs/Header.h>
+#include <geometry_msgs/Point32.h>
+#include <geometry_msgs/Vector3.h>
+
+namespace costmap_2d
+{
+    struct VoxelGrid
+    {
+        std_msgs::Header header;
+        std::vector<uint32_t> data;
+        geometry_msgs::Point32 origin;
+        geometry_msgs::Vector3 resolutions;
+        uint32_t size_x;
+        uint32_t size_y;
+        uint32_t size_z;
+    };
+    
+}
+
+#endif // VOXEL_GRID_COSTMAP_2D_H

include/costmap_2d/voxel_layer.h

@@ -38,11 +38,13 @@
 #ifndef COSTMAP_2D_VOXEL_LAYER_H_
 #define COSTMAP_2D_VOXEL_LAYER_H_
 
-// #include <ros/ros.h>
 #include <costmap_2d/layer.h>
 #include <costmap_2d/layered_costmap.h>
 #include <costmap_2d/observation_buffer.h>
-// #include <costmap_2d/voxel_grid.h>
+#include <costmap_2d/voxel_grid.h>
+#include <costmap_2d/obstacle_layer.h>
+#include <costmap_2d/utils.h>
+
 #include <nav_msgs/OccupancyGrid.h>
 #include <sensor_msgs/LaserScan.h>
 // #include <laser_geometry/laser_geometry.h>
@@ -52,16 +54,16 @@
 // #include <message_filters/subscriber.h>
 // #include <dynamic_reconfigure/server.h>
 // #include <costmap_2d/VoxelPluginConfig.h>
-#include <costmap_2d/obstacle_layer.h>
 #include <voxel_grid/voxel_grid.h>
 
+
 namespace costmap_2d
 {
 
 class VoxelLayer : public ObstacleLayer
 {
 public:
-  VoxelLayer() //: voxel_grid_(0, 0, 0)
+  VoxelLayer() : voxel_grid_(0, 0, 0)
   {
     costmap_ = NULL;  // this is the unsigned char* member of parent class's parent class Costmap2D.
   }
@@ -77,29 +79,23 @@ public:
   {
     return true;
   }
+  
   virtual void matchSize();
   virtual void reset();
 
 
 protected:
-  // virtual void setupDynamicReconfigure(ros::NodeHandle& nh);
-
   virtual void resetMaps();
 
 private:
-  // void reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level);
   void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
   virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
                                  double* max_x, double* max_y);
 
-  // dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig> *voxel_dsrv_;
-
   bool publish_voxel_;
-  // ros::Publisher voxel_pub_;
-  // voxel_grid::VoxelGrid voxel_grid_;
+  voxel_grid::VoxelGrid voxel_grid_;
   double z_resolution_, origin_z_;
   unsigned int unknown_threshold_, mark_threshold_, size_z_;
-  // ros::Publisher clearing_endpoints_pub_;
   sensor_msgs::PointCloud clearing_endpoints_;
 
   inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
@@ -142,6 +138,7 @@ private:
   {
     return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
   }
+  bool getParams();
 };
 
 }  // namespace costmap_2d