diff --git a/octomap_server/.vscode/settings.json b/octomap_server/.vscode/settings.json new file mode 100644 index 00000000..a1ce6e7c --- /dev/null +++ b/octomap_server/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "limits": "cpp" + } +} \ No newline at end of file diff --git a/octomap_server/include/octomap_server/OctomapServer.h b/octomap_server/include/octomap_server/OctomapServer.h index 414566b3..f3412a68 100644 --- a/octomap_server/include/octomap_server/OctomapServer.h +++ b/octomap_server/include/octomap_server/OctomapServer.h @@ -243,6 +243,8 @@ class OctomapServer { double m_pointcloudMaxZ; double m_occupancyMinZ; double m_occupancyMaxZ; + double m_projectedMapMinZ; + double m_projectedMapMaxZ; double m_minSizeX; double m_minSizeY; bool m_filterSpeckles; diff --git a/octomap_server/src/OctomapServer.cpp b/octomap_server/src/OctomapServer.cpp index 0a2176eb..4b9fb723 100644 --- a/octomap_server/src/OctomapServer.cpp +++ b/octomap_server/src/OctomapServer.cpp @@ -32,46 +32,47 @@ using namespace octomap; using octomap_msgs::Octomap; -bool is_equal (double a, double b, double epsilon = 1.0e-7) -{ - return std::abs(a - b) < epsilon; -} - -namespace octomap_server{ - -OctomapServer::OctomapServer(const ros::NodeHandle private_nh_, const ros::NodeHandle &nh_) -: m_nh(nh_), - m_nh_private(private_nh_), - m_pointCloudSub(NULL), - m_tfPointCloudSub(NULL), - m_reconfigureServer(m_config_mutex, private_nh_), - m_octree(NULL), - m_maxRange(-1.0), - m_minRange(-1.0), - m_worldFrameId("map"), m_baseFrameId("base_footprint"), - m_useHeightMap(true), - m_useColoredMap(false), - m_colorFactor(0.8), - m_latchedTopics(true), - m_publishFreeSpace(false), - m_res(0.05), - m_treeDepth(0), - m_maxTreeDepth(0), - m_pointcloudMinX(-std::numeric_limits::max()), - m_pointcloudMaxX(std::numeric_limits::max()), - m_pointcloudMinY(-std::numeric_limits::max()), - m_pointcloudMaxY(std::numeric_limits::max()), - m_pointcloudMinZ(-std::numeric_limits::max()), - m_pointcloudMaxZ(std::numeric_limits::max()), - m_occupancyMinZ(-std::numeric_limits::max()), - m_occupancyMaxZ(std::numeric_limits::max()), - m_minSizeX(0.0), m_minSizeY(0.0), - m_filterSpeckles(false), m_filterGroundPlane(false), - m_groundFilterDistance(0.04), m_groundFilterAngle(0.15), m_groundFilterPlaneDistance(0.07), - m_compressMap(true), - m_incrementalUpdate(false), - m_initConfig(true) -{ +bool is_equal(double a, double b, double epsilon = 1.0e-7) { return std::abs(a - b) < epsilon; } + +namespace octomap_server { + +OctomapServer::OctomapServer(const ros::NodeHandle private_nh_, const ros::NodeHandle& nh_) + : m_nh(nh_), + m_nh_private(private_nh_), + m_pointCloudSub(NULL), + m_tfPointCloudSub(NULL), + m_reconfigureServer(m_config_mutex, private_nh_), + m_octree(NULL), + m_maxRange(-1.0), + m_minRange(-1.0), + m_worldFrameId("map"), + m_baseFrameId("base_footprint"), + m_useHeightMap(true), + m_useColoredMap(false), + m_colorFactor(0.8), + m_latchedTopics(true), + m_publishFreeSpace(false), + m_res(0.05), + m_treeDepth(0), + m_maxTreeDepth(0), + m_pointcloudMinX(-std::numeric_limits::max()), + m_pointcloudMaxX(std::numeric_limits::max()), + m_pointcloudMinY(-std::numeric_limits::max()), + m_pointcloudMaxY(std::numeric_limits::max()), + m_pointcloudMinZ(-std::numeric_limits::max()), + m_pointcloudMaxZ(std::numeric_limits::max()), + m_occupancyMinZ(-std::numeric_limits::max()), + m_occupancyMaxZ(std::numeric_limits::max()), + m_minSizeX(0.0), + m_minSizeY(0.0), + m_filterSpeckles(false), + m_filterGroundPlane(false), + m_groundFilterDistance(0.04), + m_groundFilterAngle(0.15), + m_groundFilterPlaneDistance(0.07), + m_compressMap(true), + m_incrementalUpdate(false), + m_initConfig(true) { double probHit, probMiss, thresMin, thresMax; m_nh_private.param("frame_id", m_worldFrameId, m_worldFrameId); @@ -80,16 +81,18 @@ OctomapServer::OctomapServer(const ros::NodeHandle private_nh_, const ros::NodeH m_nh_private.param("colored_map", m_useColoredMap, m_useColoredMap); m_nh_private.param("color_factor", m_colorFactor, m_colorFactor); - m_nh_private.param("pointcloud_min_x", m_pointcloudMinX,m_pointcloudMinX); - m_nh_private.param("pointcloud_max_x", m_pointcloudMaxX,m_pointcloudMaxX); - m_nh_private.param("pointcloud_min_y", m_pointcloudMinY,m_pointcloudMinY); - m_nh_private.param("pointcloud_max_y", m_pointcloudMaxY,m_pointcloudMaxY); - m_nh_private.param("pointcloud_min_z", m_pointcloudMinZ,m_pointcloudMinZ); - m_nh_private.param("pointcloud_max_z", m_pointcloudMaxZ,m_pointcloudMaxZ); - m_nh_private.param("occupancy_min_z", m_occupancyMinZ,m_occupancyMinZ); - m_nh_private.param("occupancy_max_z", m_occupancyMaxZ,m_occupancyMaxZ); - m_nh_private.param("min_x_size", m_minSizeX,m_minSizeX); - m_nh_private.param("min_y_size", m_minSizeY,m_minSizeY); + m_nh_private.param("pointcloud_min_x", m_pointcloudMinX, m_pointcloudMinX); + m_nh_private.param("pointcloud_max_x", m_pointcloudMaxX, m_pointcloudMaxX); + m_nh_private.param("pointcloud_min_y", m_pointcloudMinY, m_pointcloudMinY); + m_nh_private.param("pointcloud_max_y", m_pointcloudMaxY, m_pointcloudMaxY); + m_nh_private.param("pointcloud_min_z", m_pointcloudMinZ, m_pointcloudMinZ); + m_nh_private.param("pointcloud_max_z", m_pointcloudMaxZ, m_pointcloudMaxZ); + m_nh_private.param("occupancy_min_z", m_occupancyMinZ, m_occupancyMinZ); + m_nh_private.param("occupancy_max_z", m_occupancyMaxZ, m_occupancyMaxZ); + m_nh_private.param("projected_map_min_z", m_projectedMapMinZ, -1); + m_nh_private.param("projected_map_max_z", m_projectedMapMaxZ, -1); + m_nh_private.param("min_x_size", m_minSizeX, m_minSizeX); + m_nh_private.param("min_y_size", m_minSizeY, m_minSizeY); m_nh_private.param("filter_speckles", m_filterSpeckles, m_filterSpeckles); m_nh_private.param("filter_ground", m_filterGroundPlane, m_filterGroundPlane); @@ -111,10 +114,10 @@ OctomapServer::OctomapServer(const ros::NodeHandle private_nh_, const ros::NodeH m_nh_private.param("compress_map", m_compressMap, m_compressMap); m_nh_private.param("incremental_2D_projection", m_incrementalUpdate, m_incrementalUpdate); - if (m_filterGroundPlane && (m_pointcloudMinZ > 0.0 || m_pointcloudMaxZ < 0.0)){ - ROS_WARN_STREAM("You enabled ground filtering but incoming pointclouds will be pre-filtered in [" - < 0.0 || m_pointcloudMaxZ < 0.0)) { + ROS_WARN_STREAM("You enabled ground filtering but incoming pointclouds will be pre-filtered in [" << m_pointcloudMinZ << ", " << m_pointcloudMaxZ + << "], excluding the ground level z=0. " + << "This will not work."); } if (m_useHeightMap && m_useColoredMap) { @@ -126,7 +129,9 @@ OctomapServer::OctomapServer(const ros::NodeHandle private_nh_, const ros::NodeH #ifdef COLOR_OCTOMAP_SERVER ROS_INFO_STREAM("Using RGB color registration (if information available)"); #else - ROS_ERROR_STREAM("Colored map requested in launch file - node not running/compiled to support colors, please define COLOR_OCTOMAP_SERVER and recompile or launch the octomap_color_server node"); + ROS_ERROR_STREAM( + "Colored map requested in launch file - node not running/compiled to support colors, please define COLOR_OCTOMAP_SERVER and recompile or launch the " + "octomap_color_server node"); #endif } @@ -162,7 +167,7 @@ OctomapServer::OctomapServer(const ros::NodeHandle private_nh_, const ros::NodeH m_nh_private.param("publish_free_space", m_publishFreeSpace, m_publishFreeSpace); m_nh_private.param("latch", m_latchedTopics, m_latchedTopics); - if (m_latchedTopics){ + if (m_latchedTopics) { ROS_INFO("Publishing latched (single publish will take longer, all topics are prepared)"); } else ROS_INFO("Publishing non-latched (topics are only prepared as needed, will only be re-published on map change"); @@ -174,8 +179,8 @@ OctomapServer::OctomapServer(const ros::NodeHandle private_nh_, const ros::NodeH m_mapPub = m_nh.advertise("projected_map", 5, m_latchedTopics); m_fmarkerPub = m_nh.advertise("free_cells_vis_array", 1, m_latchedTopics); - m_pointCloudSub = new message_filters::Subscriber (m_nh, "cloud_in", 5); - m_tfPointCloudSub = new tf::MessageFilter (*m_pointCloudSub, m_tfListener, m_worldFrameId, 5); + m_pointCloudSub = new message_filters::Subscriber(m_nh, "cloud_in", 5); + m_tfPointCloudSub = new tf::MessageFilter(*m_pointCloudSub, m_tfListener, m_worldFrameId, 5); m_tfPointCloudSub->registerCallback(boost::bind(&OctomapServer::insertCloudCallback, this, boost::placeholders::_1)); m_octomapBinaryService = m_nh.advertiseService("octomap_binary", &OctomapServer::octomapBinarySrv, this); @@ -188,54 +193,51 @@ OctomapServer::OctomapServer(const ros::NodeHandle private_nh_, const ros::NodeH m_reconfigureServer.setCallback(f); } -OctomapServer::~OctomapServer(){ - if (m_tfPointCloudSub){ +OctomapServer::~OctomapServer() { + if (m_tfPointCloudSub) { delete m_tfPointCloudSub; m_tfPointCloudSub = NULL; } - if (m_pointCloudSub){ + if (m_pointCloudSub) { delete m_pointCloudSub; m_pointCloudSub = NULL; } - - if (m_octree){ + if (m_octree) { delete m_octree; m_octree = NULL; } - } -bool OctomapServer::openFile(const std::string& filename){ - if (filename.length() <= 3) - return false; +bool OctomapServer::openFile(const std::string& filename) { + if (filename.length() <= 3) return false; - std::string suffix = filename.substr(filename.length()-3, 3); - if (suffix== ".bt"){ - if (!m_octree->readBinary(filename)){ + std::string suffix = filename.substr(filename.length() - 3, 3); + if (suffix == ".bt") { + if (!m_octree->readBinary(filename)) { return false; } - } else if (suffix == ".ot"){ + } else if (suffix == ".ot") { AbstractOcTree* tree = AbstractOcTree::read(filename); - if (!tree){ + if (!tree) { return false; } - if (m_octree){ + if (m_octree) { delete m_octree; m_octree = NULL; } m_octree = dynamic_cast(tree); - if (!m_octree){ + if (!m_octree) { ROS_ERROR("Could not read OcTree in file, currently there are no other types supported in .ot"); return false; } - } else{ + } else { return false; } - ROS_INFO("Octomap file %s loaded (%zu nodes).", filename.c_str(),m_octree->size()); + ROS_INFO("Octomap file %s loaded (%zu nodes).", filename.c_str(), m_octree->size()); m_treeDepth = m_octree->getTreeDepth(); m_maxTreeDepth = m_treeDepth; @@ -257,31 +259,28 @@ bool OctomapServer::openFile(const std::string& filename){ publishAll(); return true; - } -void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud){ +void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud) { ros::WallTime startTime = ros::WallTime::now(); - // // ground filtering in base frame // - PCLPointCloud pc; // input cloud for filtering and ground-detection + PCLPointCloud pc; // input cloud for filtering and ground-detection pcl::fromROSMsg(*cloud, pc); tf::StampedTransform sensorToWorldTf; try { m_tfListener.lookupTransform(m_worldFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf); - } catch(tf::TransformException& ex){ - ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback"); + } catch (tf::TransformException& ex) { + ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << ", quitting callback"); return; } Eigen::Matrix4f sensorToWorld; pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld); - // set up filter for height range, also removes NANs: pcl::PassThrough pass_x; pass_x.setFilterFieldName("x"); @@ -293,23 +292,22 @@ void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr pass_z.setFilterFieldName("z"); pass_z.setFilterLimits(m_pointcloudMinZ, m_pointcloudMaxZ); - PCLPointCloud pc_ground; // segmented ground plane - PCLPointCloud pc_nonground; // everything else + PCLPointCloud pc_ground; // segmented ground plane + PCLPointCloud pc_nonground; // everything else - if (m_filterGroundPlane){ + if (m_filterGroundPlane) { tf::StampedTransform sensorToBaseTf, baseToWorldTf; - try{ + try { m_tfListener.waitForTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, ros::Duration(0.2)); m_tfListener.lookupTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToBaseTf); m_tfListener.lookupTransform(m_worldFrameId, m_baseFrameId, cloud->header.stamp, baseToWorldTf); - - }catch(tf::TransformException& ex){ - ROS_ERROR_STREAM( "Transform error for ground plane filter: " << ex.what() << ", quitting callback.\n" - "You need to set the base_frame_id or disable filter_ground."); + } catch (tf::TransformException& ex) { + ROS_ERROR_STREAM("Transform error for ground plane filter: " << ex.what() + << ", quitting callback.\n" + "You need to set the base_frame_id or disable filter_ground."); } - Eigen::Matrix4f sensorToBase, baseToWorld; pcl_ros::transformAsMatrix(sensorToBaseTf, sensorToBase); pcl_ros::transformAsMatrix(baseToWorldTf, baseToWorld); @@ -345,7 +343,6 @@ void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr pc_nonground.header = pc.header; } - insertScan(sensorToWorldTf.getOrigin(), pc_ground, pc_nonground); double total_elapsed = (ros::WallTime::now() - startTime).toSec(); @@ -354,13 +351,11 @@ void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr publishAll(cloud->header.stamp); } -void OctomapServer::insertScan(const tf::Point& sensorOriginTf, const PCLPointCloud& ground, const PCLPointCloud& nonground){ +void OctomapServer::insertScan(const tf::Point& sensorOriginTf, const PCLPointCloud& ground, const PCLPointCloud& nonground) { point3d sensorOrigin = pointTfToOctomap(sensorOriginTf); - if (!m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMin) - || !m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMax)) - { - ROS_ERROR_STREAM("Could not generate Key for origin "<coordToKeyChecked(sensorOrigin, m_updateBBXMin) || !m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMax)) { + ROS_ERROR_STREAM("Could not generate Key for origin " << sensorOrigin); } #ifdef COLOR_OCTOMAP_SERVER @@ -370,116 +365,112 @@ void OctomapServer::insertScan(const tf::Point& sensorOriginTf, const PCLPointCl // instead of direct scan insertion, compute update to filter ground: KeySet free_cells, occupied_cells; // insert ground points only as free: - for (PCLPointCloud::const_iterator it = ground.begin(); it != ground.end(); ++it){ + for (PCLPointCloud::const_iterator it = ground.begin(); it != ground.end(); ++it) { point3d point(it->x, it->y, it->z); if ((m_minRange > 0) && (point - sensorOrigin).norm() < m_minRange) continue; // maxrange check - if ((m_maxRange > 0.0) && ((point - sensorOrigin).norm() > m_maxRange) ) { + if ((m_maxRange > 0.0) && ((point - sensorOrigin).norm() > m_maxRange)) { point = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRange; } // only clear space (ground points) - if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){ + if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)) { free_cells.insert(m_keyRay.begin(), m_keyRay.end()); } octomap::OcTreeKey endKey; - if (m_octree->coordToKeyChecked(point, endKey)){ + if (m_octree->coordToKeyChecked(point, endKey)) { updateMinKey(endKey, m_updateBBXMin); updateMaxKey(endKey, m_updateBBXMax); - } else{ - ROS_ERROR_STREAM("Could not generate Key for endpoint "<x, it->y, it->z); - + if ((m_minRange > 0) && (point - sensorOrigin).norm() < m_minRange) continue; - - // maxrange check - if ((m_maxRange < 0.0) || ((point - sensorOrigin).norm() <= m_maxRange) ) { + // maxrange check + if ((m_maxRange < 0.0) || ((point - sensorOrigin).norm() <= m_maxRange)) { // free cells - if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){ + if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)) { free_cells.insert(m_keyRay.begin(), m_keyRay.end()); } // occupied endpoint OcTreeKey key; - if (m_octree->coordToKeyChecked(point, key)){ + if (m_octree->coordToKeyChecked(point, key)) { occupied_cells.insert(key); updateMinKey(key, m_updateBBXMin); updateMaxKey(key, m_updateBBXMax); -#ifdef COLOR_OCTOMAP_SERVER // NB: Only read and interpret color if it's an occupied node +#ifdef COLOR_OCTOMAP_SERVER // NB: Only read and interpret color if it's an occupied node m_octree->averageNodeColor(it->x, it->y, it->z, /*r=*/it->r, /*g=*/it->g, /*b=*/it->b); #endif } - } else {// ray longer than maxrange:; + } else { // ray longer than maxrange:; point3d new_end = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRange; - if (m_octree->computeRayKeys(sensorOrigin, new_end, m_keyRay)){ + if (m_octree->computeRayKeys(sensorOrigin, new_end, m_keyRay)) { free_cells.insert(m_keyRay.begin(), m_keyRay.end()); octomap::OcTreeKey endKey; - if (m_octree->coordToKeyChecked(new_end, endKey)){ + if (m_octree->coordToKeyChecked(new_end, endKey)) { free_cells.insert(endKey); updateMinKey(endKey, m_updateBBXMin); updateMaxKey(endKey, m_updateBBXMax); - } else{ - ROS_ERROR_STREAM("Could not generate Key for endpoint "<updateNode(*it, false); } } // now mark all occupied cells: - for (KeySet::iterator it = occupied_cells.begin(), end=occupied_cells.end(); it!= end; it++) { + for (KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; it++) { m_octree->updateNode(*it, true); } // TODO: eval lazy+updateInner vs. proper insertion // non-lazy by default (updateInnerOccupancy() too slow for large maps) - //m_octree->updateInnerOccupancy(); + // m_octree->updateInnerOccupancy(); octomap::point3d minPt, maxPt; - ROS_DEBUG_STREAM("Bounding box keys (before): " << m_updateBBXMin[0] << " " <getNodeSize( m_maxTreeDepth ) - 1; -// tmpMax[1]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1; -// tmpMax[2]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1; -// m_updateBBXMin = tmpMin; -// m_updateBBXMax = tmpMax; -// } + // if (m_maxTreeDepth < 16) + // { + // OcTreeKey tmpMin = getIndexKey(m_updateBBXMin, m_maxTreeDepth); // this should give us the first key at depth m_maxTreeDepth that is smaller or equal + // to m_updateBBXMin (i.e. lower left in 2D grid coordinates) OcTreeKey tmpMax = getIndexKey(m_updateBBXMax, m_maxTreeDepth); // see above, now add + // something to find upper right tmpMax[0]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1; tmpMax[1]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1; + // tmpMax[2]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1; + // m_updateBBXMin = tmpMin; + // m_updateBBXMax = tmpMax; + // } // TODO: we could also limit the bbx to be within the map bounds here (see publishing check) minPt = m_octree->keyToCoord(m_updateBBXMin); maxPt = m_octree->keyToCoord(m_updateBBXMax); - ROS_DEBUG_STREAM("Updated area bounding box: "<< minPt << " - "<prune(); + if (m_compressMap) m_octree->prune(); #ifdef COLOR_OCTOMAP_SERVER - if (colors) - { + if (colors) { delete[] colors; colors = NULL; } @@ -494,11 +485,11 @@ void OctomapServer::publishProjected2DMap(const ros::Time& rostime) { } } -void OctomapServer::publishAll(const ros::Time& rostime){ +void OctomapServer::publishAll(const ros::Time& rostime) { ros::WallTime startTime = ros::WallTime::now(); size_t octomapSize = m_octree->size(); // TODO: estimate num occ. voxels for size of arrays (reserve) - if (octomapSize <= 1){ + if (octomapSize <= 1) { ROS_WARN("Nothing to publish, octree is empty"); return; } @@ -512,7 +503,7 @@ void OctomapServer::publishAll(const ros::Time& rostime){ // init markers for free space: visualization_msgs::MarkerArray freeNodesVis; // each array stores all cubes of a different size, one for each depth level: - freeNodesVis.markers.resize(m_treeDepth+1); + freeNodesVis.markers.resize(m_treeDepth + 1); geometry_msgs::Pose pose; pose.orientation = tf::createQuaternionMsgFromYaw(0.0); @@ -520,7 +511,7 @@ void OctomapServer::publishAll(const ros::Time& rostime){ // init markers: visualization_msgs::MarkerArray occupiedNodesVis; // each array stores all cubes of a different size, one for each depth level: - occupiedNodesVis.markers.resize(m_treeDepth+1); + occupiedNodesVis.markers.resize(m_treeDepth + 1); // init pointcloud: pcl::PointCloud pclCloud; @@ -529,21 +520,17 @@ void OctomapServer::publishAll(const ros::Time& rostime){ handlePreNodeTraversal(rostime); // now, traverse all leafs in the tree: - for (OcTreeT::iterator it = m_octree->begin(m_maxTreeDepth), - end = m_octree->end(); it != end; ++it) - { + for (OcTreeT::iterator it = m_octree->begin(m_maxTreeDepth), end = m_octree->end(); it != end; ++it) { bool inUpdateBBX = isInUpdateBBX(it); // call general hook: handleNode(it); - if (inUpdateBBX) - handleNodeInBBX(it); + if (inUpdateBBX) handleNodeInBBX(it); - if (m_octree->isNodeOccupied(*it)){ + if (m_octree->isNodeOccupied(*it)) { double z = it.getZ(); double half_size = it.getSize() / 2.0; - if (z + half_size > m_occupancyMinZ && z - half_size < m_occupancyMaxZ) - { + if (z + half_size > m_occupancyMinZ && z - half_size < m_occupancyMaxZ) { double size = it.getSize(); double x = it.getX(); double y = it.getY(); @@ -554,18 +541,16 @@ void OctomapServer::publishAll(const ros::Time& rostime){ #endif // Ignore speckles in the map: - if (m_filterSpeckles && (it.getDepth() == m_treeDepth +1) && isSpeckleNode(it.getKey())){ + if (m_filterSpeckles && (it.getDepth() == m_treeDepth + 1) && isSpeckleNode(it.getKey())) { ROS_DEBUG("Ignoring single speckle at (%f,%f,%f)", x, y, z); continue; - } // else: current octree node is no speckle, send it out + } // else: current octree node is no speckle, send it out handleOccupiedNode(it); - if (inUpdateBBX) - handleOccupiedNodeInBBX(it); - + if (inUpdateBBX) handleOccupiedNodeInBBX(it); - //create marker: - if (publishMarkerArray){ + // create marker: + if (publishMarkerArray) { unsigned idx = it.getDepth(); assert(idx < occupiedNodesVis.markers.size()); @@ -575,18 +560,22 @@ void OctomapServer::publishAll(const ros::Time& rostime){ cubeCenter.z = z; occupiedNodesVis.markers[idx].points.push_back(cubeCenter); - if (m_useHeightMap){ + if (m_useHeightMap) { double minX, minY, minZ, maxX, maxY, maxZ; m_octree->getMetricMin(minX, minY, minZ); m_octree->getMetricMax(maxX, maxY, maxZ); - double h = (1.0 - std::min(std::max((cubeCenter.z-minZ)/ (maxZ - minZ), 0.0), 1.0)) *m_colorFactor; + double h = (1.0 - std::min(std::max((cubeCenter.z - minZ) / (maxZ - minZ), 0.0), 1.0)) * m_colorFactor; occupiedNodesVis.markers[idx].colors.push_back(heightMapColor(h)); } #ifdef COLOR_OCTOMAP_SERVER if (m_useColoredMap) { - std_msgs::ColorRGBA _color; _color.r = (r / 255.); _color.g = (g / 255.); _color.b = (b / 255.); _color.a = 1.0; // TODO/EVALUATE: potentially use occupancy as measure for alpha channel? + std_msgs::ColorRGBA _color; + _color.r = (r / 255.); + _color.g = (g / 255.); + _color.b = (b / 255.); + _color.a = 1.0; // TODO/EVALUATE: potentially use occupancy as measure for alpha channel? occupiedNodesVis.markers[idx].colors.push_back(_color); } #endif @@ -596,30 +585,31 @@ void OctomapServer::publishAll(const ros::Time& rostime){ if (publishPointCloud) { #ifdef COLOR_OCTOMAP_SERVER PCLPoint _point = PCLPoint(); - _point.x = x; _point.y = y; _point.z = z; - _point.r = r; _point.g = g; _point.b = b; + _point.x = x; + _point.y = y; + _point.z = z; + _point.r = r; + _point.g = g; + _point.b = b; pclCloud.push_back(_point); #else pclCloud.push_back(PCLPoint(x, y, z)); #endif } - } - } else{ // node not occupied => mark as free in 2D map if unknown so far + } else { // node not occupied => mark as free in 2D map if unknown so far double z = it.getZ(); double half_size = it.getSize() / 2.0; - if (z + half_size > m_occupancyMinZ && z - half_size < m_occupancyMaxZ) - { + if (z + half_size > m_occupancyMinZ && z - half_size < m_occupancyMaxZ) { handleFreeNode(it); - if (inUpdateBBX) - handleFreeNodeInBBX(it); + if (inUpdateBBX) handleFreeNodeInBBX(it); - if (m_publishFreeSpace){ + if (m_publishFreeSpace) { double x = it.getX(); double y = it.getY(); - //create marker for free space: - if (publishFreeMarkerArray){ + // create marker for free space: + if (publishFreeMarkerArray) { unsigned idx = it.getDepth(); assert(idx < freeNodesVis.markers.size()); @@ -631,7 +621,6 @@ void OctomapServer::publishAll(const ros::Time& rostime){ freeNodesVis.markers[idx].points.push_back(cubeCenter); } } - } } } @@ -640,8 +629,8 @@ void OctomapServer::publishAll(const ros::Time& rostime){ handlePostNodeTraversal(rostime); // finish MarkerArray: - if (publishMarkerArray){ - for (unsigned i= 0; i < occupiedNodesVis.markers.size(); ++i){ + if (publishMarkerArray) { + for (unsigned i = 0; i < occupiedNodesVis.markers.size(); ++i) { double size = m_octree->getNodeSize(i); occupiedNodesVis.markers[i].header.frame_id = m_worldFrameId; @@ -652,13 +641,11 @@ void OctomapServer::publishAll(const ros::Time& rostime){ occupiedNodesVis.markers[i].scale.x = size; occupiedNodesVis.markers[i].scale.y = size; occupiedNodesVis.markers[i].scale.z = size; - occupiedNodesVis.markers[i].pose.orientation.x=0; - occupiedNodesVis.markers[i].pose.orientation.y=0; - occupiedNodesVis.markers[i].pose.orientation.z=0; - occupiedNodesVis.markers[i].pose.orientation.w=1; - if (!m_useColoredMap) - occupiedNodesVis.markers[i].color = m_color; - + occupiedNodesVis.markers[i].pose.orientation.x = 0; + occupiedNodesVis.markers[i].pose.orientation.y = 0; + occupiedNodesVis.markers[i].pose.orientation.z = 0; + occupiedNodesVis.markers[i].pose.orientation.w = 1; + if (!m_useColoredMap) occupiedNodesVis.markers[i].color = m_color; if (occupiedNodesVis.markers[i].points.size() > 0) occupiedNodesVis.markers[i].action = visualization_msgs::Marker::ADD; @@ -669,10 +656,9 @@ void OctomapServer::publishAll(const ros::Time& rostime){ m_markerPub.publish(occupiedNodesVis); } - // finish FreeMarkerArray: - if (publishFreeMarkerArray){ - for (unsigned i= 0; i < freeNodesVis.markers.size(); ++i){ + if (publishFreeMarkerArray) { + for (unsigned i = 0; i < freeNodesVis.markers.size(); ++i) { double size = m_octree->getNodeSize(i); freeNodesVis.markers[i].header.frame_id = m_worldFrameId; @@ -685,7 +671,6 @@ void OctomapServer::publishAll(const ros::Time& rostime){ freeNodesVis.markers[i].scale.z = size; freeNodesVis.markers[i].color = m_colorFree; - if (freeNodesVis.markers[i].points.size() > 0) freeNodesVis.markers[i].action = visualization_msgs::Marker::ADD; else @@ -695,64 +680,51 @@ void OctomapServer::publishAll(const ros::Time& rostime){ m_fmarkerPub.publish(freeNodesVis); } - // finish pointcloud: - if (publishPointCloud){ + if (publishPointCloud) { sensor_msgs::PointCloud2 cloud; - pcl::toROSMsg (pclCloud, cloud); + pcl::toROSMsg(pclCloud, cloud); cloud.header.frame_id = m_worldFrameId; cloud.header.stamp = rostime; m_pointCloudPub.publish(cloud); } - if (publishBinaryMap) - publishBinaryOctoMap(rostime); - - if (publishFullMap) - publishFullOctoMap(rostime); + if (publishBinaryMap) publishBinaryOctoMap(rostime); + if (publishFullMap) publishFullOctoMap(rostime); double total_elapsed = (ros::WallTime::now() - startTime).toSec(); ROS_DEBUG("Map publishing in OctomapServer took %f sec", total_elapsed); } -bool OctomapServer::octomapBinarySrv(OctomapSrv::Request &req, - OctomapSrv::Response &res) -{ +bool OctomapServer::octomapBinarySrv(OctomapSrv::Request& req, OctomapSrv::Response& res) { ros::WallTime startTime = ros::WallTime::now(); ROS_INFO("Sending binary map data on service request"); res.map.header.frame_id = m_worldFrameId; res.map.header.stamp = ros::Time::now(); - if (!octomap_msgs::binaryMapToMsg(*m_octree, res.map)) - return false; + if (!octomap_msgs::binaryMapToMsg(*m_octree, res.map)) return false; double total_elapsed = (ros::WallTime::now() - startTime).toSec(); ROS_INFO("Binary octomap sent in %f sec", total_elapsed); return true; } -bool OctomapServer::octomapFullSrv(OctomapSrv::Request &req, - OctomapSrv::Response &res) -{ +bool OctomapServer::octomapFullSrv(OctomapSrv::Request& req, OctomapSrv::Response& res) { ROS_INFO("Sending full map data on service request"); res.map.header.frame_id = m_worldFrameId; res.map.header.stamp = ros::Time::now(); - - if (!octomap_msgs::fullMapToMsg(*m_octree, res.map)) - return false; + if (!octomap_msgs::fullMapToMsg(*m_octree, res.map)) return false; return true; } -bool OctomapServer::clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp){ +bool OctomapServer::clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp) { point3d min = pointMsgToOctomap(req.min); point3d max = pointMsgToOctomap(req.max); double thresMin = m_octree->getClampingThresMin(); - for(OcTreeT::leaf_bbx_iterator it = m_octree->begin_leafs_bbx(min,max), - end=m_octree->end_leafs_bbx(); it!= end; ++it){ - + for (OcTreeT::leaf_bbx_iterator it = m_octree->begin_leafs_bbx(min, max), end = m_octree->end_leafs_bbx(); it != end; ++it) { it->setLogOdds(octomap::logodds(thresMin)); // m_octree->updateNode(it.getKey(), -6.0f); } @@ -766,7 +738,7 @@ bool OctomapServer::clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp){ bool OctomapServer::resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp) { visualization_msgs::MarkerArray occupiedNodesVis; - occupiedNodesVis.markers.resize(m_treeDepth +1); + occupiedNodesVis.markers.resize(m_treeDepth + 1); ros::Time rostime = ros::Time::now(); m_octree->clear(); // clear 2D map: @@ -784,7 +756,7 @@ bool OctomapServer::resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Res publishBinaryOctoMap(rostime); - for (std::size_t i = 0; i < occupiedNodesVis.markers.size(); ++i){ + for (std::size_t i = 0; i < occupiedNodesVis.markers.size(); ++i) { occupiedNodesVis.markers[i].header.frame_id = m_worldFrameId; occupiedNodesVis.markers[i].header.stamp = rostime; occupiedNodesVis.markers[i].ns = "map"; @@ -795,8 +767,8 @@ bool OctomapServer::resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Res m_markerPub.publish(occupiedNodesVis); visualization_msgs::MarkerArray freeNodesVis; - freeNodesVis.markers.resize(m_treeDepth +1); - for (std::size_t i = 0; i < freeNodesVis.markers.size(); ++i){ + freeNodesVis.markers.resize(m_treeDepth + 1); + for (std::size_t i = 0; i < freeNodesVis.markers.size(); ++i) { freeNodesVis.markers[i].header.frame_id = m_worldFrameId; freeNodesVis.markers[i].header.stamp = rostime; freeNodesVis.markers[i].ns = "map"; @@ -809,8 +781,7 @@ bool OctomapServer::resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Res return true; } -void OctomapServer::publishBinaryOctoMap(const ros::Time& rostime) const{ - +void OctomapServer::publishBinaryOctoMap(const ros::Time& rostime) const { Octomap map; map.header.frame_id = m_worldFrameId; map.header.stamp = rostime; @@ -821,8 +792,7 @@ void OctomapServer::publishBinaryOctoMap(const ros::Time& rostime) const{ ROS_ERROR("Error serializing OctoMap"); } -void OctomapServer::publishFullOctoMap(const ros::Time& rostime) const{ - +void OctomapServer::publishFullOctoMap(const ros::Time& rostime) const { Octomap map; map.header.frame_id = m_worldFrameId; map.header.stamp = rostime; @@ -831,43 +801,40 @@ void OctomapServer::publishFullOctoMap(const ros::Time& rostime) const{ m_fullMapPub.publish(map); else ROS_ERROR("Error serializing OctoMap"); - } - -void OctomapServer::filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& ground, PCLPointCloud& nonground) const{ +void OctomapServer::filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& ground, PCLPointCloud& nonground) const { ground.header = pc.header; nonground.header = pc.header; - if (pc.size() < 50){ + if (pc.size() < 50) { ROS_WARN("Pointcloud in OctomapServer too small, skipping ground plane extraction"); nonground = pc; } else { // plane detection for ground plane removal: - pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); - pcl::PointIndices::Ptr inliers (new pcl::PointIndices); + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // Create the segmentation object and set up: pcl::SACSegmentation seg; - seg.setOptimizeCoefficients (true); + seg.setOptimizeCoefficients(true); // TODO: maybe a filtering based on the surface normals might be more robust / accurate? seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(200); - seg.setDistanceThreshold (m_groundFilterDistance); - seg.setAxis(Eigen::Vector3f(0,0,1)); + seg.setDistanceThreshold(m_groundFilterDistance); + seg.setAxis(Eigen::Vector3f(0, 0, 1)); seg.setEpsAngle(m_groundFilterAngle); - PCLPointCloud cloud_filtered(pc); // Create the filtering object pcl::ExtractIndices extract; bool groundPlaneFound = false; - while(cloud_filtered.size() > 10 && !groundPlaneFound){ + while (cloud_filtered.size() > 10 && !groundPlaneFound) { seg.setInputCloud(cloud_filtered.makeShared()); - seg.segment (*inliers, *coefficients); - if (inliers->indices.size () == 0){ + seg.segment(*inliers, *coefficients); + if (inliers->indices.size() == 0) { ROS_INFO("PCL segmentation did not find any plane."); break; @@ -876,15 +843,15 @@ void OctomapServer::filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& gr extract.setInputCloud(cloud_filtered.makeShared()); extract.setIndices(inliers); - if (std::abs(coefficients->values.at(3)) < m_groundFilterPlaneDistance){ - ROS_DEBUG("Ground plane found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(), - coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3)); - extract.setNegative (false); - extract.filter (ground); + if (std::abs(coefficients->values.at(3)) < m_groundFilterPlaneDistance) { + ROS_DEBUG("Ground plane found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(), coefficients->values.at(0), + coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3)); + extract.setNegative(false); + extract.filter(ground); // remove ground points from full pointcloud: // workaround for PCL bug: - if(inliers->indices.size() != cloud_filtered.size()){ + if (inliers->indices.size() != cloud_filtered.size()) { extract.setNegative(true); PCLPointCloud cloud_out; extract.filter(cloud_out); @@ -893,32 +860,31 @@ void OctomapServer::filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& gr } groundPlaneFound = true; - } else{ + } else { ROS_DEBUG("Horizontal plane (not ground) found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(), coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3)); pcl::PointCloud cloud_out; - extract.setNegative (false); + extract.setNegative(false); extract.filter(cloud_out); - nonground +=cloud_out; + nonground += cloud_out; // debug // pcl::PCDWriter writer; // writer.write("nonground_plane.pcd",cloud_out, false); // remove current plane from scan for next iteration: // workaround for PCL bug: - if(inliers->indices.size() != cloud_filtered.size()){ + if (inliers->indices.size() != cloud_filtered.size()) { extract.setNegative(true); cloud_out.points.clear(); extract.filter(cloud_out); cloud_filtered = cloud_out; - } else{ + } else { cloud_filtered.points.clear(); } } - } // TODO: also do this if overall starting pointcloud too small? - if (!groundPlaneFound){ // no plane found or remaining points too small + if (!groundPlaneFound) { // no plane found or remaining points too small ROS_WARN("No ground plane found in scan"); // do a rough fitlering on height to prevent spurious obstacles @@ -928,7 +894,7 @@ void OctomapServer::filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& gr second_pass.setInputCloud(pc.makeShared()); second_pass.filter(ground); - second_pass.setNegative (true); + second_pass.setNegative(true); second_pass.filter(nonground); } @@ -938,14 +904,11 @@ void OctomapServer::filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& gr // writer.write("ground.pcd",pc_ground, false); // if (pc_nonground.size() > 0) // writer.write("nonground.pcd",pc_nonground, false); - } - - } -void OctomapServer::handlePreNodeTraversal(const ros::Time& rostime){ - if (m_publish2DMap){ +void OctomapServer::handlePreNodeTraversal(const ros::Time& rostime) { + if (m_publish2DMap) { // init projected 2D map: m_gridmap.header.frame_id = m_worldFrameId; m_gridmap.header.stamp = rostime; @@ -964,8 +927,8 @@ void OctomapServer::handlePreNodeTraversal(const ros::Time& rostime){ ROS_DEBUG("MinKey: %d %d %d / MaxKey: %d %d %d", minKey[0], minKey[1], minKey[2], maxKey[0], maxKey[1], maxKey[2]); // add padding if requested (= new min/maxPts in x&y): - double halfPaddedX = 0.5*m_minSizeX; - double halfPaddedY = 0.5*m_minSizeY; + double halfPaddedX = 0.5 * m_minSizeX; + double halfPaddedY = 0.5 * m_minSizeY; minX = std::min(minX, -halfPaddedX); maxX = std::max(maxX, halfPaddedX); minY = std::min(minY, -halfPaddedY); @@ -974,21 +937,22 @@ void OctomapServer::handlePreNodeTraversal(const ros::Time& rostime){ maxPt = octomap::point3d(maxX, maxY, maxZ); OcTreeKey paddedMaxKey; - if (!m_octree->coordToKeyChecked(minPt, m_maxTreeDepth, m_paddedMinKey)){ + if (!m_octree->coordToKeyChecked(minPt, m_maxTreeDepth, m_paddedMinKey)) { ROS_ERROR("Could not create padded min OcTree key at %f %f %f", minPt.x(), minPt.y(), minPt.z()); return; } - if (!m_octree->coordToKeyChecked(maxPt, m_maxTreeDepth, paddedMaxKey)){ + if (!m_octree->coordToKeyChecked(maxPt, m_maxTreeDepth, paddedMaxKey)) { ROS_ERROR("Could not create padded max OcTree key at %f %f %f", maxPt.x(), maxPt.y(), maxPt.z()); return; } - ROS_DEBUG("Padded MinKey: %d %d %d / padded MaxKey: %d %d %d", m_paddedMinKey[0], m_paddedMinKey[1], m_paddedMinKey[2], paddedMaxKey[0], paddedMaxKey[1], paddedMaxKey[2]); + ROS_DEBUG("Padded MinKey: %d %d %d / padded MaxKey: %d %d %d", m_paddedMinKey[0], m_paddedMinKey[1], m_paddedMinKey[2], paddedMaxKey[0], paddedMaxKey[1], + paddedMaxKey[2]); assert(paddedMaxKey[0] >= maxKey[0] && paddedMaxKey[1] >= maxKey[1]); m_multires2DScale = 1 << (m_treeDepth - m_maxTreeDepth); - m_gridmap.info.width = (paddedMaxKey[0] - m_paddedMinKey[0])/m_multires2DScale +1; - m_gridmap.info.height = (paddedMaxKey[1] - m_paddedMinKey[1])/m_multires2DScale +1; + m_gridmap.info.width = (paddedMaxKey[0] - m_paddedMinKey[0]) / m_multires2DScale + 1; + m_gridmap.info.height = (paddedMaxKey[1] - m_paddedMinKey[1]) / m_multires2DScale + 1; int mapOriginX = minKey[0] - m_paddedMinKey[0]; int mapOriginY = minKey[1] - m_paddedMinKey[1]; @@ -997,132 +961,128 @@ void OctomapServer::handlePreNodeTraversal(const ros::Time& rostime){ // might not exactly be min / max of octree: octomap::point3d origin = m_octree->keyToCoord(m_paddedMinKey, m_treeDepth); double gridRes = m_octree->getNodeSize(m_maxTreeDepth); - m_projectCompleteMap = (!m_incrementalUpdate || (std::abs(gridRes-m_gridmap.info.resolution) > 1e-6)); + m_projectCompleteMap = (!m_incrementalUpdate || (std::abs(gridRes - m_gridmap.info.resolution) > 1e-6)); m_gridmap.info.resolution = gridRes; - m_gridmap.info.origin.position.x = origin.x() - gridRes*0.5; - m_gridmap.info.origin.position.y = origin.y() - gridRes*0.5; - if (m_maxTreeDepth != m_treeDepth){ - m_gridmap.info.origin.position.x -= m_res/2.0; - m_gridmap.info.origin.position.y -= m_res/2.0; + m_gridmap.info.origin.position.x = origin.x() - gridRes * 0.5; + m_gridmap.info.origin.position.y = origin.y() - gridRes * 0.5; + if (m_maxTreeDepth != m_treeDepth) { + m_gridmap.info.origin.position.x -= m_res / 2.0; + m_gridmap.info.origin.position.y -= m_res / 2.0; } // workaround for multires. projection not working properly for inner nodes: // force re-building complete map - if (m_maxTreeDepth < m_treeDepth) - m_projectCompleteMap = true; + if (m_maxTreeDepth < m_treeDepth) m_projectCompleteMap = true; - - if(m_projectCompleteMap){ + if (m_projectCompleteMap) { ROS_DEBUG("Rebuilding complete 2D map"); m_gridmap.data.clear(); // init to unknown: m_gridmap.data.resize(m_gridmap.info.width * m_gridmap.info.height, -1); } else { - - if (mapChanged(oldMapInfo, m_gridmap.info)){ - ROS_DEBUG("2D grid map size changed to %dx%d", m_gridmap.info.width, m_gridmap.info.height); - adjustMapData(m_gridmap, oldMapInfo); - } - nav_msgs::OccupancyGrid::_data_type::iterator startIt; - size_t mapUpdateBBXMinX = std::max(0, (int(m_updateBBXMin[0]) - int(m_paddedMinKey[0]))/int(m_multires2DScale)); - size_t mapUpdateBBXMinY = std::max(0, (int(m_updateBBXMin[1]) - int(m_paddedMinKey[1]))/int(m_multires2DScale)); - size_t mapUpdateBBXMaxX = std::min(int(m_gridmap.info.width-1), (int(m_updateBBXMax[0]) - int(m_paddedMinKey[0]))/int(m_multires2DScale)); - size_t mapUpdateBBXMaxY = std::min(int(m_gridmap.info.height-1), (int(m_updateBBXMax[1]) - int(m_paddedMinKey[1]))/int(m_multires2DScale)); - - assert(mapUpdateBBXMaxX > mapUpdateBBXMinX); - assert(mapUpdateBBXMaxY > mapUpdateBBXMinY); - - size_t numCols = mapUpdateBBXMaxX-mapUpdateBBXMinX +1; - - // test for max idx: - uint max_idx = m_gridmap.info.width*mapUpdateBBXMaxY + mapUpdateBBXMaxX; - if (max_idx >= m_gridmap.data.size()) - ROS_ERROR("BBX index not valid: %d (max index %zu for size %d x %d) update-BBX is: [%zu %zu]-[%zu %zu]", max_idx, m_gridmap.data.size(), m_gridmap.info.width, m_gridmap.info.height, mapUpdateBBXMinX, mapUpdateBBXMinY, mapUpdateBBXMaxX, mapUpdateBBXMaxY); - - // reset proj. 2D map in bounding box: - for (unsigned int j = mapUpdateBBXMinY; j <= mapUpdateBBXMaxY; ++j){ - std::fill_n(m_gridmap.data.begin() + m_gridmap.info.width*j+mapUpdateBBXMinX, - numCols, -1); - } - + if (mapChanged(oldMapInfo, m_gridmap.info)) { + ROS_DEBUG("2D grid map size changed to %dx%d", m_gridmap.info.width, m_gridmap.info.height); + adjustMapData(m_gridmap, oldMapInfo); + } + nav_msgs::OccupancyGrid::_data_type::iterator startIt; + size_t mapUpdateBBXMinX = std::max(0, (int(m_updateBBXMin[0]) - int(m_paddedMinKey[0])) / int(m_multires2DScale)); + size_t mapUpdateBBXMinY = std::max(0, (int(m_updateBBXMin[1]) - int(m_paddedMinKey[1])) / int(m_multires2DScale)); + size_t mapUpdateBBXMaxX = std::min(int(m_gridmap.info.width - 1), (int(m_updateBBXMax[0]) - int(m_paddedMinKey[0])) / int(m_multires2DScale)); + size_t mapUpdateBBXMaxY = std::min(int(m_gridmap.info.height - 1), (int(m_updateBBXMax[1]) - int(m_paddedMinKey[1])) / int(m_multires2DScale)); + + assert(mapUpdateBBXMaxX > mapUpdateBBXMinX); + assert(mapUpdateBBXMaxY > mapUpdateBBXMinY); + + size_t numCols = mapUpdateBBXMaxX - mapUpdateBBXMinX + 1; + + // test for max idx: + uint max_idx = m_gridmap.info.width * mapUpdateBBXMaxY + mapUpdateBBXMaxX; + if (max_idx >= m_gridmap.data.size()) + ROS_ERROR("BBX index not valid: %d (max index %zu for size %d x %d) update-BBX is: [%zu %zu]-[%zu %zu]", max_idx, m_gridmap.data.size(), + m_gridmap.info.width, m_gridmap.info.height, mapUpdateBBXMinX, mapUpdateBBXMinY, mapUpdateBBXMaxX, mapUpdateBBXMaxY); + + // reset proj. 2D map in bounding box: + for (unsigned int j = mapUpdateBBXMinY; j <= mapUpdateBBXMaxY; ++j) { + std::fill_n(m_gridmap.data.begin() + m_gridmap.info.width * j + mapUpdateBBXMinX, numCols, -1); + } } - - - } - } -void OctomapServer::handlePostNodeTraversal(const ros::Time& rostime){ - publishProjected2DMap(rostime); -} +void OctomapServer::handlePostNodeTraversal(const ros::Time& rostime) { publishProjected2DMap(rostime); } -void OctomapServer::handleOccupiedNode(const OcTreeT::iterator& it){ - if (m_publish2DMap && m_projectCompleteMap){ +void OctomapServer::handleOccupiedNode(const OcTreeT::iterator& it) { + if (m_publish2DMap && m_projectCompleteMap) { update2DMap(it, true); } } -void OctomapServer::handleFreeNode(const OcTreeT::iterator& it){ - if (m_publish2DMap && m_projectCompleteMap){ +void OctomapServer::handleFreeNode(const OcTreeT::iterator& it) { + if (m_publish2DMap && m_projectCompleteMap) { update2DMap(it, false); } } -void OctomapServer::handleOccupiedNodeInBBX(const OcTreeT::iterator& it){ - if (m_publish2DMap && !m_projectCompleteMap){ +void OctomapServer::handleOccupiedNodeInBBX(const OcTreeT::iterator& it) { + if (m_publish2DMap && !m_projectCompleteMap) { update2DMap(it, true); } } -void OctomapServer::handleFreeNodeInBBX(const OcTreeT::iterator& it){ - if (m_publish2DMap && !m_projectCompleteMap){ +void OctomapServer::handleFreeNodeInBBX(const OcTreeT::iterator& it) { + if (m_publish2DMap && !m_projectCompleteMap) { update2DMap(it, false); } } -void OctomapServer::update2DMap(const OcTreeT::iterator& it, bool occupied){ - // update 2D map (occupied always overrides): - - if (it.getDepth() == m_maxTreeDepth){ - unsigned idx = mapIdx(it.getKey()); - if (occupied) - m_gridmap.data[mapIdx(it.getKey())] = 100; - else if (m_gridmap.data[idx] == -1){ - m_gridmap.data[idx] = 0; - } - - } else{ - int intSize = 1 << (m_maxTreeDepth - it.getDepth()); - octomap::OcTreeKey minKey=it.getIndexKey(); - for(int dx=0; dx < intSize; dx++){ - int i = (minKey[0]+dx - m_paddedMinKey[0])/m_multires2DScale; - for(int dy=0; dy < intSize; dy++){ - unsigned idx = mapIdx(i, (minKey[1]+dy - m_paddedMinKey[1])/m_multires2DScale); - if (occupied) - m_gridmap.data[idx] = 100; - else if (m_gridmap.data[idx] == -1){ - m_gridmap.data[idx] = 0; +void OctomapServer::update2DMap(const OcTreeT::iterator& it, bool occupied) { + // Determine whether to consider the Z-axis limits based on whether they are set to -1 + bool considerMinZ = m_projectedMapMinZ != -1; + bool considerMaxZ = m_projectedMapMaxZ != -1; + + // Check if the node's Z-coordinate is within the specified range + // True if we don't consider the limit or if the coordinate meets the limit condition + bool withinMinZ = !considerMinZ || it.getZ() >= m_projectedMapMinZ; + bool withinMaxZ = !considerMaxZ || it.getZ() <= m_projectedMapMaxZ; + + if (withinMinZ && withinMaxZ) { + // The node is within the Z-axis limits or the limits are ignored + if (it.getDepth() == m_maxTreeDepth) { + unsigned idx = mapIdx(it.getKey()); + if (occupied) + m_gridmap.data[idx] = 100; // Mark as occupied + else if (m_gridmap.data[idx] == -1) { + m_gridmap.data[idx] = 0; // Mark as free if previously unknown + } + } else { + int intSize = 1 << (m_maxTreeDepth - it.getDepth()); + octomap::OcTreeKey minKey = it.getIndexKey(); + for (int dx = 0; dx < intSize; dx++) { + int i = (minKey[0] + dx - m_paddedMinKey[0]) / m_multires2DScale; + for (int dy = 0; dy < intSize; dy++) { + unsigned idx = mapIdx(i, (minKey[1] + dy - m_paddedMinKey[1]) / m_multires2DScale); + if (occupied) + m_gridmap.data[idx] = 100; // Mark as occupied + else if (m_gridmap.data[idx] == -1) { + m_gridmap.data[idx] = 0; // Mark as free if previously unknown + } + } + } } - } } - } - - } - -bool OctomapServer::isSpeckleNode(const OcTreeKey&nKey) const { +bool OctomapServer::isSpeckleNode(const OcTreeKey& nKey) const { OcTreeKey key; bool neighborFound = false; - for (key[2] = nKey[2] - 1; !neighborFound && key[2] <= nKey[2] + 1; ++key[2]){ - for (key[1] = nKey[1] - 1; !neighborFound && key[1] <= nKey[1] + 1; ++key[1]){ - for (key[0] = nKey[0] - 1; !neighborFound && key[0] <= nKey[0] + 1; ++key[0]){ - if (key != nKey){ + for (key[2] = nKey[2] - 1; !neighborFound && key[2] <= nKey[2] + 1; ++key[2]) { + for (key[1] = nKey[1] - 1; !neighborFound && key[1] <= nKey[1] + 1; ++key[1]) { + for (key[0] = nKey[0] - 1; !neighborFound && key[0] <= nKey[0] + 1; ++key[0]) { + if (key != nKey) { OcTreeNode* node = m_octree->search(key); - if (node && m_octree->isNodeOccupied(node)){ + if (node && m_octree->isNodeOccupied(node)) { // we have a neighbor => break! neighborFound = true; } @@ -1134,79 +1094,64 @@ bool OctomapServer::isSpeckleNode(const OcTreeKey&nKey) const { return neighborFound; } -void OctomapServer::reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level){ +void OctomapServer::reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level) { if (m_maxTreeDepth != unsigned(config.max_depth)) m_maxTreeDepth = unsigned(config.max_depth); - else{ - m_pointcloudMinZ = config.pointcloud_min_z; - m_pointcloudMaxZ = config.pointcloud_max_z; - m_occupancyMinZ = config.occupancy_min_z; - m_occupancyMaxZ = config.occupancy_max_z; - m_filterSpeckles = config.filter_speckles; - m_filterGroundPlane = config.filter_ground; - m_compressMap = config.compress_map; - m_incrementalUpdate = config.incremental_2D_projection; + else { + m_pointcloudMinZ = config.pointcloud_min_z; + m_pointcloudMaxZ = config.pointcloud_max_z; + m_occupancyMinZ = config.occupancy_min_z; + m_occupancyMaxZ = config.occupancy_max_z; + m_filterSpeckles = config.filter_speckles; + m_filterGroundPlane = config.filter_ground; + m_compressMap = config.compress_map; + m_incrementalUpdate = config.incremental_2D_projection; // Parameters with a namespace require an special treatment at the beginning, as dynamic reconfigure // will overwrite them because the server is not able to match parameters' names. - if (m_initConfig){ - // If parameters do not have the default value, dynamic reconfigure server should be updated. - if(!is_equal(m_groundFilterDistance, 0.04)) - config.ground_filter_distance = m_groundFilterDistance; - if(!is_equal(m_groundFilterAngle, 0.15)) - config.ground_filter_angle = m_groundFilterAngle; - if(!is_equal( m_groundFilterPlaneDistance, 0.07)) - config.ground_filter_plane_distance = m_groundFilterPlaneDistance; - if(!is_equal(m_maxRange, -1.0)) - config.sensor_model_max_range = m_maxRange; - if(!is_equal(m_minRange, -1.0)) - config.sensor_model_min_range = m_minRange; - if(!is_equal(m_octree->getProbHit(), 0.7)) - config.sensor_model_hit = m_octree->getProbHit(); - if(!is_equal(m_octree->getProbMiss(), 0.4)) - config.sensor_model_miss = m_octree->getProbMiss(); - if(!is_equal(m_octree->getClampingThresMin(), 0.12)) - config.sensor_model_min = m_octree->getClampingThresMin(); - if(!is_equal(m_octree->getClampingThresMax(), 0.97)) - config.sensor_model_max = m_octree->getClampingThresMax(); - m_initConfig = false; - - boost::recursive_mutex::scoped_lock reconf_lock(m_config_mutex); - m_reconfigureServer.updateConfig(config); - } - else{ - m_groundFilterDistance = config.ground_filter_distance; - m_groundFilterAngle = config.ground_filter_angle; + if (m_initConfig) { + // If parameters do not have the default value, dynamic reconfigure server should be updated. + if (!is_equal(m_groundFilterDistance, 0.04)) config.ground_filter_distance = m_groundFilterDistance; + if (!is_equal(m_groundFilterAngle, 0.15)) config.ground_filter_angle = m_groundFilterAngle; + if (!is_equal(m_groundFilterPlaneDistance, 0.07)) config.ground_filter_plane_distance = m_groundFilterPlaneDistance; + if (!is_equal(m_maxRange, -1.0)) config.sensor_model_max_range = m_maxRange; + if (!is_equal(m_minRange, -1.0)) config.sensor_model_min_range = m_minRange; + if (!is_equal(m_octree->getProbHit(), 0.7)) config.sensor_model_hit = m_octree->getProbHit(); + if (!is_equal(m_octree->getProbMiss(), 0.4)) config.sensor_model_miss = m_octree->getProbMiss(); + if (!is_equal(m_octree->getClampingThresMin(), 0.12)) config.sensor_model_min = m_octree->getClampingThresMin(); + if (!is_equal(m_octree->getClampingThresMax(), 0.97)) config.sensor_model_max = m_octree->getClampingThresMax(); + m_initConfig = false; + + boost::recursive_mutex::scoped_lock reconf_lock(m_config_mutex); + m_reconfigureServer.updateConfig(config); + } else { + m_groundFilterDistance = config.ground_filter_distance; + m_groundFilterAngle = config.ground_filter_angle; m_groundFilterPlaneDistance = config.ground_filter_plane_distance; - m_maxRange = config.sensor_model_max_range; + m_maxRange = config.sensor_model_max_range; m_octree->setClampingThresMin(config.sensor_model_min); m_octree->setClampingThresMax(config.sensor_model_max); - // Checking values that might create unexpected behaviors. - if (is_equal(config.sensor_model_hit, 1.0)) - config.sensor_model_hit -= 1.0e-6; + // Checking values that might create unexpected behaviors. + if (is_equal(config.sensor_model_hit, 1.0)) config.sensor_model_hit -= 1.0e-6; m_octree->setProbHit(config.sensor_model_hit); - if (is_equal(config.sensor_model_miss, 0.0)) - config.sensor_model_miss += 1.0e-6; + if (is_equal(config.sensor_model_miss, 0.0)) config.sensor_model_miss += 1.0e-6; m_octree->setProbMiss(config.sensor_model_miss); - } + } } publishAll(); } -void OctomapServer::adjustMapData(nav_msgs::OccupancyGrid& map, const nav_msgs::MapMetaData& oldMapInfo) const{ - if (map.info.resolution != oldMapInfo.resolution){ +void OctomapServer::adjustMapData(nav_msgs::OccupancyGrid& map, const nav_msgs::MapMetaData& oldMapInfo) const { + if (map.info.resolution != oldMapInfo.resolution) { ROS_ERROR("Resolution of map changed, cannot be adjusted"); return; } - int i_off = int((oldMapInfo.origin.position.x - map.info.origin.position.x)/map.info.resolution +0.5); - int j_off = int((oldMapInfo.origin.position.y - map.info.origin.position.y)/map.info.resolution +0.5); + int i_off = int((oldMapInfo.origin.position.x - map.info.origin.position.x) / map.info.resolution + 0.5); + int j_off = int((oldMapInfo.origin.position.y - map.info.origin.position.y) / map.info.resolution + 0.5); - if (i_off < 0 || j_off < 0 - || oldMapInfo.width + i_off > map.info.width - || oldMapInfo.height + j_off > map.info.height) - { + if (i_off < 0 || j_off < 0 || oldMapInfo.width + i_off > map.info.width || oldMapInfo.height + j_off > map.info.height) { ROS_ERROR("New 2D map does not contain old map area, this case is not implemented"); return; } @@ -1219,24 +1164,20 @@ void OctomapServer::adjustMapData(nav_msgs::OccupancyGrid& map, const nav_msgs:: nav_msgs::OccupancyGrid::_data_type::iterator fromStart, fromEnd, toStart; - for (int j =0; j < int(oldMapInfo.height); ++j ){ + for (int j = 0; j < int(oldMapInfo.height); ++j) { // copy chunks, row by row: - fromStart = oldMapData.begin() + j*oldMapInfo.width; + fromStart = oldMapData.begin() + j * oldMapInfo.width; fromEnd = fromStart + oldMapInfo.width; - toStart = map.data.begin() + ((j+j_off)*m_gridmap.info.width + i_off); + toStart = map.data.begin() + ((j + j_off) * m_gridmap.info.width + i_off); copy(fromStart, fromEnd, toStart); -// for (int i =0; i < int(oldMapInfo.width); ++i){ -// map.data[m_gridmap.info.width*(j+j_off) +i+i_off] = oldMapData[oldMapInfo.width*j +i]; -// } - + // for (int i =0; i < int(oldMapInfo.width); ++i){ + // map.data[m_gridmap.info.width*(j+j_off) +i+i_off] = oldMapData[oldMapInfo.width*j +i]; + // } } - } - std_msgs::ColorRGBA OctomapServer::heightMapColor(double h) { - std_msgs::ColorRGBA color; color.a = 1.0; // blend over HSV-values (more colors) @@ -1251,39 +1192,49 @@ std_msgs::ColorRGBA OctomapServer::heightMapColor(double h) { i = floor(h); f = h - i; - if (!(i & 1)) - f = 1 - f; // if i is even + if (!(i & 1)) f = 1 - f; // if i is even m = v * (1 - s); n = v * (1 - s * f); switch (i) { case 6: case 0: - color.r = v; color.g = n; color.b = m; + color.r = v; + color.g = n; + color.b = m; break; case 1: - color.r = n; color.g = v; color.b = m; + color.r = n; + color.g = v; + color.b = m; break; case 2: - color.r = m; color.g = v; color.b = n; + color.r = m; + color.g = v; + color.b = n; break; case 3: - color.r = m; color.g = n; color.b = v; + color.r = m; + color.g = n; + color.b = v; break; case 4: - color.r = n; color.g = m; color.b = v; + color.r = n; + color.g = m; + color.b = v; break; case 5: - color.r = v; color.g = m; color.b = n; + color.r = v; + color.g = m; + color.b = n; break; default: - color.r = 1; color.g = 0.5; color.b = 0.5; + color.r = 1; + color.g = 0.5; + color.b = 0.5; break; } return color; } -} - - - +} // namespace octomap_server