costmap_2d: fixed voxel_layer memory corruption (and refactored to make it easier to compare with original version). Added voxel_marker node with option to show occupied, free or unknown cells of a voxel grid msg.

CMakeLists.txt

@@ -500,6 +500,9 @@ IF(costmap_2d_FOUND)
     target_link_libraries(rtabmap_costmap_plugins2
       ${costmap_2d_LIBRARIES}
     )
+    add_executable(rtabmap_costmap_voxel_markers src/costmap_2d/voxel_markers.cpp)
+    target_link_libraries(rtabmap_costmap_voxel_markers ${costmap_2d_LIBRARIES})
+    set_target_properties(rtabmap_costmap_voxel_markers PROPERTIES OUTPUT_NAME "voxel_markers")
 ENDIF(costmap_2d_FOUND)
 
 #############
@@ -600,6 +603,7 @@ IF(costmap_2d_FOUND)
     install(TARGETS 
        rtabmap_costmap_plugins
        rtabmap_costmap_plugins2
+       rtabmap_costmap_voxel_markers
        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

src/costmap_2d/voxel_layer.cpp

@@ -48,10 +48,8 @@ using costmap_2d::NO_INFORMATION;
 using costmap_2d::LETHAL_OBSTACLE;
 using costmap_2d::FREE_SPACE;
 
-using costmap_2d::Costmap2D;
 using costmap_2d::ObservationBuffer;
 using costmap_2d::Observation;
-using costmap_2d::VoxelGrid;
 
 
 namespace rtabmap_ros
@@ -62,22 +60,23 @@ void VoxelLayer::onInitialize()
   ObstacleLayer::onInitialize();
   ros::NodeHandle private_nh("~/" + name_);
   
-  std::string costmap_name_ = name_.substr(0, name_.find("/"));
+  std::string costmap_name = name_.substr(0, name_.find("/"));
-  ros::NodeHandle pnh("~/" + costmap_name_);
+  ros::NodeHandle pnh("~/" + costmap_name);
 
   private_nh.param("publish_voxel_map", publish_voxel_, false);
-  pnh.param("robot_frame", robot_base_frame_, std::string("base_link"));
+  // param from parent costmap group
+  pnh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
 
   if (publish_voxel_)
-    voxel_pub_ = private_nh.advertise <VoxelGrid > ("voxel_grid", 1);
+    voxel_pub_ = private_nh.advertise < costmap_2d::VoxelGrid > ("voxel_grid", 1);
 
   clearing_endpoints_pub_ = private_nh.advertise<sensor_msgs::PointCloud>("clearing_endpoints", 1);
 }
 
 void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
 {
-  voxel_dsrv_ = new dynamic_reconfigure::Server<VoxelPluginConfig>(nh);
+  voxel_dsrv_ = new dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>(nh);
-  dynamic_reconfigure::Server<VoxelPluginConfig>::CallbackType cb = boost::bind(
+  dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>::CallbackType cb = boost::bind(
       &VoxelLayer::reconfigureCB, this, _1, _2);
   voxel_dsrv_->setCallback(cb);
 }
@@ -88,7 +87,7 @@ VoxelLayer::~VoxelLayer()
     delete voxel_dsrv_;
 }
 
-void VoxelLayer::reconfigureCB(VoxelPluginConfig &config, uint32_t level)
+void VoxelLayer::reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level)
 {
   enabled_ = config.enabled;
   footprint_clearing_enabled_ = config.footprint_clearing_enabled;
@@ -106,7 +105,7 @@ void VoxelLayer::matchSize()
 {
   ObstacleLayer::matchSize();
   voxel_grid_.resize(size_x_, size_y_, size_z_);
- // ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_ && voxel_grid_.sizeZ() == size_z_);
+  ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);
 }
 
 void VoxelLayer::reset()
@@ -126,13 +125,10 @@ void VoxelLayer::resetMaps()
 void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                        double* min_y, double* max_x, double* max_y)
 {
-  if (rolling_window_) {
+  if (rolling_window_)
-      updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2 );
+    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
-  }
-
   if (!enabled_)
     return;
-
   useExtraBounds(min_x, min_y, max_x, max_y);
 
   bool current = true;
@@ -195,7 +191,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
 
   if (publish_voxel_)
   {
-    VoxelGrid grid_msg;
+    costmap_2d::VoxelGrid grid_msg;
     unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
     grid_msg.size_x = voxel_grid_.sizeX();
     grid_msg.size_y = voxel_grid_.sizeY();
@@ -404,86 +400,89 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
 
 void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
 {
-    int cell_oz;
+  int cell_oz;
-    // get the global pose of the robot
+  // get the global pose of the robot
-    try
+  try
-    {
+  {
-        geometry_msgs::TransformStamped transformStamped;
+      geometry_msgs::TransformStamped transformStamped;
 #ifdef COSTMAP_2D_POINTCLOUD2
-        transformStamped = tf_->lookupTransform(global_frame_, robot_base_frame_, ros::Time(0));
+      transformStamped = tf_->lookupTransform(global_frame_, robot_base_frame_, ros::Time(0));
 #else
-        tf::StampedTransform stampedTransform;
+      tf::StampedTransform stampedTransform;
-        tf_->lookupTransform(global_frame_, robot_base_frame_, ros::Time(0), stampedTransform);
+      tf_->lookupTransform(global_frame_, robot_base_frame_, ros::Time(0), stampedTransform);
-        tf::transformStampedTFToMsg(stampedTransform, transformStamped);
+      tf::transformStampedTFToMsg(stampedTransform, transformStamped);
 #endif
-        const double robot_z = transformStamped.transform.translation.z;
+      const double robot_z = transformStamped.transform.translation.z;
-        const double z_grid_height = z_resolution_ * size_z_;
+      const double z_grid_height = z_resolution_ * size_z_;
-        const double new_origin_z = robot_z - z_grid_height / 2;
+      const double new_origin_z = robot_z - z_grid_height / 2;
-        cell_oz = int((new_origin_z - origin_z_) / z_resolution_);
+      cell_oz = int((new_origin_z - origin_z_) / z_resolution_);
-    }
+  }
 #ifdef COSTMAP_2D_POINTCLOUD2
-    catch (tf2::TransformException& ex)
+  catch (tf2::TransformException& ex)
 #else
-    catch(tf::TransformException& ex)
+  catch(tf::TransformException& ex)
 #endif
-    {
+  {
-        ROS_ERROR("%s", ex.what());
+      ROS_ERROR("%s", ex.what());
-        // If the robot pose is not detected, the origin_z_ will remain the same.
+      // If the robot pose is not detected, the origin_z_ will remain the same.
-        cell_oz = 0;
+      cell_oz = 0;
-    }
+  }
 
-    // project the new origin into the grid
+  // project the new origin into the grid
-    int cell_ox, cell_oy;
+  int cell_ox, cell_oy;
-    cell_ox = int((new_origin_x - origin_x_) / resolution_);
+  cell_ox = int((new_origin_x - origin_x_) / resolution_);
-    cell_oy = int((new_origin_y - origin_y_) / resolution_);
+  cell_oy = int((new_origin_y - origin_y_) / resolution_);
 
-    // compute the associated world coordinates for the origin cell
+  // compute the associated world coordinates for the origin cell
-    // because we want to keep things grid-aligned
+  // because we want to keep things grid-aligned
-    double new_grid_ox, new_grid_oy, new_grid_oz;
+  double new_grid_ox, new_grid_oy, new_grid_oz;
-    new_grid_ox = origin_x_ + cell_ox * resolution_;
+  new_grid_ox = origin_x_ + cell_ox * resolution_;
-    new_grid_oy = origin_y_ + cell_oy * resolution_;
+  new_grid_oy = origin_y_ + cell_oy * resolution_;
-    new_grid_oz = origin_z_ + cell_oz * z_resolution_;
+  new_grid_oz = origin_z_ + cell_oz * z_resolution_;
 
-    // to avoid casting from unsigned int to int a bunch of times
+  // to avoid casting from unsigned int to int a bunch of times
-    int size_x = size_x_;
+  int size_x = size_x_;
-    int size_y = size_y_;
+  int size_y = size_y_;
 
-    // we need to compute the overlap of the new and existing windows
+  // we need to compute the overlap of the new and existing windows
-    int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
+  int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
-    lower_left_x = std::min(std::max(cell_ox, 0), size_x);
+  lower_left_x = std::min(std::max(cell_ox, 0), size_x);
-    lower_left_y = std::min(std::max(cell_oy, 0), size_y);
+  lower_left_y = std::min(std::max(cell_oy, 0), size_y);
-    upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
+  upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
-    upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
+  upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
 
-    unsigned int cell_size_x = upper_right_x - lower_left_x;
+  unsigned int cell_size_x = upper_right_x - lower_left_x;
-    unsigned int cell_size_y = upper_right_y - lower_left_y;
+  unsigned int cell_size_y = upper_right_y - lower_left_y;
 
-    // we need a map to store the obstacles in the window temporarily
+  // we need a map to store the obstacles in the window temporarily
-    //unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
+  unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
-    unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
+  unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
-    unsigned int* voxel_map = voxel_grid_.getData();
+  unsigned int* voxel_map = voxel_grid_.getData();
 
-    // copy the local window in the costmap to the local map
+  // copy the local window in the costmap to the local map
-    copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x,
+  copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
+  copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x,
                 cell_size_y);
 
-    // we'll reset our maps to unknown space if appropriate
+  // we'll reset our maps to unknown space if appropriate
-    resetMaps();
+  resetMaps();
 
-    // update the origin with the appropriate world coordinates
+  // update the origin with the appropriate world coordinates
-    origin_x_ = new_grid_ox;
+  origin_x_ = new_grid_ox;
-    origin_y_ = new_grid_oy;
+  origin_y_ = new_grid_oy;
-    origin_z_ = new_grid_oz;
+  origin_z_ = new_grid_oz;
 
-    // compute the starting cell location for copying data back in
+  // compute the starting cell location for copying data back in
-    int start_x = lower_left_x - cell_ox;
+  int start_x = lower_left_x - cell_ox;
-    int start_y = lower_left_y - cell_oy;
+  int start_y = lower_left_y - cell_oy;
 
-    // now we want to copy the overlapping information back into the map, but in its new location
+  // now we want to copy the overlapping information back into the map, but in its new location
-    copyMapRegion3D(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y, cell_oz);
+  copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
+  copyMapRegion3D(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y, cell_oz);
 
-    // make sure to clean up
+  // make sure to clean up
-    delete[] local_voxel_map;
+  delete[] local_map;
+  delete[] local_voxel_map;
 }
 
 }  // namespace rtabmap_ros

src/costmap_2d/voxel_layer.h

@@ -54,8 +54,6 @@
 #include <costmap_2d/obstacle_layer.h>
 #include <voxel_grid/voxel_grid.h>
 
-using costmap_2d::VoxelPluginConfig;
-
 namespace rtabmap_ros
 {
 
@@ -65,7 +63,6 @@ public:
   VoxelLayer() :
       voxel_grid_(0, 0, 0)
   {
-
     costmap_ = NULL;  // this is the unsigned char* member of parent class's parent class Costmap2D.
   }
 
@@ -83,18 +80,19 @@ public:
   virtual void matchSize();
   virtual void reset();
 
+
 protected:
   virtual void setupDynamicReconfigure(ros::NodeHandle& nh);
 
   virtual void resetMaps();
 
 private:
-  void reconfigureCB(VoxelPluginConfig &config, uint32_t level);
+  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<VoxelPluginConfig> *voxel_dsrv_;
+  dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig> *voxel_dsrv_;
 
   bool publish_voxel_;
   std::string robot_base_frame_;
@@ -109,11 +107,9 @@ private:
   {
     if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
       return false;
-
     mx = ((wx - origin_x_) / resolution_);
     my = ((wy - origin_y_) / resolution_);
     mz = ((wz - origin_z_) / z_resolution_);
-
     if (mx < size_x_ && my < size_y_ && mz < size_z_)
       return true;
 
@@ -168,7 +164,6 @@ private:
                        unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x,
                        unsigned int region_size_y, int z_shift)
     {
-
         // we'll first need to compute the starting points for each map
         // this is like getting voxel column. We are not taking into account the z position of the voxel
         data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
@@ -181,18 +176,24 @@ private:
         for (unsigned int i = 0; i < region_size_y; ++i)
         {
             memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
-            for (unsigned int j = 0; j < dm_size_x; j++) {
+            for (unsigned int j = 0; j < region_size_x; j++) {
-                if (z_shift > 0) {
+            	// known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits
-                    dm_index[j] = ((dm_index[j] & marked_bits_mask) >> z_shift & marked_bits_mask) |
+				if (z_shift > 0) {
-                                  ((~((data_type) 0) << sizeof(data_type) * 4 - z_shift) |
+					dm_index[j] =
-                                   (dm_index[j] & unknown_bits_mask) >> z_shift) & unknown_bits_mask;
+							// Shift marked cells, insert zeros for new unknowns
+							((dm_index[j] & marked_bits_mask) >> z_shift & marked_bits_mask) |
+							// Shift empty/unknown cells, insert ones for new unknowns
+						    (((dm_index[j] & unknown_bits_mask) >> z_shift | (~((data_type) 0) << sizeof(data_type) * 4 - z_shift)) & unknown_bits_mask);
+
+				} else if (z_shift < 0) {
+					dm_index[j] =
+							// Shift marked cells, insert zeros for new unknowns
+							(dm_index[j] & marked_bits_mask) << z_shift * -1 |
+							// Shift empty/unknown cells, insert ones for new unknowns
+						    ((dm_index[j] << z_shift * -1 & unknown_bits_mask) | ~(~((data_type) 0) << z_shift * -1));
+				}
+			}
 
-                } else if (z_shift < 0) {
-                    dm_index[j] = (dm_index[j] & marked_bits_mask) << z_shift * -1 |
-                                  (dm_index[j] << z_shift * -1 & unknown_bits_mask |
-                                   ~(~((unsigned int) 0) << z_shift * -1));
-                }
-            }
             dm_index += dm_size_x;
             sm_index += sm_size_x;
         }

src/costmap_2d/voxel_markers.cpp

@@ -0,0 +1,166 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *********************************************************************/
+
+/**
+ * Modified matlabbe:
+ * Added option to choose between unknown, free and marked cells
+ */
+
+#include <ros/ros.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <costmap_2d/VoxelGrid.h>
+#include <voxel_grid/voxel_grid.h>
+
+struct Cell
+{
+  double x;
+  double y;
+  double z;
+  voxel_grid::VoxelStatus status;
+};
+typedef std::vector<Cell> V_Cell;
+
+float g_colors_r[] = {0.0f, 1.0f, 1.0f};
+float g_colors_g[] = {1.0f, 1.0f, 0.0f};
+float g_colors_b[] = {1.0f, 1.0f, 0.0f};
+float g_colors_a[] = {0.5f, 0.1f, 0.5f};
+
+std::string g_marker_ns;
+V_Cell g_cells;
+int g_cell_type;
+void voxelCallback(const ros::Publisher& pub, const costmap_2d::VoxelGridConstPtr& grid)
+{
+  if (grid->data.empty())
+  {
+    ROS_ERROR("Received empty voxel grid");
+    return;
+  }
+
+  ros::WallTime start = ros::WallTime::now();
+
+  ROS_DEBUG("Received voxel grid");
+  const std::string frame_id = grid->header.frame_id;
+  const ros::Time stamp = grid->header.stamp;
+  const uint32_t* data = &grid->data.front();
+  const double x_origin = grid->origin.x;
+  const double y_origin = grid->origin.y;
+  const double z_origin = grid->origin.z;
+  const double x_res = grid->resolutions.x;
+  const double y_res = grid->resolutions.y;
+  const double z_res = grid->resolutions.z;
+  const uint32_t x_size = grid->size_x;
+  const uint32_t y_size = grid->size_y;
+  const uint32_t z_size = grid->size_z;
+
+  g_cells.clear();
+  uint32_t num_markers = 0;
+  for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid)
+  {
+    for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid)
+    {
+      for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid)
+      {
+        voxel_grid::VoxelStatus status = voxel_grid::VoxelGrid::getVoxel(x_grid, y_grid, z_grid, x_size, y_size, z_size,
+                                                                         data);
+
+        if (status == (voxel_grid::VoxelStatus)g_cell_type)
+        {
+          Cell c;
+          c.status = status;
+          c.x = x_origin + (x_grid + 0.5) * x_res;
+          c.y = y_origin + (y_grid + 0.5) * y_res;
+          c.z = z_origin + (z_grid + 0.5) * z_res;
+          g_cells.push_back(c);
+
+          ++num_markers;
+        }
+      }
+    }
+  }
+
+  visualization_msgs::Marker m;
+  m.header.frame_id = frame_id;
+  m.header.stamp = stamp;
+  m.ns = g_marker_ns;
+  m.id = 0;
+  m.type = visualization_msgs::Marker::CUBE_LIST;
+  m.action = visualization_msgs::Marker::ADD;
+  m.pose.orientation.w = 1.0;
+  m.scale.x = x_res;
+  m.scale.y = y_res;
+  m.scale.z = z_res;
+  m.color.r = g_colors_r[g_cell_type];
+  m.color.g = g_colors_g[g_cell_type];
+  m.color.b = g_colors_b[g_cell_type];
+  m.color.a = g_colors_a[g_cell_type];
+  m.points.resize(num_markers);
+  for (uint32_t i = 0; i < num_markers; ++i)
+  {
+    Cell& c = g_cells[i];
+    geometry_msgs::Point& p = m.points[i];
+    p.x = c.x;
+    p.y = c.y;
+    p.z = c.z;
+  }
+
+  pub.publish(m);
+
+  ros::WallTime end = ros::WallTime::now();
+  ROS_DEBUG("Published %d markers in %f seconds", num_markers, (end - start).toSec());
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "3d_markers");
+  ros::NodeHandle n;
+  ros::NodeHandle pnh("~");
+ 
+  pnh.param("cell_type", g_cell_type, (int)voxel_grid::MARKED);
+  pnh.param("r", g_colors_r[g_cell_type], g_colors_r[g_cell_type]);
+  pnh.param("g", g_colors_g[g_cell_type], g_colors_g[g_cell_type]);
+  pnh.param("b", g_colors_b[g_cell_type], g_colors_b[g_cell_type]);
+  pnh.param("a", g_colors_a[g_cell_type], g_colors_a[g_cell_type]);
+
+  ROS_DEBUG("Startup");
+
+  ros::Publisher pub = n.advertise < visualization_msgs::Marker > ("visualization_marker", 1);
+  ros::Subscriber sub = n.subscribe < costmap_2d::VoxelGrid > ("voxel_grid", 1, boost::bind(voxelCallback, pub, _1));
+  g_marker_ns = n.resolveName("voxel_grid");
+
+  ros::spin();
+}