From e919f66640fb5ad9cd5335c8cc02e82992703b21 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 27 Aug 2020 18:25:15 -0400 Subject: [PATCH] costmap_2d: fixed voxel_layer memory corruption (and refactored to make it easier to compare with original version). Added voxel_marker node with option to show occupied, free or unknown cells of a voxel grid msg. --- CMakeLists.txt | 4 + src/costmap_2d/voxel_layer.cpp | 153 ++++++++++++++-------------- src/costmap_2d/voxel_layer.h | 39 ++++---- src/costmap_2d/voxel_markers.cpp | 166 +++++++++++++++++++++++++++++++ 4 files changed, 266 insertions(+), 96 deletions(-) create mode 100644 src/costmap_2d/voxel_markers.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index bb264cd1..2add7fbd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -500,6 +500,9 @@ IF(costmap_2d_FOUND) target_link_libraries(rtabmap_costmap_plugins2 ${costmap_2d_LIBRARIES} ) + add_executable(rtabmap_costmap_voxel_markers src/costmap_2d/voxel_markers.cpp) + target_link_libraries(rtabmap_costmap_voxel_markers ${costmap_2d_LIBRARIES}) + set_target_properties(rtabmap_costmap_voxel_markers PROPERTIES OUTPUT_NAME "voxel_markers") ENDIF(costmap_2d_FOUND) ############# @@ -600,6 +603,7 @@ IF(costmap_2d_FOUND) install(TARGETS rtabmap_costmap_plugins rtabmap_costmap_plugins2 + rtabmap_costmap_voxel_markers ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/src/costmap_2d/voxel_layer.cpp b/src/costmap_2d/voxel_layer.cpp index 78d3560a..0e035cdf 100644 --- a/src/costmap_2d/voxel_layer.cpp +++ b/src/costmap_2d/voxel_layer.cpp @@ -48,10 +48,8 @@ using costmap_2d::NO_INFORMATION; using costmap_2d::LETHAL_OBSTACLE; using costmap_2d::FREE_SPACE; -using costmap_2d::Costmap2D; using costmap_2d::ObservationBuffer; using costmap_2d::Observation; -using costmap_2d::VoxelGrid; namespace rtabmap_ros @@ -62,22 +60,23 @@ void VoxelLayer::onInitialize() ObstacleLayer::onInitialize(); ros::NodeHandle private_nh("~/" + name_); - std::string costmap_name_ = name_.substr(0, name_.find("/")); - ros::NodeHandle pnh("~/" + costmap_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")); + // 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 ("voxel_grid", 1); + voxel_pub_ = private_nh.advertise < costmap_2d::VoxelGrid > ("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( + voxel_dsrv_ = new dynamic_reconfigure::Server(nh); + dynamic_reconfigure::Server::CallbackType cb = boost::bind( &VoxelLayer::reconfigureCB, this, _1, _2); voxel_dsrv_->setCallback(cb); } @@ -88,7 +87,7 @@ VoxelLayer::~VoxelLayer() delete voxel_dsrv_; } -void VoxelLayer::reconfigureCB(VoxelPluginConfig &config, uint32_t level) +void VoxelLayer::reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level) { enabled_ = config.enabled; footprint_clearing_enabled_ = config.footprint_clearing_enabled; @@ -106,7 +105,7 @@ void VoxelLayer::matchSize() { ObstacleLayer::matchSize(); voxel_grid_.resize(size_x_, size_y_, size_z_); - // ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_ && voxel_grid_.sizeZ() == size_z_); + ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_); } void VoxelLayer::reset() @@ -126,13 +125,10 @@ void VoxelLayer::resetMaps() void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y) { - if (rolling_window_) { - updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2 ); - } - + 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; @@ -195,7 +191,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, if (publish_voxel_) { - VoxelGrid grid_msg; + costmap_2d::VoxelGrid grid_msg; unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY(); grid_msg.size_x = voxel_grid_.sizeX(); grid_msg.size_y = voxel_grid_.sizeY(); @@ -404,86 +400,89 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) { - int cell_oz; - // get the global pose of the robot - try - { - geometry_msgs::TransformStamped transformStamped; + 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)); + 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); + 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_); - } + 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) + catch (tf2::TransformException& ex) #else - catch(tf::TransformException& ex) + 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; - } + { + 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_); + // 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_; + // 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_; + // 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); + // 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; + 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(); + // 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, + // 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(); + // 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; + // 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; + // 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); + // 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_voxel_map; + // make sure to clean up + delete[] local_map; + delete[] local_voxel_map; } } // namespace rtabmap_ros diff --git a/src/costmap_2d/voxel_layer.h b/src/costmap_2d/voxel_layer.h index d4c2b0cc..74bc07b2 100644 --- a/src/costmap_2d/voxel_layer.h +++ b/src/costmap_2d/voxel_layer.h @@ -54,8 +54,6 @@ #include #include -using costmap_2d::VoxelPluginConfig; - namespace rtabmap_ros { @@ -65,7 +63,6 @@ public: VoxelLayer() : voxel_grid_(0, 0, 0) { - costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D. } @@ -83,18 +80,19 @@ public: virtual void matchSize(); virtual void reset(); + protected: virtual void setupDynamicReconfigure(ros::NodeHandle& nh); virtual void resetMaps(); private: - void reconfigureCB(VoxelPluginConfig &config, uint32_t level); + void reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level); void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info); virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y, double* max_x, double* max_y); - dynamic_reconfigure::Server *voxel_dsrv_; + dynamic_reconfigure::Server *voxel_dsrv_; bool publish_voxel_; std::string robot_base_frame_; @@ -109,11 +107,9 @@ private: { if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_) return false; - mx = ((wx - origin_x_) / resolution_); my = ((wy - origin_y_) / resolution_); mz = ((wz - origin_z_) / z_resolution_); - if (mx < size_x_ && my < size_y_ && mz < size_z_) return true; @@ -168,7 +164,6 @@ private: unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x, unsigned int region_size_y, int z_shift) { - // we'll first need to compute the starting points for each map // this is like getting voxel column. We are not taking into account the z position of the voxel data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x); @@ -181,18 +176,24 @@ private: for (unsigned int i = 0; i < region_size_y; ++i) { memcpy(dm_index, sm_index, region_size_x * sizeof(data_type)); - for (unsigned int j = 0; j < dm_size_x; j++) { - 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; + 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)); + } + } - } 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; } diff --git a/src/costmap_2d/voxel_markers.cpp b/src/costmap_2d/voxel_markers.cpp new file mode 100644 index 00000000..1f93a887 --- /dev/null +++ b/src/costmap_2d/voxel_markers.cpp @@ -0,0 +1,166 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ + +/** + * Modified matlabbe: + * Added option to choose between unknown, free and marked cells + */ + +#include +#include +#include +#include + +struct Cell +{ + double x; + double y; + double z; + voxel_grid::VoxelStatus status; +}; +typedef std::vector V_Cell; + +float g_colors_r[] = {0.0f, 1.0f, 1.0f}; +float g_colors_g[] = {1.0f, 1.0f, 0.0f}; +float g_colors_b[] = {1.0f, 1.0f, 0.0f}; +float g_colors_a[] = {0.5f, 0.1f, 0.5f}; + +std::string g_marker_ns; +V_Cell g_cells; +int g_cell_type; +void voxelCallback(const ros::Publisher& pub, const costmap_2d::VoxelGridConstPtr& grid) +{ + if (grid->data.empty()) + { + ROS_ERROR("Received empty voxel grid"); + return; + } + + ros::WallTime start = ros::WallTime::now(); + + ROS_DEBUG("Received voxel grid"); + const std::string frame_id = grid->header.frame_id; + const ros::Time stamp = grid->header.stamp; + const uint32_t* data = &grid->data.front(); + const double x_origin = grid->origin.x; + const double y_origin = grid->origin.y; + const double z_origin = grid->origin.z; + const double x_res = grid->resolutions.x; + const double y_res = grid->resolutions.y; + const double z_res = grid->resolutions.z; + const uint32_t x_size = grid->size_x; + const uint32_t y_size = grid->size_y; + const uint32_t z_size = grid->size_z; + + g_cells.clear(); + uint32_t num_markers = 0; + for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid) + { + for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid) + { + for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid) + { + voxel_grid::VoxelStatus status = voxel_grid::VoxelGrid::getVoxel(x_grid, y_grid, z_grid, x_size, y_size, z_size, + data); + + if (status == (voxel_grid::VoxelStatus)g_cell_type) + { + Cell c; + c.status = status; + c.x = x_origin + (x_grid + 0.5) * x_res; + c.y = y_origin + (y_grid + 0.5) * y_res; + c.z = z_origin + (z_grid + 0.5) * z_res; + g_cells.push_back(c); + + ++num_markers; + } + } + } + } + + visualization_msgs::Marker m; + m.header.frame_id = frame_id; + m.header.stamp = stamp; + m.ns = g_marker_ns; + m.id = 0; + m.type = visualization_msgs::Marker::CUBE_LIST; + m.action = visualization_msgs::Marker::ADD; + m.pose.orientation.w = 1.0; + m.scale.x = x_res; + m.scale.y = y_res; + m.scale.z = z_res; + m.color.r = g_colors_r[g_cell_type]; + m.color.g = g_colors_g[g_cell_type]; + m.color.b = g_colors_b[g_cell_type]; + m.color.a = g_colors_a[g_cell_type]; + m.points.resize(num_markers); + for (uint32_t i = 0; i < num_markers; ++i) + { + Cell& c = g_cells[i]; + geometry_msgs::Point& p = m.points[i]; + p.x = c.x; + p.y = c.y; + p.z = c.z; + } + + pub.publish(m); + + ros::WallTime end = ros::WallTime::now(); + ROS_DEBUG("Published %d markers in %f seconds", num_markers, (end - start).toSec()); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "3d_markers"); + ros::NodeHandle n; + ros::NodeHandle pnh("~"); + + pnh.param("cell_type", g_cell_type, (int)voxel_grid::MARKED); + pnh.param("r", g_colors_r[g_cell_type], g_colors_r[g_cell_type]); + pnh.param("g", g_colors_g[g_cell_type], g_colors_g[g_cell_type]); + pnh.param("b", g_colors_b[g_cell_type], g_colors_b[g_cell_type]); + pnh.param("a", g_colors_a[g_cell_type], g_colors_a[g_cell_type]); + + ROS_DEBUG("Startup"); + + ros::Publisher pub = n.advertise < visualization_msgs::Marker > ("visualization_marker", 1); + ros::Subscriber sub = n.subscribe < costmap_2d::VoxelGrid > ("voxel_grid", 1, boost::bind(voxelCallback, pub, _1)); + g_marker_ns = n.resolveName("voxel_grid"); + + ros::spin(); +}