00001
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
00087 private_nh.param("ground_filter/distance", m_groundFilterDistance, m_groundFilterDistance);
00088
00089 private_nh.param("ground_filter/angle", m_groundFilterAngle, m_groundFilterAngle);
00090
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
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
00251
00252 PCLPointCloud pc;
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
00268 pcl::PassThrough<pcl::PointXYZ> pass;
00269 pass.setFilterFieldName("z");
00270 pass.setFilterLimits(m_pointcloudMinZ, m_pointcloudMaxZ);
00271
00272 PCLPointCloud pc_ground;
00273 PCLPointCloud pc_nonground;
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
00294 pcl::transformPointCloud(pc, pc, sensorToBase);
00295 pass.setInputCloud(pc.makeShared());
00296 pass.filter(pc);
00297 filterGroundPlane(pc, pc_ground, pc_nonground);
00298
00299
00300 pcl::transformPointCloud(pc_ground, pc_ground, baseToWorld);
00301 pcl::transformPointCloud(pc_nonground, pc_nonground, baseToWorld);
00302 } else {
00303
00304 pcl::transformPointCloud(pc, pc, sensorToWorld);
00305
00306
00307 pass.setInputCloud(pc.makeShared());
00308 pass.filter(pc);
00309
00310 pc_nonground = pc;
00311
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
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
00350 KeySet free_cells, occupied_cells;
00351
00352 for (PCLPointCloud::const_iterator it = ground.begin(); it != ground.end(); ++it){
00353 point3d point(it->x, it->y, it->z);
00354
00355 if ((m_maxRange > 0.0) && ((point - sensorOrigin).norm() > m_maxRange) ) {
00356 point = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRange;
00357 }
00358
00359
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
00374 for (PCLPointCloud::const_iterator it = nonground.begin(); it != nonground.end(); ++it){
00375 point3d point(it->x, it->y, it->z);
00376
00377 if ((m_maxRange < 0.0) || ((point - sensorOrigin).norm() <= m_maxRange) ) {
00378
00379
00380 if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){
00381 free_cells.insert(m_keyRay.begin(), m_keyRay.end());
00382 }
00383
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 {
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
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
00417 for (KeySet::iterator it = occupied_cells.begin(), end=free_cells.end(); it!= end; it++) {
00418 m_octree->updateNode(*it, true);
00419 }
00420
00421
00422
00423
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
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
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
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
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
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
00492 visualization_msgs::MarkerArray freeNodesVis;
00493
00494 freeNodesVis.markers.resize(m_treeDepth+1);
00495
00496
00497
00498
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
00510 visualization_msgs::MarkerArray occupiedNodesVis;
00511
00512 occupiedNodesVis.markers.resize(m_treeDepth+1);
00513
00514
00515 pcl::PointCloud<pcl::PointXYZ> pclCloud;
00516
00517
00518 handlePreNodeTraversal(rostime);
00519
00520
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
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
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 }
00544
00545 handleOccupiedNode(it);
00546 if (inUpdateBBX)
00547 handleOccupiedNodeInBBX(it);
00548
00549
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
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
00591 if (publishPointCloud)
00592 pclCloud.push_back(pcl::PointXYZ(x, y, z));
00593
00594 }
00595 } else{
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
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
00634 handlePostNodeTraversal(rostime);
00635
00636
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
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
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
00755 }
00756
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
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
00847 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00848 pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00849
00850
00851 pcl::SACSegmentation<pcl::PointXYZ> seg;
00852 seg.setOptimizeCoefficients (true);
00853
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
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
00886
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
00904
00905
00906
00907
00908
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
00921 if (!groundPlaneFound){
00922 ROS_WARN("No ground plane found in scan");
00923
00924
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
00936
00937
00938
00939
00940
00941
00942 }
00943
00944
00945 }
00946
00947 void OctomapServer::handlePreNodeTraversal(const ros::Time& rostime){
00948 if (m_publish2DMap){
00949
00950 m_gridmap.header.frame_id = m_worldFrameId;
00951 m_gridmap.header.stamp = rostime;
00952 nav_msgs::MapMetaData oldMapInfo = m_gridmap.info;
00953
00954
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
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
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
01023
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
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
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
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
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
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
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
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
01200
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
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;
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