From 420d7403dda0e4ce008ea1868afb509f7e261d9a Mon Sep 17 00:00:00 2001 From: Prescillia Date: Mon, 4 Nov 2019 14:18:16 -0500 Subject: [PATCH] Voxel layer from costmap_2d package takes into account robot z position --- CMakeLists.txt | 6 +- costmap_plugins.xml | 3 + src/costmap_2d/voxel_layer.cpp | 489 +++++++++++++++++++++++++++++++++ src/costmap_2d/voxel_layer.h | 204 ++++++++++++++ 4 files changed, 701 insertions(+), 1 deletion(-) create mode 100644 src/costmap_2d/voxel_layer.cpp create mode 100644 src/costmap_2d/voxel_layer.h 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_