octomap_server_contact_nodelet.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 #define BOOST_PARAMETER_MAX_ARITY 7
38 #include <algorithm>
39 
40 using namespace octomap;
41 using octomap_msgs::Octomap;
42 
43 namespace jsk_pcl_ros
44 {
45  OctomapServerContact::OctomapServerContact(const ros::NodeHandle &privateNh)
46  : OctomapServer(privateNh),
47  DiagnosticNodelet("OctomapServerContact"),
48  m_octreeContact(NULL),
49  m_publishUnknownSpace(false),
50  m_publishFrontierSpace(false),
51  m_offsetVisualizeUnknown(0),
52  m_maxRangeProximity(0.05),
53  m_occupancyMinX(-std::numeric_limits<double>::max()),
54  m_occupancyMaxX(std::numeric_limits<double>::max()),
55  m_occupancyMinY(-std::numeric_limits<double>::max()),
56  m_occupancyMaxY(std::numeric_limits<double>::max()),
57  m_useContactSurface(true)
58  {
59  delete m_octree;
60  m_octree = NULL;
62  m_octreeContact = dynamic_cast<OcTreeContact*>(m_octree);
63  if (!m_octreeContact) {
64  ROS_ERROR("Could not convert OcTreeContact from OcTree");
65  assert(m_octreeContact);
66  }
67 
68  m_useHeightMap = false;
69 
70  privateNh.param("publish_unknown_space", m_publishUnknownSpace, m_publishUnknownSpace);
71  privateNh.param("offset_vis_unknown", m_offsetVisualizeUnknown, m_offsetVisualizeUnknown);
72  privateNh.param("sensor_model/max_range_proximity", m_maxRangeProximity, m_maxRangeProximity);
73  privateNh.param("publish_frontier_space", m_publishFrontierSpace, m_publishFrontierSpace);
74 
75  privateNh.param("occupancy_min_x", m_occupancyMinX,m_occupancyMinX);
76  privateNh.param("occupancy_max_x", m_occupancyMaxX,m_occupancyMaxX);
77  privateNh.param("occupancy_min_y", m_occupancyMinY,m_occupancyMinY);
78  privateNh.param("occupancy_max_y", m_occupancyMaxY,m_occupancyMaxY);
79 
80  privateNh.param("use_contact_surface", m_useContactSurface,m_useContactSurface);
81 
82  double r, g, b, a;
83  privateNh.param("color_unknown/r", r, 0.5);
84  privateNh.param("color_unknown/g", g, 0.5);
85  privateNh.param("color_unknown/b", b, 0.7);
86  privateNh.param("color_unknown/a", a, 1.0);
87  m_colorUnknown.r = r;
88  m_colorUnknown.g = g;
89  m_colorUnknown.b = b;
90  m_colorUnknown.a = a;
91  privateNh.param("color_frontier/r", r, 1.0);
92  privateNh.param("color_frontier/g", g, 0.0);
93  privateNh.param("color_frontier/b", b, 0.0);
94  privateNh.param("color_frontier/a", a, 1.0);
95  m_colorFrontier.r = r;
96  m_colorFrontier.g = g;
97  m_colorFrontier.b = b;
98  m_colorFrontier.a = a;
99 
100  m_unknownPointCloudPub = m_nh.advertise<sensor_msgs::PointCloud2>("octomap_unknown_point_cloud_centers", 1, m_latchedTopics);
101  m_umarkerPub = m_nh.advertise<visualization_msgs::MarkerArray>("unknown_cells_vis_array", 1, m_latchedTopics);
102 
105  m_tfPointProximitySub->registerCallback(boost::bind(&OctomapServerContact::insertProximityCallback, this, _1));
106 
107  m_frontierPointCloudPub = m_nh.advertise<sensor_msgs::PointCloud2>("octomap_frontier_point_cloud_centers", 1, m_latchedTopics);
108  m_fromarkerPub = m_nh.advertise<visualization_msgs::MarkerArray>("frontier_cells_vis_array", 1, m_latchedTopics);
109 
110  m_contactSensorSub.subscribe(m_nh, "contact_sensors_in", 2);
112  m_contactSensorSub, m_tfListener, m_worldFrameId, 2));
113  m_tfContactSensorSub->registerCallback(boost::bind(&OctomapServerContact::insertContactSensorCallback, this, _1));
114 
115  initContactSensor(privateNh);
116  }
117 
119  }
120 
122  double defaultPadding, defaultScale;
123  privateNh.param<double>("self_see_defaultPadding", defaultPadding, .01);
124  privateNh.param<double>("self_see_defaultScale", defaultScale, 1.0);
125  std::vector<robot_self_filter::LinkInfo> links;
126 
127  if (!privateNh.hasParam("self_see_links")) {
128  ROS_WARN("No links specified for self filtering.");
129  }
130  else {
131  XmlRpc::XmlRpcValue sslVals;;
132  privateNh.getParam("self_see_links", sslVals);
133  if (sslVals.getType() != XmlRpc::XmlRpcValue::TypeArray) {
134  ROS_WARN("Self see links need to be an array");
135  }
136  else {
137  if (sslVals.size() == 0) {
138  ROS_WARN("No values in self see links array");
139  }
140  else {
141  for (int i = 0; i < sslVals.size(); i++) {
142  robot_self_filter::LinkInfo li;
143  if (sslVals[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) {
144  ROS_WARN("Self see links entry %d is not a structure. Stopping processing of self see links",i);
145  break;
146  }
147  if (!sslVals[i].hasMember("name")) {
148  ROS_WARN("Self see links entry %d has no name. Stopping processing of self see links",i);
149  break;
150  }
151  li.name = std::string(sslVals[i]["name"]);
152  if (!sslVals[i].hasMember("padding")) {
153  ROS_DEBUG("Self see links entry %d has no padding. Assuming default padding of %g",i,defaultPadding);
154  li.padding = defaultPadding;
155  }
156  else {
157  li.padding = sslVals[i]["padding"];
158  }
159  if (!sslVals[i].hasMember("scale")) {
160  ROS_DEBUG("Self see links entry %d has no scale. Assuming default scale of %g",i,defaultScale);
161  li.scale = defaultScale;
162  }
163  else {
164  li.scale = sslVals[i]["scale"];
165  }
166  links.push_back(li);
167  }
168  }
169  }
170  }
172  }
173 
174  void OctomapServerContact::insertProximityCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud) {
175  // ROS_INFO("insertProximityCallback is called");
176 
177  ros::WallTime startTime = ros::WallTime::now();
178 
179  //
180  // ground filtering in base frame
181  //
182  PCLPointCloud pc; // input cloud for filtering and ground-detection
183  pcl::fromROSMsg(*cloud, pc);
184 
185  tf::StampedTransform sensorToWorldTf;
186  try {
187  m_tfListener.lookupTransform(m_worldFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf);
188  } catch(tf::TransformException& ex){
189  ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback");
190  return;
191  }
192 
193  Eigen::Matrix4f sensorToWorld;
194  pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld);
195 
196  // directly transform to map frame:
197  pcl::transformPointCloud(pc, pc, sensorToWorld);
198 
199  pc.header = pc.header;
200 
201  insertScanProximity(sensorToWorldTf.getOrigin(), pc);
202 
203  double total_elapsed = (ros::WallTime::now() - startTime).toSec();
204  ROS_DEBUG("Pointcloud insertion in OctomapServer done (%zu pts, %f sec)", pc.size(), total_elapsed);
205 
206  publishAll(cloud->header.stamp);
207  }
208 
209  void OctomapServerContact::insertScanProximity(const tf::Point& sensorOriginTf, const PCLPointCloud& pc) {
210  point3d sensorOrigin = pointTfToOctomap(sensorOriginTf);
211 
212  if (!m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMin)
213  || !m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMax))
214  {
215  ROS_ERROR_STREAM("Could not generate Key for origin "<<sensorOrigin);
216  }
217 
218 #ifdef COLOR_OCTOMAP_SERVER
219  unsigned char* colors = new unsigned char[3];
220 #endif
221 
222  // instead of direct scan insertion, compute update to filter ground:
223  KeySet free_cells, occupied_cells;
224 
225  // all other points: free on ray, occupied on endpoint:
226  for (PCLPointCloud::const_iterator it = pc.begin(); it != pc.end(); ++it) {
227  point3d point(it->x, it->y, it->z);
228  // maxrange check
229  if ((m_maxRangeProximity < 0.0) || ((point - sensorOrigin).norm() <= m_maxRangeProximity) ) {
230 
231  // free cells
232  if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){
233  free_cells.insert(m_keyRay.begin(), m_keyRay.end());
234  }
235  // occupied endpoint
236  OcTreeKey key;
237  if (m_octree->coordToKeyChecked(point, key)){
238  occupied_cells.insert(key);
239 
242 
243 #ifdef COLOR_OCTOMAP_SERVER // NB: Only read and interpret color if it's an occupied node
244  const int rgb = *reinterpret_cast<const int*>(&(it->rgb)); // TODO: there are other ways to encode color than this one
245  colors[0] = ((rgb >> 16) & 0xff);
246  colors[1] = ((rgb >> 8) & 0xff);
247  colors[2] = (rgb & 0xff);
248  m_octree->averageNodeColor(it->x, it->y, it->z, colors[0], colors[1], colors[2]);
249 #endif
250  }
251  } else {// ray longer than maxrange:;
252  point3d new_end = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRangeProximity;
253  if (m_octree->computeRayKeys(sensorOrigin, new_end, m_keyRay)){
254  free_cells.insert(m_keyRay.begin(), m_keyRay.end());
255 
256  octomap::OcTreeKey endKey;
257  if (m_octree->coordToKeyChecked(new_end, endKey)){
258  updateMinKey(endKey, m_updateBBXMin);
259  updateMaxKey(endKey, m_updateBBXMax);
260  } else{
261  ROS_ERROR_STREAM("Could not generate Key for endpoint "<<new_end);
262  }
263  }
264  }
265  }
266 
267  // mark free cells only if not seen occupied in this cloud
268  for(KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it){
269  if (occupied_cells.find(*it) == occupied_cells.end()){
270  m_octree->updateNode(*it, false);
271  }
272  }
273 
274  // now mark all occupied cells:
275  for (KeySet::iterator it = occupied_cells.begin(), end=occupied_cells.end(); it!= end; it++) {
276  m_octree->updateNode(*it, true);
277  }
278 
279  // TODO: eval lazy+updateInner vs. proper insertion
280  // non-lazy by default (updateInnerOccupancy() too slow for large maps)
281  //m_octree->updateInnerOccupancy();
282  octomap::point3d minPt, maxPt;
283  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]);
284 
285  // TODO: snap max / min keys to larger voxels by m_maxTreeDepth
286  // if (m_maxTreeDepth < 16)
287  // {
288  // 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)
289  // OcTreeKey tmpMax = getIndexKey(m_updateBBXMax, m_maxTreeDepth); // see above, now add something to find upper right
290  // tmpMax[0]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1;
291  // tmpMax[1]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1;
292  // tmpMax[2]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1;
293  // m_updateBBXMin = tmpMin;
294  // m_updateBBXMax = tmpMax;
295  // }
296 
297  // TODO: we could also limit the bbx to be within the map bounds here (see publishing check)
300  ROS_DEBUG_STREAM("Updated area bounding box: "<< minPt << " - "<<maxPt);
301  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]);
302 
303  if (m_compressMap)
304  m_octree->prune();
305  }
306 
307  void OctomapServerContact::insertContactSensor(const jsk_recognition_msgs::ContactSensorArray::ConstPtr& msg) {
308  std::vector<jsk_recognition_msgs::ContactSensor> datas = msg->datas;
309 
310  // setup tf transformation between octomap and each link
311  {
312  std_msgs::Header tmpHeader;
313  tmpHeader.frame_id = m_worldFrameId;
314  tmpHeader.stamp = msg->header.stamp;
315  if(!m_selfMask->assumeFrame(tmpHeader)) {
316  ROS_ERROR_STREAM("failed tf transformation in insertContactSensor");
317  return;
318  }
319  }
320 
321  // clamp min and max points cf. https://github.com/OctoMap/octomap/issues/146
324  point3d pmin = m_octree->keyToCoord(m_octree->coordToKey(pmin_raw));
325  point3d pmax = m_octree->keyToCoord(m_octree->coordToKey(pmax_raw));
326  float diff[3];
327  unsigned int steps[3];
329  for (int i = 0; i < 3; ++i) {
330  diff[i] = pmax(i) - pmin(i);
331  steps[i] = floor(diff[i] / resolution);
332  // std::cout << "bbx " << i << " size: " << diff[i] << " " << steps[i] << " steps\n";
333  }
334 
335  // loop for grids of octomap
336  if (m_useContactSurface) {
337  std::vector< std::vector<bool> > containFlag(datas.size(), std::vector<bool>(8));
338  point3d p = pmin;
339  for (unsigned int x = 0; x < steps[0]; ++x) {
340  p.x() += resolution;
341  for (unsigned int y = 0; y < steps[1]; ++y) {
342  p.y() += resolution;
343  for (unsigned int z = 0; z < steps[2]; ++z) {
344  // std::cout << "querying p=" << p << std::endl;
345  p.z() += resolution;
346  // loop for vertices of each gird
347  point3d vertexOffset(-resolution/2.0, -resolution/2.0, -resolution/2.0);
348  point3d vertex;
349  for (int i = 0; i < 2; i++) {
350  if (i == 1) { vertexOffset.z() += resolution; }
351  for (int j = 0; j < 2; j++) {
352  if (j == 1) { vertexOffset.y() += resolution; }
353  for (int k = 0; k < 2; k++) {
354  if (k == 1) { vertexOffset.x() += resolution; }
355  vertex = p + vertexOffset;
356  // std::cout << "vertex = " << vertex << std::endl;
357  // loop for each body link
358  for (int l=0; l<datas.size(); l++) {
359  if (m_selfMask->getMaskContainmentforNamedLink(vertex(0), vertex(1), vertex(2), datas[l].link_name) == robot_self_filter::INSIDE) {
360  // std::cout << "inside vertex = " << vertex << std::endl;
361  containFlag[l][i+j*2+k*4] = true;
362  }
363  else {
364  containFlag[l][i+j*2+k*4] = false;
365  }
366  }
367  }
368  vertexOffset.x() -= resolution;
369  }
370  vertexOffset.y() -= resolution;
371  }
372 
373  // update probability of grid
374  std::vector<bool> containFlagLinkSum(8, false);
375  std::vector<bool> containFlagVerticesSum(datas.size(), false);
376  std::vector<bool> containFlagVerticesProd(datas.size(), true);
377  bool insideFlag = false;
378  bool surfaceFlag = false;
379  for (int l = 0; l < datas.size(); l++) {
380  for (int i = 0; i < 8; i++) {
381  if (containFlag[l][i]) {
382  containFlagLinkSum[i] = true;
383  containFlagVerticesSum[l] = true;
384  }
385  else {
386  containFlagVerticesProd[l] = false;
387  }
388  }
389  }
390  insideFlag = (std::find(containFlagLinkSum.begin(), containFlagLinkSum.end(), false) == containFlagLinkSum.end()); // when all elements is true
391  for (int l = 0; l < datas.size(); l++) {
392  if (containFlagVerticesSum[l] && !(containFlagVerticesProd[l]) ) {
393  if (datas[l].contact) {
394  surfaceFlag = true;
395  }
396  }
397  }
398  if (insideFlag) { // inside
399  octomap::OcTreeKey pKey;
400  if (m_octreeContact->coordToKeyChecked(p, pKey)) {
402  // std::cout << "find inside grid and find key. p = " << vertex << std::endl;
403  }
404  }
405  else if (surfaceFlag) { // surface
406  octomap::OcTreeKey pKey;
407  if (m_octreeContact->coordToKeyChecked(p, pKey)) {
409  // std::cout << "find surface grid and find key. p = " << vertex << std::endl;
410  }
411  }
412  }
413  p.z() = pmin.z();
414  }
415  p.y() = pmin.y();
416  }
417  }
418  else {
419  point3d vertexOffset(-resolution/2.0, -resolution/2.0, -resolution/2.0);
420 #pragma omp parallel for
421  for (unsigned int cnt = 0; cnt < steps[0] * steps[1] * steps[2]; ++cnt) {
422  // get grid center
423  point3d p;
424  {
425  unsigned int id[3];
426  id[0] = cnt / (steps[1] * steps[2]);
427  id[1] = (cnt % (steps[1] * steps[2])) / steps[2];
428  id[2] = (cnt % (steps[1] * steps[2])) % steps[2];
429  p.x() = pmin(0) + resolution * id[0];
430  p.y() = pmin(1) + resolution * id[1];
431  p.z() = pmin(2) + resolution * id[2];
432  }
433  point3d vertex;
434  vertex = p + vertexOffset;
435  // loop for each body link
436  for (int l=0; l<datas.size(); l++) {
437  if (m_selfMask->getMaskContainmentforNamedLink(vertex(0), vertex(1), vertex(2), datas[l].link_name) == robot_self_filter::INSIDE) {
438  octomap::OcTreeKey pKey;
439  if (m_octreeContact->coordToKeyChecked(p, pKey)) {
440 #pragma omp critical
442  }
443  break;
444  }
445  }
446  }
447  }
449  }
450 
451  void OctomapServerContact::insertContactSensorCallback(const jsk_recognition_msgs::ContactSensorArray::ConstPtr& msg) {
452  NODELET_INFO("insert contact sensor");
453  insertContactSensor(msg);
454 
455  publishAll(msg->header.stamp);
456  }
457 
459  ros::WallTime startTime = ros::WallTime::now();
460  size_t octomapSize = m_octreeContact->size();
461  // TODO: estimate num occ. voxels for size of arrays (reserve)
462  if (octomapSize <= 1) {
463  ROS_WARN("Nothing to publish, octree is empty");
464  return;
465  }
466 
467  bool publishFreeMarkerArray = m_publishFreeSpace && (m_latchedTopics || m_fmarkerPub.getNumSubscribers() > 0);
468  bool publishUnknownMarkerArray = m_publishUnknownSpace && (m_latchedTopics || m_umarkerPub.getNumSubscribers() > 0);
469  bool publishFrontierMarkerArray = m_publishFrontierSpace && (m_latchedTopics || m_fromarkerPub.getNumSubscribers() > 0);
470  bool publishMarkerArray = (m_latchedTopics || m_markerPub.getNumSubscribers() > 0);
471  bool publishPointCloud = (m_latchedTopics || m_pointCloudPub.getNumSubscribers() > 0);
472  bool publishBinaryMap = (m_latchedTopics || m_binaryMapPub.getNumSubscribers() > 0);
473  bool publishFullMap = (m_latchedTopics || m_fullMapPub.getNumSubscribers() > 0);
475 
476  // init markers for free space:
477  visualization_msgs::MarkerArray freeNodesVis;
478  // each array stores all cubes of a different size, one for each depth level:
479  freeNodesVis.markers.resize(m_treeDepth+1);
480 
481  geometry_msgs::Pose pose;
482  pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
483 
484  // init markers:
485  visualization_msgs::MarkerArray occupiedNodesVis;
486  // each array stores all cubes of a different size, one for each depth level:
487  occupiedNodesVis.markers.resize(m_treeDepth+1);
488 
489  // init pointcloud:
490  pcl::PointCloud<pcl::PointXYZ> pclCloud;
491 
492  // call pre-traversal hook:
493  handlePreNodeTraversal(rostime);
494 
495  // now, traverse all leafs in the tree:
496  for (OcTree::iterator it = m_octreeContact->begin(m_maxTreeDepth), end = m_octreeContact->end(); it != end; ++it) {
497  bool inUpdateBBX = isInUpdateBBX(it);
498 
499  // call general hook:
500  handleNode(it);
501  if (inUpdateBBX) {
502  handleNodeInBBX(it);
503  }
504 
505  if (m_octreeContact->isNodeOccupied(*it)) {
506  double x = it.getX();
507  double y = it.getY();
508  double z = it.getZ();
509  if (x > m_occupancyMinX && x < m_occupancyMaxX && y > m_occupancyMinY && y < m_occupancyMaxY && z > m_occupancyMinZ && z < m_occupancyMaxZ) {
510  double size = it.getSize();
511  double x = it.getX();
512  double y = it.getY();
513 
514  // Ignore speckles in the map:
515  if (m_filterSpeckles && (it.getDepth() == m_treeDepth +1) && isSpeckleNode(it.getKey())) {
516  ROS_DEBUG("Ignoring single speckle at (%f,%f,%f)", x, y, z);
517  continue;
518  } // else: current octree node is no speckle, send it out
519 
520  handleOccupiedNode(it);
521  if (inUpdateBBX) {
523  }
524 
525  //create marker:
526  if (publishMarkerArray) {
527  unsigned idx = it.getDepth();
528  assert(idx < occupiedNodesVis.markers.size());
529 
530  geometry_msgs::Point cubeCenter;
531  cubeCenter.x = x;
532  cubeCenter.y = y;
533  cubeCenter.z = z;
534 
535  occupiedNodesVis.markers[idx].points.push_back(cubeCenter);
536  if (m_useHeightMap) {
537  double minX, minY, minZ, maxX, maxY, maxZ;
538  m_octreeContact->getMetricMin(minX, minY, minZ);
539  m_octreeContact->getMetricMax(maxX, maxY, maxZ);
540 
541  double h = (1.0 - std::min(std::max((cubeCenter.z-minZ)/ (maxZ - minZ), 0.0), 1.0)) *m_colorFactor;
542  occupiedNodesVis.markers[idx].colors.push_back(heightMapColor(h));
543  }
544  }
545 
546  // insert into pointcloud:
547  if (publishPointCloud) {
548  pclCloud.push_back(pcl::PointXYZ(x, y, z));
549  }
550 
551  }
552  } else { // node not occupied => mark as free in 2D map if unknown so far
553  double x = it.getX();
554  double y = it.getY();
555  double z = it.getZ();
556  if (x > m_occupancyMinX && x < m_occupancyMaxX && y > m_occupancyMinY && y < m_occupancyMaxY && z > m_occupancyMinZ && z < m_occupancyMaxZ) {
557  handleFreeNode(it);
558  if (inUpdateBBX)
560 
561  if (m_publishFreeSpace) {
562  double x = it.getX();
563  double y = it.getY();
564 
565  //create marker for free space:
566  if (publishFreeMarkerArray) {
567  unsigned idx = it.getDepth();
568  assert(idx < freeNodesVis.markers.size());
569 
570  geometry_msgs::Point cubeCenter;
571  cubeCenter.x = x;
572  cubeCenter.y = y;
573  cubeCenter.z = z;
574 
575  freeNodesVis.markers[idx].points.push_back(cubeCenter);
576  }
577  }
578  }
579  }
580  }
581 
582  // call post-traversal hook:
583  handlePostNodeTraversal(rostime);
584 
585  // finish MarkerArray:
586  if (publishMarkerArray) {
587  for (unsigned i = 0; i < occupiedNodesVis.markers.size(); ++i) {
588  double size = m_octreeContact->getNodeSize(i);
589 
590  occupiedNodesVis.markers[i].header.frame_id = m_worldFrameId;
591  occupiedNodesVis.markers[i].header.stamp = rostime;
592  occupiedNodesVis.markers[i].ns = m_worldFrameId;
593  occupiedNodesVis.markers[i].id = i;
594  occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
595  occupiedNodesVis.markers[i].scale.x = size;
596  occupiedNodesVis.markers[i].scale.y = size;
597  occupiedNodesVis.markers[i].scale.z = size;
598  occupiedNodesVis.markers[i].color = m_color;
599 
600 
601  if (occupiedNodesVis.markers[i].points.size() > 0) {
602  occupiedNodesVis.markers[i].action = visualization_msgs::Marker::ADD;
603  }
604  else {
605  occupiedNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
606  }
607  }
608 
609  m_markerPub.publish(occupiedNodesVis);
610  }
611 
612 
613  // finish FreeMarkerArray:
614  if (publishFreeMarkerArray) {
615  for (unsigned i = 0; i < freeNodesVis.markers.size(); ++i) {
616  double size = m_octreeContact->getNodeSize(i);
617 
618  freeNodesVis.markers[i].header.frame_id = m_worldFrameId;
619  freeNodesVis.markers[i].header.stamp = rostime;
620  freeNodesVis.markers[i].ns = m_worldFrameId;
621  freeNodesVis.markers[i].id = i;
622  freeNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
623  freeNodesVis.markers[i].scale.x = size;
624  freeNodesVis.markers[i].scale.y = size;
625  freeNodesVis.markers[i].scale.z = size;
626  freeNodesVis.markers[i].color = m_colorFree;
627 
628 
629  if (freeNodesVis.markers[i].points.size() > 0) {
630  freeNodesVis.markers[i].action = visualization_msgs::Marker::ADD;
631  }
632  else {
633  freeNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
634  }
635  }
636 
637  m_fmarkerPub.publish(freeNodesVis);
638  }
639 
640 
641  // publish unknown grid as marker
642  if (publishUnknownMarkerArray) {
643  visualization_msgs::MarkerArray unknownNodesVis;
644  unknownNodesVis.markers.resize(1);
645 
646  point3d_list unknownLeaves;
647  double offset = m_offsetVisualizeUnknown;
648  point3d pMin(m_occupancyMinX + offset, m_occupancyMinY + offset, m_occupancyMinZ + offset);
649  point3d pMax(m_occupancyMaxX - offset, m_occupancyMaxY - offset, m_occupancyMaxZ - offset);
650 
651  m_octreeContact->getUnknownLeafCenters(unknownLeaves, pMin, pMax);
652  pcl::PointCloud<pcl::PointXYZ> unknownCloud;
653 
654  for (point3d_list::iterator it = unknownLeaves.begin(); it != unknownLeaves.end(); it++) {
655  float x = (*it).x();
656  float y = (*it).y();
657  float z = (*it).z();
658  unknownCloud.push_back(pcl::PointXYZ(x, y, z));
659 
660  geometry_msgs::Point cubeCenter;
661  cubeCenter.x = x;
662  cubeCenter.y = y;
663  cubeCenter.z = z;
664  if (m_useHeightMap) {
665  double minX, minY, minZ, maxX, maxY, maxZ;
666  m_octreeContact->getMetricMin(minX, minY, minZ);
667  m_octreeContact->getMetricMax(maxX, maxY, maxZ);
668  double h = (1.0 - std::min(std::max((cubeCenter.z-minZ)/ (maxZ - minZ), 0.0), 1.0)) *m_colorFactor;
669  unknownNodesVis.markers[0].colors.push_back(heightMapColor(h));
670  }
671  unknownNodesVis.markers[0].points.push_back(cubeCenter);
672  }
673 
675  unknownNodesVis.markers[0].header.frame_id = m_worldFrameId;
676  unknownNodesVis.markers[0].header.stamp = rostime;
677  unknownNodesVis.markers[0].ns = m_worldFrameId;
678  unknownNodesVis.markers[0].id = 0;
679  unknownNodesVis.markers[0].type = visualization_msgs::Marker::CUBE_LIST;
680  unknownNodesVis.markers[0].scale.x = size;
681  unknownNodesVis.markers[0].scale.y = size;
682  unknownNodesVis.markers[0].scale.z = size;
683  unknownNodesVis.markers[0].color = m_colorUnknown;
684 
685  if (unknownNodesVis.markers[0].points.size() > 0) {
686  unknownNodesVis.markers[0].action = visualization_msgs::Marker::ADD;
687  }
688  else {
689  unknownNodesVis.markers[0].action = visualization_msgs::Marker::DELETE;
690  }
691  m_umarkerPub.publish(unknownNodesVis);
692 
693  // publish unknown grid as pointcloud
694  sensor_msgs::PointCloud2 unknownRosCloud;
695  pcl::toROSMsg (unknownCloud, unknownRosCloud);
696  unknownRosCloud.header.frame_id = m_worldFrameId;
697  unknownRosCloud.header.stamp = rostime;
698  m_unknownPointCloudPub.publish(unknownRosCloud);
699 
700  // publish frontier grid as marker
701  if (publishFrontierMarkerArray) {
702  visualization_msgs::MarkerArray frontierNodesVis;
703  frontierNodesVis.markers.resize(1);
704  pcl::PointCloud<pcl::PointXYZ> frontierCloud;
706  // how many resolution-size grids are in one edge
707  int x_num = int(((m_occupancyMaxX - m_occupancyMinX) / resolution));
708  int y_num = int(((m_occupancyMaxY - m_occupancyMinY) / resolution));
709  int z_num = int(((m_occupancyMaxZ - m_occupancyMinZ) / resolution));
710  std::vector< std::vector< std::vector<int> > > check_unknown(x_num, std::vector< std::vector<int> >(y_num, std::vector<int>(z_num)));
711  std::vector< std::vector< std::vector<int> > > check_occupied(x_num, std::vector< std::vector<int> >(y_num, std::vector<int>(z_num)));
712  std::vector< std::vector< std::vector<int> > > check_frontier(x_num, std::vector< std::vector<int> >(y_num, std::vector<int>(z_num)));
713 
714  for (int i = 0; i < x_num; i++) {
715  for (int j = 0; j < y_num; j++) {
716  for (int k = 0; k < z_num; k++) {
717  check_unknown[i][j][k] = 0;
718  check_occupied[i][j][k] = 0;
719  check_frontier[i][j][k] = 0;
720  }
721  }
722  }
723 
724  // for all unknown grids, store its information to array
725  for (point3d_list::iterator it_unknown = unknownLeaves.begin();
726  it_unknown != unknownLeaves.end();
727  it_unknown++) {
728  // get center of unknown grids
729  double x_unknown = it_unknown->x();
730  double y_unknown = it_unknown->y();
731  double z_unknown = it_unknown->z();
732  int x_index = int(std::round((x_unknown - m_occupancyMinX) / resolution - 1));
733  int y_index = int(std::round((y_unknown - m_occupancyMinY) / resolution - 1));
734  int z_index = int(std::round((z_unknown - m_occupancyMinZ) / resolution - 1));
735  check_unknown[x_index][y_index][z_index] = 1;
736  }
737 
738  // for all occupied grids, store its information to array
739  for (int idx = 0; idx < occupiedNodesVis.markers.size(); idx++) {
740  double size_occupied = occupiedNodesVis.markers[idx].scale.x;
741  for (int id = 0; id < occupiedNodesVis.markers[idx].points.size(); id++) {
742  double x_occupied = occupiedNodesVis.markers[idx].points[id].x;
743  double y_occupied = occupiedNodesVis.markers[idx].points[id].y;
744  double z_occupied = occupiedNodesVis.markers[idx].points[id].z;
745  int x_min_index = std::round((x_occupied - (size_occupied / 2.0) - m_occupancyMinX) / resolution);
746  int y_min_index = std::round((y_occupied - (size_occupied / 2.0) - m_occupancyMinY) / resolution);
747  int z_min_index = std::round((z_occupied - (size_occupied / 2.0) - m_occupancyMinZ) / resolution);
748  for (int i = x_min_index; i < x_min_index + int(size_occupied/resolution); i++) {
749  for (int j = y_min_index; j < y_min_index + int(size_occupied/resolution); j++) {
750  for (int k = z_min_index; k < z_min_index + int(size_occupied/resolution); k++) {
751  check_occupied[i][j][k] = 1;
752  }
753  }
754  }
755  }
756  }
757 
758  // for all grids except occupied and unknown, (NOTE there are grids which are not free, nor occupied, nor unknown)
759  // check whether they are frontier, namely, adjecent to unknown grids
760  // NOTE all unknown grids are displaced half from the other grids
761  geometry_msgs::Point cubeCenter;
762  for (int i = 0; i < x_num; i++) {
763  for (int j = 0; j < y_num; j++) {
764  for (int k = 0; k < z_num-1; k++) {
765  for (int l = -1; l <= 1; l++) {
766  if ( i+l < 0 || x_num <= i+l ) continue;
767  for (int m = -1; m <= 1; m++) {
768  if ( j+m < 0 || y_num <= j+m ) continue;
769  for (int n = -1; n <= 1; n++) {
770  if ( k+n < 0 || z_num <= k+n ) continue;
771  if (l == 0 && m == 0 && n== 0) continue;
772  if (check_unknown[i+l][j+m][k+n] == 1 && check_unknown[i][j][k] == 0 && check_occupied[i][j][k] == 0 && check_frontier[i][j][k] == 0) {
773  check_frontier[i][j][k] = 1;
774  cubeCenter.x = (i+0.5)*resolution + m_occupancyMinX;
775  cubeCenter.y = (j+0.5)*resolution + m_occupancyMinY;
776  cubeCenter.z = (k+0.5)*resolution + m_occupancyMinZ;
777  if (m_useHeightMap) {
778  double minX, minY, minZ, maxX, maxY, maxZ;
779  m_octreeContact->getMetricMin(minX, minY, minZ);
780  m_octreeContact->getMetricMax(maxX, maxY, maxZ);
781  double h = (1.0 - std::min(std::max((cubeCenter.z-minZ)/ (maxZ - minZ), 0.0), 1.0)) *m_colorFactor;
782  frontierNodesVis.markers[0].colors.push_back(heightMapColor(h));
783  }
784  frontierNodesVis.markers[0].points.push_back(cubeCenter);
785  }
786  }
787  }
788  }
789  }
790  }
791  }
792 
793  // publish frontier grid as marker
795  frontierNodesVis.markers[0].header.frame_id = m_worldFrameId;
796  frontierNodesVis.markers[0].header.stamp = rostime;
797  frontierNodesVis.markers[0].ns = m_worldFrameId;
798  frontierNodesVis.markers[0].id = 0;
799  frontierNodesVis.markers[0].type = visualization_msgs::Marker::CUBE_LIST;
800  frontierNodesVis.markers[0].scale.x = size;
801  frontierNodesVis.markers[0].scale.y = size;
802  frontierNodesVis.markers[0].scale.z = size;
803  frontierNodesVis.markers[0].color = m_colorFrontier;
804 
805  if (frontierNodesVis.markers[0].points.size() > 0) {
806  frontierNodesVis.markers[0].action = visualization_msgs::Marker::ADD;
807  }
808  else {
809  frontierNodesVis.markers[0].action = visualization_msgs::Marker::DELETE;
810  }
811 
812  m_fromarkerPub.publish(frontierNodesVis);
813 
814  // publish frontier grid as pointcloud
815  sensor_msgs::PointCloud2 frontierRosCloud;
816  pcl::toROSMsg (frontierCloud, frontierRosCloud);
817  frontierRosCloud.header.frame_id = m_worldFrameId;
818  frontierRosCloud.header.stamp = rostime;
819  m_frontierPointCloudPub.publish(frontierRosCloud);
820  }
821  }
822 
823  // finish pointcloud:
824  if (publishPointCloud) {
825  sensor_msgs::PointCloud2 cloud;
826  pcl::toROSMsg (pclCloud, cloud);
827  cloud.header.frame_id = m_worldFrameId;
828  cloud.header.stamp = rostime;
829  m_pointCloudPub.publish(cloud);
830  }
831 
832  if (publishBinaryMap) {
833  publishBinaryOctoMap(rostime);
834  }
835 
836  if (publishFullMap) {
837  publishFullOctoMap(rostime);
838  }
839 
840  double totalElapsed = (ros::WallTime::now() - startTime).toSec();
841  ROS_DEBUG("Map publishing in OctomapServer took %f sec", totalElapsed);
842  }
843 
845  {
846  DiagnosticNodelet::onInit();
848  }
849 }
850 
resolution
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::OctomapServerContact, nodelet::Nodelet)
message_filters::Subscriber< sensor_msgs::PointCloud2 > * m_pointProximitySub
double min(const OneDataStat &d)
wrapper function for min method for boost::function
Definition: one_data_stat.h:94
tf::TransformListener m_tfListener
iterator begin()
iterator begin(unsigned char maxDepth=0) const
tf::MessageFilter< sensor_msgs::PointCloud2 > * m_tfPointProximitySub
virtual void insertContactSensor(const jsk_recognition_msgs::ContactSensorArray::ConstPtr &msg)
void publish(const boost::shared_ptr< M > &message) const
octomap::OcTreeKey m_updateBBXMax
virtual void insertScanProximity(const tf::Point &sensorOriginTf, const PCLPointCloud &pc)
unsigned short int coordToKey(double coordinate) const
end
static void updateMaxKey(const octomap::OcTreeKey &in, octomap::OcTreeKey &max)
octomap::OcTreeKey m_updateBBXMin
boost::shared_ptr< robot_self_filter::SelfMaskNamedLink > m_selfMask
float getProbHitContactSensorLog() const
Definition: OcTreeContact.h:72
virtual OcTreeNode * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
int size() const
GLfloat n[6][3]
double max(const OneDataStat &d)
wrapper function for max method for boost::function
void transformAsMatrix(const tf::Transform &bt, Eigen::Matrix4f &out_mat)
virtual void handleFreeNodeInBBX(const OcTreeT::iterator &it)
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
void publishBinaryOctoMap(const ros::Time &rostime=ros::Time::now()) const
bool isInUpdateBBX(const OcTreeT::iterator &it) const
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
Type const & getType() const
pose
#define ROS_WARN(...)
void getUnknownLeafCenters(point3d_list &node_centers, point3d pmin, point3d pmax, unsigned int depth=0) const
virtual void initContactSensor(const ros::NodeHandle &privateNh)
double keyToCoord(unsigned short int key, unsigned depth) const
static void updateMinKey(const octomap::OcTreeKey &in, octomap::OcTreeKey &min)
size
bool computeRayKeys(const point3d &origin, const point3d &end, KeyRay &ray) const
TFSIMD_FORCE_INLINE Vector3 normalized() const
static std_msgs::ColorRGBA heightMapColor(double h)
virtual void handlePreNodeTraversal(const ros::Time &rostime)
virtual void publishAll(const ros::Time &rostime)
virtual void handleOccupiedNode(const OcTreeT::iterator &it)
long l
boost::shared_ptr< tf::MessageFilter< jsk_recognition_msgs::ContactSensorArray > > m_tfContactSensorSub
bool isSpeckleNode(const octomap::OcTreeKey &key) const
virtual void insertContactSensorCallback(const jsk_recognition_msgs::ContactSensorArray::ConstPtr &msg)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
std_msgs::ColorRGBA m_color
virtual void getMetricMin(double &x, double &y, double &z)
unordered_ns::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
void publishFullOctoMap(const ros::Time &rostime=ros::Time::now()) const
std_msgs::ColorRGBA m_colorFree
float getProbMissContactSensorLog() const
Definition: OcTreeContact.h:74
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
rgb
octomap::OcTreeContact * m_octreeContact
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
#define ROS_DEBUG_STREAM(args)
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
std::list< octomath::Vector3 > point3d_list
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
iterator end()
virtual void getMetricMax(double &x, double &y, double &z)
#define NODELET_INFO(...)
uint32_t getNumSubscribers() const
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
static WallTime now()
virtual void handleNode(const OcTreeT::iterator &it)
bool getParam(const std::string &key, std::string &s) const
p
#define NULL
virtual void insertProximityCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud)
message_filters::Subscriber< jsk_recognition_msgs::ContactSensorArray > m_contactSensorSub
virtual void handlePostNodeTraversal(const ros::Time &rostime)
bool hasParam(const std::string &key) const
GdkColor colors[]
#define ROS_ERROR_STREAM(args)
cloud
This is a inherited class of OcTree. The probability of contact sensor model is defined.
Definition: OcTreeContact.h:48
pcl::PointCloud< pcl::PointXYZ > PCLPointCloud
virtual void handleNodeInBBX(const OcTreeT::iterator &it)
#define ROS_ERROR(...)
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
char a[26]
virtual void handleFreeNode(const OcTreeT::iterator &it)
virtual void handleOccupiedNodeInBBX(const OcTreeT::iterator &it)
Connection registerCallback(const C &callback)
#define ROS_DEBUG(...)


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:47