OctomapServer.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2010-2013, A. Hornung, University of Freiburg
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the University of Freiburg nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
31 
32 using namespace octomap;
33 using octomap_msgs::Octomap;
34 
35 bool is_equal (double a, double b, double epsilon = 1.0e-7)
36 {
37  return std::abs(a - b) < epsilon;
38 }
39 
40 namespace octomap_server{
41 
42 OctomapServer::OctomapServer(const ros::NodeHandle private_nh_, const ros::NodeHandle &nh_)
43 : m_nh(nh_),
44  m_nh_private(private_nh_),
45  m_pointCloudSub(NULL),
46  m_tfPointCloudSub(NULL),
47  m_reconfigureServer(m_config_mutex, private_nh_),
48  m_octree(NULL),
49  m_maxRange(-1.0),
50  m_minRange(-1.0),
51  m_worldFrameId("/map"), m_baseFrameId("base_footprint"),
52  m_useHeightMap(true),
53  m_useColoredMap(false),
54  m_colorFactor(0.8),
55  m_latchedTopics(true),
56  m_publishFreeSpace(false),
57  m_res(0.05),
58  m_treeDepth(0),
59  m_maxTreeDepth(0),
60  m_pointcloudMinX(-std::numeric_limits<double>::max()),
61  m_pointcloudMaxX(std::numeric_limits<double>::max()),
62  m_pointcloudMinY(-std::numeric_limits<double>::max()),
63  m_pointcloudMaxY(std::numeric_limits<double>::max()),
64  m_pointcloudMinZ(-std::numeric_limits<double>::max()),
65  m_pointcloudMaxZ(std::numeric_limits<double>::max()),
66  m_occupancyMinZ(-std::numeric_limits<double>::max()),
67  m_occupancyMaxZ(std::numeric_limits<double>::max()),
68  m_minSizeX(0.0), m_minSizeY(0.0),
69  m_filterSpeckles(false), m_filterGroundPlane(false),
70  m_groundFilterDistance(0.04), m_groundFilterAngle(0.15), m_groundFilterPlaneDistance(0.07),
71  m_compressMap(true),
72  m_incrementalUpdate(false),
73  m_initConfig(true)
74 {
75  double probHit, probMiss, thresMin, thresMax;
76 
78  m_nh_private.param("base_frame_id", m_baseFrameId, m_baseFrameId);
82 
91  m_nh_private.param("min_x_size", m_minSizeX,m_minSizeX);
92  m_nh_private.param("min_y_size", m_minSizeY,m_minSizeY);
93 
96  // distance of points from plane for RANSAC
98  // angular derivation of found plane:
100  // distance of found plane from z=0 to be detected as ground (e.g. to exclude tables)
102 
103  m_nh_private.param("sensor_model/max_range", m_maxRange, m_maxRange);
104  m_nh_private.param("sensor_model/min_range", m_minRange, m_minRange);
105 
106  m_nh_private.param("resolution", m_res, m_res);
107  m_nh_private.param("sensor_model/hit", probHit, 0.7);
108  m_nh_private.param("sensor_model/miss", probMiss, 0.4);
109  m_nh_private.param("sensor_model/min", thresMin, 0.12);
110  m_nh_private.param("sensor_model/max", thresMax, 0.97);
111  m_nh_private.param("compress_map", m_compressMap, m_compressMap);
112  m_nh_private.param("incremental_2D_projection", m_incrementalUpdate, m_incrementalUpdate);
113 
114  if (m_filterGroundPlane && (m_pointcloudMinZ > 0.0 || m_pointcloudMaxZ < 0.0)){
115  ROS_WARN_STREAM("You enabled ground filtering but incoming pointclouds will be pre-filtered in ["
116  <<m_pointcloudMinZ <<", "<< m_pointcloudMaxZ << "], excluding the ground level z=0. "
117  << "This will not work.");
118  }
119 
121  ROS_WARN_STREAM("You enabled both height map and RGB color registration. This is contradictory. Defaulting to height map.");
122  m_useColoredMap = false;
123  }
124 
125  if (m_useColoredMap) {
126 #ifdef COLOR_OCTOMAP_SERVER
127  ROS_INFO_STREAM("Using RGB color registration (if information available)");
128 #else
129  ROS_ERROR_STREAM("Colored map requested in launch file - node not running/compiled to support colors, please define COLOR_OCTOMAP_SERVER and recompile or launch the octomap_color_server node");
130 #endif
131  }
132 
133  // initialize octomap object & params
134  m_octree = new OcTreeT(m_res);
135  m_octree->setProbHit(probHit);
136  m_octree->setProbMiss(probMiss);
137  m_octree->setClampingThresMin(thresMin);
138  m_octree->setClampingThresMax(thresMax);
141  m_gridmap.info.resolution = m_res;
142 
143  double r, g, b, a;
144  m_nh_private.param("color/r", r, 0.0);
145  m_nh_private.param("color/g", g, 0.0);
146  m_nh_private.param("color/b", b, 1.0);
147  m_nh_private.param("color/a", a, 1.0);
148  m_color.r = r;
149  m_color.g = g;
150  m_color.b = b;
151  m_color.a = a;
152 
153  m_nh_private.param("color_free/r", r, 0.0);
154  m_nh_private.param("color_free/g", g, 1.0);
155  m_nh_private.param("color_free/b", b, 0.0);
156  m_nh_private.param("color_free/a", a, 1.0);
157  m_colorFree.r = r;
158  m_colorFree.g = g;
159  m_colorFree.b = b;
160  m_colorFree.a = a;
161 
163 
165  if (m_latchedTopics){
166  ROS_INFO("Publishing latched (single publish will take longer, all topics are prepared)");
167  } else
168  ROS_INFO("Publishing non-latched (topics are only prepared as needed, will only be re-published on map change");
169 
170  m_markerPub = m_nh.advertise<visualization_msgs::MarkerArray>("occupied_cells_vis_array", 1, m_latchedTopics);
171  m_binaryMapPub = m_nh.advertise<Octomap>("octomap_binary", 1, m_latchedTopics);
172  m_fullMapPub = m_nh.advertise<Octomap>("octomap_full", 1, m_latchedTopics);
173  m_pointCloudPub = m_nh.advertise<sensor_msgs::PointCloud2>("octomap_point_cloud_centers", 1, m_latchedTopics);
174  m_mapPub = m_nh.advertise<nav_msgs::OccupancyGrid>("projected_map", 5, m_latchedTopics);
175  m_fmarkerPub = m_nh.advertise<visualization_msgs::MarkerArray>("free_cells_vis_array", 1, m_latchedTopics);
176 
179  m_tfPointCloudSub->registerCallback(boost::bind(&OctomapServer::insertCloudCallback, this, boost::placeholders::_1));
180 
185 
186  dynamic_reconfigure::Server<OctomapServerConfig>::CallbackType f;
187  f = boost::bind(&OctomapServer::reconfigureCallback, this, boost::placeholders::_1, boost::placeholders::_2);
188  m_reconfigureServer.setCallback(f);
189 }
190 
192  if (m_tfPointCloudSub){
193  delete m_tfPointCloudSub;
194  m_tfPointCloudSub = NULL;
195  }
196 
197  if (m_pointCloudSub){
198  delete m_pointCloudSub;
199  m_pointCloudSub = NULL;
200  }
201 
202 
203  if (m_octree){
204  delete m_octree;
205  m_octree = NULL;
206  }
207 
208 }
209 
210 bool OctomapServer::openFile(const std::string& filename){
211  if (filename.length() <= 3)
212  return false;
213 
214  std::string suffix = filename.substr(filename.length()-3, 3);
215  if (suffix== ".bt"){
216  if (!m_octree->readBinary(filename)){
217  return false;
218  }
219  } else if (suffix == ".ot"){
220  AbstractOcTree* tree = AbstractOcTree::read(filename);
221  if (!tree){
222  return false;
223  }
224  if (m_octree){
225  delete m_octree;
226  m_octree = NULL;
227  }
228  m_octree = dynamic_cast<OcTreeT*>(tree);
229  if (!m_octree){
230  ROS_ERROR("Could not read OcTree in file, currently there are no other types supported in .ot");
231  return false;
232  }
233 
234  } else{
235  return false;
236  }
237 
238  ROS_INFO("Octomap file %s loaded (%zu nodes).", filename.c_str(),m_octree->size());
239 
243  m_gridmap.info.resolution = m_res;
244  double minX, minY, minZ;
245  double maxX, maxY, maxZ;
246  m_octree->getMetricMin(minX, minY, minZ);
247  m_octree->getMetricMax(maxX, maxY, maxZ);
248 
249  m_updateBBXMin[0] = m_octree->coordToKey(minX);
250  m_updateBBXMin[1] = m_octree->coordToKey(minY);
251  m_updateBBXMin[2] = m_octree->coordToKey(minZ);
252 
253  m_updateBBXMax[0] = m_octree->coordToKey(maxX);
254  m_updateBBXMax[1] = m_octree->coordToKey(maxY);
255  m_updateBBXMax[2] = m_octree->coordToKey(maxZ);
256 
257  publishAll();
258 
259  return true;
260 
261 }
262 
263 void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud){
264  ros::WallTime startTime = ros::WallTime::now();
265 
266 
267  //
268  // ground filtering in base frame
269  //
270  PCLPointCloud pc; // input cloud for filtering and ground-detection
271  pcl::fromROSMsg(*cloud, pc);
272 
273  tf::StampedTransform sensorToWorldTf;
274  try {
275  m_tfListener.lookupTransform(m_worldFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf);
276  } catch(tf::TransformException& ex){
277  ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback");
278  return;
279  }
280 
281  Eigen::Matrix4f sensorToWorld;
282  pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld);
283 
284 
285  // set up filter for height range, also removes NANs:
286  pcl::PassThrough<PCLPoint> pass_x;
287  pass_x.setFilterFieldName("x");
288  pass_x.setFilterLimits(m_pointcloudMinX, m_pointcloudMaxX);
289  pcl::PassThrough<PCLPoint> pass_y;
290  pass_y.setFilterFieldName("y");
291  pass_y.setFilterLimits(m_pointcloudMinY, m_pointcloudMaxY);
292  pcl::PassThrough<PCLPoint> pass_z;
293  pass_z.setFilterFieldName("z");
294  pass_z.setFilterLimits(m_pointcloudMinZ, m_pointcloudMaxZ);
295 
296  PCLPointCloud pc_ground; // segmented ground plane
297  PCLPointCloud pc_nonground; // everything else
298 
299  if (m_filterGroundPlane){
300  tf::StampedTransform sensorToBaseTf, baseToWorldTf;
301  try{
302  m_tfListener.waitForTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, ros::Duration(0.2));
303  m_tfListener.lookupTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToBaseTf);
304  m_tfListener.lookupTransform(m_worldFrameId, m_baseFrameId, cloud->header.stamp, baseToWorldTf);
305 
306 
307  }catch(tf::TransformException& ex){
308  ROS_ERROR_STREAM( "Transform error for ground plane filter: " << ex.what() << ", quitting callback.\n"
309  "You need to set the base_frame_id or disable filter_ground.");
310  }
311 
312 
313  Eigen::Matrix4f sensorToBase, baseToWorld;
314  pcl_ros::transformAsMatrix(sensorToBaseTf, sensorToBase);
315  pcl_ros::transformAsMatrix(baseToWorldTf, baseToWorld);
316 
317  // transform pointcloud from sensor frame to fixed robot frame
318  pcl::transformPointCloud(pc, pc, sensorToBase);
319  pass_x.setInputCloud(pc.makeShared());
320  pass_x.filter(pc);
321  pass_y.setInputCloud(pc.makeShared());
322  pass_y.filter(pc);
323  pass_z.setInputCloud(pc.makeShared());
324  pass_z.filter(pc);
325  filterGroundPlane(pc, pc_ground, pc_nonground);
326 
327  // transform clouds to world frame for insertion
328  pcl::transformPointCloud(pc_ground, pc_ground, baseToWorld);
329  pcl::transformPointCloud(pc_nonground, pc_nonground, baseToWorld);
330  } else {
331  // directly transform to map frame:
332  pcl::transformPointCloud(pc, pc, sensorToWorld);
333 
334  // just filter height range:
335  pass_x.setInputCloud(pc.makeShared());
336  pass_x.filter(pc);
337  pass_y.setInputCloud(pc.makeShared());
338  pass_y.filter(pc);
339  pass_z.setInputCloud(pc.makeShared());
340  pass_z.filter(pc);
341 
342  pc_nonground = pc;
343  // pc_nonground is empty without ground segmentation
344  pc_ground.header = pc.header;
345  pc_nonground.header = pc.header;
346  }
347 
348 
349  insertScan(sensorToWorldTf.getOrigin(), pc_ground, pc_nonground);
350 
351  double total_elapsed = (ros::WallTime::now() - startTime).toSec();
352  ROS_DEBUG("Pointcloud insertion in OctomapServer done (%zu+%zu pts (ground/nonground), %f sec)", pc_ground.size(), pc_nonground.size(), total_elapsed);
353 
354  publishAll(cloud->header.stamp);
355 }
356 
357 void OctomapServer::insertScan(const tf::Point& sensorOriginTf, const PCLPointCloud& ground, const PCLPointCloud& nonground){
358  point3d sensorOrigin = pointTfToOctomap(sensorOriginTf);
359 
360  if (!m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMin)
361  || !m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMax))
362  {
363  ROS_ERROR_STREAM("Could not generate Key for origin "<<sensorOrigin);
364  }
365 
366 #ifdef COLOR_OCTOMAP_SERVER
367  unsigned char* colors = new unsigned char[3];
368 #endif
369 
370  // instead of direct scan insertion, compute update to filter ground:
371  KeySet free_cells, occupied_cells;
372  // insert ground points only as free:
373  for (PCLPointCloud::const_iterator it = ground.begin(); it != ground.end(); ++it){
374  point3d point(it->x, it->y, it->z);
375 
376  if ((m_minRange > 0) && (point - sensorOrigin).norm() < m_minRange) continue;
377 
378  // maxrange check
379  if ((m_maxRange > 0.0) && ((point - sensorOrigin).norm() > m_maxRange) ) {
380  point = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRange;
381  }
382 
383  // only clear space (ground points)
384  if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){
385  free_cells.insert(m_keyRay.begin(), m_keyRay.end());
386  }
387 
388  octomap::OcTreeKey endKey;
389  if (m_octree->coordToKeyChecked(point, endKey)){
390  updateMinKey(endKey, m_updateBBXMin);
391  updateMaxKey(endKey, m_updateBBXMax);
392  } else{
393  ROS_ERROR_STREAM("Could not generate Key for endpoint "<<point);
394  }
395  }
396 
397  // all other points: free on ray, occupied on endpoint:
398  for (PCLPointCloud::const_iterator it = nonground.begin(); it != nonground.end(); ++it){
399  point3d point(it->x, it->y, it->z);
400 
401  if ((m_minRange > 0) && (point - sensorOrigin).norm() < m_minRange) continue;
402 
403  // maxrange check
404  if ((m_maxRange < 0.0) || ((point - sensorOrigin).norm() <= m_maxRange) ) {
405 
406  // free cells
407  if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){
408  free_cells.insert(m_keyRay.begin(), m_keyRay.end());
409  }
410  // occupied endpoint
411  OcTreeKey key;
412  if (m_octree->coordToKeyChecked(point, key)){
413  occupied_cells.insert(key);
414 
417 
418 #ifdef COLOR_OCTOMAP_SERVER // NB: Only read and interpret color if it's an occupied node
419  m_octree->averageNodeColor(it->x, it->y, it->z, /*r=*/it->r, /*g=*/it->g, /*b=*/it->b);
420 #endif
421  }
422  } else {// ray longer than maxrange:;
423  point3d new_end = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRange;
424  if (m_octree->computeRayKeys(sensorOrigin, new_end, m_keyRay)){
425  free_cells.insert(m_keyRay.begin(), m_keyRay.end());
426 
427  octomap::OcTreeKey endKey;
428  if (m_octree->coordToKeyChecked(new_end, endKey)){
429  free_cells.insert(endKey);
430  updateMinKey(endKey, m_updateBBXMin);
431  updateMaxKey(endKey, m_updateBBXMax);
432  } else{
433  ROS_ERROR_STREAM("Could not generate Key for endpoint "<<new_end);
434  }
435 
436 
437  }
438  }
439  }
440 
441  // mark free cells only if not seen occupied in this cloud
442  for(KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it){
443  if (occupied_cells.find(*it) == occupied_cells.end()){
444  m_octree->updateNode(*it, false);
445  }
446  }
447 
448  // now mark all occupied cells:
449  for (KeySet::iterator it = occupied_cells.begin(), end=occupied_cells.end(); it!= end; it++) {
450  m_octree->updateNode(*it, true);
451  }
452 
453  // TODO: eval lazy+updateInner vs. proper insertion
454  // non-lazy by default (updateInnerOccupancy() too slow for large maps)
455  //m_octree->updateInnerOccupancy();
456  octomap::point3d minPt, maxPt;
457  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]);
458 
459  // TODO: snap max / min keys to larger voxels by m_maxTreeDepth
460 // if (m_maxTreeDepth < 16)
461 // {
462 // 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)
463 // OcTreeKey tmpMax = getIndexKey(m_updateBBXMax, m_maxTreeDepth); // see above, now add something to find upper right
464 // tmpMax[0]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1;
465 // tmpMax[1]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1;
466 // tmpMax[2]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1;
467 // m_updateBBXMin = tmpMin;
468 // m_updateBBXMax = tmpMax;
469 // }
470 
471  // TODO: we could also limit the bbx to be within the map bounds here (see publishing check)
474  ROS_DEBUG_STREAM("Updated area bounding box: "<< minPt << " - "<<maxPt);
475  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]);
476 
477  if (m_compressMap)
478  m_octree->prune();
479 
480 #ifdef COLOR_OCTOMAP_SERVER
481  if (colors)
482  {
483  delete[] colors;
484  colors = NULL;
485  }
486 #endif
487 }
488 
491  if (m_publish2DMap) {
492  m_gridmap.header.stamp = rostime;
494  }
495 }
496 
498  ros::WallTime startTime = ros::WallTime::now();
499  size_t octomapSize = m_octree->size();
500  // TODO: estimate num occ. voxels for size of arrays (reserve)
501  if (octomapSize <= 1){
502  ROS_WARN("Nothing to publish, octree is empty");
503  return;
504  }
505 
506  bool publishFreeMarkerArray = m_publishFreeSpace && (m_latchedTopics || m_fmarkerPub.getNumSubscribers() > 0);
507  bool publishMarkerArray = (m_latchedTopics || m_markerPub.getNumSubscribers() > 0);
508  bool publishPointCloud = (m_latchedTopics || m_pointCloudPub.getNumSubscribers() > 0);
509  bool publishBinaryMap = (m_latchedTopics || m_binaryMapPub.getNumSubscribers() > 0);
510  bool publishFullMap = (m_latchedTopics || m_fullMapPub.getNumSubscribers() > 0);
511 
512  // init markers for free space:
513  visualization_msgs::MarkerArray freeNodesVis;
514  // each array stores all cubes of a different size, one for each depth level:
515  freeNodesVis.markers.resize(m_treeDepth+1);
516 
517  geometry_msgs::Pose pose;
518  pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
519 
520  // init markers:
521  visualization_msgs::MarkerArray occupiedNodesVis;
522  // each array stores all cubes of a different size, one for each depth level:
523  occupiedNodesVis.markers.resize(m_treeDepth+1);
524 
525  // init pointcloud:
526  pcl::PointCloud<PCLPoint> pclCloud;
527 
528  // call pre-traversal hook:
529  handlePreNodeTraversal(rostime);
530 
531  // now, traverse all leafs in the tree:
533  end = m_octree->end(); it != end; ++it)
534  {
535  bool inUpdateBBX = isInUpdateBBX(it);
536 
537  // call general hook:
538  handleNode(it);
539  if (inUpdateBBX)
540  handleNodeInBBX(it);
541 
542  if (m_octree->isNodeOccupied(*it)){
543  double z = it.getZ();
544  double half_size = it.getSize() / 2.0;
545  if (z + half_size > m_occupancyMinZ && z - half_size < m_occupancyMaxZ)
546  {
547  double size = it.getSize();
548  double x = it.getX();
549  double y = it.getY();
550 #ifdef COLOR_OCTOMAP_SERVER
551  int r = it->getColor().r;
552  int g = it->getColor().g;
553  int b = it->getColor().b;
554 #endif
555 
556  // Ignore speckles in the map:
557  if (m_filterSpeckles && (it.getDepth() == m_treeDepth +1) && isSpeckleNode(it.getKey())){
558  ROS_DEBUG("Ignoring single speckle at (%f,%f,%f)", x, y, z);
559  continue;
560  } // else: current octree node is no speckle, send it out
561 
562  handleOccupiedNode(it);
563  if (inUpdateBBX)
565 
566 
567  //create marker:
568  if (publishMarkerArray){
569  unsigned idx = it.getDepth();
570  assert(idx < occupiedNodesVis.markers.size());
571 
572  geometry_msgs::Point cubeCenter;
573  cubeCenter.x = x;
574  cubeCenter.y = y;
575  cubeCenter.z = z;
576 
577  occupiedNodesVis.markers[idx].points.push_back(cubeCenter);
578  if (m_useHeightMap){
579  double minX, minY, minZ, maxX, maxY, maxZ;
580  m_octree->getMetricMin(minX, minY, minZ);
581  m_octree->getMetricMax(maxX, maxY, maxZ);
582 
583  double h = (1.0 - std::min(std::max((cubeCenter.z-minZ)/ (maxZ - minZ), 0.0), 1.0)) *m_colorFactor;
584  occupiedNodesVis.markers[idx].colors.push_back(heightMapColor(h));
585  }
586 
587 #ifdef COLOR_OCTOMAP_SERVER
588  if (m_useColoredMap) {
589  std_msgs::ColorRGBA _color; _color.r = (r / 255.); _color.g = (g / 255.); _color.b = (b / 255.); _color.a = 1.0; // TODO/EVALUATE: potentially use occupancy as measure for alpha channel?
590  occupiedNodesVis.markers[idx].colors.push_back(_color);
591  }
592 #endif
593  }
594 
595  // insert into pointcloud:
596  if (publishPointCloud) {
597 #ifdef COLOR_OCTOMAP_SERVER
598  PCLPoint _point = PCLPoint();
599  _point.x = x; _point.y = y; _point.z = z;
600  _point.r = r; _point.g = g; _point.b = b;
601  pclCloud.push_back(_point);
602 #else
603  pclCloud.push_back(PCLPoint(x, y, z));
604 #endif
605  }
606 
607  }
608  } else{ // node not occupied => mark as free in 2D map if unknown so far
609  double z = it.getZ();
610  double half_size = it.getSize() / 2.0;
611  if (z + half_size > m_occupancyMinZ && z - half_size < m_occupancyMaxZ)
612  {
613  handleFreeNode(it);
614  if (inUpdateBBX)
616 
617  if (m_publishFreeSpace){
618  double x = it.getX();
619  double y = it.getY();
620 
621  //create marker for free space:
622  if (publishFreeMarkerArray){
623  unsigned idx = it.getDepth();
624  assert(idx < freeNodesVis.markers.size());
625 
626  geometry_msgs::Point cubeCenter;
627  cubeCenter.x = x;
628  cubeCenter.y = y;
629  cubeCenter.z = z;
630 
631  freeNodesVis.markers[idx].points.push_back(cubeCenter);
632  }
633  }
634 
635  }
636  }
637  }
638 
639  // call post-traversal hook:
640  handlePostNodeTraversal(rostime);
641 
642  // finish MarkerArray:
643  if (publishMarkerArray){
644  for (unsigned i= 0; i < occupiedNodesVis.markers.size(); ++i){
645  double size = m_octree->getNodeSize(i);
646 
647  occupiedNodesVis.markers[i].header.frame_id = m_worldFrameId;
648  occupiedNodesVis.markers[i].header.stamp = rostime;
649  occupiedNodesVis.markers[i].ns = "map";
650  occupiedNodesVis.markers[i].id = i;
651  occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
652  occupiedNodesVis.markers[i].scale.x = size;
653  occupiedNodesVis.markers[i].scale.y = size;
654  occupiedNodesVis.markers[i].scale.z = size;
655  if (!m_useColoredMap)
656  occupiedNodesVis.markers[i].color = m_color;
657 
658 
659  if (occupiedNodesVis.markers[i].points.size() > 0)
660  occupiedNodesVis.markers[i].action = visualization_msgs::Marker::ADD;
661  else
662  occupiedNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
663  }
664 
665  m_markerPub.publish(occupiedNodesVis);
666  }
667 
668 
669  // finish FreeMarkerArray:
670  if (publishFreeMarkerArray){
671  for (unsigned i= 0; i < freeNodesVis.markers.size(); ++i){
672  double size = m_octree->getNodeSize(i);
673 
674  freeNodesVis.markers[i].header.frame_id = m_worldFrameId;
675  freeNodesVis.markers[i].header.stamp = rostime;
676  freeNodesVis.markers[i].ns = "map";
677  freeNodesVis.markers[i].id = i;
678  freeNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
679  freeNodesVis.markers[i].scale.x = size;
680  freeNodesVis.markers[i].scale.y = size;
681  freeNodesVis.markers[i].scale.z = size;
682  freeNodesVis.markers[i].color = m_colorFree;
683 
684 
685  if (freeNodesVis.markers[i].points.size() > 0)
686  freeNodesVis.markers[i].action = visualization_msgs::Marker::ADD;
687  else
688  freeNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
689  }
690 
691  m_fmarkerPub.publish(freeNodesVis);
692  }
693 
694 
695  // finish pointcloud:
696  if (publishPointCloud){
697  sensor_msgs::PointCloud2 cloud;
698  pcl::toROSMsg (pclCloud, cloud);
699  cloud.header.frame_id = m_worldFrameId;
700  cloud.header.stamp = rostime;
701  m_pointCloudPub.publish(cloud);
702  }
703 
704  if (publishBinaryMap)
705  publishBinaryOctoMap(rostime);
706 
707  if (publishFullMap)
708  publishFullOctoMap(rostime);
709 
710 
711  double total_elapsed = (ros::WallTime::now() - startTime).toSec();
712  ROS_DEBUG("Map publishing in OctomapServer took %f sec", total_elapsed);
713 }
714 
715 bool OctomapServer::octomapBinarySrv(OctomapSrv::Request &req,
716  OctomapSrv::Response &res)
717 {
718  ros::WallTime startTime = ros::WallTime::now();
719  ROS_INFO("Sending binary map data on service request");
720  res.map.header.frame_id = m_worldFrameId;
721  res.map.header.stamp = ros::Time::now();
722  if (!octomap_msgs::binaryMapToMsg(*m_octree, res.map))
723  return false;
724 
725  double total_elapsed = (ros::WallTime::now() - startTime).toSec();
726  ROS_INFO("Binary octomap sent in %f sec", total_elapsed);
727  return true;
728 }
729 
730 bool OctomapServer::octomapFullSrv(OctomapSrv::Request &req,
731  OctomapSrv::Response &res)
732 {
733  ROS_INFO("Sending full map data on service request");
734  res.map.header.frame_id = m_worldFrameId;
735  res.map.header.stamp = ros::Time::now();
736 
737 
738  if (!octomap_msgs::fullMapToMsg(*m_octree, res.map))
739  return false;
740 
741  return true;
742 }
743 
744 bool OctomapServer::clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp){
745  point3d min = pointMsgToOctomap(req.min);
746  point3d max = pointMsgToOctomap(req.max);
747 
748  double thresMin = m_octree->getClampingThresMin();
749  for(OcTreeT::leaf_bbx_iterator it = m_octree->begin_leafs_bbx(min,max),
750  end=m_octree->end_leafs_bbx(); it!= end; ++it){
751 
752  it->setLogOdds(octomap::logodds(thresMin));
753  // m_octree->updateNode(it.getKey(), -6.0f);
754  }
755  // TODO: eval which is faster (setLogOdds+updateInner or updateNode)
757 
759 
760  return true;
761 }
762 
763 bool OctomapServer::resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp) {
764  visualization_msgs::MarkerArray occupiedNodesVis;
765  occupiedNodesVis.markers.resize(m_treeDepth +1);
766  ros::Time rostime = ros::Time::now();
767  m_octree->clear();
768  // clear 2D map:
769  m_gridmap.data.clear();
770  m_gridmap.info.height = 0.0;
771  m_gridmap.info.width = 0.0;
772  m_gridmap.info.resolution = 0.0;
773  m_gridmap.info.origin.position.x = 0.0;
774  m_gridmap.info.origin.position.y = 0.0;
775 
776  ROS_INFO("Cleared octomap");
777  publishAll(rostime); // Note: This will return as the octree is empty
778 
779  publishProjected2DMap(rostime);
780 
781  publishBinaryOctoMap(rostime);
782 
783  for (std::size_t i = 0; i < occupiedNodesVis.markers.size(); ++i){
784  occupiedNodesVis.markers[i].header.frame_id = m_worldFrameId;
785  occupiedNodesVis.markers[i].header.stamp = rostime;
786  occupiedNodesVis.markers[i].ns = "map";
787  occupiedNodesVis.markers[i].id = i;
788  occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
789  occupiedNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
790  }
791  m_markerPub.publish(occupiedNodesVis);
792 
793  visualization_msgs::MarkerArray freeNodesVis;
794  freeNodesVis.markers.resize(m_treeDepth +1);
795  for (std::size_t i = 0; i < freeNodesVis.markers.size(); ++i){
796  freeNodesVis.markers[i].header.frame_id = m_worldFrameId;
797  freeNodesVis.markers[i].header.stamp = rostime;
798  freeNodesVis.markers[i].ns = "map";
799  freeNodesVis.markers[i].id = i;
800  freeNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
801  freeNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
802  }
803  m_fmarkerPub.publish(freeNodesVis);
804 
805  return true;
806 }
807 
809 
810  Octomap map;
811  map.header.frame_id = m_worldFrameId;
812  map.header.stamp = rostime;
813 
815  m_binaryMapPub.publish(map);
816  else
817  ROS_ERROR("Error serializing OctoMap");
818 }
819 
820 void OctomapServer::publishFullOctoMap(const ros::Time& rostime) const{
821 
822  Octomap map;
823  map.header.frame_id = m_worldFrameId;
824  map.header.stamp = rostime;
825 
827  m_fullMapPub.publish(map);
828  else
829  ROS_ERROR("Error serializing OctoMap");
830 
831 }
832 
833 
835  ground.header = pc.header;
836  nonground.header = pc.header;
837 
838  if (pc.size() < 50){
839  ROS_WARN("Pointcloud in OctomapServer too small, skipping ground plane extraction");
840  nonground = pc;
841  } else {
842  // plane detection for ground plane removal:
843  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
844  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
845 
846  // Create the segmentation object and set up:
847  pcl::SACSegmentation<PCLPoint> seg;
848  seg.setOptimizeCoefficients (true);
849  // TODO: maybe a filtering based on the surface normals might be more robust / accurate?
850  seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
851  seg.setMethodType(pcl::SAC_RANSAC);
852  seg.setMaxIterations(200);
853  seg.setDistanceThreshold (m_groundFilterDistance);
854  seg.setAxis(Eigen::Vector3f(0,0,1));
855  seg.setEpsAngle(m_groundFilterAngle);
856 
857 
858  PCLPointCloud cloud_filtered(pc);
859  // Create the filtering object
860  pcl::ExtractIndices<PCLPoint> extract;
861  bool groundPlaneFound = false;
862 
863  while(cloud_filtered.size() > 10 && !groundPlaneFound){
864  seg.setInputCloud(cloud_filtered.makeShared());
865  seg.segment (*inliers, *coefficients);
866  if (inliers->indices.size () == 0){
867  ROS_INFO("PCL segmentation did not find any plane.");
868 
869  break;
870  }
871 
872  extract.setInputCloud(cloud_filtered.makeShared());
873  extract.setIndices(inliers);
874 
875  if (std::abs(coefficients->values.at(3)) < m_groundFilterPlaneDistance){
876  ROS_DEBUG("Ground plane found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(),
877  coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3));
878  extract.setNegative (false);
879  extract.filter (ground);
880 
881  // remove ground points from full pointcloud:
882  // workaround for PCL bug:
883  if(inliers->indices.size() != cloud_filtered.size()){
884  extract.setNegative(true);
885  PCLPointCloud cloud_out;
886  extract.filter(cloud_out);
887  nonground += cloud_out;
888  cloud_filtered = cloud_out;
889  }
890 
891  groundPlaneFound = true;
892  } else{
893  ROS_DEBUG("Horizontal plane (not ground) found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(),
894  coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3));
895  pcl::PointCloud<PCLPoint> cloud_out;
896  extract.setNegative (false);
897  extract.filter(cloud_out);
898  nonground +=cloud_out;
899  // debug
900  // pcl::PCDWriter writer;
901  // writer.write<PCLPoint>("nonground_plane.pcd",cloud_out, false);
902 
903  // remove current plane from scan for next iteration:
904  // workaround for PCL bug:
905  if(inliers->indices.size() != cloud_filtered.size()){
906  extract.setNegative(true);
907  cloud_out.points.clear();
908  extract.filter(cloud_out);
909  cloud_filtered = cloud_out;
910  } else{
911  cloud_filtered.points.clear();
912  }
913  }
914 
915  }
916  // TODO: also do this if overall starting pointcloud too small?
917  if (!groundPlaneFound){ // no plane found or remaining points too small
918  ROS_WARN("No ground plane found in scan");
919 
920  // do a rough fitlering on height to prevent spurious obstacles
921  pcl::PassThrough<PCLPoint> second_pass;
922  second_pass.setFilterFieldName("z");
923  second_pass.setFilterLimits(-m_groundFilterPlaneDistance, m_groundFilterPlaneDistance);
924  second_pass.setInputCloud(pc.makeShared());
925  second_pass.filter(ground);
926 
927  second_pass.setFilterLimitsNegative (true);
928  second_pass.filter(nonground);
929  }
930 
931  // debug:
932  // pcl::PCDWriter writer;
933  // if (pc_ground.size() > 0)
934  // writer.write<PCLPoint>("ground.pcd",pc_ground, false);
935  // if (pc_nonground.size() > 0)
936  // writer.write<PCLPoint>("nonground.pcd",pc_nonground, false);
937 
938  }
939 
940 
941 }
942 
944  if (m_publish2DMap){
945  // init projected 2D map:
946  m_gridmap.header.frame_id = m_worldFrameId;
947  m_gridmap.header.stamp = rostime;
948  nav_msgs::MapMetaData oldMapInfo = m_gridmap.info;
949 
950  // TODO: move most of this stuff into c'tor and init map only once (adjust if size changes)
951  double minX, minY, minZ, maxX, maxY, maxZ;
952  m_octree->getMetricMin(minX, minY, minZ);
953  m_octree->getMetricMax(maxX, maxY, maxZ);
954 
955  octomap::point3d minPt(minX, minY, minZ);
956  octomap::point3d maxPt(maxX, maxY, maxZ);
959 
960  ROS_DEBUG("MinKey: %d %d %d / MaxKey: %d %d %d", minKey[0], minKey[1], minKey[2], maxKey[0], maxKey[1], maxKey[2]);
961 
962  // add padding if requested (= new min/maxPts in x&y):
963  double halfPaddedX = 0.5*m_minSizeX;
964  double halfPaddedY = 0.5*m_minSizeY;
965  minX = std::min(minX, -halfPaddedX);
966  maxX = std::max(maxX, halfPaddedX);
967  minY = std::min(minY, -halfPaddedY);
968  maxY = std::max(maxY, halfPaddedY);
969  minPt = octomap::point3d(minX, minY, minZ);
970  maxPt = octomap::point3d(maxX, maxY, maxZ);
971 
972  OcTreeKey paddedMaxKey;
974  ROS_ERROR("Could not create padded min OcTree key at %f %f %f", minPt.x(), minPt.y(), minPt.z());
975  return;
976  }
977  if (!m_octree->coordToKeyChecked(maxPt, m_maxTreeDepth, paddedMaxKey)){
978  ROS_ERROR("Could not create padded max OcTree key at %f %f %f", maxPt.x(), maxPt.y(), maxPt.z());
979  return;
980  }
981 
982  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]);
983  assert(paddedMaxKey[0] >= maxKey[0] && paddedMaxKey[1] >= maxKey[1]);
984 
986  m_gridmap.info.width = (paddedMaxKey[0] - m_paddedMinKey[0])/m_multires2DScale +1;
987  m_gridmap.info.height = (paddedMaxKey[1] - m_paddedMinKey[1])/m_multires2DScale +1;
988 
989  int mapOriginX = minKey[0] - m_paddedMinKey[0];
990  int mapOriginY = minKey[1] - m_paddedMinKey[1];
991  assert(mapOriginX >= 0 && mapOriginY >= 0);
992 
993  // might not exactly be min / max of octree:
995  double gridRes = m_octree->getNodeSize(m_maxTreeDepth);
996  m_projectCompleteMap = (!m_incrementalUpdate || (std::abs(gridRes-m_gridmap.info.resolution) > 1e-6));
997  m_gridmap.info.resolution = gridRes;
998  m_gridmap.info.origin.position.x = origin.x() - gridRes*0.5;
999  m_gridmap.info.origin.position.y = origin.y() - gridRes*0.5;
1000  if (m_maxTreeDepth != m_treeDepth){
1001  m_gridmap.info.origin.position.x -= m_res/2.0;
1002  m_gridmap.info.origin.position.y -= m_res/2.0;
1003  }
1004 
1005  // workaround for multires. projection not working properly for inner nodes:
1006  // force re-building complete map
1008  m_projectCompleteMap = true;
1009 
1010 
1012  ROS_DEBUG("Rebuilding complete 2D map");
1013  m_gridmap.data.clear();
1014  // init to unknown:
1015  m_gridmap.data.resize(m_gridmap.info.width * m_gridmap.info.height, -1);
1016 
1017  } else {
1018 
1019  if (mapChanged(oldMapInfo, m_gridmap.info)){
1020  ROS_DEBUG("2D grid map size changed to %dx%d", m_gridmap.info.width, m_gridmap.info.height);
1021  adjustMapData(m_gridmap, oldMapInfo);
1022  }
1023  nav_msgs::OccupancyGrid::_data_type::iterator startIt;
1024  size_t mapUpdateBBXMinX = std::max(0, (int(m_updateBBXMin[0]) - int(m_paddedMinKey[0]))/int(m_multires2DScale));
1025  size_t mapUpdateBBXMinY = std::max(0, (int(m_updateBBXMin[1]) - int(m_paddedMinKey[1]))/int(m_multires2DScale));
1026  size_t mapUpdateBBXMaxX = std::min(int(m_gridmap.info.width-1), (int(m_updateBBXMax[0]) - int(m_paddedMinKey[0]))/int(m_multires2DScale));
1027  size_t mapUpdateBBXMaxY = std::min(int(m_gridmap.info.height-1), (int(m_updateBBXMax[1]) - int(m_paddedMinKey[1]))/int(m_multires2DScale));
1028 
1029  assert(mapUpdateBBXMaxX > mapUpdateBBXMinX);
1030  assert(mapUpdateBBXMaxY > mapUpdateBBXMinY);
1031 
1032  size_t numCols = mapUpdateBBXMaxX-mapUpdateBBXMinX +1;
1033 
1034  // test for max idx:
1035  uint max_idx = m_gridmap.info.width*mapUpdateBBXMaxY + mapUpdateBBXMaxX;
1036  if (max_idx >= m_gridmap.data.size())
1037  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);
1038 
1039  // reset proj. 2D map in bounding box:
1040  for (unsigned int j = mapUpdateBBXMinY; j <= mapUpdateBBXMaxY; ++j){
1041  std::fill_n(m_gridmap.data.begin() + m_gridmap.info.width*j+mapUpdateBBXMinX,
1042  numCols, -1);
1043  }
1044 
1045  }
1046 
1047 
1048 
1049  }
1050 
1051 }
1052 
1054  publishProjected2DMap(rostime);
1055 }
1056 
1059  update2DMap(it, true);
1060  }
1061 }
1062 
1065  update2DMap(it, false);
1066  }
1067 }
1068 
1071  update2DMap(it, true);
1072  }
1073 }
1074 
1077  update2DMap(it, false);
1078  }
1079 }
1080 
1081 void OctomapServer::update2DMap(const OcTreeT::iterator& it, bool occupied){
1082  // update 2D map (occupied always overrides):
1083 
1084  if (it.getDepth() == m_maxTreeDepth){
1085  unsigned idx = mapIdx(it.getKey());
1086  if (occupied)
1087  m_gridmap.data[mapIdx(it.getKey())] = 100;
1088  else if (m_gridmap.data[idx] == -1){
1089  m_gridmap.data[idx] = 0;
1090  }
1091 
1092  } else{
1093  int intSize = 1 << (m_maxTreeDepth - it.getDepth());
1094  octomap::OcTreeKey minKey=it.getIndexKey();
1095  for(int dx=0; dx < intSize; dx++){
1096  int i = (minKey[0]+dx - m_paddedMinKey[0])/m_multires2DScale;
1097  for(int dy=0; dy < intSize; dy++){
1098  unsigned idx = mapIdx(i, (minKey[1]+dy - m_paddedMinKey[1])/m_multires2DScale);
1099  if (occupied)
1100  m_gridmap.data[idx] = 100;
1101  else if (m_gridmap.data[idx] == -1){
1102  m_gridmap.data[idx] = 0;
1103  }
1104  }
1105  }
1106  }
1107 
1108 
1109 }
1110 
1111 
1112 
1113 bool OctomapServer::isSpeckleNode(const OcTreeKey&nKey) const {
1114  OcTreeKey key;
1115  bool neighborFound = false;
1116  for (key[2] = nKey[2] - 1; !neighborFound && key[2] <= nKey[2] + 1; ++key[2]){
1117  for (key[1] = nKey[1] - 1; !neighborFound && key[1] <= nKey[1] + 1; ++key[1]){
1118  for (key[0] = nKey[0] - 1; !neighborFound && key[0] <= nKey[0] + 1; ++key[0]){
1119  if (key != nKey){
1120  OcTreeNode* node = m_octree->search(key);
1121  if (node && m_octree->isNodeOccupied(node)){
1122  // we have a neighbor => break!
1123  neighborFound = true;
1124  }
1125  }
1126  }
1127  }
1128  }
1129 
1130  return neighborFound;
1131 }
1132 
1133 void OctomapServer::reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level){
1134  if (m_maxTreeDepth != unsigned(config.max_depth))
1135  m_maxTreeDepth = unsigned(config.max_depth);
1136  else{
1137  m_pointcloudMinZ = config.pointcloud_min_z;
1138  m_pointcloudMaxZ = config.pointcloud_max_z;
1139  m_occupancyMinZ = config.occupancy_min_z;
1140  m_occupancyMaxZ = config.occupancy_max_z;
1141  m_filterSpeckles = config.filter_speckles;
1142  m_filterGroundPlane = config.filter_ground;
1143  m_compressMap = config.compress_map;
1144  m_incrementalUpdate = config.incremental_2D_projection;
1145 
1146  // Parameters with a namespace require an special treatment at the beginning, as dynamic reconfigure
1147  // will overwrite them because the server is not able to match parameters' names.
1148  if (m_initConfig){
1149  // If parameters do not have the default value, dynamic reconfigure server should be updated.
1150  if(!is_equal(m_groundFilterDistance, 0.04))
1151  config.ground_filter_distance = m_groundFilterDistance;
1152  if(!is_equal(m_groundFilterAngle, 0.15))
1153  config.ground_filter_angle = m_groundFilterAngle;
1155  config.ground_filter_plane_distance = m_groundFilterPlaneDistance;
1156  if(!is_equal(m_maxRange, -1.0))
1157  config.sensor_model_max_range = m_maxRange;
1158  if(!is_equal(m_minRange, -1.0))
1159  config.sensor_model_min_range = m_minRange;
1160  if(!is_equal(m_octree->getProbHit(), 0.7))
1161  config.sensor_model_hit = m_octree->getProbHit();
1162  if(!is_equal(m_octree->getProbMiss(), 0.4))
1163  config.sensor_model_miss = m_octree->getProbMiss();
1164  if(!is_equal(m_octree->getClampingThresMin(), 0.12))
1165  config.sensor_model_min = m_octree->getClampingThresMin();
1166  if(!is_equal(m_octree->getClampingThresMax(), 0.97))
1167  config.sensor_model_max = m_octree->getClampingThresMax();
1168  m_initConfig = false;
1169 
1170  boost::recursive_mutex::scoped_lock reconf_lock(m_config_mutex);
1171  m_reconfigureServer.updateConfig(config);
1172  }
1173  else{
1174  m_groundFilterDistance = config.ground_filter_distance;
1175  m_groundFilterAngle = config.ground_filter_angle;
1176  m_groundFilterPlaneDistance = config.ground_filter_plane_distance;
1177  m_maxRange = config.sensor_model_max_range;
1178  m_octree->setClampingThresMin(config.sensor_model_min);
1179  m_octree->setClampingThresMax(config.sensor_model_max);
1180 
1181  // Checking values that might create unexpected behaviors.
1182  if (is_equal(config.sensor_model_hit, 1.0))
1183  config.sensor_model_hit -= 1.0e-6;
1184  m_octree->setProbHit(config.sensor_model_hit);
1185  if (is_equal(config.sensor_model_miss, 0.0))
1186  config.sensor_model_miss += 1.0e-6;
1187  m_octree->setProbMiss(config.sensor_model_miss);
1188  }
1189  }
1190  publishAll();
1191 }
1192 
1193 void OctomapServer::adjustMapData(nav_msgs::OccupancyGrid& map, const nav_msgs::MapMetaData& oldMapInfo) const{
1194  if (map.info.resolution != oldMapInfo.resolution){
1195  ROS_ERROR("Resolution of map changed, cannot be adjusted");
1196  return;
1197  }
1198 
1199  int i_off = int((oldMapInfo.origin.position.x - map.info.origin.position.x)/map.info.resolution +0.5);
1200  int j_off = int((oldMapInfo.origin.position.y - map.info.origin.position.y)/map.info.resolution +0.5);
1201 
1202  if (i_off < 0 || j_off < 0
1203  || oldMapInfo.width + i_off > map.info.width
1204  || oldMapInfo.height + j_off > map.info.height)
1205  {
1206  ROS_ERROR("New 2D map does not contain old map area, this case is not implemented");
1207  return;
1208  }
1209 
1210  nav_msgs::OccupancyGrid::_data_type oldMapData = map.data;
1211 
1212  map.data.clear();
1213  // init to unknown:
1214  map.data.resize(map.info.width * map.info.height, -1);
1215 
1216  nav_msgs::OccupancyGrid::_data_type::iterator fromStart, fromEnd, toStart;
1217 
1218  for (int j =0; j < int(oldMapInfo.height); ++j ){
1219  // copy chunks, row by row:
1220  fromStart = oldMapData.begin() + j*oldMapInfo.width;
1221  fromEnd = fromStart + oldMapInfo.width;
1222  toStart = map.data.begin() + ((j+j_off)*m_gridmap.info.width + i_off);
1223  copy(fromStart, fromEnd, toStart);
1224 
1225 // for (int i =0; i < int(oldMapInfo.width); ++i){
1226 // map.data[m_gridmap.info.width*(j+j_off) +i+i_off] = oldMapData[oldMapInfo.width*j +i];
1227 // }
1228 
1229  }
1230 
1231 }
1232 
1233 
1234 std_msgs::ColorRGBA OctomapServer::heightMapColor(double h) {
1235 
1236  std_msgs::ColorRGBA color;
1237  color.a = 1.0;
1238  // blend over HSV-values (more colors)
1239 
1240  double s = 1.0;
1241  double v = 1.0;
1242 
1243  h -= floor(h);
1244  h *= 6;
1245  int i;
1246  double m, n, f;
1247 
1248  i = floor(h);
1249  f = h - i;
1250  if (!(i & 1))
1251  f = 1 - f; // if i is even
1252  m = v * (1 - s);
1253  n = v * (1 - s * f);
1254 
1255  switch (i) {
1256  case 6:
1257  case 0:
1258  color.r = v; color.g = n; color.b = m;
1259  break;
1260  case 1:
1261  color.r = n; color.g = v; color.b = m;
1262  break;
1263  case 2:
1264  color.r = m; color.g = v; color.b = n;
1265  break;
1266  case 3:
1267  color.r = m; color.g = n; color.b = v;
1268  break;
1269  case 4:
1270  color.r = n; color.g = m; color.b = v;
1271  break;
1272  case 5:
1273  color.r = v; color.g = m; color.b = n;
1274  break;
1275  default:
1276  color.r = 1; color.g = 0.5; color.b = 0.5;
1277  break;
1278  }
1279 
1280  return color;
1281 }
1282 }
1283 
1284 
1285 
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::end_leafs_bbx
const leaf_bbx_iterator end_leafs_bbx() const
octomap::AbstractOccupancyOcTree::setProbMiss
void setProbMiss(double prob)
pcl_ros::transformAsMatrix
void transformAsMatrix(const geometry_msgs::Transform &bt, Eigen::Matrix4f &out_mat)
octomap_server::OctomapServer::m_resetService
ros::ServiceServer m_resetService
Definition: OctomapServer.h:212
octomap_server::OctomapServer::octomapBinarySrv
virtual bool octomapBinarySrv(OctomapSrv::Request &req, OctomapSrv::GetOctomap::Response &res)
Definition: OctomapServer.cpp:715
octomap_server::OctomapServer::m_occupancyMaxZ
double m_occupancyMaxZ
Definition: OctomapServer.h:245
octomap_server::OctomapServer::m_reconfigureServer
dynamic_reconfigure::Server< OctomapServerConfig > m_reconfigureServer
Definition: OctomapServer.h:215
octomap_server::OctomapServer::filterGroundPlane
void filterGroundPlane(const PCLPointCloud &pc, PCLPointCloud &ground, PCLPointCloud &nonground) const
label the input cloud "pc" into ground and nonground. Should be in the robot's fixed frame (not world...
Definition: OctomapServer.cpp:834
OccupancyOcTreeBase< OcTreeNode >::updateInnerOccupancy
void updateInnerOccupancy()
octomap_server::OctomapServer::handleNode
virtual void handleNode(const OcTreeT::iterator &it)
hook that is called when traversing all nodes of the updated Octree (does nothing here)
Definition: OctomapServer.h:158
octomap_server::OctomapServer::m_pointcloudMinX
double m_pointcloudMinX
Definition: OctomapServer.h:238
octomap_server::OctomapServer::m_baseFrameId
std::string m_baseFrameId
Definition: OctomapServer.h:225
octomap_server::OctomapServer::m_latchedTopics
bool m_latchedTopics
Definition: OctomapServer.h:231
octomap_server::OctomapServer::updateMaxKey
static void updateMaxKey(const octomap::OcTreeKey &in, octomap::OcTreeKey &max)
Definition: OctomapServer.h:112
octomap_msgs::fullMapToMsg
static bool fullMapToMsg(const OctomapT &octomap, Octomap &msg)
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::search
OcTreeNode * search(const OcTreeKey &key, unsigned int depth=0) const
epsilon
double epsilon
octomap::AbstractOccupancyOcTree::getClampingThresMin
double getClampingThresMin() const
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
octomap_server::OctomapServer::PCLPointCloud
pcl::PointCloud< pcl::PointXYZ > PCLPointCloud
Definition: OctomapServer.h:90
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::size
virtual size_t size() const
octomap_server::OctomapServer::insertCloudCallback
virtual void insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud)
Definition: OctomapServer.cpp:263
octomap_server::OctomapServer::openFile
virtual bool openFile(const std::string &filename)
Definition: OctomapServer.cpp:210
octomap_server::OctomapServer::m_keyRay
octomap::KeyRay m_keyRay
Definition: OctomapServer.h:218
octomap_server::OctomapServer::m_tfPointCloudSub
tf::MessageFilter< sensor_msgs::PointCloud2 > * m_tfPointCloudSub
Definition: OctomapServer.h:211
octomap_server::OctomapServer::heightMapColor
static std_msgs::ColorRGBA heightMapColor(double h)
Definition: OctomapServer.cpp:1234
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::computeRayKeys
bool computeRayKeys(const point3d &origin, const point3d &end, KeyRay &ray) const
s
XmlRpcServer s
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::keyToCoord
point3d keyToCoord(const OcTreeKey &key) const
octomap_server::OctomapServer::m_filterSpeckles
bool m_filterSpeckles
Definition: OctomapServer.h:248
octomap_server::OctomapServer::isSpeckleNode
bool isSpeckleNode(const octomap::OcTreeKey &key) const
Find speckle nodes (single occupied voxels with no neighbors). Only works on lowest resolution!
Definition: OctomapServer.cpp:1113
octomap_server::OctomapServer::resetSrv
bool resetSrv(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
Definition: OctomapServer.cpp:763
octomap_server::OctomapServer::m_paddedMinKey
octomap::OcTreeKey m_paddedMinKey
Definition: OctomapServer.h:264
octomap::AbstractOccupancyOcTree::readBinary
bool readBinary(const std::string &filename)
octomap_server::OctomapServer::handlePreNodeTraversal
virtual void handlePreNodeTraversal(const ros::Time &rostime)
hook that is called before traversing all nodes
Definition: OctomapServer.cpp:943
octomap_server::OctomapServer::m_pointcloudMaxY
double m_pointcloudMaxY
Definition: OctomapServer.h:241
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::getMetricMax
virtual void getMetricMax(double &x, double &y, double &z)
octomap_server::OctomapServer::octomapFullSrv
virtual bool octomapFullSrv(OctomapSrv::Request &req, OctomapSrv::GetOctomap::Response &res)
Definition: OctomapServer.cpp:730
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::coordToKey
OcTreeKey coordToKey(const point3d &coord) const
octomap_server::OctomapServer::m_octree
OcTreeT * m_octree
Definition: OctomapServer.h:217
OctomapServer.h
octomap_server::OctomapServer::m_multires2DScale
unsigned m_multires2DScale
Definition: OctomapServer.h:265
octomap_server::OctomapServer::m_colorFactor
double m_colorFactor
Definition: OctomapServer.h:229
tf::StampedTransform
octomap_server::OctomapServer::publishAll
virtual void publishAll(const ros::Time &rostime=ros::Time::now())
Definition: OctomapServer.cpp:497
pcl::fromROSMsg
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
octomap_server::OctomapServer::m_initConfig
bool m_initConfig
Definition: OctomapServer.h:257
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
octomap_server::OctomapServer::m_publishFreeSpace
bool m_publishFreeSpace
Definition: OctomapServer.h:232
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
octomap_server::OctomapServer::m_nh_private
ros::NodeHandle m_nh_private
Definition: OctomapServer.h:208
f
f
octomap_server::OctomapServer::publishBinaryOctoMap
void publishBinaryOctoMap(const ros::Time &rostime=ros::Time::now()) const
Definition: OctomapServer.cpp:808
message_filters::Subscriber< sensor_msgs::PointCloud2 >
octomath::Vector3
octomap_server::OctomapServer::m_pointcloudMaxZ
double m_pointcloudMaxZ
Definition: OctomapServer.h:243
octomap_server::OctomapServer::m_incrementalUpdate
bool m_incrementalUpdate
Definition: OctomapServer.h:260
octomap_server::OctomapServer::m_publish2DMap
bool m_publish2DMap
Definition: OctomapServer.h:262
octomap::OcTree
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::getResolution
double getResolution() const
octomap_server::OctomapServer::handleFreeNodeInBBX
virtual void handleFreeNodeInBBX(const OcTreeT::iterator &it)
hook that is called when traversing free nodes in the updated area (updates 2D map projection here)
Definition: OctomapServer.cpp:1075
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
octomap_server::OctomapServer::publishProjected2DMap
void publishProjected2DMap(const ros::Time &rostime=ros::Time::now())
Definition: OctomapServer.cpp:489
octomap_msgs::binaryMapToMsg
static bool binaryMapToMsg(const OctomapT &octomap, Octomap &msg)
octomap_server::OctomapServer::~OctomapServer
virtual ~OctomapServer()
Definition: OctomapServer.cpp:191
octomap_server::OctomapServer::m_res
double m_res
Definition: OctomapServer.h:234
octomap_server::OctomapServer::m_groundFilterAngle
double m_groundFilterAngle
Definition: OctomapServer.h:252
tf::Point
tf::Vector3 Point
KeySet
unordered_ns::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
octomap_eraser_cli.max
max
Definition: octomap_eraser_cli.py:24
octomap_server::OctomapServer::m_octomapFullService
ros::ServiceServer m_octomapFullService
Definition: OctomapServer.h:212
octomap_server::OctomapServer::mapChanged
bool mapChanged(const nav_msgs::MapMetaData &oldMapInfo, const nav_msgs::MapMetaData &newMapInfo)
Definition: OctomapServer.h:199
octomap::OcTreeNode
octomap::AbstractOcTree
ROS_DEBUG
#define ROS_DEBUG(...)
octomap_server::OctomapServer::m_octomapBinaryService
ros::ServiceServer m_octomapBinaryService
Definition: OctomapServer.h:212
octomap_server::OctomapServer::clearBBXSrv
bool clearBBXSrv(BBXSrv::Request &req, BBXSrv::Response &resp)
Definition: OctomapServer.cpp:744
octomap_server::OctomapServer::m_nh
ros::NodeHandle m_nh
Definition: OctomapServer.h:207
octomap_server::OctomapServer::updateMinKey
static void updateMinKey(const octomap::OcTreeKey &in, octomap::OcTreeKey &min)
Definition: OctomapServer.h:107
octomap_server::OctomapServer::m_fmarkerPub
ros::Publisher m_fmarkerPub
Definition: OctomapServer.h:209
octomap_server::OctomapServer::adjustMapData
void adjustMapData(nav_msgs::OccupancyGrid &map, const nav_msgs::MapMetaData &oldMapInfo) const
Definition: OctomapServer.cpp:1193
ros::WallTime::now
static WallTime now()
octomap::AbstractOccupancyOcTree::setClampingThresMin
void setClampingThresMin(double thresProb)
octomap::AbstractOccupancyOcTree::getClampingThresMax
double getClampingThresMax() const
octomap_server::OctomapServer::handleFreeNode
virtual void handleFreeNode(const OcTreeT::iterator &it)
hook that is called when traversing free nodes of the updated Octree
Definition: OctomapServer.cpp:1063
octomap_server::OctomapServer::m_pointcloudMinZ
double m_pointcloudMinZ
Definition: OctomapServer.h:242
octomap_server::OctomapServer::m_gridmap
nav_msgs::OccupancyGrid m_gridmap
Definition: OctomapServer.h:261
octomap_server::OctomapServer::m_pointcloudMaxX
double m_pointcloudMaxX
Definition: OctomapServer.h:239
ROS_WARN
#define ROS_WARN(...)
octomap::KeyRay::end
iterator end()
octomap_server::OctomapServer::m_colorFree
std_msgs::ColorRGBA m_colorFree
Definition: OctomapServer.h:228
octomap::OcTreeKey
octomap_server::OctomapServer::m_updateBBXMin
octomap::OcTreeKey m_updateBBXMin
Definition: OctomapServer.h:219
octomap::AbstractOccupancyOcTree::setClampingThresMax
void setClampingThresMax(double thresProb)
octomap::OcTreeKey
tf::Transformer::waitForTransform
bool waitForTransform(const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::prune
virtual void prune()
octomap::AbstractOccupancyOcTree::setProbHit
void setProbHit(double prob)
ros::WallTime
octomap_server::OctomapServer::m_mapPub
ros::Publisher m_mapPub
Definition: OctomapServer.h:209
octomap_server::OctomapServer::insertScan
virtual void insertScan(const tf::Point &sensorOrigin, const PCLPointCloud &ground, const PCLPointCloud &nonground)
update occupancy map with a scan labeled as ground and nonground. The scans should be in the global m...
Definition: OctomapServer.cpp:357
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::begin
iterator begin(unsigned char maxDepth=0) const
octomap_server::OctomapServer::PCLPoint
pcl::PointXYZ PCLPoint
Definition: OctomapServer.h:89
octomap::AbstractOccupancyOcTree::getProbHit
double getProbHit() const
octomap_server::OctomapServer::m_pointCloudPub
ros::Publisher m_pointCloudPub
Definition: OctomapServer.h:209
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::coordToKeyChecked
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
octomap::AbstractOccupancyOcTree::isNodeOccupied
bool isNodeOccupied(const OcTreeNode &occupancyNode) const
octomap_server::OctomapServer::isInUpdateBBX
bool isInUpdateBBX(const OcTreeT::iterator &it) const
Test if key is within update area of map (2D, ignores height)
Definition: OctomapServer.h:118
octomap_server
Definition: OctomapServer.h:80
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::iterator
leaf_iterator iterator
octomap::AbstractOccupancyOcTree::getProbMiss
double getProbMiss() const
octomap_server::OctomapServer::m_compressMap
bool m_compressMap
Definition: OctomapServer.h:255
octomap::logodds
float logodds(double probability)
octomap_server::OctomapServer::m_updateBBXMax
octomap::OcTreeKey m_updateBBXMax
Definition: OctomapServer.h:220
octomath::Vector3::y
float & y()
ros::Time
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::getNodeSize
double getNodeSize(unsigned depth) const
octomap_server::OctomapServer::m_fullMapPub
ros::Publisher m_fullMapPub
Definition: OctomapServer.h:209
octomap_server::OctomapServer::m_groundFilterDistance
double m_groundFilterDistance
Definition: OctomapServer.h:251
is_equal
bool is_equal(double a, double b, double epsilon=1.0e-7)
Definition: OctomapServer.cpp:35
octomap_server::OctomapServer::handleOccupiedNode
virtual void handleOccupiedNode(const OcTreeT::iterator &it)
hook that is called when traversing occupied nodes of the updated Octree
Definition: OctomapServer.cpp:1057
std
octomap_server::OctomapServer::m_occupancyMinZ
double m_occupancyMinZ
Definition: OctomapServer.h:244
tf::MessageFilter< sensor_msgs::PointCloud2 >
ROS_ERROR
#define ROS_ERROR(...)
octomap_server::OctomapServer::m_minRange
double m_minRange
Definition: OctomapServer.h:222
octomap_server::OctomapServer::m_treeDepth
unsigned m_treeDepth
Definition: OctomapServer.h:235
octomap_server::OctomapServer::mapIdx
unsigned mapIdx(int i, int j) const
Definition: OctomapServer.h:181
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::end
const iterator end() const
octomap_server::OctomapServer::m_clearBBXService
ros::ServiceServer m_clearBBXService
Definition: OctomapServer.h:212
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
octomap_server::OctomapServer::OcTreeT
octomap::OcTree OcTreeT
Definition: OctomapServer.h:91
octomap_server::OctomapServer::m_minSizeY
double m_minSizeY
Definition: OctomapServer.h:247
pcl::toROSMsg
void toROSMsg(const pcl::PointCloud< T > &cloud, sensor_msgs::Image &msg)
octomap_server::OctomapServer::m_tfListener
tf::TransformListener m_tfListener
Definition: OctomapServer.h:213
octomap_server::OctomapServer::m_pointCloudSub
message_filters::Subscriber< sensor_msgs::PointCloud2 > * m_pointCloudSub
Definition: OctomapServer.h:210
octomap_server::OctomapServer::m_groundFilterPlaneDistance
double m_groundFilterPlaneDistance
Definition: OctomapServer.h:253
octomap_server::OctomapServer::m_projectCompleteMap
bool m_projectCompleteMap
Definition: OctomapServer.h:266
octomap_server::OctomapServer::handleOccupiedNodeInBBX
virtual void handleOccupiedNodeInBBX(const OcTreeT::iterator &it)
hook that is called when traversing occupied nodes in the updated area (updates 2D map projection her...
Definition: OctomapServer.cpp:1069
octomap
octomap::KeyRay::begin
iterator begin()
octomap_server::OctomapServer::m_maxRange
double m_maxRange
Definition: OctomapServer.h:223
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
octomap_server::OctomapServer::m_useColoredMap
bool m_useColoredMap
Definition: OctomapServer.h:267
octomap_server::OctomapServer::reconfigureCallback
void reconfigureCallback(octomap_server::OctomapServerConfig &config, uint32_t level)
Definition: OctomapServer.cpp:1133
tf::Transformer::lookupTransform
void lookupTransform(const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, StampedTransform &transform) const
octomap_server::OctomapServer::m_pointcloudMinY
double m_pointcloudMinY
Definition: OctomapServer.h:240
tf2::TransformException
ROS_INFO
#define ROS_INFO(...)
octomath::Vector3::z
float & z()
octomap_server::OctomapServer::m_filterGroundPlane
bool m_filterGroundPlane
Definition: OctomapServer.h:250
octomap::point3d
octomath::Vector3 point3d
OccupancyOcTreeBase< OcTreeNode >::updateNode
virtual OcTreeNode * updateNode(const OcTreeKey &key, bool occupied, bool lazy_eval=false)
tf::createQuaternionMsgFromYaw
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
octomap_server::OctomapServer::publishFullOctoMap
void publishFullOctoMap(const ros::Time &rostime=ros::Time::now()) const
Definition: OctomapServer.cpp:820
octomath::Vector3::x
float & x()
octomap_server::OctomapServer::m_maxTreeDepth
unsigned m_maxTreeDepth
Definition: OctomapServer.h:236
octomap_server::OctomapServer::m_minSizeX
double m_minSizeX
Definition: OctomapServer.h:246
ros::Duration
octomap_server::OctomapServer::handleNodeInBBX
virtual void handleNodeInBBX(const OcTreeT::iterator &it)
hook that is called when traversing all nodes of the updated Octree in the updated area (does nothing...
Definition: OctomapServer.h:161
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::clear
void clear()
octomap_server::OctomapServer::m_markerPub
ros::Publisher m_markerPub
Definition: OctomapServer.h:209
octomap_server::OctomapServer::handlePostNodeTraversal
virtual void handlePostNodeTraversal(const ros::Time &rostime)
hook that is called after traversing all nodes
Definition: OctomapServer.cpp:1053
octomap_server::OctomapServer::m_worldFrameId
std::string m_worldFrameId
Definition: OctomapServer.h:224
tf::Transform::getOrigin
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::begin_leafs_bbx
leaf_bbx_iterator begin_leafs_bbx(const OcTreeKey &min, const OcTreeKey &max, unsigned char maxDepth=0) const
ros::NodeHandle
octomap_server::OctomapServer::m_config_mutex
boost::recursive_mutex m_config_mutex
Definition: OctomapServer.h:214
octomap_server::OctomapServer::m_useHeightMap
bool m_useHeightMap
Definition: OctomapServer.h:226
octomap_server::OctomapServer::update2DMap
virtual void update2DMap(const OcTreeT::iterator &it, bool occupied)
updates the downprojected 2D map as either occupied or free
Definition: OctomapServer.cpp:1081
ros::Time::now
static Time now()
octomap_server::OctomapServer::m_color
std_msgs::ColorRGBA m_color
Definition: OctomapServer.h:227
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::getTreeDepth
unsigned int getTreeDepth() const
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::getMetricMin
virtual void getMetricMin(double &x, double &y, double &z)
octomap_server::OctomapServer::m_binaryMapPub
ros::Publisher m_binaryMapPub
Definition: OctomapServer.h:209


octomap_server
Author(s): Armin Hornung
autogenerated on Mon Jan 2 2023 03:18:15