First version rtabmap_launch working

rtabmap_costmap_plugins/CMakeLists.txt

@@ -0,0 +1,67 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rtabmap_costmap_plugins)
+
+##################
+## Dependencies ##
+##################
+
+find_package(catkin REQUIRED COMPONENTS roscpp pcl_conversions costmap_2d)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES rtabmap_costmap_plugins rtabmap_costmap_plugins2
+  CATKIN_DEPENDS roscpp pcl_conversions costmap_2d
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+  ${catkin_INCLUDE_DIRS}
+)
+
+IF(${costmap_2d_VERSION_MAJOR} GREATER 1 OR ${costmap_2d_VERSION_MINOR} GREATER 15)
+  ADD_DEFINITIONS("-DCOSTMAP_2D_POINTCLOUD2")
+ENDIF(${costmap_2d_VERSION_MAJOR} GREATER 1 OR ${costmap_2d_VERSION_MINOR} GREATER 15)
+add_library(rtabmap_costmap_plugins
+   src/static_layer.cpp
+)
+add_library(rtabmap_costmap_plugins2
+   src/voxel_layer.cpp
+)
+target_link_libraries(rtabmap_costmap_plugins
+  ${catkin_LIBRARIES}
+)
+target_link_libraries(rtabmap_costmap_plugins2
+  ${catkin_LIBRARIES}
+)
+add_executable(rtabmap_costmap_voxel_markers src/voxel_markers.cpp)
+target_link_libraries(rtabmap_costmap_voxel_markers ${catkin_LIBRARIES})
+set_target_properties(rtabmap_costmap_voxel_markers PROPERTIES OUTPUT_NAME "voxel_markers")
+
+
+#############
+## Install ##
+#############
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+   FILES_MATCHING PATTERN "*.h"
+)
+
+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}
+ )
+install(FILES
+   costmap_plugins.xml
+   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+

rtabmap_costmap_plugins/costmap_plugins.xml

@@ -0,0 +1,13 @@
+<class_libraries>
+  <library path="lib/librtabmap_costmap_plugins">
+    <class type="rtabmap_costmap_plugins::StaticLayer"     base_class_type="costmap_2d::Layer">
+      <description>Listens to OccupancyGrid messages and copies them in, like from map_server.</description>
+    </class>
+  </library>
+  <library path="lib/librtabmap_costmap_plugins2">
+    <class type="rtabmap_costmap_plugins::VoxelLayer"     base_class_type="costmap_2d::Layer">
+      <description>Similar to obstacle costmap, but uses 3D voxel grid to store data.</description>
+    </class>
+  </library>
+</class_libraries>
+

rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/static_layer.h

@@ -0,0 +1,100 @@
+/*********************************************************************
+ *
+ * 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!!
+ *********************************************************************/
+#ifndef RTABMAP_ROS_STATIC_LAYER_H_
+#define RTABMAP_ROS_STATIC_LAYER_H_
+
+#include <ros/ros.h>
+#include <costmap_2d/costmap_layer.h>
+#include <costmap_2d/layered_costmap.h>
+#include <costmap_2d/GenericPluginConfig.h>
+#include <dynamic_reconfigure/server.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <map_msgs/OccupancyGridUpdate.h>
+#include <message_filters/subscriber.h>
+
+namespace rtabmap_costmap_plugins
+{
+
+class StaticLayer : public costmap_2d::CostmapLayer
+{
+public:
+  StaticLayer();
+  virtual ~StaticLayer();
+  virtual void onInitialize();
+  virtual void activate();
+  virtual void deactivate();
+  virtual void reset();
+
+  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x,
+                             double* max_y);
+  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+
+  virtual void matchSize();
+
+private:
+  /**
+   * @brief  Callback to update the costmap's map from the map_server
+   * @param new_map The map to put into the costmap. The origin of the new
+   * map along with its size will determine what parts of the costmap's
+   * static map are overwritten.
+   */
+  void incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map);
+  void incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update);
+  void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);
+
+  unsigned char interpretValue(unsigned char value);
+
+  std::string global_frame_; ///< @brief The global frame for the costmap
+  bool subscribe_to_updates_;
+  bool map_received_;
+  bool has_updated_data_;
+  unsigned int x_,y_,width_,height_;
+  bool track_unknown_space_;
+  bool use_maximum_;
+  bool trinary_costmap_;
+  ros::Subscriber map_sub_, map_update_sub_;
+
+  unsigned char lethal_threshold_, unknown_cost_value_;
+
+  mutable boost::recursive_mutex lock_;
+  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
+};
+
+}  // namespace rtabmap_ros
+
+#endif  // RTABMAP_ROS_DYNAMIC_LAYER_H_

rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.h

@@ -0,0 +1,205 @@
+/*********************************************************************
+ *
+ * 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!!
+ *********************************************************************/
+#ifndef RTABMAP_ROS_VOXEL_LAYER_H_
+#define RTABMAP_ROS_VOXEL_LAYER_H_
+
+#include <costmap_2d/layer.h>
+#include <costmap_2d/layered_costmap.h>
+#include <costmap_2d/observation_buffer.h>
+#include <costmap_2d/VoxelGrid.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <sensor_msgs/LaserScan.h>
+#include <laser_geometry/laser_geometry.h>
+#include <sensor_msgs/PointCloud.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/point_cloud_conversion.h>
+#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 rtabmap_costmap_plugins
+{
+
+class VoxelLayer : public costmap_2d::ObstacleLayer
+{
+public:
+  VoxelLayer() :
+      voxel_grid_(0, 0, 0)
+  {
+    costmap_ = NULL;  // this is the unsigned char* member of parent class's parent class Costmap2D.
+  }
+
+  virtual ~VoxelLayer();
+
+  virtual void onInitialize();
+  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
+                            double* max_x, double* max_y);
+
+  void updateOrigin(double new_origin_x, double new_origin_y);
+  bool isDiscretized()
+  {
+    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_;
+  std::string robot_base_frame_;
+  ros::Publisher voxel_pub_;
+  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)
+  {
+    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;
+
+    return false;
+  }
+
+  inline bool worldToMap3D(double wx, double wy, double wz, unsigned int& mx, unsigned int& my, unsigned int& mz)
+  {
+    if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
+      return false;
+
+    mx = (int)((wx - origin_x_) / resolution_);
+    my = (int)((wy - origin_y_) / resolution_);
+    mz = (int)((wz - origin_z_) / z_resolution_);
+
+    if (mx < size_x_ && my < size_y_ && mz < size_z_)
+      return true;
+
+    return false;
+  }
+
+  inline void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double& wx, double& wy, double& wz)
+  {
+    // returns the center point of the cell
+    wx = origin_x_ + (mx + 0.5) * resolution_;
+    wy = origin_y_ + (my + 0.5) * resolution_;
+    wz = origin_z_ + (mz + 0.5) * z_resolution_;
+  }
+
+  inline double dist(double x0, double y0, double z0, double x1, double y1, double z1)
+  {
+    return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
+  }
+
+
+  /**
+     * @brief  Copy a region of a source map into a destination map
+     * @param  source_map The source map
+     * @param sm_lower_left_x The lower left x point of the source map to start the copy
+     * @param sm_lower_left_y The lower left y point of the source map to start the copy
+     * @param sm_size_x The x size of the source map
+     * @param  dest_map The destination map
+     * @param dm_lower_left_x The lower left x point of the destination map to start the copy
+     * @param dm_lower_left_y The lower left y point of the destination map to start the copy
+     * @param dm_size_x The x size of the destination map
+     * @param region_size_x The x size of the region to copy
+     * @param region_size_y The y size of the region to copy
+     */
+    template<typename data_type>
+    void copyMapRegion3D(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y,
+                       unsigned int sm_size_x, data_type* dest_map, unsigned int dm_lower_left_x,
+                       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);
+        data_type* dm_index = dest_map + (dm_lower_left_y * dm_size_x  + dm_lower_left_x);
+
+        uint32_t marked_bits_mask = (data_type) 0xFFFF0000;
+        uint32_t unknown_bits_mask = (data_type) 0x0000FFFF;
+
+        // now, we'll copy the source map into the destination map
+        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 < region_size_x; j++) {
+            	// known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits
+				if (z_shift > 0) {
+					dm_index[j] =
+							// 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));
+				}
+			}
+
+            dm_index += dm_size_x;
+            sm_index += sm_size_x;
+        }
+    }
+};
+
+}  // namespace rtabmap_ros
+
+#endif  // RTABMAP_ROS_VOXEL_LAYER_H_

rtabmap_costmap_plugins/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>rtabmap_costmap_plugins</name>
+  <version>0.1.0</version>
+  <description>RTAB-Map's costmap_2d plugins</description>
+  <maintainer email="[email protected]">Mathieu Labbe</maintainer>
+  <author>Mathieu Labbe</author>
+  <license>BSD</license>
+  <url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
+  <url type="repository">https://github.com/introlab/rtabmap_ros</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>genmsg</buildtool_depend>
+
+  <build_depend>libpcl-all-dev</build_depend>
+  <build_depend>message_generation</build_depend>
+  <depend>costmap_2d</depend>
+  <depend>pcl_conversions</depend>
+  <depend>roscpp</depend>
+
+  <export>
+    <costmap_2d plugin="${prefix}/costmap_plugins.xml"/>
+  </export>
+</package>

rtabmap_costmap_plugins/src/static_layer.cpp

@@ -0,0 +1,266 @@
+/*********************************************************************
+ *
+ * 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: added "layered_costmap_->updateMap(0,0,0);" below
+ */
+
+#include "rtabmap_costmap_plugins/static_layer.h"
+#include <costmap_2d/costmap_math.h>
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_costmap_plugins::StaticLayer, costmap_2d::Layer)
+
+using costmap_2d::NO_INFORMATION;
+using costmap_2d::LETHAL_OBSTACLE;
+using costmap_2d::FREE_SPACE;
+
+namespace rtabmap_costmap_plugins
+{
+
+StaticLayer::StaticLayer() : dsrv_(NULL) {}
+
+StaticLayer::~StaticLayer()
+{
+    if(dsrv_)
+        delete dsrv_;
+}
+
+void StaticLayer::onInitialize()
+{
+  ros::NodeHandle nh("~/" + name_), g_nh;
+  current_ = true;
+
+  global_frame_ = layered_costmap_->getGlobalFrameID();
+
+  std::string map_topic;
+  nh.param("map_topic", map_topic, std::string("map"));
+  nh.param("subscribe_to_updates", subscribe_to_updates_, false);
+  
+  nh.param("track_unknown_space", track_unknown_space_, true);
+  nh.param("use_maximum", use_maximum_, false);
+
+  int temp_lethal_threshold, temp_unknown_cost_value;
+  nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
+  nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
+  nh.param("trinary_costmap", trinary_costmap_, true);
+
+  lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
+  unknown_cost_value_ = temp_unknown_cost_value;
+  //we'll subscribe to the latched topic that the map server uses
+  ROS_INFO("Requesting the map...");
+  map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this);
+  map_received_ = false;
+  has_updated_data_ = false;
+
+  ros::Rate r(10);
+  while (!map_received_ && g_nh.ok())
+  {
+    ros::spinOnce();
+    r.sleep();
+  }
+
+  ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
+  
+  if(subscribe_to_updates_)
+  {
+    ROS_INFO("Subscribing to updates");
+    map_update_sub_ = g_nh.subscribe(map_topic + "_updates", 10, &StaticLayer::incomingUpdate, this);
+  }
+
+  if(dsrv_)
+  {
+    delete dsrv_;
+  }
+
+  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
+  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
+      &StaticLayer::reconfigureCB, this, boost::placeholders::_1, boost::placeholders::_2);
+  dsrv_->setCallback(cb);
+}
+
+void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
+{
+  if (config.enabled != enabled_)
+  {
+    enabled_ = config.enabled;
+    has_updated_data_ = true;
+    x_ = y_ = 0;
+    width_ = size_x_;
+    height_ = size_y_;
+  }
+}
+
+void StaticLayer::matchSize()
+{
+  Costmap2D* master = layered_costmap_->getCostmap();
+  resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
+            master->getOriginX(), master->getOriginY());
+}
+
+unsigned char StaticLayer::interpretValue(unsigned char value)
+{
+  //check if the static value is above the unknown or lethal thresholds
+  if (track_unknown_space_ && value == unknown_cost_value_)
+    return NO_INFORMATION;
+  else if (value >= lethal_threshold_)
+    return LETHAL_OBSTACLE;
+  else if (trinary_costmap_)
+    return FREE_SPACE;
+
+  double scale = (double) value / lethal_threshold_;
+  return scale * LETHAL_OBSTACLE;
+}
+
+void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
+{
+  unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
+
+  ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);
+
+  // resize costmap if size, resolution or origin do not match
+  Costmap2D* master = layered_costmap_->getCostmap();
+  if (master->getSizeInCellsX() != size_x ||
+      master->getSizeInCellsY() != size_y ||
+      master->getResolution() != new_map->info.resolution ||
+      master->getOriginX() != new_map->info.origin.position.x ||
+      master->getOriginY() != new_map->info.origin.position.y ||
+      !layered_costmap_->isSizeLocked())
+  {
+    ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
+    layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
+                                new_map->info.origin.position.y, true);
+  }else if(size_x_ != size_x || size_y_ != size_y ||
+      resolution_ != new_map->info.resolution ||
+      origin_x_ != new_map->info.origin.position.x ||
+      origin_y_ != new_map->info.origin.position.y){
+    matchSize();
+  }
+
+  unsigned int index = 0;
+
+  //initialize the costmap with static data
+  for (unsigned int i = 0; i < size_y; ++i)
+  {
+    for (unsigned int j = 0; j < size_x; ++j)
+    {
+      unsigned char value = new_map->data[index];
+      costmap_[index] = interpretValue(value);
+      ++index;
+    }
+  }
+  x_ = y_ = 0;
+  width_ = size_x_;
+  height_ = size_y_;
+  map_received_ = true;
+  has_updated_data_ = true;
+
+  layered_costmap_->updateMap(0,0,0);
+}
+
+void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update)
+{
+    unsigned int di = 0;
+    for (unsigned int y = 0; y < update->height ; y++)
+    {
+        unsigned int index_base = (update->y + y) * size_x_;
+        for (unsigned int x = 0; x < update->width ; x++)
+        {
+            unsigned int index = index_base + x + update->x;
+            costmap_[index] = interpretValue( update->data[di++] );
+        }
+    }
+    x_ = update->x;
+    y_ = update->y;
+    width_ = update->width;
+    height_ = update->height;
+    has_updated_data_ = true;
+
+    layered_costmap_->updateMap(0,0,0);
+}
+
+void StaticLayer::activate()
+{
+    onInitialize();
+}
+
+void StaticLayer::deactivate()
+{
+    map_sub_.shutdown();
+    if (subscribe_to_updates_)
+        map_update_sub_.shutdown();
+}
+
+void StaticLayer::reset()
+{
+    deactivate();
+    activate();
+}
+
+void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
+                               double* max_x, double* max_y)
+{
+  if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))
+    return;
+    
+  useExtraBounds(min_x, min_y, max_x, max_y);
+
+  double mx, my;
+  
+  mapToWorld(x_, y_, mx, my);
+  *min_x = std::min(mx, *min_x);
+  *min_y = std::min(my, *min_y);
+  
+  mapToWorld(x_ + width_, y_ + height_, mx, my);
+  *max_x = std::max(mx, *max_x);
+  *max_y = std::max(my, *max_y);
+  
+  has_updated_data_ = false;
+}
+
+void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+{
+  if (!map_received_)
+    return;
+  if (!use_maximum_)
+    updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
+  else
+    updateWithMax(master_grid, min_i, min_j, max_i, max_j);
+}
+
+}  // namespace rtabmap_ros

rtabmap_costmap_plugins/src/voxel_layer.cpp

@@ -0,0 +1,488 @@
+/*********************************************************************
+ *
+ * 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!!
+ *********************************************************************/
+#include "rtabmap_costmap_plugins/voxel_layer.h"
+#include <pluginlib/class_list_macros.hpp>
+#include <sensor_msgs/point_cloud2_iterator.h>
+#include <boost/thread.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+
+#define VOXEL_BITS 16
+PLUGINLIB_EXPORT_CLASS(rtabmap_costmap_plugins::VoxelLayer, costmap_2d::Layer)
+
+using costmap_2d::NO_INFORMATION;
+using costmap_2d::LETHAL_OBSTACLE;
+using costmap_2d::FREE_SPACE;
+
+using costmap_2d::ObservationBuffer;
+using costmap_2d::Observation;
+
+
+namespace rtabmap_costmap_plugins
+{
+
+void VoxelLayer::onInitialize()
+{
+  ObstacleLayer::onInitialize();
+  ros::NodeHandle private_nh("~/" + name_);
+  
+  std::string costmap_name = name_.substr(0, name_.find("/"));
+  ros::NodeHandle pnh("~/" + costmap_name);
+
+  private_nh.param("publish_voxel_map", publish_voxel_, false);
+  // 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 < 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<costmap_2d::VoxelPluginConfig>(nh);
+  dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>::CallbackType cb = boost::bind(
+      &VoxelLayer::reconfigureCB, this, boost::placeholders::_1, boost::placeholders::_2);
+  voxel_dsrv_->setCallback(cb);
+}
+
+VoxelLayer::~VoxelLayer()
+{
+  if (voxel_dsrv_)
+    delete voxel_dsrv_;
+}
+
+void VoxelLayer::reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level)
+{
+  enabled_ = config.enabled;
+  footprint_clearing_enabled_ = config.footprint_clearing_enabled;
+  max_obstacle_height_ = config.max_obstacle_height;
+  size_z_ = config.z_voxels;
+  origin_z_ = config.origin_z;
+  z_resolution_ = config.z_resolution;
+  unknown_threshold_ = config.unknown_threshold + (VOXEL_BITS - size_z_);
+  mark_threshold_ = config.mark_threshold;
+  combination_method_ = config.combination_method;
+  matchSize();
+}
+
+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_);
+}
+
+void VoxelLayer::reset()
+{
+  deactivate();
+  resetMaps();
+  voxel_grid_.reset();
+  activate();
+}
+
+void VoxelLayer::resetMaps()
+{
+  Costmap2D::resetMaps();
+  voxel_grid_.reset();
+}
+
+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_)
+    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
+  if (!enabled_)
+    return;
+  useExtraBounds(min_x, min_y, max_x, max_y);
+
+  bool current = true;
+  std::vector<Observation> observations, clearing_observations;
+
+  // get the marking observations
+  current = getMarkingObservations(observations) && current;
+
+  // get the clearing observations
+  current = getClearingObservations(clearing_observations) && current;
+
+  // update the global current status
+  current_ = current;
+
+  // raytrace freespace
+  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
+  {
+    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
+  }
+
+  // place the new obstacles into a priority queue... each with a priority of zero to begin with
+  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
+  {
+    const Observation& obs = *it;
+
+#ifdef COSTMAP_2D_POINTCLOUD2
+    const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
+    // Note that these points are already filtered with max and min obstacle heights
+    // You will have to adjust param file to see points beyond default settings
+    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
+
+    for (unsigned int i = 0; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+        const double x = *iter_x;
+        const double y = *iter_y;
+        const double z = *iter_z;
+#else
+    pcl::PointCloud<pcl::PointXYZ>::const_iterator point_it = obs.cloud_->begin();
+    for (;point_it < obs.cloud_->end(); ++point_it) {
+        const double x = point_it->x;
+        const double y = point_it->y;
+        const double z = point_it->z;
+#endif
+      // now we need to compute the map coordinates for the observation
+      unsigned int mx, my, mz;
+
+      if (!worldToMap3D(x, y, z, mx, my, mz))
+        continue;
+
+      if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
+      {
+        unsigned int index = getIndex(mx, my);
+
+        costmap_[index] = LETHAL_OBSTACLE;
+        touch(x, y, min_x, min_y, max_x, max_y);
+      }
+    }
+  }
+
+  if (publish_voxel_)
+  {
+    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();
+    grid_msg.size_z = voxel_grid_.sizeZ();
+    grid_msg.data.resize(size);
+    memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
+
+    grid_msg.origin.x = origin_x_;
+    grid_msg.origin.y = origin_y_;
+    grid_msg.origin.z = origin_z_;
+
+    grid_msg.resolutions.x = resolution_;
+    grid_msg.resolutions.y = resolution_;
+    grid_msg.resolutions.z = z_resolution_;
+    grid_msg.header.frame_id = global_frame_;
+    grid_msg.header.stamp = ros::Time::now();
+    voxel_pub_.publish(grid_msg);
+  }
+
+  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
+}
+
+void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
+{
+  // get the cell coordinates of the center point of the window
+  unsigned int mx, my;
+  if (!worldToMap(wx, wy, mx, my))
+    return;
+
+  // compute the bounds of the window
+  double start_x = wx - w_size_x / 2;
+  double start_y = wy - w_size_y / 2;
+  double end_x = start_x + w_size_x;
+  double end_y = start_y + w_size_y;
+
+  // scale the window based on the bounds of the costmap
+  start_x = std::max(origin_x_, start_x);
+  start_y = std::max(origin_y_, start_y);
+
+  end_x = std::min(origin_x_ + getSizeInMetersX(), end_x);
+  end_y = std::min(origin_y_ + getSizeInMetersY(), end_y);
+
+  // get the map coordinates of the bounds of the window
+  unsigned int map_sx, map_sy, map_ex, map_ey;
+
+  // check for legality just in case
+  if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
+    return;
+
+  // we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
+  unsigned int index = getIndex(map_sx, map_sy);
+  unsigned char* current = &costmap_[index];
+  for (unsigned int j = map_sy; j <= map_ey; ++j)
+  {
+    for (unsigned int i = map_sx; i <= map_ex; ++i)
+    {
+      // if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it
+      if (*current != LETHAL_OBSTACLE)
+      {
+        if (clear_no_info || *current != NO_INFORMATION)
+        {
+          *current = FREE_SPACE;
+          voxel_grid_.clearVoxelColumn(index);
+        }
+      }
+      current++;
+      index++;
+    }
+    current += size_x_ - (map_ex - map_sx) - 1;
+    index += size_x_ - (map_ex - map_sx) - 1;
+  }
+}
+
+void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
+                                           double* max_x, double* max_y)
+{
+  size_t clearing_observation_cloud_size = clearing_observation.cloud_->height * clearing_observation.cloud_->width;
+  if (clearing_observation_cloud_size == 0)
+    return;
+
+  double sensor_x, sensor_y, sensor_z;
+  double ox = clearing_observation.origin_.x;
+  double oy = clearing_observation.origin_.y;
+  double oz = clearing_observation.origin_.z;
+
+  if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
+  {
+    ROS_WARN_THROTTLE(
+        1.0,
+        "The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
+        ox, oy, oz);
+    return;
+  }
+
+  bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
+  if (publish_clearing_points)
+  {
+    clearing_endpoints_.points.clear();
+    clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
+  }
+
+  // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
+  double map_end_x = origin_x_ + getSizeInMetersX();
+  double map_end_y = origin_y_ + getSizeInMetersY();
+  double map_end_z = origin_z_ + size_z_ * z_resolution_;
+
+#ifdef COSTMAP_2D_POINTCLOUD2
+    sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
+
+    for (;iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
+    {
+    double wpx = *iter_x;
+    double wpy = *iter_y;
+    double wpz = *iter_z;
+#else
+    pcl::PointCloud<pcl::PointXYZ>::const_iterator point_it = clearing_observation.cloud_->begin();
+    for (;point_it < clearing_observation.cloud_->end(); ++point_it) {
+        double wpx = point_it->x;
+        double wpy = point_it->y;
+        double wpz = point_it->z;
+#endif
+
+    double distance = dist(ox, oy, oz, wpx, wpy, wpz);
+    double scaling_fact = 1.0, scaling_fact_z = 1.0;
+    scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
+    scaling_fact_z = std::max(std::min(scaling_fact_z, (distance - 2 * z_resolution_) / distance), 0.0);
+    wpx = scaling_fact * (wpx - ox) + ox;
+    wpy = scaling_fact * (wpy - oy) + oy;
+    wpz = scaling_fact_z * (wpz - oz) + oz;
+
+    double a = wpx - ox;
+    double b = wpy - oy;
+    double c = wpz - oz;
+    double t = 1.0;
+
+    // the minimum value to raytrace from is the origin
+    if (wpz < origin_z_)
+    {
+      t = std::min(t, (origin_z_ - oz) / c);
+    }
+    if (wpx < origin_x_)
+    {
+      t = std::min(t, (origin_x_ - ox) / a);
+    }
+    if (wpy < origin_y_)
+    {
+      t = std::min(t, (origin_y_ - oy) / b);
+    }
+
+    // the maximum value to raytrace to is the end of the map
+    if (wpx > map_end_x)
+    {
+      t = std::min(t, (map_end_x - ox) / a);
+    }
+    if (wpy > map_end_y)
+    {
+      t = std::min(t, (map_end_y - oy) / b);
+    }
+    if (wpz > map_end_z)
+    {
+      t = std::min(t, (map_end_z - oz) / c);
+    }
+
+    wpx = ox + a * t;
+    wpy = oy + b * t;
+    wpz = oz + c * t;
+
+    double point_x, point_y, point_z;
+    if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z))
+    {
+      unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
+
+      // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
+      voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
+                                      unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
+                                      cell_raytrace_range);
+
+      updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
+
+      if (publish_clearing_points)
+      {
+        geometry_msgs::Point32 point;
+        point.x = wpx;
+        point.y = wpy;
+        point.z = wpz;
+        clearing_endpoints_.points.push_back(point);
+      }
+    }
+  }
+
+  if (publish_clearing_points)
+  {
+    clearing_endpoints_.header.frame_id = global_frame_;
+#ifdef COSTMAP_2D_POINTCLOUD2
+    clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
+#else
+    clearing_endpoints_.header.stamp = pcl_conversions::fromPCL(clearing_observation.cloud_->header.stamp);
+#endif
+    clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
+
+    clearing_endpoints_pub_.publish(clearing_endpoints_);
+  }
+}
+
+void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
+{
+  int cell_oz;
+  // get the global pose of the robot
+  try
+  {
+      geometry_msgs::TransformStamped transformStamped;
+#ifdef COSTMAP_2D_POINTCLOUD2
+      transformStamped = tf_->lookupTransform(global_frame_, robot_base_frame_, ros::Time(0));
+#else
+      tf::StampedTransform stampedTransform;
+      tf_->lookupTransform(global_frame_, robot_base_frame_, ros::Time(0), stampedTransform);
+      tf::transformStampedTFToMsg(stampedTransform, transformStamped);
+#endif
+      const double robot_z = transformStamped.transform.translation.z;
+      const double z_grid_height = z_resolution_ * size_z_;
+      const double new_origin_z = robot_z - z_grid_height / 2;
+      cell_oz = int((new_origin_z - origin_z_) / z_resolution_);
+  }
+#ifdef COSTMAP_2D_POINTCLOUD2
+  catch (tf2::TransformException& ex)
+#else
+  catch(tf::TransformException& ex)
+#endif
+  {
+      ROS_ERROR("%s", ex.what());
+      // If the robot pose is not detected, the origin_z_ will remain the same.
+      cell_oz = 0;
+  }
+
+  // project the new origin into the grid
+  int cell_ox, cell_oy;
+  cell_ox = int((new_origin_x - origin_x_) / resolution_);
+  cell_oy = int((new_origin_y - origin_y_) / resolution_);
+
+  // compute the associated world coordinates for the origin cell
+  // because we want to keep things grid-aligned
+  double new_grid_ox, new_grid_oy, new_grid_oz;
+  new_grid_ox = origin_x_ + cell_ox * resolution_;
+  new_grid_oy = origin_y_ + cell_oy * resolution_;
+  new_grid_oz = origin_z_ + cell_oz * z_resolution_;
+
+  // to avoid casting from unsigned int to int a bunch of times
+  int size_x = size_x_;
+  int size_y = size_y_;
+
+  // 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;
+  lower_left_x = std::min(std::max(cell_ox, 0), size_x);
+  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_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_y = upper_right_y - lower_left_y;
+
+  // 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 int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
+  unsigned int* voxel_map = voxel_grid_.getData();
+
+  // copy the local window in the costmap to the local map
+  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
+  resetMaps();
+
+  // update the origin with the appropriate world coordinates
+  origin_x_ = new_grid_ox;
+  origin_y_ = new_grid_oy;
+  origin_z_ = new_grid_oz;
+
+  // compute the starting cell location for copying data back in
+  int start_x = lower_left_x - cell_ox;
+  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
+  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
+  delete[] local_map;
+  delete[] local_voxel_map;
+}
+
+}  // namespace rtabmap_ros

rtabmap_costmap_plugins/src/voxel_markers.cpp

@@ -0,0 +1,182 @@
+/*********************************************************************
+ *
+ * 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());
+}
+
+ros::Publisher pub;
+ros::Subscriber sub;
+void connectCb()
+{
+	ros::NodeHandle n;
+	sub = n.subscribe < costmap_2d::VoxelGrid > ("voxel_grid", 1, boost::bind(voxelCallback, pub, boost::placeholders::_1));
+}
+
+void disconnectCb()
+{
+	if(pub.getNumSubscribers()==0)
+		sub.shutdown();
+}
+
+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::SubscriberStatusCallback connect_cb = boost::bind(connectCb);
+  ros::SubscriberStatusCallback disconnect_cb = boost::bind(disconnectCb);
+
+  pub = n.advertise < visualization_msgs::Marker > ("visualization_marker", 1, connect_cb, disconnect_cb);
+  g_marker_ns = n.resolveName("voxel_grid");
+
+  ros::spin();
+}