OctomapServer.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright (c) 2010-2012, A. Hornung, University of Freiburg
00011  * All rights reserved.
00012  *
00013  * Redistribution and use in source and binary forms, with or without
00014  * modification, are permitted provided that the following conditions are met:
00015  *
00016  *     * Redistributions of source code must retain the above copyright
00017  *       notice, this list of conditions and the following disclaimer.
00018  *     * Redistributions in binary form must reproduce the above copyright
00019  *       notice, this list of conditions and the following disclaimer in the
00020  *       documentation and/or other materials provided with the distribution.
00021  *     * Neither the name of the University of Freiburg nor the names of its
00022  *       contributors may be used to endorse or promote products derived from
00023  *       this software without specific prior written permission.
00024  *
00025  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00026  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00027  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00028  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00029  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00030  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00031  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00032  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00033  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00034  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  * POSSIBILITY OF SUCH DAMAGE.
00036  */
00037 
00038 #include <octomap_server/OctomapServer.h>
00039 
00040 using namespace octomap;
00041 using octomap_msgs::OctomapBinary;
00042 
00043 namespace octomap_server{
00044 
00045 OctomapServer::OctomapServer(ros::NodeHandle private_nh_)
00046 : m_nh(),
00047   m_pointCloudSub(NULL),
00048   m_tfPointCloudSub(NULL),
00049   m_octree(NULL),
00050   m_maxRange(-1.0),
00051   m_worldFrameId("/map"), m_baseFrameId("base_footprint"),
00052   m_useHeightMap(true),
00053   m_colorFactor(0.8),
00054   m_latchedTopics(true),
00055   m_publishFreeSpace(false),
00056   m_res(0.05),
00057   m_treeDepth(0),
00058   m_maxTreeDepth(0),
00059   m_probHit(0.7), m_probMiss(0.4),
00060   m_thresMin(0.12), m_thresMax(0.97),
00061   m_pointcloudMinZ(-std::numeric_limits<double>::max()),
00062   m_pointcloudMaxZ(std::numeric_limits<double>::max()),
00063   m_occupancyMinZ(-std::numeric_limits<double>::max()),
00064   m_occupancyMaxZ(std::numeric_limits<double>::max()),
00065   m_minSizeX(0.0), m_minSizeY(0.0),
00066   m_filterSpeckles(false), m_filterGroundPlane(false),
00067   m_groundFilterDistance(0.04), m_groundFilterAngle(0.15), m_groundFilterPlaneDistance(0.07),
00068   m_compressMap(true),
00069   m_incrementalUpdate(false)
00070 {
00071   ros::NodeHandle private_nh(private_nh_);
00072   private_nh.param("frame_id", m_worldFrameId, m_worldFrameId);
00073   private_nh.param("base_frame_id", m_baseFrameId, m_baseFrameId);
00074   private_nh.param("height_map", m_useHeightMap, m_useHeightMap);
00075   private_nh.param("color_factor", m_colorFactor, m_colorFactor);
00076 
00077   private_nh.param("pointcloud_min_z", m_pointcloudMinZ,m_pointcloudMinZ);
00078   private_nh.param("pointcloud_max_z", m_pointcloudMaxZ,m_pointcloudMaxZ);
00079   private_nh.param("occupancy_min_z", m_occupancyMinZ,m_occupancyMinZ);
00080   private_nh.param("occupancy_max_z", m_occupancyMaxZ,m_occupancyMaxZ);
00081   private_nh.param("min_x_size", m_minSizeX,m_minSizeX);
00082   private_nh.param("min_y_size", m_minSizeY,m_minSizeY);
00083 
00084   private_nh.param("filter_speckles", m_filterSpeckles, m_filterSpeckles);
00085   private_nh.param("filter_ground", m_filterGroundPlane, m_filterGroundPlane);
00086   // distance of points from plane for RANSAC
00087   private_nh.param("ground_filter/distance", m_groundFilterDistance, m_groundFilterDistance);
00088   // angular derivation of found plane:
00089   private_nh.param("ground_filter/angle", m_groundFilterAngle, m_groundFilterAngle);
00090   // distance of found plane from z=0 to be detected as ground (e.g. to exclude tables)
00091   private_nh.param("ground_filter/plane_distance", m_groundFilterPlaneDistance, m_groundFilterPlaneDistance);
00092 
00093   private_nh.param("sensor_model/max_range", m_maxRange, m_maxRange);
00094 
00095   private_nh.param("resolution", m_res, m_res);
00096   private_nh.param("sensor_model/hit", m_probHit, m_probHit);
00097   private_nh.param("sensor_model/miss", m_probMiss, m_probMiss);
00098   private_nh.param("sensor_model/min", m_thresMin, m_thresMin);
00099   private_nh.param("sensor_model/max", m_thresMax, m_thresMax);
00100   private_nh.param("compress_map", m_compressMap, m_compressMap);
00101   private_nh.param("incremental_2D_projection", m_incrementalUpdate, m_incrementalUpdate);
00102 
00103   if (m_filterGroundPlane && (m_pointcloudMinZ > 0.0 || m_pointcloudMaxZ < 0.0)){
00104           ROS_WARN_STREAM("You enabled ground filtering but incoming pointclouds will be pre-filtered in ["
00105                           <<m_pointcloudMinZ <<", "<< m_pointcloudMaxZ << "], excluding the ground level z=0. "
00106                           << "This will not work.");
00107 
00108   }
00109 
00110 
00111 
00112   // initialize octomap object & params
00113   m_octree = new OcTree(m_res);
00114   m_octree->setProbHit(m_probHit);
00115   m_octree->setProbMiss(m_probMiss);
00116   m_octree->setClampingThresMin(m_thresMin);
00117   m_octree->setClampingThresMax(m_thresMax);
00118   m_treeDepth = m_octree->getTreeDepth();
00119   m_maxTreeDepth = m_treeDepth;
00120   m_gridmap.info.resolution = m_res;
00121 
00122   double r, g, b, a;
00123   private_nh.param("color/r", r, 0.0);
00124   private_nh.param("color/g", g, 0.0);
00125   private_nh.param("color/b", b, 1.0);
00126   private_nh.param("color/a", a, 1.0);
00127   m_color.r = r;
00128   m_color.g = g;
00129   m_color.b = b;
00130   m_color.a = a;
00131 
00132   private_nh.param("color_free/r", r, 0.0);
00133   private_nh.param("color_free/g", g, 1.0);
00134   private_nh.param("color_free/b", b, 0.0);
00135   private_nh.param("color_free/a", a, 1.0);
00136   m_colorFree.r = r;
00137   m_colorFree.g = g;
00138   m_colorFree.b = b;
00139   m_colorFree.a = a;
00140 
00141   private_nh.param("publish_free_space", m_publishFreeSpace, m_publishFreeSpace);
00142 
00143   private_nh.param("latch", m_latchedTopics, m_latchedTopics);
00144   if (m_latchedTopics){
00145     ROS_INFO("Publishing latched (single publish will take longer, all topics are prepared)");
00146   } else
00147     ROS_INFO("Publishing non-latched (topics are only prepared as needed, will only be re-published on map change");
00148 
00149   m_markerPub = m_nh.advertise<visualization_msgs::MarkerArray>("occupied_cells_vis_array", 1, m_latchedTopics);
00150   m_binaryMapPub = m_nh.advertise<OctomapBinary>("octomap_binary", 1, m_latchedTopics);
00151   m_fullMapPub = m_nh.advertise<OctomapBinary>("octomap_full", 1, m_latchedTopics);
00152   m_pointCloudPub = m_nh.advertise<sensor_msgs::PointCloud2>("octomap_point_cloud_centers", 1, m_latchedTopics);
00153   m_collisionObjectPub = m_nh.advertise<arm_navigation_msgs::CollisionObject>("octomap_collision_object", 1, m_latchedTopics);
00154   m_mapPub = m_nh.advertise<nav_msgs::OccupancyGrid>("projected_map", 5, m_latchedTopics);
00155   m_cmapPub = m_nh.advertise<arm_navigation_msgs::CollisionMap>("collision_map_out", 1, m_latchedTopics);
00156   m_fmapPub = m_nh.advertise<arm_navigation_msgs::CollisionMap>("free_map_out", 1, m_latchedTopics);    
00157   m_fmarkerPub = m_nh.advertise<visualization_msgs::MarkerArray>("free_cells_vis_array", 1, m_latchedTopics);
00158 
00159   m_pointCloudSub = new message_filters::Subscriber<sensor_msgs::PointCloud2> (m_nh, "cloud_in", 5);
00160   m_tfPointCloudSub = new tf::MessageFilter<sensor_msgs::PointCloud2> (*m_pointCloudSub, m_tfListener, m_worldFrameId, 5);
00161   m_tfPointCloudSub->registerCallback(boost::bind(&OctomapServer::insertCloudCallback, this, _1));
00162 
00163   m_octomapBinaryService = m_nh.advertiseService("octomap_binary", &OctomapServer::octomapBinarySrv, this);
00164   m_octomapFullService = m_nh.advertiseService("octomap_full", &OctomapServer::octomapFullSrv, this);
00165   m_clearBBXService = private_nh.advertiseService("clear_bbx", &OctomapServer::clearBBXSrv, this);
00166   m_resetService = private_nh.advertiseService("reset", &OctomapServer::resetSrv, this);
00167 
00168   dynamic_reconfigure::Server<OctomapServerConfig>::CallbackType f;
00169 
00170   f = boost::bind(&OctomapServer::reconfigureCallback, this, _1, _2);
00171   m_reconfigureServer.setCallback(f);
00172 }
00173 
00174 OctomapServer::~OctomapServer(){
00175   if (m_tfPointCloudSub){
00176     delete m_tfPointCloudSub;
00177     m_tfPointCloudSub = NULL;
00178   }
00179 
00180   if (m_pointCloudSub){
00181     delete m_pointCloudSub;
00182     m_pointCloudSub = NULL;
00183   }
00184 
00185 
00186   if (m_octree){
00187     delete m_octree;
00188     m_octree = NULL;
00189   }
00190 
00191 }
00192 
00193 bool OctomapServer::openFile(const std::string& filename){
00194   if (filename.length() <= 3)
00195     return false;
00196 
00197   std::string suffix = filename.substr(filename.length()-3, 3);
00198   if (suffix== ".bt"){
00199     if (!m_octree->readBinary(filename)){
00200       return false;
00201     }
00202   } else if (suffix == ".ot"){
00203     AbstractOcTree* tree = AbstractOcTree::read(filename);
00204     if (!tree){
00205       return false;
00206     }
00207     if (m_octree){
00208       delete m_octree;
00209       m_octree = NULL;
00210     }
00211     m_octree = dynamic_cast<OcTree*>(tree);
00212     if (!m_octree){
00213       ROS_ERROR("Could not read OcTree in file, currently there are no other types supported in .ot");
00214       return false;
00215     }
00216 
00217   } else{
00218     return false;
00219   }
00220 
00221   ROS_INFO("Octomap file %s loaded (%zu nodes).", filename.c_str(),m_octree->size());
00222 
00223   m_treeDepth = m_octree->getTreeDepth();
00224   m_maxTreeDepth = m_treeDepth;
00225   m_res = m_octree->getResolution();
00226   m_gridmap.info.resolution = m_res;
00227   double minX, minY, minZ;
00228   double maxX, maxY, maxZ;
00229   m_octree->getMetricMin(minX, minY, minZ);
00230   m_octree->getMetricMax(maxX, maxY, maxZ);
00231 
00232   m_octree->genKeyValue(minX, m_updateBBXMin[0]);
00233   m_octree->genKeyValue(minY, m_updateBBXMin[1]);
00234   m_octree->genKeyValue(minZ, m_updateBBXMin[2]);
00235   m_octree->genKeyValue(maxX, m_updateBBXMax[0]);
00236   m_octree->genKeyValue(maxY, m_updateBBXMax[1]);
00237   m_octree->genKeyValue(maxZ, m_updateBBXMax[2]);
00238 
00239   publishAll();
00240 
00241   return true;
00242 
00243 }
00244 
00245 void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud){
00246   ros::WallTime startTime = ros::WallTime::now();
00247 
00248 
00249   //
00250   // ground filtering in base frame
00251   //
00252   PCLPointCloud pc; // input cloud for filtering and ground-detection
00253   pcl::fromROSMsg(*cloud, pc);
00254 
00255   tf::StampedTransform sensorToWorldTf;
00256   try {
00257     m_tfListener.lookupTransform(m_worldFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf);
00258   } catch(tf::TransformException& ex){
00259     ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback");
00260     return;
00261   }
00262 
00263   Eigen::Matrix4f sensorToWorld;
00264   pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld);
00265 
00266 
00267   // set up filter for height range, also removes NANs:
00268   pcl::PassThrough<pcl::PointXYZ> pass;
00269   pass.setFilterFieldName("z");
00270   pass.setFilterLimits(m_pointcloudMinZ, m_pointcloudMaxZ);
00271 
00272   PCLPointCloud pc_ground; // segmented ground plane
00273   PCLPointCloud pc_nonground; // everything else
00274 
00275   if (m_filterGroundPlane){
00276     tf::StampedTransform sensorToBaseTf, baseToWorldTf;
00277     try{
00278       m_tfListener.waitForTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, ros::Duration(0.2));
00279       m_tfListener.lookupTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToBaseTf);
00280       m_tfListener.lookupTransform(m_worldFrameId, m_baseFrameId, cloud->header.stamp, baseToWorldTf);
00281 
00282 
00283     }catch(tf::TransformException& ex){
00284       ROS_ERROR_STREAM( "Transform error for ground plane filter: " << ex.what() << ", quitting callback.\n"
00285                         "You need to set the base_frame_id or disable filter_ground.");
00286     }
00287 
00288 
00289     Eigen::Matrix4f sensorToBase, baseToWorld;
00290     pcl_ros::transformAsMatrix(sensorToBaseTf, sensorToBase);
00291     pcl_ros::transformAsMatrix(baseToWorldTf, baseToWorld);
00292 
00293     // transform pointcloud from sensor frame to fixed robot frame
00294     pcl::transformPointCloud(pc, pc, sensorToBase);
00295     pass.setInputCloud(pc.makeShared());
00296     pass.filter(pc);
00297     filterGroundPlane(pc, pc_ground, pc_nonground);
00298 
00299     // transform clouds to world frame for insertion
00300     pcl::transformPointCloud(pc_ground, pc_ground, baseToWorld);
00301     pcl::transformPointCloud(pc_nonground, pc_nonground, baseToWorld);
00302   } else {
00303     // directly transform to map frame:
00304     pcl::transformPointCloud(pc, pc, sensorToWorld);
00305 
00306     // just filter height range:
00307     pass.setInputCloud(pc.makeShared());
00308     pass.filter(pc);
00309 
00310     pc_nonground = pc;
00311     // pc_nonground is empty without ground segmentation
00312     pc_ground.header = pc.header;
00313     pc_nonground.header = pc.header;
00314   }
00315 
00316 
00317   insertScan(sensorToWorldTf.getOrigin(), pc_ground, pc_nonground);
00318 
00319   double total_elapsed = (ros::WallTime::now() - startTime).toSec();
00320   ROS_DEBUG("Pointcloud insertion in OctomapServer done (%zu+%zu pts (ground/nonground), %f sec)", pc_ground.size(), pc_nonground.size(), total_elapsed);
00321 
00322   publishAll(cloud->header.stamp);
00323 }
00324 
00325 
00326 OcTreeKey getIndexKey(const OcTreeKey & key, unsigned short depth ) {
00327    // TODO: replace with helper function from next octomap (> 1.5)
00328    unsigned short int mask = 65535 << (16 - depth);
00329    OcTreeKey result = key;
00330    result[0] &= mask;
00331    result[1] &= mask;
00332    result[2] &= mask;
00333    return result;
00334 }
00335 
00336 
00337 
00338 void OctomapServer::insertScan(const tf::Point& sensorOriginTf, const PCLPointCloud& ground, const PCLPointCloud& nonground){
00339   point3d sensorOrigin = pointTfToOctomap(sensorOriginTf);
00340 
00341   if (!m_octree->genKey(sensorOrigin, m_updateBBXMin)
00342       || !m_octree->genKey(sensorOrigin, m_updateBBXMax))
00343   {
00344     ROS_ERROR_STREAM("Could not generate Key for origin "<<sensorOrigin);
00345   }
00346 
00347 
00348 
00349   // instead of direct scan insertion, compute update to filter ground:
00350   KeySet free_cells, occupied_cells;
00351   // insert ground points only as free:
00352   for (PCLPointCloud::const_iterator it = ground.begin(); it != ground.end(); ++it){
00353     point3d point(it->x, it->y, it->z);
00354     // maxrange check
00355     if ((m_maxRange > 0.0) && ((point - sensorOrigin).norm() > m_maxRange) ) {
00356       point = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRange;
00357     }
00358 
00359     // only clear space (ground points)
00360     if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){
00361       free_cells.insert(m_keyRay.begin(), m_keyRay.end());
00362     }
00363 
00364     octomap::OcTreeKey endKey;
00365     if (m_octree->genKey(point, endKey)){
00366       updateMinKey(endKey, m_updateBBXMin);
00367       updateMaxKey(endKey, m_updateBBXMax);
00368     } else{
00369       ROS_ERROR_STREAM("Could not generate Key for endpoint "<<point);
00370     }
00371   }
00372 
00373   // all other points: free on ray, occupied on endpoint:
00374   for (PCLPointCloud::const_iterator it = nonground.begin(); it != nonground.end(); ++it){
00375     point3d point(it->x, it->y, it->z);
00376     // maxrange check
00377     if ((m_maxRange < 0.0) || ((point - sensorOrigin).norm() <= m_maxRange) ) {
00378 
00379       // free cells
00380       if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){
00381         free_cells.insert(m_keyRay.begin(), m_keyRay.end());
00382       }
00383       // occupied endpoint
00384       OcTreeKey key;
00385       if (m_octree->genKey(point, key)){
00386         occupied_cells.insert(key);
00387 
00388         updateMinKey(key, m_updateBBXMin);
00389         updateMaxKey(key, m_updateBBXMax);
00390       }
00391     } else {// ray longer than maxrange:;
00392       point3d new_end = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRange;
00393       if (m_octree->computeRayKeys(sensorOrigin, new_end, m_keyRay)){
00394         free_cells.insert(m_keyRay.begin(), m_keyRay.end());
00395 
00396         octomap::OcTreeKey endKey;
00397         if (m_octree->genKey(new_end, endKey)){
00398           updateMinKey(endKey, m_updateBBXMin);
00399           updateMaxKey(endKey, m_updateBBXMax);
00400         } else{
00401           ROS_ERROR_STREAM("Could not generate Key for endpoint "<<new_end);
00402         }
00403 
00404 
00405       }
00406     }
00407   }
00408 
00409   // mark free cells only if not seen occupied in this cloud
00410   for(KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it){
00411     if (occupied_cells.find(*it) == occupied_cells.end()){
00412       m_octree->updateNode(*it, false);
00413     }
00414   }
00415 
00416   // now mark all occupied cells:
00417   for (KeySet::iterator it = occupied_cells.begin(), end=free_cells.end(); it!= end; it++) {
00418     m_octree->updateNode(*it, true);
00419   }
00420 
00421   // TODO: eval lazy+updateInner vs. proper insertion
00422   // non-lazy by default (updateInnerOccupancy() too slow for large maps)
00423   //m_octree->updateInnerOccupancy();
00424   octomap::point3d minPt, maxPt;
00425   ROS_DEBUG_STREAM("Bounding box keys (before): " << m_updateBBXMin[0] << " " <<m_updateBBXMin[1] << " " << m_updateBBXMin[2] << " / " <<m_updateBBXMax[0] << " "<<m_updateBBXMax[1] << " "<< m_updateBBXMax[2]);
00426 
00427   // TODO: snap max / min keys to larger voxels by m_maxTreeDepth
00428 //   if (m_maxTreeDepth < 16)
00429 //   {
00430 //      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)
00431 //      OcTreeKey tmpMax = getIndexKey(m_updateBBXMax, m_maxTreeDepth); // see above, now add something to find upper right
00432 //      tmpMax[0]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1;
00433 //      tmpMax[1]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1;
00434 //      tmpMax[2]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1;
00435 //      m_updateBBXMin = tmpMin;
00436 //      m_updateBBXMax = tmpMax;
00437 //   }
00438 
00439   // TODO: we could also limit the bbx to be within the map bounds here (see publishing check)
00440   m_octree->genCoords(m_updateBBXMin, m_octree->getTreeDepth(), minPt);
00441   m_octree->genCoords(m_updateBBXMax, m_octree->getTreeDepth(), maxPt);
00442   ROS_DEBUG_STREAM("Updated area bounding box: "<< minPt << " - "<<maxPt);
00443   ROS_DEBUG_STREAM("Bounding box keys (after): " << m_updateBBXMin[0] << " " <<m_updateBBXMin[1] << " " << m_updateBBXMin[2] << " / " <<m_updateBBXMax[0] << " "<<m_updateBBXMax[1] << " "<< m_updateBBXMax[2]);
00444 
00445   if (m_compressMap)
00446     m_octree->prune();
00447 
00448 
00449 }
00450 
00451 
00452 
00453 void OctomapServer::publishAll(const ros::Time& rostime){
00454   ros::WallTime startTime = ros::WallTime::now();
00455   size_t octomapSize = m_octree->size();
00456   // TODO: estimate num occ. voxels for size of arrays (reserve)
00457   if (octomapSize <= 1){
00458     ROS_WARN("Nothing to publish, octree is empty");
00459     return;
00460   }
00461 
00462   bool publishFreeMap = m_publishFreeSpace && (m_latchedTopics || m_fmapPub.getNumSubscribers() > 0);
00463   bool publishFreeMarkerArray = m_publishFreeSpace && (m_latchedTopics || m_fmarkerPub.getNumSubscribers() > 0);
00464   bool publishCollisionMap = (m_latchedTopics || m_cmapPub.getNumSubscribers() > 0);
00465   bool publishCollisionObject = (m_latchedTopics || m_collisionObjectPub.getNumSubscribers() > 0);
00466   bool publishMarkerArray = (m_latchedTopics || m_markerPub.getNumSubscribers() > 0);
00467   bool publishPointCloud = (m_latchedTopics || m_pointCloudPub.getNumSubscribers() > 0);
00468   bool publishBinaryMap = (m_latchedTopics || m_binaryMapPub.getNumSubscribers() > 0);
00469   bool publishFullMap = (m_latchedTopics || m_fullMapPub.getNumSubscribers() > 0);
00470   m_publish2DMap = (m_latchedTopics || m_mapPub.getNumSubscribers() > 0);
00471 
00472   // init collision object:
00473   arm_navigation_msgs::CollisionObject collisionObject;
00474   collisionObject.header.frame_id = m_worldFrameId;
00475   collisionObject.header.stamp = rostime;
00476   collisionObject.id = "map";
00477   arm_navigation_msgs::OrientedBoundingBox collObjBox;
00478   collObjBox.axis.x = collObjBox.axis.y = 0.0;
00479   collObjBox.axis.z = 1.0;
00480   collObjBox.angle = 0.0;
00481 
00482   // init free map and box:
00483   arm_navigation_msgs::CollisionMap freeMap;
00484   freeMap.header.frame_id = m_worldFrameId;
00485   freeMap.header.stamp = rostime;
00486   arm_navigation_msgs::OrientedBoundingBox freeObjBox;
00487   freeObjBox.axis.x = freeObjBox.axis.y = 0.0;
00488   freeObjBox.axis.z = 1.0;
00489   freeObjBox.angle = 0.0;
00490 
00491   // init markers for free space:
00492   visualization_msgs::MarkerArray freeNodesVis;
00493   // each array stores all cubes of a different size, one for each depth level:
00494   freeNodesVis.markers.resize(m_treeDepth+1);
00495 
00496 
00497 
00498   //init collision map:
00499   arm_navigation_msgs::CollisionMap collisionMap;
00500   collisionMap.header.frame_id = m_worldFrameId;
00501   collisionMap.header.stamp = rostime;
00502   arm_navigation_msgs::Shape collObjShape;
00503   collObjShape.type = arm_navigation_msgs::Shape::BOX;
00504   collObjShape.dimensions.resize(3);
00505 
00506   geometry_msgs::Pose pose;
00507   pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
00508 
00509   // init markers:
00510   visualization_msgs::MarkerArray occupiedNodesVis;
00511   // each array stores all cubes of a different size, one for each depth level:
00512   occupiedNodesVis.markers.resize(m_treeDepth+1);
00513 
00514   // init pointcloud:
00515   pcl::PointCloud<pcl::PointXYZ> pclCloud;
00516 
00517   // call pre-traversal hook:
00518   handlePreNodeTraversal(rostime);
00519 
00520   // now, traverse all leafs in the tree:
00521   for (OcTree::iterator it = m_octree->begin(m_maxTreeDepth),
00522       end = m_octree->end(); it != end; ++it)
00523   {
00524     bool inUpdateBBX = isInUpdateBBX(it);
00525 
00526     // call general hook:
00527     handleNode(it);
00528     if (inUpdateBBX)
00529       handleNodeInBBX(it);
00530 
00531     if (m_octree->isNodeOccupied(*it)){
00532       double z = it.getZ();
00533       if (z > m_occupancyMinZ && z < m_occupancyMaxZ)
00534       {
00535         double size = it.getSize();
00536         double x = it.getX();
00537         double y = it.getY();
00538 
00539         // Ignore speckles in the map:
00540         if (m_filterSpeckles && (it.getDepth() == m_treeDepth +1) && isSpeckleNode(it.getKey())){
00541           ROS_DEBUG("Ignoring single speckle at (%f,%f,%f)", x, y, z);
00542           continue;
00543         } // else: current octree node is no speckle, send it out
00544 
00545         handleOccupiedNode(it);
00546         if (inUpdateBBX)
00547           handleOccupiedNodeInBBX(it);
00548 
00549         // create collision object:
00550         if (publishCollisionObject){
00551           collObjShape.dimensions[0] = collObjShape.dimensions[1] = collObjShape.dimensions[2] = size;
00552           collisionObject.shapes.push_back(collObjShape);
00553           pose.position.x = x;
00554           pose.position.y = y;
00555           pose.position.z = z;
00556           collisionObject.poses.push_back(pose);
00557         }
00558 
00559         if (publishCollisionMap){
00560           collObjBox.extents.x = collObjBox.extents.y = collObjBox.extents.z = size;
00561 
00562           collObjBox.center.x = x;
00563           collObjBox.center.y = y;
00564           collObjBox.center.z = z;
00565           collisionMap.boxes.push_back(collObjBox);
00566 
00567         }
00568 
00569         //create marker:
00570         if (publishMarkerArray){
00571           unsigned idx = it.getDepth();
00572           assert(idx < occupiedNodesVis.markers.size());
00573 
00574           geometry_msgs::Point cubeCenter;
00575           cubeCenter.x = x;
00576           cubeCenter.y = y;
00577           cubeCenter.z = z;
00578 
00579           occupiedNodesVis.markers[idx].points.push_back(cubeCenter);
00580           if (m_useHeightMap){
00581             double minX, minY, minZ, maxX, maxY, maxZ;
00582             m_octree->getMetricMin(minX, minY, minZ);
00583             m_octree->getMetricMax(maxX, maxY, maxZ);
00584 
00585             double h = (1.0 - std::min(std::max((cubeCenter.z-minZ)/ (maxZ - minZ), 0.0), 1.0)) *m_colorFactor;
00586             occupiedNodesVis.markers[idx].colors.push_back(heightMapColor(h));
00587           }
00588         }
00589 
00590         // insert into pointcloud:
00591         if (publishPointCloud)
00592           pclCloud.push_back(pcl::PointXYZ(x, y, z));
00593 
00594       }
00595     } else{ // node not occupied => mark as free in 2D map if unknown so far
00596       double z = it.getZ();
00597       if (z > m_occupancyMinZ && z < m_occupancyMaxZ)
00598       {
00599         handleFreeNode(it);
00600         if (inUpdateBBX)
00601           handleFreeNodeInBBX(it);
00602 
00603         if (m_publishFreeSpace){
00604           double x = it.getX();
00605           double y = it.getY();
00606 
00607           if (publishFreeMap){
00608             freeObjBox.extents.x = freeObjBox.extents.y = freeObjBox.extents.z = it.getSize();
00609             freeObjBox.center.x = x;
00610             freeObjBox.center.y = y;
00611             freeObjBox.center.z = z;
00612             freeMap.boxes.push_back(freeObjBox);
00613           }
00614 
00615           //create marker for free space:
00616           if (publishFreeMarkerArray){
00617             unsigned idx = it.getDepth();
00618             assert(idx < freeNodesVis.markers.size());
00619 
00620             geometry_msgs::Point cubeCenter;
00621             cubeCenter.x = x;
00622             cubeCenter.y = y;
00623             cubeCenter.z = z;
00624 
00625             freeNodesVis.markers[idx].points.push_back(cubeCenter);
00626           }
00627         }
00628 
00629       }
00630     }
00631   }
00632 
00633   // call post-traversal hook:
00634   handlePostNodeTraversal(rostime);
00635 
00636   // finish MarkerArray:
00637   if (publishMarkerArray){
00638     for (unsigned i= 0; i < occupiedNodesVis.markers.size(); ++i){
00639       double size = m_octree->getNodeSize(i);
00640 
00641       occupiedNodesVis.markers[i].header.frame_id = m_worldFrameId;
00642       occupiedNodesVis.markers[i].header.stamp = rostime;
00643       occupiedNodesVis.markers[i].ns = "map";
00644       occupiedNodesVis.markers[i].id = i;
00645       occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
00646       occupiedNodesVis.markers[i].scale.x = size;
00647       occupiedNodesVis.markers[i].scale.y = size;
00648       occupiedNodesVis.markers[i].scale.z = size;
00649       occupiedNodesVis.markers[i].color = m_color;
00650 
00651 
00652       if (occupiedNodesVis.markers[i].points.size() > 0)
00653         occupiedNodesVis.markers[i].action = visualization_msgs::Marker::ADD;
00654       else
00655         occupiedNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
00656     }
00657 
00658     m_markerPub.publish(occupiedNodesVis);
00659   }
00660 
00661 
00662   // finish FreeMarkerArray:
00663   if (publishFreeMarkerArray){
00664     for (unsigned i= 0; i < freeNodesVis.markers.size(); ++i){
00665       double size = m_octree->getNodeSize(i);
00666 
00667       freeNodesVis.markers[i].header.frame_id = m_worldFrameId;
00668       freeNodesVis.markers[i].header.stamp = rostime;
00669       freeNodesVis.markers[i].ns = "map";
00670       freeNodesVis.markers[i].id = i;
00671       freeNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
00672       freeNodesVis.markers[i].scale.x = size;
00673       freeNodesVis.markers[i].scale.y = size;
00674       freeNodesVis.markers[i].scale.z = size;
00675       freeNodesVis.markers[i].color = m_colorFree;
00676 
00677 
00678       if (freeNodesVis.markers[i].points.size() > 0)
00679         freeNodesVis.markers[i].action = visualization_msgs::Marker::ADD;
00680       else
00681         freeNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
00682     }
00683 
00684     m_fmarkerPub.publish(freeNodesVis);
00685   }
00686 
00687 
00688   // finish pointcloud:
00689   if (publishPointCloud){
00690     sensor_msgs::PointCloud2 cloud;
00691     pcl::toROSMsg (pclCloud, cloud);
00692     cloud.header.frame_id = m_worldFrameId;
00693     cloud.header.stamp = rostime;
00694     m_pointCloudPub.publish(cloud);
00695   }
00696 
00697   if (publishCollisionObject)
00698     m_collisionObjectPub.publish(collisionObject);
00699 
00700   if (publishCollisionMap)
00701     m_cmapPub.publish(collisionMap);
00702 
00703   if (publishBinaryMap)
00704     publishBinaryOctoMap(rostime);
00705 
00706   if (publishFullMap)
00707     publishFullOctoMap(rostime);
00708 
00709   if (publishFreeMap)
00710     m_fmapPub.publish(freeMap);
00711 
00712 
00713 
00714   double total_elapsed = (ros::WallTime::now() - startTime).toSec();
00715   ROS_DEBUG("Map publishing in OctomapServer took %f sec", total_elapsed);
00716 
00717 }
00718 
00719 
00720 bool OctomapServer::octomapBinarySrv(OctomapSrv::Request  &req,
00721                                     OctomapSrv::Response &res)
00722 {
00723   ROS_INFO("Sending binary map data on service request");
00724   res.map.header.frame_id = m_worldFrameId;
00725   res.map.header.stamp = ros::Time::now();
00726   if (!octomap_msgs::binaryMapToMsgData(*m_octree, res.map.data))
00727     return false;
00728 
00729   return true;
00730 }
00731 
00732 bool OctomapServer::octomapFullSrv(OctomapSrv::Request  &req,
00733                                     OctomapSrv::Response &res)
00734 {
00735   ROS_INFO("Sending full map data on service request");
00736   res.map.header.frame_id = m_worldFrameId;
00737   res.map.header.stamp = ros::Time::now();
00738 
00739 
00740   if (!octomap_msgs::fullMapToMsgData(*m_octree, res.map.data))
00741     return false;
00742 
00743   return true;
00744 }
00745 
00746 bool OctomapServer::clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp){
00747   point3d min = pointMsgToOctomap(req.min);
00748   point3d max = pointMsgToOctomap(req.max);
00749 
00750   for(OcTree::leaf_bbx_iterator it = m_octree->begin_leafs_bbx(min,max),
00751       end=m_octree->end_leafs_bbx(); it!= end; ++it){
00752 
00753     it->setLogOdds(octomap::logodds(m_thresMin));
00754     //                  m_octree->updateNode(it.getKey(), -6.0f);
00755   }
00756   // TODO: eval which is faster (setLogOdds+updateInner or updateNode)
00757   m_octree->updateInnerOccupancy();
00758 
00759   publishAll(ros::Time::now());
00760 
00761   return true;
00762 }
00763 
00764 bool OctomapServer::resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp) {
00765   visualization_msgs::MarkerArray occupiedNodesVis;
00766   occupiedNodesVis.markers.resize(m_treeDepth +1);
00767   ros::Time rostime = ros::Time::now();
00768   m_octree->clear();
00769   // clear 2D map:
00770   m_gridmap.data.clear();
00771   m_gridmap.info.height = 0.0;
00772   m_gridmap.info.width = 0.0;
00773   m_gridmap.info.resolution = 0.0;
00774   m_gridmap.info.origin.position.x = 0.0;
00775   m_gridmap.info.origin.position.y = 0.0;
00776 
00777   ROS_INFO("Cleared octomap");
00778   publishAll(rostime);
00779 
00780   publishBinaryOctoMap(rostime);
00781   for (unsigned i= 0; i < occupiedNodesVis.markers.size(); ++i){
00782 
00783     occupiedNodesVis.markers[i].header.frame_id = m_worldFrameId;
00784     occupiedNodesVis.markers[i].header.stamp = rostime;
00785     occupiedNodesVis.markers[i].ns = "map";
00786     occupiedNodesVis.markers[i].id = i;
00787     occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
00788     occupiedNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
00789   }
00790 
00791 
00792   m_markerPub.publish(occupiedNodesVis);
00793 
00794 
00795   visualization_msgs::MarkerArray freeNodesVis;
00796   freeNodesVis.markers.resize(m_treeDepth +1);
00797 
00798   for (unsigned i= 0; i < freeNodesVis.markers.size(); ++i){
00799 
00800     freeNodesVis.markers[i].header.frame_id = m_worldFrameId;
00801     freeNodesVis.markers[i].header.stamp = rostime;
00802     freeNodesVis.markers[i].ns = "map";
00803     freeNodesVis.markers[i].id = i;
00804     freeNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
00805     freeNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
00806   }
00807   m_fmarkerPub.publish(freeNodesVis);
00808 
00809   return true;
00810 }
00811 
00812 void OctomapServer::publishBinaryOctoMap(const ros::Time& rostime) const{
00813 
00814   OctomapBinary map;
00815   map.header.frame_id = m_worldFrameId;
00816   map.header.stamp = rostime;
00817 
00818   if (octomap_msgs::binaryMapToMsgData(*m_octree, map.data))
00819     m_binaryMapPub.publish(map);
00820   else
00821     ROS_ERROR("Error serializing OctoMap");
00822 }
00823 
00824 void OctomapServer::publishFullOctoMap(const ros::Time& rostime) const{
00825 
00826   OctomapBinary map;
00827   map.header.frame_id = m_worldFrameId;
00828   map.header.stamp = rostime;
00829 
00830   if (octomap_msgs::fullMapToMsgData(*m_octree, map.data))
00831     m_fullMapPub.publish(map);
00832   else
00833     ROS_ERROR("Error serializing OctoMap");
00834 
00835 }
00836 
00837 
00838 void OctomapServer::filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& ground, PCLPointCloud& nonground) const{
00839   ground.header = pc.header;
00840   nonground.header = pc.header;
00841 
00842   if (pc.size() < 50){
00843     ROS_WARN("Pointcloud in OctomapServer too small, skipping ground plane extraction");
00844     nonground = pc;
00845   } else {
00846     // plane detection for ground plane removal:
00847     pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00848     pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00849 
00850     // Create the segmentation object and set up:
00851     pcl::SACSegmentation<pcl::PointXYZ> seg;
00852     seg.setOptimizeCoefficients (true);
00853     // TODO: maybe a filtering based on the surface normals might be more robust / accurate?
00854     seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
00855     seg.setMethodType(pcl::SAC_RANSAC);
00856     seg.setMaxIterations(200);
00857     seg.setDistanceThreshold (m_groundFilterDistance);
00858     seg.setAxis(Eigen::Vector3f(0,0,1));
00859     seg.setEpsAngle(m_groundFilterAngle);
00860 
00861 
00862     PCLPointCloud cloud_filtered(pc);
00863     // Create the filtering object
00864     pcl::ExtractIndices<pcl::PointXYZ> extract;
00865     bool groundPlaneFound = false;
00866 
00867     while(cloud_filtered.size() > 10 && !groundPlaneFound){
00868       seg.setInputCloud(cloud_filtered.makeShared());
00869       seg.segment (*inliers, *coefficients);
00870       if (inliers->indices.size () == 0){
00871         ROS_INFO("PCL segmentation did not find any plane.");
00872 
00873         break;
00874       }
00875 
00876       extract.setInputCloud(cloud_filtered.makeShared());
00877       extract.setIndices(inliers);
00878 
00879       if (std::abs(coefficients->values.at(3)) < m_groundFilterPlaneDistance){
00880         ROS_DEBUG("Ground plane found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(),
00881                   coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3));
00882         extract.setNegative (false);
00883         extract.filter (ground);
00884 
00885         // remove ground points from full pointcloud:
00886         // workaround for PCL bug:
00887         if(inliers->indices.size() != cloud_filtered.size()){
00888           extract.setNegative(true);
00889           PCLPointCloud cloud_out;
00890           extract.filter(cloud_out);
00891           nonground += cloud_out;
00892           cloud_filtered = cloud_out;
00893         }
00894 
00895         groundPlaneFound = true;
00896       } else{
00897         ROS_DEBUG("Horizontal plane (not ground) found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(),
00898                   coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3));
00899         pcl::PointCloud<pcl::PointXYZ> cloud_out;
00900         extract.setNegative (false);
00901         extract.filter(cloud_out);
00902         nonground +=cloud_out;
00903         // debug
00904         //            pcl::PCDWriter writer;
00905         //            writer.write<pcl::PointXYZ>("nonground_plane.pcd",cloud_out, false);
00906 
00907         // remove current plane from scan for next iteration:
00908         // workaround for PCL bug:
00909         if(inliers->indices.size() != cloud_filtered.size()){
00910           extract.setNegative(true);
00911           cloud_out.points.clear();
00912           extract.filter(cloud_out);
00913           cloud_filtered = cloud_out;
00914         } else{
00915           cloud_filtered.points.clear();
00916         }
00917       }
00918 
00919     }
00920     // TODO: also do this if overall starting pointcloud too small?
00921     if (!groundPlaneFound){ // no plane found or remaining points too small
00922       ROS_WARN("No ground plane found in scan");
00923 
00924       // do a rough fitlering on height to prevent spurious obstacles
00925       pcl::PassThrough<pcl::PointXYZ> second_pass;
00926       second_pass.setFilterFieldName("z");
00927       second_pass.setFilterLimits(-m_groundFilterPlaneDistance, m_groundFilterPlaneDistance);
00928       second_pass.setInputCloud(pc.makeShared());
00929       second_pass.filter(ground);
00930 
00931       second_pass.setFilterLimitsNegative (true);
00932       second_pass.filter(nonground);
00933     }
00934 
00935     // debug:
00936     //        pcl::PCDWriter writer;
00937     //        if (pc_ground.size() > 0)
00938     //          writer.write<pcl::PointXYZ>("ground.pcd",pc_ground, false);
00939     //        if (pc_nonground.size() > 0)
00940     //          writer.write<pcl::PointXYZ>("nonground.pcd",pc_nonground, false);
00941 
00942   }
00943 
00944 
00945 }
00946 
00947 void OctomapServer::handlePreNodeTraversal(const ros::Time& rostime){
00948   if (m_publish2DMap){
00949     // init projected 2D map:
00950     m_gridmap.header.frame_id = m_worldFrameId;
00951     m_gridmap.header.stamp = rostime;
00952     nav_msgs::MapMetaData oldMapInfo = m_gridmap.info;
00953 
00954     // TODO: move most of this stuff into c'tor and init map only once (adjust if size changes)
00955     double minX, minY, minZ, maxX, maxY, maxZ;
00956     m_octree->getMetricMin(minX, minY, minZ);
00957     m_octree->getMetricMax(maxX, maxY, maxZ);
00958 
00959     octomap::point3d minPt(minX, minY, minZ);
00960     octomap::point3d maxPt(maxX, maxY, maxZ);
00961     octomap::OcTreeKey minKey, maxKey, curKey;
00962     if (!m_octree->genKey(minPt, minKey)){
00963       ROS_ERROR("Could not create min OcTree key at %f %f %f", minPt.x(), minPt.y(), minPt.z());
00964       return;
00965     }
00966 
00967     if (!m_octree->genKey(maxPt, maxKey)){
00968       ROS_ERROR("Could not create max OcTree key at %f %f %f", maxPt.x(), maxPt.y(), maxPt.z());
00969       return;
00970     }
00971     m_octree->genKeyAtDepth(minKey, m_maxTreeDepth, minKey);
00972     m_octree->genKeyAtDepth(maxKey, m_maxTreeDepth, maxKey);
00973 
00974     ROS_DEBUG("MinKey: %d %d %d / MaxKey: %d %d %d", minKey[0], minKey[1], minKey[2], maxKey[0], maxKey[1], maxKey[2]);
00975 
00976     // add padding if requested (= new min/maxPts in x&y):
00977     double halfPaddedX = 0.5*m_minSizeX;
00978     double halfPaddedY = 0.5*m_minSizeY;
00979     minX = std::min(minX, -halfPaddedX);
00980     maxX = std::max(maxX, halfPaddedX);
00981     minY = std::min(minY, -halfPaddedY);
00982     maxY = std::max(maxY, halfPaddedY);
00983     minPt = octomap::point3d(minX, minY, minZ);
00984     maxPt = octomap::point3d(maxX, maxY, maxZ);
00985 
00986     OcTreeKey paddedMaxKey;
00987     if (!m_octree->genKey(minPt, m_paddedMinKey)){
00988       ROS_ERROR("Could not create padded min OcTree key at %f %f %f", minPt.x(), minPt.y(), minPt.z());
00989       return;
00990     }
00991     if (!m_octree->genKey(maxPt, paddedMaxKey)){
00992       ROS_ERROR("Could not create padded max OcTree key at %f %f %f", maxPt.x(), maxPt.y(), maxPt.z());
00993       return;
00994     }
00995     m_octree->genKeyAtDepth(m_paddedMinKey, m_maxTreeDepth, m_paddedMinKey);
00996     m_octree->genKeyAtDepth(paddedMaxKey, m_maxTreeDepth, paddedMaxKey);
00997 
00998     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]);
00999     assert(paddedMaxKey[0] >= maxKey[0] && paddedMaxKey[1] >= maxKey[1]);
01000 
01001     m_multires2DScale = 1 << (m_treeDepth - m_maxTreeDepth);
01002     m_gridmap.info.width = (paddedMaxKey[0] - m_paddedMinKey[0])/m_multires2DScale +1;
01003     m_gridmap.info.height = (paddedMaxKey[1] - m_paddedMinKey[1])/m_multires2DScale +1;
01004 
01005     int mapOriginX = minKey[0] - m_paddedMinKey[0];
01006     int mapOriginY = minKey[1] - m_paddedMinKey[1];
01007     assert(mapOriginX >= 0 && mapOriginY >= 0);
01008 
01009     // might not exactly be min / max of octree:
01010     octomap::point3d origin;
01011     m_octree->genCoords(m_paddedMinKey, m_treeDepth, origin);
01012     double gridRes = m_octree->getNodeSize(m_maxTreeDepth);
01013     m_projectCompleteMap = (!m_incrementalUpdate || (std::abs(gridRes-m_gridmap.info.resolution) > 1e-6));
01014     m_gridmap.info.resolution = gridRes;
01015     m_gridmap.info.origin.position.x = origin.x() - gridRes*0.5;
01016     m_gridmap.info.origin.position.y = origin.y() - gridRes*0.5;
01017     if (m_maxTreeDepth != m_treeDepth){
01018       m_gridmap.info.origin.position.x -= m_res/2.0;
01019       m_gridmap.info.origin.position.y -= m_res/2.0;
01020     }
01021     
01022     // workaround for  multires. projection not working properly for inner nodes:
01023     // force re-building complete map
01024     if (m_maxTreeDepth < m_treeDepth)
01025       m_projectCompleteMap = true;
01026 
01027 
01028     if(m_projectCompleteMap){
01029       ROS_DEBUG("Rebuilding complete 2D map");
01030       m_gridmap.data.clear();
01031       // init to unknown:
01032       m_gridmap.data.resize(m_gridmap.info.width * m_gridmap.info.height, -1);
01033 
01034     } else {
01035 
01036        if (mapChanged(oldMapInfo, m_gridmap.info)){
01037           ROS_DEBUG("2D grid map size changed to %dx%d", m_gridmap.info.width, m_gridmap.info.height);
01038           adjustMapData(m_gridmap, oldMapInfo);
01039        }
01040        nav_msgs::OccupancyGrid::_data_type::iterator startIt;
01041        size_t mapUpdateBBXMinX = std::max(0, (int(m_updateBBXMin[0]) - int(m_paddedMinKey[0]))/int(m_multires2DScale));
01042        size_t mapUpdateBBXMinY = std::max(0, (int(m_updateBBXMin[1]) - int(m_paddedMinKey[1]))/int(m_multires2DScale));
01043        size_t mapUpdateBBXMaxX = std::min(int(m_gridmap.info.width-1), (int(m_updateBBXMax[0]) - int(m_paddedMinKey[0]))/int(m_multires2DScale));
01044        size_t mapUpdateBBXMaxY = std::min(int(m_gridmap.info.height-1), (int(m_updateBBXMax[1]) - int(m_paddedMinKey[1]))/int(m_multires2DScale));
01045 
01046        assert(mapUpdateBBXMaxX > mapUpdateBBXMinX);
01047        assert(mapUpdateBBXMaxY > mapUpdateBBXMinY);
01048 
01049        size_t numCols = mapUpdateBBXMaxX-mapUpdateBBXMinX +1;
01050 
01051        // test for max idx:
01052        uint max_idx = m_gridmap.info.width*mapUpdateBBXMaxY + mapUpdateBBXMaxX;
01053        if (max_idx  >= m_gridmap.data.size())
01054          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);
01055 
01056        // reset proj. 2D map in bounding box:
01057        for (unsigned int j = mapUpdateBBXMinY; j <= mapUpdateBBXMaxY; ++j){
01058           std::fill_n(m_gridmap.data.begin() + m_gridmap.info.width*j+mapUpdateBBXMinX,
01059                       numCols, -1);
01060        }
01061 
01062     }
01063        
01064 
01065 
01066   }
01067 
01068 }
01069 
01070 void OctomapServer::handlePostNodeTraversal(const ros::Time& rostime){
01071 
01072   if (m_publish2DMap)
01073     m_mapPub.publish(m_gridmap);
01074 }
01075 
01076 void OctomapServer::handleOccupiedNode(const OcTreeT::iterator& it){
01077 
01078   if (m_publish2DMap && m_projectCompleteMap){
01079     update2DMap(it, true);
01080   }
01081 }
01082 
01083 void OctomapServer::handleFreeNode(const OcTreeT::iterator& it){
01084 
01085   if (m_publish2DMap && m_projectCompleteMap){
01086     update2DMap(it, false);
01087   }
01088 }
01089 
01090 void OctomapServer::handleOccupiedNodeInBBX(const OcTreeT::iterator& it){
01091 
01092   if (m_publish2DMap && !m_projectCompleteMap){
01093     update2DMap(it, true);
01094   }
01095 }
01096 
01097 void OctomapServer::handleFreeNodeInBBX(const OcTreeT::iterator& it){
01098 
01099   if (m_publish2DMap && !m_projectCompleteMap){
01100     update2DMap(it, false);
01101   }
01102 }
01103 
01104 void OctomapServer::update2DMap(const OcTreeT::iterator& it, bool occupied){
01105 
01106   // update 2D map (occupied always overrides):
01107 
01108   if (it.getDepth() == m_maxTreeDepth){
01109     unsigned idx = mapIdx(it.getKey());
01110     if (occupied)
01111       m_gridmap.data[mapIdx(it.getKey())] = 100;
01112     else if (m_gridmap.data[idx] == -1){
01113       m_gridmap.data[idx] = 0;
01114     }
01115 
01116   } else{
01117     int intSize = 1 << (m_maxTreeDepth - it.getDepth());
01118     octomap::OcTreeKey minKey=it.getIndexKey();
01119     for(int dx=0; dx < intSize; dx++){
01120       int i = (minKey[0]+dx - m_paddedMinKey[0])/m_multires2DScale;
01121       for(int dy=0; dy < intSize; dy++){
01122         unsigned idx = mapIdx(i, (minKey[1]+dy - m_paddedMinKey[1])/m_multires2DScale);
01123         if (occupied)
01124           m_gridmap.data[idx] = 100;
01125         else if (m_gridmap.data[idx] == -1){
01126           m_gridmap.data[idx] = 0;
01127         }
01128       }
01129     }
01130   }
01131 
01132 
01133 }
01134 
01135 
01136 
01137 bool OctomapServer::isSpeckleNode(const OcTreeKey&nKey) const {
01138   OcTreeKey key;
01139   bool neighborFound = false;
01140   for (key[2] = nKey[2] - 1; !neighborFound && key[2] <= nKey[2] + 1; ++key[2]){
01141     for (key[1] = nKey[1] - 1; !neighborFound && key[1] <= nKey[1] + 1; ++key[1]){
01142       for (key[0] = nKey[0] - 1; !neighborFound && key[0] <= nKey[0] + 1; ++key[0]){
01143         if (key != nKey){
01144           OcTreeNode* node = m_octree->search(key);
01145           if (node && m_octree->isNodeOccupied(node)){
01146             // we have a neighbor => break!
01147             neighborFound = true;
01148           }
01149         }
01150       }
01151     }
01152   }
01153 
01154   return neighborFound;
01155 }
01156 
01157 void OctomapServer::reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level){
01158   if (m_maxTreeDepth != unsigned(config.max_depth)){
01159     m_maxTreeDepth = unsigned(config.max_depth);
01160 
01161     publishAll();
01162   }
01163 
01164 
01165 }
01166 
01167 void OctomapServer::adjustMapData(nav_msgs::OccupancyGrid& map, const nav_msgs::MapMetaData& oldMapInfo) const{
01168   if (map.info.resolution != oldMapInfo.resolution){
01169     ROS_ERROR("Resolution of map changed, cannot be adjusted");
01170     return;
01171   }
01172 
01173   int i_off = int((oldMapInfo.origin.position.x - map.info.origin.position.x)/map.info.resolution +0.5);
01174   int j_off = int((oldMapInfo.origin.position.y - map.info.origin.position.y)/map.info.resolution +0.5);
01175 
01176   if (i_off < 0 || j_off < 0
01177       || oldMapInfo.width  + i_off > map.info.width
01178       || oldMapInfo.height + j_off > map.info.height)
01179   {
01180     ROS_ERROR("New 2D map does not contain old map area, this case is not implemented");
01181     return;
01182   }
01183 
01184   nav_msgs::OccupancyGrid::_data_type oldMapData = map.data;
01185 
01186   map.data.clear();
01187   // init to unknown:
01188   map.data.resize(map.info.width * map.info.height, -1);
01189 
01190   nav_msgs::OccupancyGrid::_data_type::iterator fromStart, fromEnd, toStart;
01191 
01192   for (int j =0; j < int(oldMapInfo.height); ++j ){
01193     // copy chunks, row by row:
01194     fromStart = oldMapData.begin() + j*oldMapInfo.width;
01195     fromEnd = fromStart + oldMapInfo.width;
01196     toStart = map.data.begin() + ((j+j_off)*m_gridmap.info.width + i_off);
01197     copy(fromStart, fromEnd, toStart);
01198 
01199 //    for (int i =0; i < int(oldMapInfo.width); ++i){
01200 //      map.data[m_gridmap.info.width*(j+j_off) +i+i_off] = oldMapData[oldMapInfo.width*j +i];
01201 //    }
01202 
01203   }
01204 
01205 }
01206 
01207 
01208 std_msgs::ColorRGBA OctomapServer::heightMapColor(double h) {
01209 
01210   std_msgs::ColorRGBA color;
01211   color.a = 1.0;
01212   // blend over HSV-values (more colors)
01213 
01214   double s = 1.0;
01215   double v = 1.0;
01216 
01217   h -= floor(h);
01218   h *= 6;
01219   int i;
01220   double m, n, f;
01221 
01222   i = floor(h);
01223   f = h - i;
01224   if (!(i & 1))
01225     f = 1 - f; // if i is even
01226   m = v * (1 - s);
01227   n = v * (1 - s * f);
01228 
01229   switch (i) {
01230     case 6:
01231     case 0:
01232       color.r = v; color.g = n; color.b = m;
01233       break;
01234     case 1:
01235       color.r = n; color.g = v; color.b = m;
01236       break;
01237     case 2:
01238       color.r = m; color.g = v; color.b = n;
01239       break;
01240     case 3:
01241       color.r = m; color.g = n; color.b = v;
01242       break;
01243     case 4:
01244       color.r = n; color.g = m; color.b = v;
01245       break;
01246     case 5:
01247       color.r = v; color.g = m; color.b = n;
01248       break;
01249     default:
01250       color.r = 1; color.g = 0.5; color.b = 0.5;
01251       break;
01252   }
01253 
01254   return color;
01255 }
01256 }
01257 
01258 
01259 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


octomap_server
Author(s): Armin Hornung
autogenerated on Tue Jul 9 2013 10:21:26