Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Efficiency improvements and customizable occupancy 2D map projection. #9

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
Open

Efficiency improvements and customizable occupancy 2D map projection. #9

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
29d32c1
Efficiency improvements and customizable occupancy 2D map projection.
carlosmccosta Jun 16, 2014
cfe8293
Improved efficiency of pointcloud integration and better parametrization
carlosmccosta Jun 18, 2014
dbb4055
Configuration of occupancy grid projection (min and max z) using dynamic
carlosmccosta Aug 13, 2014
03f1c1c
Added option to load the initial OctoMap from nav_msgs/OccupancyGrid msg
carlosmccosta Feb 18, 2015
87d6004
Added service to trigger the publish of octomap information into ros
carlosmccosta Mar 4, 2015
e102f8b
Improved integration of nav_msgs::OccupancyGrid msgs.
carlosmccosta Jul 9, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions octomap_server/include/octomap_server/OctomapServer.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -233,6 +237,7 @@ class OctomapServer{
double m_groundFilterPlaneDistance;

bool m_compressMap;
size_t m_numberCloudsPublished;

// downprojected 2D map:
bool m_incrementalUpdate;
Expand Down
122 changes: 85 additions & 37 deletions octomap_server/src/OctomapServer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"),
Expand All @@ -54,22 +55,30 @@ OctomapServer::OctomapServer(ros::NodeHandle private_nh_)
m_pointcloudMaxZ(std::numeric_limits<double>::max()),
m_occupancyMinZ(-std::numeric_limits<double>::max()),
m_occupancyMaxZ(std::numeric_limits<double>::max()),
m_occupancyGrid2DMinZ(-std::numeric_limits<double>::max()),
m_occupancyGrid2DMaxZ(std::numeric_limits<double>::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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

m_sensorFrameId is used with its default value (empty string) if the user does not explicitly set it. This will break existing code / launch files. I would also discourage using a parameter for frame IDs since all incoming point clouds already have a valid frame ID.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This parameter is needed when the pointclouds arrive already in the map frame (for example in a 3D pointcloud assembler from 2D laser scans). This way a pointless pointcloud transformation will be avoided.
And if the pointclouds do arrive in the map frame, it is necessary to find the sensor pose to integrate the pointcloud in the octree.
It will not break existing code, because it is checked in line 252
bool cloudAlreadyInWorldFrameId = cloud->header.frame_id == m_worldFrameId;
and the parameter passed to the tf listener updated accordingly
m_tfListener.lookupTransform(m_worldFrameId, cloudAlreadyInWorldFrameId ? m_sensorFrameId : cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf);

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If I start the code as it is and my points are already in the map frame (e.g., static sensor), then your PR will in the best case end up with a different result (if the param is set) and by default (param not set) will break since the TF listener looks up the empty frame "".

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I didn't consider that case, because i assumed no one would want to insert scans in the m_worldFrameId without providing the sensor frame id.
In fact, i introduced this extra parameter, because if the cloud is in the m_worldFrameId (map), the raytracing algorithms won't work as expected because they would be casting rays from the wrong origin.
Nevertheless, i see your point, and the code should be robust to handle incorrect parameterization.
Moreover, it may be useful to provide point clouds in other frames other than map and sensor, so this parameter would be useful for fixing the raytracing origin.

private_nh.param("height_map", m_useHeightMap, m_useHeightMap);
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("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);

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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<pcl::PointXYZ> pclCloud;
// init pointcloud:
sensor_msgs::PointCloud2Ptr cloud = sensor_msgs::PointCloud2Ptr(new sensor_msgs::PointCloud2());
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

cloud is only used locally here, so no need to create on the Heap + Ptr.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I tend to use shared ptrs because if in the future the nodes are converted to nodelets, the transition is easier (and the overhead of the Ptr is negligible).

size_t numberPointsInCloud = 0;
size_t numberReservedPoints = m_octree->size();
float* pointcloudDataPosition = NULL;
if (publishPointCloud) {
cloud->header.seq = m_numberCloudsPublished++;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This line (and the variable m_numberCloudsPublished) is not needed, the publisher should automatically increase .seq in the header.

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);
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Resizing back and forth is a bad idea. Best leave that to the actual vector implementation and instead use data.reserve from the beginning.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

With large pointclouds, letting the vector resize automatically may lead to lots of memory reallocations, and since we have an estimate of the desired size, the efficiency gains will outweigh the memory consumption overhead.

Initially i used resize because i wanted to track the size of the internal vector (cloud->points), and reserve doesnt affect the vector size, but then i decided to track it manually with
size_t numberPointsInCloud
This way i avoided a division by the cloud->point_step.

Nevertheless reserve is more efficient than resize, because doesn't insert default value elements, so i will changed it.

m_pointCloudPub.publish(cloud);
}

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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 {

Expand All @@ -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);
}

}
Expand Down Expand Up @@ -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;
}
}
}
}
}


}


Expand Down Expand Up @@ -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;

Expand Down