From 29d32c1190f5633b47f513fabea90c8802a7c126 Mon Sep 17 00:00:00 2001 From: Carlos Miguel Correia da Costa Date: Mon, 16 Jun 2014 10:16:50 +0100 Subject: [PATCH 1/6] Efficiency improvements and customizable occupancy 2D map projection. + Efficiency improvements: | avoid transforming clouds already in map frame (added additional parameter to specify sensor frame_id for calculating sensor pose) | create occupancy cloud directly as ros message instead of pcl cloud (avoids conversion) + Projection of nav_msgs::OccupancyGrid with customizable height and initialization as free or unknown. + Increased tf listener buffer to 30 sec. --- .../include/octomap_server/OctomapServer.h | 5 + octomap_server/src/OctomapServer.cpp | 122 ++++++++++++------ 2 files changed, 90 insertions(+), 37 deletions(-) diff --git a/octomap_server/include/octomap_server/OctomapServer.h b/octomap_server/include/octomap_server/OctomapServer.h index ce56708b..969bb905 100644 --- a/octomap_server/include/octomap_server/OctomapServer.h +++ b/octomap_server/include/octomap_server/OctomapServer.h @@ -203,6 +203,7 @@ class OctomapServer{ double m_maxRange; std::string m_worldFrameId; // the map frame std::string m_baseFrameId; // base of the robot for ground plane filtering + std::string m_sensorFrameId; // the sensor frame bool m_useHeightMap; std_msgs::ColorRGBA m_color; std_msgs::ColorRGBA m_colorFree; @@ -223,6 +224,9 @@ class OctomapServer{ double m_pointcloudMaxZ; double m_occupancyMinZ; double m_occupancyMaxZ; + double m_occupancyGrid2DMinZ; + double m_occupancyGrid2DMaxZ; + bool m_occupancyGrid2DInitializedAsFree; double m_minSizeX; double m_minSizeY; bool m_filterSpeckles; @@ -233,6 +237,7 @@ class OctomapServer{ double m_groundFilterPlaneDistance; bool m_compressMap; + size_t m_numberCloudsPublished; // downprojected 2D map: bool m_incrementalUpdate; diff --git a/octomap_server/src/OctomapServer.cpp b/octomap_server/src/OctomapServer.cpp index ae7c5650..289c3f02 100644 --- a/octomap_server/src/OctomapServer.cpp +++ b/octomap_server/src/OctomapServer.cpp @@ -38,6 +38,7 @@ OctomapServer::OctomapServer(ros::NodeHandle private_nh_) : m_nh(), m_pointCloudSub(NULL), m_tfPointCloudSub(NULL), + m_tfListener(ros::Duration(30)), m_octree(NULL), m_maxRange(-1.0), m_worldFrameId("/map"), m_baseFrameId("base_footprint"), @@ -54,15 +55,20 @@ OctomapServer::OctomapServer(ros::NodeHandle private_nh_) m_pointcloudMaxZ(std::numeric_limits::max()), m_occupancyMinZ(-std::numeric_limits::max()), m_occupancyMaxZ(std::numeric_limits::max()), + m_occupancyGrid2DMinZ(-std::numeric_limits::max()), + m_occupancyGrid2DMaxZ(std::numeric_limits::max()), + m_occupancyGrid2DInitializedAsFree(false), 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_numberCloudsPublished(0), m_incrementalUpdate(false) { ros::NodeHandle private_nh(private_nh_); private_nh.param("frame_id", m_worldFrameId, m_worldFrameId); private_nh.param("base_frame_id", m_baseFrameId, m_baseFrameId); + private_nh.param("sensor_frame_id", m_sensorFrameId, m_sensorFrameId); private_nh.param("height_map", m_useHeightMap, m_useHeightMap); private_nh.param("color_factor", m_colorFactor, m_colorFactor); @@ -70,6 +76,9 @@ OctomapServer::OctomapServer(ros::NodeHandle private_nh_) private_nh.param("pointcloud_max_z", m_pointcloudMaxZ,m_pointcloudMaxZ); private_nh.param("occupancy_min_z", m_occupancyMinZ,m_occupancyMinZ); private_nh.param("occupancy_max_z", m_occupancyMaxZ,m_occupancyMaxZ); + private_nh.param("occupancy_grid_2d_min_z", m_occupancyGrid2DMinZ, m_occupancyGrid2DMinZ); + private_nh.param("occupancy_grid_2d_max_z", m_occupancyGrid2DMaxZ, m_occupancyGrid2DMaxZ); + private_nh.param("occupancy_grid_2d_initialized_as_free", m_occupancyGrid2DInitializedAsFree, m_occupancyGrid2DInitializedAsFree); private_nh.param("min_x_size", m_minSizeX,m_minSizeX); private_nh.param("min_y_size", m_minSizeY,m_minSizeY); @@ -241,10 +250,11 @@ void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr // PCLPointCloud pc; // input cloud for filtering and ground-detection pcl::fromROSMsg(*cloud, pc); + bool cloudAlreadyInWorldFrameId = cloud->header.frame_id == m_worldFrameId; tf::StampedTransform sensorToWorldTf; try { - m_tfListener.lookupTransform(m_worldFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf); + m_tfListener.lookupTransform(m_worldFrameId, cloudAlreadyInWorldFrameId ? m_sensorFrameId : cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf); } catch(tf::TransformException& ex){ ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback"); return; @@ -291,7 +301,7 @@ void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr pcl::transformPointCloud(pc_nonground, pc_nonground, baseToWorld); } else { // directly transform to map frame: - pcl::transformPointCloud(pc, pc, sensorToWorld); + if (!cloudAlreadyInWorldFrameId) pcl::transformPointCloud(pc, pc, sensorToWorld); // just filter height range: pass.setInputCloud(pc.makeShared()); @@ -456,8 +466,37 @@ void OctomapServer::publishAll(const ros::Time& rostime){ // each array stores all cubes of a different size, one for each depth level: occupiedNodesVis.markers.resize(m_treeDepth+1); - // init pointcloud: - pcl::PointCloud pclCloud; +// init pointcloud: + sensor_msgs::PointCloud2Ptr cloud = sensor_msgs::PointCloud2Ptr(new sensor_msgs::PointCloud2()); + size_t numberPointsInCloud = 0; + size_t numberReservedPoints = m_octree->size(); + float* pointcloudDataPosition = NULL; + if (publishPointCloud) { + cloud->header.seq = m_numberCloudsPublished++; + cloud->header.stamp = rostime; + cloud->header.frame_id = m_worldFrameId; + cloud->height = 1; + cloud->width = 0; + cloud->fields.clear(); + cloud->fields.resize(3); + cloud->fields[0].name = "x"; + cloud->fields[0].offset = 0; + cloud->fields[0].datatype = sensor_msgs::PointField::FLOAT32; + cloud->fields[0].count = 1; + cloud->fields[1].name = "y"; + cloud->fields[1].offset = 4; + cloud->fields[1].datatype = sensor_msgs::PointField::FLOAT32; + cloud->fields[1].count = 1; + cloud->fields[2].name = "z"; + cloud->fields[2].offset = 8; + cloud->fields[2].datatype = sensor_msgs::PointField::FLOAT32; + cloud->fields[2].count = 1; + cloud->point_step = 12; + + if (numberReservedPoints < 1) numberReservedPoints = 8; + cloud->data.resize(numberReservedPoints * cloud->point_step); // resize to fit all points and avoid memory moves + pointcloudDataPosition = (float*)(&cloud->data[0]); + } // call pre-traversal hook: handlePreNodeTraversal(rostime); @@ -514,8 +553,18 @@ void OctomapServer::publishAll(const ros::Time& rostime){ } // insert into pointcloud: - if (publishPointCloud) - pclCloud.push_back(pcl::PointXYZ(x, y, z)); + if (publishPointCloud) { + if (numberPointsInCloud >= numberReservedPoints) { + numberReservedPoints += 16; + cloud->data.resize(numberReservedPoints * cloud->point_step); + pointcloudDataPosition = (float*)(&cloud->data[numberPointsInCloud * cloud->point_step]); + } + + *pointcloudDataPosition++ = (float)x; + *pointcloudDataPosition++ = (float)y; + *pointcloudDataPosition++ = (float)z; + ++numberPointsInCloud; + } } } else{ // node not occupied => mark as free in 2D map if unknown so far @@ -604,11 +653,10 @@ void OctomapServer::publishAll(const ros::Time& rostime){ // finish pointcloud: - if (publishPointCloud){ - sensor_msgs::PointCloud2 cloud; - pcl::toROSMsg (pclCloud, cloud); - cloud.header.frame_id = m_worldFrameId; - cloud.header.stamp = rostime; + if (publishPointCloud) { + cloud->width = numberPointsInCloud; + cloud->row_step = cloud->width * cloud->point_step; + cloud->data.resize(cloud->height * cloud->row_step); // resize to shrink the vector size to the real number of points inserted m_pointCloudPub.publish(cloud); } @@ -863,6 +911,8 @@ void OctomapServer::handlePreNodeTraversal(const ros::Time& rostime){ double minX, minY, minZ, maxX, maxY, maxZ; m_octree->getMetricMin(minX, minY, minZ); m_octree->getMetricMax(maxX, maxY, maxZ); + if (m_occupancyGrid2DMinZ > minZ) minZ = m_occupancyGrid2DMinZ; + if (m_occupancyGrid2DMaxZ < maxZ) maxZ = m_occupancyGrid2DMaxZ; octomap::point3d minPt(minX, minY, minZ); octomap::point3d maxPt(maxX, maxY, maxZ); @@ -924,7 +974,7 @@ void OctomapServer::handlePreNodeTraversal(const ros::Time& rostime){ 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); + m_gridmap.data.resize(m_gridmap.info.width * m_gridmap.info.height, m_occupancyGrid2DInitializedAsFree ? 0 : -1); } else { @@ -950,8 +1000,7 @@ void OctomapServer::handlePreNodeTraversal(const ros::Time& rostime){ // 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); + std::fill_n(m_gridmap.data.begin() + m_gridmap.info.width*j+mapUpdateBBXMinX, numCols, m_occupancyGrid2DInitializedAsFree ? 0 : -1); } } @@ -997,34 +1046,33 @@ void OctomapServer::handleFreeNodeInBBX(const OcTreeT::iterator& it){ } void OctomapServer::update2DMap(const OcTreeT::iterator& it, bool occupied){ - // update 2D map (occupied always overrides): + double z = it.getZ(); + if (z >= m_occupancyGrid2DMinZ && z <= m_occupancyGrid2DMaxZ) { + 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; + } - 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; + } 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; + } } } } } - - } @@ -1080,7 +1128,7 @@ void OctomapServer::adjustMapData(nav_msgs::OccupancyGrid& map, const nav_msgs:: map.data.clear(); // init to unknown: - map.data.resize(map.info.width * map.info.height, -1); + map.data.resize(map.info.width * map.info.height, m_occupancyGrid2DInitializedAsFree ? 0 : -1); nav_msgs::OccupancyGrid::_data_type::iterator fromStart, fromEnd, toStart; From cfe8293df2cbcaa0747abd4154d2ca6f40e18abe Mon Sep 17 00:00:00 2001 From: Carlos Miguel Correia da Costa Date: Wed, 18 Jun 2014 14:20:10 +0100 Subject: [PATCH 2/6] Improved efficiency of pointcloud integration and better parametrization of sensor frame id. - Line 310 of OctomapServer.cpp was doing an unnecessary internal copy (pc_nonground = pc), that can be avoided by using shared pointers. - Improved assembly of pointcloud from octree by using vector reserve instead of resize. - Improved parametrization of sensor frame id. This way the input cloud can be sent in any frame id (map, sensor, other), and the parameter sensor_frame_id can be used to specify which is the sensor frame to compute the origin for the raytracing algorithms (used to integrate the pointcloud into the octree). --- .../include/octomap_server/OctomapServer.h | 1 - octomap_server/src/OctomapServer.cpp | 93 ++++++++++--------- 2 files changed, 50 insertions(+), 44 deletions(-) diff --git a/octomap_server/include/octomap_server/OctomapServer.h b/octomap_server/include/octomap_server/OctomapServer.h index 969bb905..f83dce7a 100644 --- a/octomap_server/include/octomap_server/OctomapServer.h +++ b/octomap_server/include/octomap_server/OctomapServer.h @@ -237,7 +237,6 @@ class OctomapServer{ double m_groundFilterPlaneDistance; bool m_compressMap; - size_t m_numberCloudsPublished; // downprojected 2D map: bool m_incrementalUpdate; diff --git a/octomap_server/src/OctomapServer.cpp b/octomap_server/src/OctomapServer.cpp index 289c3f02..1d79a4af 100644 --- a/octomap_server/src/OctomapServer.cpp +++ b/octomap_server/src/OctomapServer.cpp @@ -38,7 +38,7 @@ OctomapServer::OctomapServer(ros::NodeHandle private_nh_) : m_nh(), m_pointCloudSub(NULL), m_tfPointCloudSub(NULL), - m_tfListener(ros::Duration(30)), + m_tfListener(ros::Duration(60)), m_octree(NULL), m_maxRange(-1.0), m_worldFrameId("/map"), m_baseFrameId("base_footprint"), @@ -62,7 +62,6 @@ OctomapServer::OctomapServer(ros::NodeHandle private_nh_) m_filterSpeckles(false), m_filterGroundPlane(false), m_groundFilterDistance(0.04), m_groundFilterAngle(0.15), m_groundFilterPlaneDistance(0.07), m_compressMap(true), - m_numberCloudsPublished(0), m_incrementalUpdate(false) { ros::NodeHandle private_nh(private_nh_); @@ -248,20 +247,26 @@ void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr // // ground filtering in base frame // - PCLPointCloud pc; // input cloud for filtering and ground-detection - pcl::fromROSMsg(*cloud, pc); + PCLPointCloud::Ptr pc(new PCLPointCloud()); // input cloud for filtering and ground-detection + pcl::fromROSMsg(*cloud, *pc); + bool cloudAlreadyInWorldFrameId = cloud->header.frame_id == m_worldFrameId; + std::string targetFrame = cloud->header.frame_id; + if (cloudAlreadyInWorldFrameId && !m_sensorFrameId.empty()) { + targetFrame = m_sensorFrameId; + } - tf::StampedTransform sensorToWorldTf; + tf::StampedTransform cloudToWorldTf; try { - m_tfListener.lookupTransform(m_worldFrameId, cloudAlreadyInWorldFrameId ? m_sensorFrameId : cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf); + m_tfListener.waitForTransform(m_worldFrameId, targetFrame, cloud->header.stamp, ros::Duration(0.2)); + m_tfListener.lookupTransform(m_worldFrameId, targetFrame, cloud->header.stamp, cloudToWorldTf); } 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); + Eigen::Matrix4f cloudToWorld; + pcl_ros::transformAsMatrix(cloudToWorldTf, cloudToWorld); // set up filter for height range, also removes NANs: @@ -269,14 +274,15 @@ void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr pass.setFilterFieldName("z"); pass.setFilterLimits(m_pointcloudMinZ, m_pointcloudMaxZ); - PCLPointCloud pc_ground; // segmented ground plane - PCLPointCloud pc_nonground; // everything else + PCLPointCloud::Ptr pc_ground(new PCLPointCloud()); // segmented ground plane + PCLPointCloud::Ptr pc_nonground(new PCLPointCloud()); // everything else + + tf::StampedTransform cloudToBaseTf, baseToWorldTf; if (m_filterGroundPlane){ - tf::StampedTransform sensorToBaseTf, baseToWorldTf; 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_baseFrameId, cloud->header.frame_id, cloud->header.stamp, cloudToBaseTf); m_tfListener.lookupTransform(m_worldFrameId, m_baseFrameId, cloud->header.stamp, baseToWorldTf); @@ -286,38 +292,43 @@ void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr } - Eigen::Matrix4f sensorToBase, baseToWorld; - pcl_ros::transformAsMatrix(sensorToBaseTf, sensorToBase); + Eigen::Matrix4f cloudToBase, baseToWorld; + pcl_ros::transformAsMatrix(cloudToBaseTf, cloudToBase); pcl_ros::transformAsMatrix(baseToWorldTf, baseToWorld); - // transform pointcloud from sensor frame to fixed robot frame - pcl::transformPointCloud(pc, pc, sensorToBase); - pass.setInputCloud(pc.makeShared()); - pass.filter(pc); - filterGroundPlane(pc, pc_ground, pc_nonground); + // transform pointcloud from cloud frame to fixed robot frame + pcl::transformPointCloud(*pc, *pc, cloudToBase); + pass.setInputCloud(pc); + pass.filter(*pc); + filterGroundPlane(*pc, *pc_ground, *pc_nonground); // transform clouds to world frame for insertion - pcl::transformPointCloud(pc_ground, pc_ground, baseToWorld); - pcl::transformPointCloud(pc_nonground, pc_nonground, baseToWorld); + pcl::transformPointCloud(*pc_ground, *pc_ground, baseToWorld); + pcl::transformPointCloud(*pc_nonground, *pc_nonground, baseToWorld); } else { // directly transform to map frame: - if (!cloudAlreadyInWorldFrameId) pcl::transformPointCloud(pc, pc, sensorToWorld); + if (!cloudAlreadyInWorldFrameId) pcl::transformPointCloud(*pc, *pc, cloudToWorld); // just filter height range: - pass.setInputCloud(pc.makeShared()); - pass.filter(pc); + pass.setInputCloud(pc); + pass.filter(*pc); - pc_nonground = pc; + pc_nonground = pc; // switch pointers // pc_nonground is empty without ground segmentation - pc_ground.header = pc.header; - pc_nonground.header = pc.header; + pc_ground->header = pc->header; + pc_nonground->header = pc->header; } + // change sensor origin to the frame specified + if (!m_sensorFrameId.empty() && m_sensorFrameId != targetFrame) { + m_tfListener.waitForTransform(m_worldFrameId, m_sensorFrameId, cloud->header.stamp, ros::Duration(0.2)); + m_tfListener.lookupTransform(m_worldFrameId, m_sensorFrameId, cloud->header.stamp, cloudToWorldTf); + } - insertScan(sensorToWorldTf.getOrigin(), pc_ground, pc_nonground); + insertScan(cloudToWorldTf.getOrigin(), *pc_ground, *pc_nonground); double total_elapsed = (ros::WallTime::now() - startTime).toSec(); - ROS_DEBUG("Pointcloud insertion in OctomapServer done (%zu+%zu pts (ground/nonground), %f sec)", pc_ground.size(), pc_nonground.size(), total_elapsed); + ROS_DEBUG("Pointcloud insertion in OctomapServer done (%zu+%zu pts (ground/nonground), %f sec)", pc_ground->size(), pc_nonground->size(), total_elapsed); publishAll(cloud->header.stamp); } @@ -469,10 +480,8 @@ void OctomapServer::publishAll(const ros::Time& rostime){ // init pointcloud: sensor_msgs::PointCloud2Ptr cloud = sensor_msgs::PointCloud2Ptr(new sensor_msgs::PointCloud2()); size_t numberPointsInCloud = 0; - size_t numberReservedPoints = m_octree->size(); - float* pointcloudDataPosition = NULL; + size_t numberReservedPoints = m_octree->getNumLeafNodes(); if (publishPointCloud) { - cloud->header.seq = m_numberCloudsPublished++; cloud->header.stamp = rostime; cloud->header.frame_id = m_worldFrameId; cloud->height = 1; @@ -493,9 +502,7 @@ void OctomapServer::publishAll(const ros::Time& rostime){ cloud->fields[2].count = 1; cloud->point_step = 12; - if (numberReservedPoints < 1) numberReservedPoints = 8; - cloud->data.resize(numberReservedPoints * cloud->point_step); // resize to fit all points and avoid memory moves - pointcloudDataPosition = (float*)(&cloud->data[0]); + cloud->data.reserve(numberReservedPoints * cloud->point_step); // reserve memory to avoid reallocations } // call pre-traversal hook: @@ -554,15 +561,13 @@ void OctomapServer::publishAll(const ros::Time& rostime){ // insert into pointcloud: if (publishPointCloud) { - if (numberPointsInCloud >= numberReservedPoints) { - numberReservedPoints += 16; - cloud->data.resize(numberReservedPoints * cloud->point_step); - pointcloudDataPosition = (float*)(&cloud->data[numberPointsInCloud * cloud->point_step]); - } + float pointData[3]; + pointData[0] = x; + pointData[1] = y; + pointData[2] = z; + unsigned char* pointDataBytes = (unsigned char*)(&pointData[0]); + cloud->data.insert(cloud->data.end(), &pointDataBytes[0], &pointDataBytes[3 * sizeof(float)]); - *pointcloudDataPosition++ = (float)x; - *pointcloudDataPosition++ = (float)y; - *pointcloudDataPosition++ = (float)z; ++numberPointsInCloud; } @@ -658,6 +663,8 @@ void OctomapServer::publishAll(const ros::Time& rostime){ cloud->row_step = cloud->width * cloud->point_step; cloud->data.resize(cloud->height * cloud->row_step); // resize to shrink the vector size to the real number of points inserted m_pointCloudPub.publish(cloud); + + ROS_DEBUG_STREAM("Published pointcloud with " << numberPointsInCloud << " points"); } if (publishBinaryMap) From dbb405550577487ea94d8092641a3781d8d77efe Mon Sep 17 00:00:00 2001 From: Carlos Miguel Correia da Costa Date: Wed, 13 Aug 2014 14:44:00 +0100 Subject: [PATCH 3/6] Configuration of occupancy grid projection (min and max z) using dynamic reconfigure. --- octomap_server/cfg/OctomapServer.cfg | 3 ++- octomap_server/src/OctomapServer.cpp | 15 +++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/octomap_server/cfg/OctomapServer.cfg b/octomap_server/cfg/OctomapServer.cfg index a473767e..a9f62855 100755 --- a/octomap_server/cfg/OctomapServer.cfg +++ b/octomap_server/cfg/OctomapServer.cfg @@ -6,5 +6,6 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("max_depth", int_t, 0, "Maximum depth when traversing the octree to send out markers. 16: full depth / max. resolution", 16, 1, 16) - +gen.add("occupancy_grid_2d_min_z", double_t, 0, "Minimum height for 2D occupancy map projection", 0) +gen.add("occupancy_grid_2d_max_z", double_t, 0, "Maiximum height for 2D occupancy map projection", 3) exit(gen.generate(PACKAGE, "octomap_server_node", "OctomapServer")) \ No newline at end of file diff --git a/octomap_server/src/OctomapServer.cpp b/octomap_server/src/OctomapServer.cpp index 1d79a4af..05bd8a52 100644 --- a/octomap_server/src/OctomapServer.cpp +++ b/octomap_server/src/OctomapServer.cpp @@ -1104,14 +1104,13 @@ bool OctomapServer::isSpeckleNode(const OcTreeKey&nKey) const { return neighborFound; } -void OctomapServer::reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level){ - if (m_maxTreeDepth != unsigned(config.max_depth)){ - m_maxTreeDepth = unsigned(config.max_depth); - - publishAll(); - } - - +void OctomapServer::reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level) { + if (m_maxTreeDepth != config.max_depth || m_occupancyGrid2DMinZ != config.occupancy_grid_2d_min_z || m_occupancyGrid2DMaxZ != config.occupancy_grid_2d_max_z) { + m_maxTreeDepth = config.max_depth; + m_occupancyGrid2DMinZ = config.occupancy_grid_2d_min_z; + m_occupancyGrid2DMaxZ = config.occupancy_grid_2d_max_z; + publishAll(); + } } void OctomapServer::adjustMapData(nav_msgs::OccupancyGrid& map, const nav_msgs::MapMetaData& oldMapInfo) const{ From 03f1c1c193df174ccb7e6d2f944a88445873ae45 Mon Sep 17 00:00:00 2001 From: Carlos Miguel Correia da Costa Date: Wed, 18 Feb 2015 00:09:45 +0000 Subject: [PATCH 4/6] Added option to load the initial OctoMap from nav_msgs/OccupancyGrid msg and regulate how often the 2D projection map is published. --- .../include/octomap_server/OctomapServer.h | 17 +++- octomap_server/src/OctomapServer.cpp | 86 +++++++++++++++++-- 2 files changed, 96 insertions(+), 7 deletions(-) diff --git a/octomap_server/include/octomap_server/OctomapServer.h b/octomap_server/include/octomap_server/OctomapServer.h index f83dce7a..70709e01 100644 --- a/octomap_server/include/octomap_server/OctomapServer.h +++ b/octomap_server/include/octomap_server/OctomapServer.h @@ -65,6 +65,7 @@ #include #include #include +#include namespace octomap_server { @@ -112,7 +113,9 @@ class OctomapServer{ void reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level); void publishBinaryOctoMap(const ros::Time& rostime = ros::Time::now()) const; void publishFullOctoMap(const ros::Time& rostime = ros::Time::now()) const; - void publishAll(const ros::Time& rostime = ros::Time::now()); + void publishAll(const ros::Time& rostime = ros::Time::now(), bool forceMsgPublish = false); + + void insertReferenceOccupancyGrid(const nav_msgs::OccupancyGridConstPtr& occupancy_grid_msg); /** * @brief update occupancy map with a scan labeled as ground and nonground. @@ -191,6 +194,7 @@ class OctomapServer{ ros::Publisher m_markerPub, m_binaryMapPub, m_fullMapPub, m_pointCloudPub, m_collisionObjectPub, m_mapPub, m_cmapPub, m_fmapPub, m_fmarkerPub; message_filters::Subscriber* m_pointCloudSub; tf::MessageFilter* m_tfPointCloudSub; + ros::Subscriber m_OccupancyGridSub_; ros::ServiceServer m_octomapBinaryService, m_octomapFullService, m_clearBBXService, m_resetService; tf::TransformListener m_tfListener; dynamic_reconfigure::Server m_reconfigureServer; @@ -219,7 +223,10 @@ class OctomapServer{ double m_probMiss; double m_thresMin; double m_thresMax; + double m_thresOccupancy; + bool m_overrideSensorZ; + double m_overrideSensorZValue; double m_pointcloudMinZ; double m_pointcloudMaxZ; double m_occupancyMinZ; @@ -238,6 +245,14 @@ class OctomapServer{ bool m_compressMap; + + ros::Duration m_miniumAmountOfTimeBetweenROSMsgPublishing; + int m_minimumNumberOfIntegrationsBeforeROSMsgPublishing; + ros::Time m_timeLastPublishedROSMsgs; + int m_numberIntegrationsSinceLastPublishedROSMsgs; + + + // downprojected 2D map: bool m_incrementalUpdate; nav_msgs::OccupancyGrid m_gridmap; diff --git a/octomap_server/src/OctomapServer.cpp b/octomap_server/src/OctomapServer.cpp index 05bd8a52..bb996f8f 100644 --- a/octomap_server/src/OctomapServer.cpp +++ b/octomap_server/src/OctomapServer.cpp @@ -51,6 +51,9 @@ OctomapServer::OctomapServer(ros::NodeHandle private_nh_) m_maxTreeDepth(0), m_probHit(0.7), m_probMiss(0.4), m_thresMin(0.12), m_thresMax(0.97), + m_thresOccupancy(0.5), + m_overrideSensorZ(false), + m_overrideSensorZValue(0.0), m_pointcloudMinZ(-std::numeric_limits::max()), m_pointcloudMaxZ(std::numeric_limits::max()), m_occupancyMinZ(-std::numeric_limits::max()), @@ -62,7 +65,9 @@ OctomapServer::OctomapServer(ros::NodeHandle private_nh_) 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_incrementalUpdate(false), + m_timeLastPublishedROSMsgs(0), + m_numberIntegrationsSinceLastPublishedROSMsgs(0) { ros::NodeHandle private_nh(private_nh_); private_nh.param("frame_id", m_worldFrameId, m_worldFrameId); @@ -72,7 +77,10 @@ OctomapServer::OctomapServer(ros::NodeHandle private_nh_) private_nh.param("color_factor", m_colorFactor, m_colorFactor); private_nh.param("pointcloud_min_z", m_pointcloudMinZ,m_pointcloudMinZ); - private_nh.param("pointcloud_max_z", m_pointcloudMaxZ,m_pointcloudMaxZ); + private_nh.param("pointcloud_min_z", m_pointcloudMinZ,m_pointcloudMinZ); + + private_nh.param("override_sensor_z", m_overrideSensorZ,m_overrideSensorZ); + private_nh.param("override_sensor_z_value", m_overrideSensorZValue,m_overrideSensorZValue); private_nh.param("occupancy_min_z", m_occupancyMinZ,m_occupancyMinZ); private_nh.param("occupancy_max_z", m_occupancyMaxZ,m_occupancyMaxZ); private_nh.param("occupancy_grid_2d_min_z", m_occupancyGrid2DMinZ, m_occupancyGrid2DMinZ); @@ -97,6 +105,8 @@ OctomapServer::OctomapServer(ros::NodeHandle private_nh_) private_nh.param("sensor_model/miss", m_probMiss, m_probMiss); private_nh.param("sensor_model/min", m_thresMin, m_thresMin); private_nh.param("sensor_model/max", m_thresMax, m_thresMax); + private_nh.param("sensor_model/occupancy_treshold", m_thresOccupancy, m_thresOccupancy); + private_nh.param("compress_map", m_compressMap, m_compressMap); private_nh.param("incremental_2D_projection", m_incrementalUpdate, m_incrementalUpdate); @@ -115,6 +125,7 @@ OctomapServer::OctomapServer(ros::NodeHandle private_nh_) m_octree->setProbMiss(m_probMiss); m_octree->setClampingThresMin(m_thresMin); m_octree->setClampingThresMax(m_thresMax); + m_octree->setOccupancyThres(m_thresOccupancy); m_treeDepth = m_octree->getTreeDepth(); m_maxTreeDepth = m_treeDepth; m_gridmap.info.resolution = m_res; @@ -157,6 +168,16 @@ OctomapServer::OctomapServer(ros::NodeHandle private_nh_) m_tfPointCloudSub = new tf::MessageFilter (*m_pointCloudSub, m_tfListener, m_worldFrameId, 5); m_tfPointCloudSub->registerCallback(boost::bind(&OctomapServer::insertCloudCallback, this, _1)); + std::string occupancyGridTopic; + private_nh.param("occupancy_grid_in", occupancyGridTopic, std::string("occupancy_grid_in")); + m_OccupancyGridSub_ = m_nh.subscribe(occupancyGridTopic, 1, &OctomapServer::insertReferenceOccupancyGrid, this); + + double minimumAmountOfTimeBetweenROSMsgPublishing; + private_nh.param("minimum_amount_of_time_between_ros_msg_publishing", minimumAmountOfTimeBetweenROSMsgPublishing, 5.0); + m_miniumAmountOfTimeBetweenROSMsgPublishing.fromSec(minimumAmountOfTimeBetweenROSMsgPublishing); + private_nh.param("minimum_number_of_integrations_before_ros_msg_publishing", m_minimumNumberOfIntegrationsBeforeROSMsgPublishing, 10); + + m_octomapBinaryService = m_nh.advertiseService("octomap_binary", &OctomapServer::octomapBinarySrv, this); m_octomapFullService = m_nh.advertiseService("octomap_full", &OctomapServer::octomapFullSrv, this); m_clearBBXService = private_nh.advertiseService("clear_bbx", &OctomapServer::clearBBXSrv, this); @@ -234,7 +255,7 @@ bool OctomapServer::openFile(const std::string& filename){ m_updateBBXMax[1] = m_octree->coordToKey(maxY); m_updateBBXMax[2] = m_octree->coordToKey(maxZ); - publishAll(); + publishAll(ros::Time::now(), true); return true; @@ -330,12 +351,14 @@ void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr double total_elapsed = (ros::WallTime::now() - startTime).toSec(); ROS_DEBUG("Pointcloud insertion in OctomapServer done (%zu+%zu pts (ground/nonground), %f sec)", pc_ground->size(), pc_nonground->size(), total_elapsed); - publishAll(cloud->header.stamp); + publishAll(cloud->header.stamp, false); } void OctomapServer::insertScan(const tf::Point& sensorOriginTf, const PCLPointCloud& ground, const PCLPointCloud& nonground){ point3d sensorOrigin = pointTfToOctomap(sensorOriginTf); + if (m_overrideSensorZ) { sensorOrigin.z() = m_overrideSensorZValue; } + if (!m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMin) || !m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMax)) { @@ -412,7 +435,7 @@ void OctomapServer::insertScan(const tf::Point& sensorOriginTf, const PCLPointCl } // now mark all occupied cells: - for (KeySet::iterator it = occupied_cells.begin(), end=free_cells.end(); it!= end; it++) { + for (KeySet::iterator it = occupied_cells.begin(), end=occupied_cells.end(); it!= end; it++) { m_octree->updateNode(*it, true); } @@ -443,12 +466,63 @@ void OctomapServer::insertScan(const tf::Point& sensorOriginTf, const PCLPointCl if (m_compressMap) m_octree->prune(); + ++m_numberIntegrationsSinceLastPublishedROSMsgs; +} + +void OctomapServer::insertReferenceOccupancyGrid(const nav_msgs::OccupancyGridConstPtr& occupancy_grid_msg) { + size_t number_cells = occupancy_grid_msg->info.width * occupancy_grid_msg->info.height; + if (occupancy_grid_msg->data.size() > 0 && (occupancy_grid_msg->data.size() == number_cells)) { + float map_resolution = occupancy_grid_msg->info.resolution; + unsigned int map_width = occupancy_grid_msg->info.width; + unsigned int map_height = occupancy_grid_msg->info.height; + + float map_origin_x = occupancy_grid_msg->info.origin.position.x + map_resolution / 2.0; + float map_origin_y = occupancy_grid_msg->info.origin.position.y + map_resolution / 2.0; + + Eigen::Transform transform = + Eigen::Transform(Eigen::Translation3f(map_origin_x, map_origin_y, 0)) * + Eigen::Transform(Eigen::Quaternionf(occupancy_grid_msg->info.origin.orientation.w, occupancy_grid_msg->info.origin.orientation.x, occupancy_grid_msg->info.origin.orientation.y, occupancy_grid_msg->info.origin.orientation.z)); + + size_t data_position = 0; + double new_x, new_y; + for (unsigned int y = 0; y < map_height; ++y) { + float x_map = 0.0; + float y_map = (float)y * map_resolution; + for (unsigned int x = 0; x < map_width; ++x) { + x_map = (float)x * map_resolution; + new_x = static_cast (transform (0, 0) * x_map + transform (0, 1) * y_map + transform (0, 3)); + new_y = static_cast (transform (1, 0) * x_map + transform (1, 1) * y_map + transform (1, 3)); + if (occupancy_grid_msg->data[data_position] >= 0) { + double cell_occupation_probability = ((double)occupancy_grid_msg->data[data_position]) / 100.0; + m_octree->setNodeValue(new_x, new_y, 0.0, cell_occupation_probability); + m_octree->updateNode(new_x, new_y, 0.0, cell_occupation_probability > 0.5); + } + + ++data_position; + } + } + + m_octree->updateInnerOccupancy(); + m_octree->toMaxLikelihood(); + m_octree->prune(); + + ROS_INFO_STREAM("OctoMap loaded OccupancyGrid with " << occupancy_grid_msg->info.width << " x " << occupancy_grid_msg->info.height << " cells"); + + publishAll(ros::Time::now(), true); + } } -void OctomapServer::publishAll(const ros::Time& rostime){ +void OctomapServer::publishAll(const ros::Time& rostime, bool forceMsgPublish) { + if(!forceMsgPublish && (m_numberIntegrationsSinceLastPublishedROSMsgs < m_minimumNumberOfIntegrationsBeforeROSMsgPublishing || (rostime - m_timeLastPublishedROSMsgs) < m_miniumAmountOfTimeBetweenROSMsgPublishing)) { + return; + } + + m_numberIntegrationsSinceLastPublishedROSMsgs = 0; + m_timeLastPublishedROSMsgs = rostime; + ros::WallTime startTime = ros::WallTime::now(); size_t octomapSize = m_octree->size(); // TODO: estimate num occ. voxels for size of arrays (reserve) From 87d6004f49676b4cb0166dae03345fba644a7486 Mon Sep 17 00:00:00 2001 From: Carlos Miguel Correia da Costa Date: Wed, 4 Mar 2015 16:49:43 +0000 Subject: [PATCH 5/6] Added service to trigger the publish of octomap information into ros messages. --- octomap_server/include/octomap_server/OctomapServer.h | 3 ++- octomap_server/src/OctomapServer.cpp | 8 ++++++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/octomap_server/include/octomap_server/OctomapServer.h b/octomap_server/include/octomap_server/OctomapServer.h index 70709e01..8c9d1034 100644 --- a/octomap_server/include/octomap_server/OctomapServer.h +++ b/octomap_server/include/octomap_server/OctomapServer.h @@ -82,6 +82,7 @@ class OctomapServer{ virtual ~OctomapServer(); virtual bool octomapBinarySrv(OctomapSrv::Request &req, OctomapSrv::GetOctomap::Response &res); virtual bool octomapFullSrv(OctomapSrv::Request &req, OctomapSrv::GetOctomap::Response &res); + virtual bool octomapPublishAllSrv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); bool clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp); bool resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp); @@ -195,7 +196,7 @@ class OctomapServer{ message_filters::Subscriber* m_pointCloudSub; tf::MessageFilter* m_tfPointCloudSub; ros::Subscriber m_OccupancyGridSub_; - ros::ServiceServer m_octomapBinaryService, m_octomapFullService, m_clearBBXService, m_resetService; + ros::ServiceServer m_octomapBinaryService, m_octomapFullService, m_octomapPublishAllService, m_clearBBXService, m_resetService; tf::TransformListener m_tfListener; dynamic_reconfigure::Server m_reconfigureServer; diff --git a/octomap_server/src/OctomapServer.cpp b/octomap_server/src/OctomapServer.cpp index bb996f8f..a56731f1 100644 --- a/octomap_server/src/OctomapServer.cpp +++ b/octomap_server/src/OctomapServer.cpp @@ -180,6 +180,7 @@ OctomapServer::OctomapServer(ros::NodeHandle private_nh_) m_octomapBinaryService = m_nh.advertiseService("octomap_binary", &OctomapServer::octomapBinarySrv, this); m_octomapFullService = m_nh.advertiseService("octomap_full", &OctomapServer::octomapFullSrv, this); + m_octomapPublishAllService = m_nh.advertiseService("octomap_publish_all", &OctomapServer::octomapPublishAllSrv, this); m_clearBBXService = private_nh.advertiseService("clear_bbx", &OctomapServer::clearBBXSrv, this); m_resetService = private_nh.advertiseService("reset", &OctomapServer::resetSrv, this); @@ -780,6 +781,13 @@ bool OctomapServer::octomapFullSrv(OctomapSrv::Request &req, return true; } + +bool OctomapServer::octomapPublishAllSrv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { + publishAll(ros::Time::now(), true); + return true; +} + + bool OctomapServer::clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp){ point3d min = pointMsgToOctomap(req.min); point3d max = pointMsgToOctomap(req.max); From e102f8b53511c90f5405bc650e1cc20fa1f35b9b Mon Sep 17 00:00:00 2001 From: Carlos Costa Date: Thu, 9 Jul 2015 16:35:59 +0100 Subject: [PATCH 6/6] Improved integration of nav_msgs::OccupancyGrid msgs. --- octomap_server/src/OctomapServer.cpp | 28 +++++++++++++++++++++++++--- 1 file changed, 25 insertions(+), 3 deletions(-) diff --git a/octomap_server/src/OctomapServer.cpp b/octomap_server/src/OctomapServer.cpp index a56731f1..b355c36a 100644 --- a/octomap_server/src/OctomapServer.cpp +++ b/octomap_server/src/OctomapServer.cpp @@ -487,6 +487,28 @@ void OctomapServer::insertReferenceOccupancyGrid(const nav_msgs::OccupancyGridCo size_t data_position = 0; double new_x, new_y; + + // insert free + for (unsigned int y = 0; y < map_height; ++y) { + float x_map = 0.0; + float y_map = (float)y * map_resolution; + for (unsigned int x = 0; x < map_width; ++x) { + x_map = (float)x * map_resolution; + new_x = static_cast (transform (0, 0) * x_map + transform (0, 1) * y_map + transform (0, 3)); + new_y = static_cast (transform (1, 0) * x_map + transform (1, 1) * y_map + transform (1, 3)); + double cell_occupation_probability = ((double)occupancy_grid_msg->data[data_position]) / 100.0; + if (cell_occupation_probability >= 0.0 && cell_occupation_probability < 0.5) { + m_octree->setNodeValue(new_x, new_y, 0.0, cell_occupation_probability); + m_octree->updateNode(new_x, new_y, 0.0, cell_occupation_probability >= 0.5); + } + + ++data_position; + } + } + + data_position = 0; + + // insert occupied (after inserting free cells in order to preserve all the occupied cells -> avoids free cells overwriting occupied cells) for (unsigned int y = 0; y < map_height; ++y) { float x_map = 0.0; float y_map = (float)y * map_resolution; @@ -494,10 +516,10 @@ void OctomapServer::insertReferenceOccupancyGrid(const nav_msgs::OccupancyGridCo x_map = (float)x * map_resolution; new_x = static_cast (transform (0, 0) * x_map + transform (0, 1) * y_map + transform (0, 3)); new_y = static_cast (transform (1, 0) * x_map + transform (1, 1) * y_map + transform (1, 3)); - if (occupancy_grid_msg->data[data_position] >= 0) { - double cell_occupation_probability = ((double)occupancy_grid_msg->data[data_position]) / 100.0; + double cell_occupation_probability = ((double)occupancy_grid_msg->data[data_position]) / 100.0; + if (cell_occupation_probability >= 0.5) { m_octree->setNodeValue(new_x, new_y, 0.0, cell_occupation_probability); - m_octree->updateNode(new_x, new_y, 0.0, cell_occupation_probability > 0.5); + m_octree->updateNode(new_x, new_y, 0.0, cell_occupation_probability >= 0.5); } ++data_position;