diff --git a/CMakeLists.txt b/CMakeLists.txt
index bd346f16..dbf8a459 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -129,7 +129,10 @@ generate_dynamic_reconfigure_options(cfg/Camera.cfg)
SET(optional_dependencies "")
IF(costmap_2d_FOUND)
- SET(optional_dependencies ${optional_dependencies} costmap_2d)
+ SET(optional_dependencies ${optional_dependencies} costmap_2d)
+ 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)
ENDIF(costmap_2d_FOUND)
IF(octomap_msgs_FOUND)
SET(optional_dependencies ${optional_dependencies} octomap_msgs)
@@ -230,6 +233,7 @@ SET(Libraries
)
SET(rtabmap_plugins_lib_src
src/costmap_2d/static_layer.cpp
+ src/costmap_2d/voxel_layer.cpp
${rtabmap_plugins_lib_src}
)
ENDIF(costmap_2d_FOUND)
diff --git a/costmap_plugins.xml b/costmap_plugins.xml
index f3a59e21..8988bb55 100644
--- a/costmap_plugins.xml
+++ b/costmap_plugins.xml
@@ -3,6 +3,9 @@
Listens to OccupancyGrid messages and copies them in, like from map_server.
+
+ Similar to obstacle costmap, but uses 3D voxel grid to store data.
+
diff --git a/src/costmap_2d/voxel_layer.cpp b/src/costmap_2d/voxel_layer.cpp
new file mode 100644
index 00000000..78d3560a
--- /dev/null
+++ b/src/costmap_2d/voxel_layer.cpp
@@ -0,0 +1,489 @@
+/*********************************************************************
+ *
+ * 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 "voxel_layer.h"
+#include
+#include
+#include
+#include
+
+#define VOXEL_BITS 16
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::VoxelLayer, costmap_2d::Layer)
+
+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
+{
+
+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);
+ pnh.param("robot_frame", robot_base_frame_, std::string("base_link"));
+
+ if (publish_voxel_)
+ voxel_pub_ = private_nh.advertise ("voxel_grid", 1);
+
+ clearing_endpoints_pub_ = private_nh.advertise("clearing_endpoints", 1);
+}
+
+void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
+{
+ voxel_dsrv_ = new dynamic_reconfigure::Server(nh);
+ dynamic_reconfigure::Server::CallbackType cb = boost::bind(
+ &VoxelLayer::reconfigureCB, this, _1, _2);
+ voxel_dsrv_->setCallback(cb);
+}
+
+VoxelLayer::~VoxelLayer()
+{
+ if (voxel_dsrv_)
+ delete voxel_dsrv_;
+}
+
+void VoxelLayer::reconfigureCB(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_ && voxel_grid_.sizeZ() == size_z_);
+}
+
+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 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::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 iter_x(cloud, "x");
+ sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y");
+ sensor_msgs::PointCloud2ConstIterator 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::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_)
+ {
+ 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 iter_x(*(clearing_observation.cloud_), "x");
+ sensor_msgs::PointCloud2ConstIterator iter_y(*(clearing_observation.cloud_), "y");
+ sensor_msgs::PointCloud2ConstIterator 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::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(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
+ 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_voxel_map;
+}
+
+} // namespace rtabmap_ros
diff --git a/src/costmap_2d/voxel_layer.h b/src/costmap_2d/voxel_layer.h
new file mode 100644
index 00000000..d4c2b0cc
--- /dev/null
+++ b/src/costmap_2d/voxel_layer.h
@@ -0,0 +1,204 @@
+/*********************************************************************
+ *
+ * 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
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+using costmap_2d::VoxelPluginConfig;
+
+namespace rtabmap_ros
+{
+
+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(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 *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
+ 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 < dm_size_x; j++) {
+ if (z_shift > 0) {
+ dm_index[j] = ((dm_index[j] & marked_bits_mask) >> z_shift & marked_bits_mask) |
+ ((~((data_type) 0) << sizeof(data_type) * 4 - z_shift) |
+ (dm_index[j] & unknown_bits_mask) >> z_shift) & unknown_bits_mask;
+
+ } 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;
+ }
+ }
+};
+
+} // namespace rtabmap_ros
+
+#endif // RTABMAP_ROS_VOXEL_LAYER_H_