octomap_server_contact_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #define BOOST_PARAMETER_MAX_ARITY 7
00037 #include "jsk_pcl_ros/octomap_server_contact.h"
00038 #include <algorithm>
00039 
00040 using namespace octomap;
00041 using octomap_msgs::Octomap;
00042 
00043 namespace jsk_pcl_ros
00044 {
00045   OctomapServerContact::OctomapServerContact(const ros::NodeHandle &privateNh)
00046   : OctomapServer(privateNh),
00047     DiagnosticNodelet("OctomapServerContact"),
00048     m_octreeContact(NULL),
00049     m_offsetVisualizeUnknown(0),
00050     m_occupancyMinX(-std::numeric_limits<double>::max()),
00051     m_occupancyMaxX(std::numeric_limits<double>::max()),
00052     m_occupancyMinY(-std::numeric_limits<double>::max()),
00053     m_occupancyMaxY(std::numeric_limits<double>::max())
00054   {
00055     delete m_octree;
00056     m_octree = NULL;
00057     m_octree = new OcTreeContact(m_res);
00058     m_octreeContact = dynamic_cast<OcTreeContact*>(m_octree);
00059     if (!m_octreeContact) {
00060       ROS_ERROR("Could not convert OcTreeContact from OcTree");
00061       assert(m_octreeContact);
00062     }
00063 
00064     m_useHeightMap = false;
00065 
00066     privateNh.param("offset_vis_unknown", m_offsetVisualizeUnknown,m_offsetVisualizeUnknown);
00067 
00068     privateNh.param("occupancy_min_x", m_occupancyMinX,m_occupancyMinX);
00069     privateNh.param("occupancy_max_x", m_occupancyMaxX,m_occupancyMaxX);
00070     privateNh.param("occupancy_min_y", m_occupancyMinY,m_occupancyMinY);
00071     privateNh.param("occupancy_max_y", m_occupancyMaxY,m_occupancyMaxY);
00072 
00073     double r, g, b, a;
00074     privateNh.param("color_unknown/r", r, 0.5);
00075     privateNh.param("color_unknown/g", g, 0.5);
00076     privateNh.param("color_unknown/b", b, 0.7);
00077     privateNh.param("color_unknown/a", a, 1.0);
00078     m_colorUnknown.r = r;
00079     m_colorUnknown.g = g;
00080     m_colorUnknown.b = b;
00081     m_colorUnknown.a = a;
00082 
00083     m_unknownPointCloudPub = m_nh.advertise<sensor_msgs::PointCloud2>("octomap_unknown_point_cloud_centers", 1, m_latchedTopics);
00084     m_umarkerPub = m_nh.advertise<visualization_msgs::MarkerArray>("unknown_cells_vis_array", 1, m_latchedTopics);
00085 
00086     m_contactSensorSub.subscribe(m_nh, "contact_sensors_in", 2);
00087     m_tfContactSensorSub.reset(new tf::MessageFilter<jsk_recognition_msgs::ContactSensorArray> (
00088                                  m_contactSensorSub, m_tfListener, m_worldFrameId, 2));
00089     m_tfContactSensorSub->registerCallback(boost::bind(&OctomapServerContact::insertContactSensorCallback, this, _1));
00090 
00091     initContactSensor(privateNh);
00092   }
00093 
00094   OctomapServerContact::~OctomapServerContact() {
00095   }
00096 
00097   void OctomapServerContact::initContactSensor(const ros::NodeHandle &privateNh) {
00098     double defaultPadding, defaultScale;
00099     privateNh.param<double>("self_see_defaultPadding", defaultPadding, .01);
00100     privateNh.param<double>("self_see_defaultScale", defaultScale, 1.0);
00101     std::vector<robot_self_filter::LinkInfo> links;
00102 
00103     if (!privateNh.hasParam("self_see_links")) {
00104       ROS_WARN("No links specified for self filtering.");
00105     }
00106     else {
00107       XmlRpc::XmlRpcValue sslVals;;
00108       privateNh.getParam("self_see_links", sslVals);
00109       if (sslVals.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00110         ROS_WARN("Self see links need to be an array");
00111       }
00112       else {
00113         if (sslVals.size() == 0) {
00114           ROS_WARN("No values in self see links array");
00115         }
00116         else {
00117           for (int i = 0; i < sslVals.size(); i++) {
00118             robot_self_filter::LinkInfo li;
00119             if (sslVals[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) {
00120               ROS_WARN("Self see links entry %d is not a structure.  Stopping processing of self see links",i);
00121               break;
00122             }
00123             if (!sslVals[i].hasMember("name")) {
00124               ROS_WARN("Self see links entry %d has no name.  Stopping processing of self see links",i);
00125               break;
00126             }
00127             li.name = std::string(sslVals[i]["name"]);
00128             if (!sslVals[i].hasMember("padding")) {
00129               ROS_DEBUG("Self see links entry %d has no padding.  Assuming default padding of %g",i,defaultPadding);
00130               li.padding = defaultPadding;
00131             }
00132             else {
00133               li.padding = sslVals[i]["padding"];
00134             }
00135             if (!sslVals[i].hasMember("scale")) {
00136               ROS_DEBUG("Self see links entry %d has no scale.  Assuming default scale of %g",i,defaultScale);
00137               li.scale = defaultScale;
00138             }
00139             else {
00140               li.scale = sslVals[i]["scale"];
00141             }
00142             links.push_back(li);
00143           }
00144         }
00145       }
00146     }
00147     m_selfMask = boost::shared_ptr<robot_self_filter::SelfMaskNamedLink>(new robot_self_filter::SelfMaskNamedLink(m_tfListener, links));
00148   }
00149 
00150   void OctomapServerContact::insertContactSensor(const std::vector<jsk_recognition_msgs::ContactSensor> &datas) {
00151     std_msgs::Header tmpHeader;
00152     tmpHeader.frame_id = m_worldFrameId;
00153     tmpHeader.stamp = ros::Time::now();
00154 
00155     point3d pmin( m_occupancyMinX, m_occupancyMinY, m_occupancyMinZ);
00156     point3d pmax( m_occupancyMaxX, m_occupancyMaxY, m_occupancyMaxZ);
00157     float diff[3];
00158     unsigned int steps[3];
00159     double resolution = m_octreeContact->getResolution();
00160     for (int i = 0; i < 3; ++i) {
00161       diff[i] = pmax(i) - pmin(i);
00162       steps[i] = floor(diff[i] / resolution);
00163       //      std::cout << "bbx " << i << " size: " << diff[i] << " " << steps[i] << " steps\n";
00164     }
00165 
00166     m_selfMask->assumeFrame(tmpHeader);
00167 
00168     // loop for grids of octomap
00169     std::vector< std::vector<bool> > containFlag(datas.size(), std::vector<bool>(8));
00170     point3d p = pmin;
00171     for (unsigned int x = 0; x < steps[0]; ++x) {
00172       p.x() += resolution;
00173       for (unsigned int y = 0; y < steps[1]; ++y) {
00174         p.y() += resolution;
00175         for (unsigned int z = 0; z < steps[2]; ++z) {
00176           // std::cout << "querying p=" << p << std::endl;
00177           p.z() += resolution;
00178           // loop for vertices of each gird
00179           point3d vertexOffset(-resolution/2.0, -resolution/2.0, -resolution/2.0);
00180           point3d vertex;
00181           for (int i = 0; i < 2; i++) {
00182             if (i == 1) { vertexOffset.z() += resolution; }
00183             for (int j = 0; j < 2; j++) {
00184               if (j == 1) { vertexOffset.y() += resolution; }
00185               for (int k = 0; k < 2; k++) {
00186                 if (k == 1) { vertexOffset.x() += resolution; }
00187                 vertex = p + vertexOffset;
00188                 // std::cout << "vertex = " << vertex << std::endl;
00189                 // loop for each body link
00190                 for (int l=0; l<datas.size(); l++) {
00191                   if (m_selfMask->getMaskContainmentforNamedLink(vertex(0), vertex(1), vertex(2), datas[l].link_name) == robot_self_filter::INSIDE) {
00192                     // std::cout << "inside vertex = " << vertex << std::endl;
00193                     containFlag[l][i+j*2+k*4] = true;
00194                   }
00195                   else {
00196                     containFlag[l][i+j*2+k*4] = false;
00197                   }
00198                 }
00199               }
00200               vertexOffset.x() -= resolution;
00201             }
00202             vertexOffset.y() -= resolution;
00203           }
00204 
00205           // update probability of grid
00206           std::vector<bool> containFlagLinkSum(8, false);
00207           std::vector<bool> containFlagVerticesSum(datas.size(), false);
00208           std::vector<bool> containFlagVerticesProd(datas.size(), true);
00209           bool insideFlag = false;
00210           bool surfaceFlag = false;
00211           for (int l = 0; l < datas.size(); l++) {
00212             for (int i = 0; i < 8; i++) {
00213               if (containFlag[l][i]) {
00214                 containFlagLinkSum[i] = true;
00215                 containFlagVerticesSum[l] = true;
00216               }
00217               else {
00218                 containFlagVerticesProd[l] = false;
00219               }
00220             }
00221           }
00222           insideFlag = (std::find(containFlagLinkSum.begin(), containFlagLinkSum.end(), false) == containFlagLinkSum.end()); // when all elements is true
00223           for (int l = 0; l < datas.size(); l++) {
00224             if (containFlagVerticesSum[l] && !(containFlagVerticesProd[l]) ) {
00225               if (datas[l].contact) {
00226                 surfaceFlag = true;
00227               }
00228             }
00229           }
00230           if (insideFlag) { // inside
00231             octomap::OcTreeKey pKey;
00232             if (m_octreeContact->coordToKeyChecked(p, pKey)) {
00233               m_octreeContact->updateNode(pKey, m_octreeContact->getProbMissContactSensorLog());
00234               // std::cout << "find inside grid and find key. p = " << vertex << std::endl;
00235             }
00236           }
00237           else if (surfaceFlag) { // surface
00238             octomap::OcTreeKey pKey;
00239             if (m_octreeContact->coordToKeyChecked(p, pKey)) {
00240               m_octreeContact->updateNode(pKey, m_octreeContact->getProbHitContactSensorLog());
00241               // std::cout << "find surface grid and find key. p = " << vertex << std::endl;
00242             }
00243           }
00244         }
00245         p.z() = pmin.z();
00246       }
00247       p.y() = pmin.y();
00248     }
00249     m_octreeContact->updateInnerOccupancy();
00250   }
00251 
00252   void OctomapServerContact::insertContactSensorCallback(const jsk_recognition_msgs::ContactSensorArray::ConstPtr& msg) {
00253     NODELET_INFO("insert contact sensor");
00254     std::vector<jsk_recognition_msgs::ContactSensor> datas = msg->datas;
00255     insertContactSensor(datas);
00256 
00257     publishAll(msg->header.stamp);
00258   }
00259 
00260   void OctomapServerContact::publishAll(const ros::Time& rostime) {
00261     ros::WallTime startTime = ros::WallTime::now();
00262     size_t octomapSize = m_octreeContact->size();
00263     // TODO: estimate num occ. voxels for size of arrays (reserve)
00264     if (octomapSize <= 1) {
00265       ROS_WARN("Nothing to publish, octree is empty");
00266       return;
00267     }
00268 
00269     bool publishFreeMarkerArray = m_publishFreeSpace && (m_latchedTopics || m_fmarkerPub.getNumSubscribers() > 0);
00270     bool publishMarkerArray = (m_latchedTopics || m_markerPub.getNumSubscribers() > 0);
00271     bool publishPointCloud = (m_latchedTopics || m_pointCloudPub.getNumSubscribers() > 0);
00272     bool publishBinaryMap = (m_latchedTopics || m_binaryMapPub.getNumSubscribers() > 0);
00273     bool publishFullMap = (m_latchedTopics || m_fullMapPub.getNumSubscribers() > 0);
00274     m_publish2DMap = (m_latchedTopics || m_mapPub.getNumSubscribers() > 0);
00275 
00276     // init markers for free space:
00277     visualization_msgs::MarkerArray freeNodesVis;
00278     // each array stores all cubes of a different size, one for each depth level:
00279     freeNodesVis.markers.resize(m_treeDepth+1);
00280 
00281     geometry_msgs::Pose pose;
00282     pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
00283 
00284     // init markers:
00285     visualization_msgs::MarkerArray occupiedNodesVis;
00286     // each array stores all cubes of a different size, one for each depth level:
00287     occupiedNodesVis.markers.resize(m_treeDepth+1);
00288 
00289     // init pointcloud:
00290     pcl::PointCloud<pcl::PointXYZ> pclCloud;
00291 
00292     // call pre-traversal hook:
00293     handlePreNodeTraversal(rostime);
00294 
00295     // now, traverse all leafs in the tree:
00296     for (OcTree::iterator it = m_octreeContact->begin(m_maxTreeDepth), end = m_octreeContact->end(); it != end; ++it) {
00297       bool inUpdateBBX = isInUpdateBBX(it);
00298 
00299       // call general hook:
00300       handleNode(it);
00301       if (inUpdateBBX) {
00302         handleNodeInBBX(it);
00303       }
00304 
00305       if (m_octreeContact->isNodeOccupied(*it)) {
00306         double x = it.getX();
00307         double y = it.getY();
00308         double z = it.getZ();
00309         if (x > m_occupancyMinX && x < m_occupancyMaxX && y > m_occupancyMinY && y < m_occupancyMaxY && z > m_occupancyMinZ && z < m_occupancyMaxZ) {
00310           double size = it.getSize();
00311           double x = it.getX();
00312           double y = it.getY();
00313 
00314           // Ignore speckles in the map:
00315           if (m_filterSpeckles && (it.getDepth() == m_treeDepth +1) && isSpeckleNode(it.getKey())) {
00316             ROS_DEBUG("Ignoring single speckle at (%f,%f,%f)", x, y, z);
00317             continue;
00318           } // else: current octree node is no speckle, send it out
00319 
00320           handleOccupiedNode(it);
00321           if (inUpdateBBX) {
00322             handleOccupiedNodeInBBX(it);
00323           }
00324 
00325           //create marker:
00326           if (publishMarkerArray) {
00327             unsigned idx = it.getDepth();
00328             assert(idx < occupiedNodesVis.markers.size());
00329 
00330             geometry_msgs::Point cubeCenter;
00331             cubeCenter.x = x;
00332             cubeCenter.y = y;
00333             cubeCenter.z = z;
00334 
00335             occupiedNodesVis.markers[idx].points.push_back(cubeCenter);
00336             if (m_useHeightMap) {
00337               double minX, minY, minZ, maxX, maxY, maxZ;
00338               m_octreeContact->getMetricMin(minX, minY, minZ);
00339               m_octreeContact->getMetricMax(maxX, maxY, maxZ);
00340 
00341               double h = (1.0 - std::min(std::max((cubeCenter.z-minZ)/ (maxZ - minZ), 0.0), 1.0)) *m_colorFactor;
00342               occupiedNodesVis.markers[idx].colors.push_back(heightMapColor(h));
00343             }
00344           }
00345 
00346           // insert into pointcloud:
00347           if (publishPointCloud) {
00348             pclCloud.push_back(pcl::PointXYZ(x, y, z));
00349           }
00350 
00351         }
00352       } else { // node not occupied => mark as free in 2D map if unknown so far
00353         double x = it.getX();
00354         double y = it.getY();
00355         double z = it.getZ();
00356         if (x > m_occupancyMinX && x < m_occupancyMaxX && y > m_occupancyMinY && y < m_occupancyMaxY && z > m_occupancyMinZ && z < m_occupancyMaxZ) {
00357           handleFreeNode(it);
00358           if (inUpdateBBX)
00359             handleFreeNodeInBBX(it);
00360 
00361           if (m_publishFreeSpace) {
00362             double x = it.getX();
00363             double y = it.getY();
00364 
00365             //create marker for free space:
00366             if (publishFreeMarkerArray) {
00367               unsigned idx = it.getDepth();
00368               assert(idx < freeNodesVis.markers.size());
00369 
00370               geometry_msgs::Point cubeCenter;
00371               cubeCenter.x = x;
00372               cubeCenter.y = y;
00373               cubeCenter.z = z;
00374 
00375               freeNodesVis.markers[idx].points.push_back(cubeCenter);
00376             }
00377           }
00378         }
00379       }
00380     }
00381 
00382     // call post-traversal hook:
00383     handlePostNodeTraversal(rostime);
00384 
00385     // finish MarkerArray:
00386     if (publishMarkerArray) {
00387       for (unsigned i= 0; i < occupiedNodesVis.markers.size(); ++i) {
00388         double size = m_octreeContact->getNodeSize(i);
00389 
00390         occupiedNodesVis.markers[i].header.frame_id = m_worldFrameId;
00391         occupiedNodesVis.markers[i].header.stamp = rostime;
00392         occupiedNodesVis.markers[i].ns = m_worldFrameId;
00393         occupiedNodesVis.markers[i].id = i;
00394         occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
00395         occupiedNodesVis.markers[i].scale.x = size;
00396         occupiedNodesVis.markers[i].scale.y = size;
00397         occupiedNodesVis.markers[i].scale.z = size;
00398         occupiedNodesVis.markers[i].color = m_color;
00399 
00400 
00401         if (occupiedNodesVis.markers[i].points.size() > 0) {
00402           occupiedNodesVis.markers[i].action = visualization_msgs::Marker::ADD;
00403         }
00404         else {
00405           occupiedNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
00406         }
00407       }
00408 
00409       m_markerPub.publish(occupiedNodesVis);
00410     }
00411 
00412 
00413     // finish FreeMarkerArray:
00414     if (publishFreeMarkerArray) {
00415       for (unsigned i= 0; i < freeNodesVis.markers.size(); ++i) {
00416         double size = m_octreeContact->getNodeSize(i);
00417 
00418         freeNodesVis.markers[i].header.frame_id = m_worldFrameId;
00419         freeNodesVis.markers[i].header.stamp = rostime;
00420         freeNodesVis.markers[i].ns = m_worldFrameId;
00421         freeNodesVis.markers[i].id = i;
00422         freeNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
00423         freeNodesVis.markers[i].scale.x = size;
00424         freeNodesVis.markers[i].scale.y = size;
00425         freeNodesVis.markers[i].scale.z = size;
00426         freeNodesVis.markers[i].color = m_colorFree;
00427 
00428 
00429         if (freeNodesVis.markers[i].points.size() > 0) {
00430           freeNodesVis.markers[i].action = visualization_msgs::Marker::ADD;
00431         }
00432         else {
00433           freeNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
00434         }
00435       }
00436 
00437       m_fmarkerPub.publish(freeNodesVis);
00438     }
00439 
00440     // publish unknown grid as marker
00441     visualization_msgs::MarkerArray unknownNodesVis;
00442     unknownNodesVis.markers.resize(1);
00443 
00444     point3d_list unknownLeaves;
00445     double offset = m_offsetVisualizeUnknown;
00446     point3d pMin(m_occupancyMinX + offset, m_occupancyMinY + offset, m_occupancyMinZ + offset);
00447     point3d pMax(m_occupancyMaxX - offset, m_occupancyMaxY - offset, m_occupancyMaxZ - offset);
00448 
00449     m_octreeContact->getUnknownLeafCenters(unknownLeaves, pMin, pMax);
00450     pcl::PointCloud<pcl::PointXYZ> unknownCloud;
00451 
00452     for (point3d_list::iterator it = unknownLeaves.begin(); it != unknownLeaves.end(); it++) {
00453       float x = (*it).x();
00454       float y = (*it).y();
00455       float z = (*it).z();
00456       unknownCloud.push_back(pcl::PointXYZ(x, y, z));
00457 
00458       geometry_msgs::Point cubeCenter;
00459       cubeCenter.x = x;
00460       cubeCenter.y = y;
00461       cubeCenter.z = z;
00462       if (m_useHeightMap) {
00463         double minX, minY, minZ, maxX, maxY, maxZ;
00464         m_octreeContact->getMetricMin(minX, minY, minZ);
00465         m_octreeContact->getMetricMax(maxX, maxY, maxZ);
00466         double h = (1.0 - std::min(std::max((cubeCenter.z-minZ)/ (maxZ - minZ), 0.0), 1.0)) *m_colorFactor;
00467         unknownNodesVis.markers[0].colors.push_back(heightMapColor(h));
00468       }
00469       unknownNodesVis.markers[0].points.push_back(cubeCenter);
00470     }
00471 
00472     double size = m_octreeContact->getNodeSize(m_maxTreeDepth);
00473     unknownNodesVis.markers[0].header.frame_id = m_worldFrameId;
00474     unknownNodesVis.markers[0].header.stamp = rostime;
00475     unknownNodesVis.markers[0].ns = m_worldFrameId;
00476     unknownNodesVis.markers[0].id = 0;
00477     unknownNodesVis.markers[0].type = visualization_msgs::Marker::CUBE_LIST;
00478     unknownNodesVis.markers[0].scale.x = size;
00479     unknownNodesVis.markers[0].scale.y = size;
00480     unknownNodesVis.markers[0].scale.z = size;
00481     unknownNodesVis.markers[0].color = m_colorUnknown;
00482 
00483     if (unknownNodesVis.markers[0].points.size() > 0) {
00484       unknownNodesVis.markers[0].action = visualization_msgs::Marker::ADD;
00485     }
00486     else {
00487       unknownNodesVis.markers[0].action = visualization_msgs::Marker::DELETE;
00488     }
00489     m_umarkerPub.publish(unknownNodesVis);
00490 
00491     // publish unknown grid as pointcloud
00492     sensor_msgs::PointCloud2 unknownRosCloud;
00493     pcl::toROSMsg (unknownCloud, unknownRosCloud);
00494     unknownRosCloud.header.frame_id = m_worldFrameId;
00495     unknownRosCloud.header.stamp = rostime;
00496     m_unknownPointCloudPub.publish(unknownRosCloud);
00497 
00498     // finish pointcloud:
00499     if (publishPointCloud) {
00500       sensor_msgs::PointCloud2 cloud;
00501       pcl::toROSMsg (pclCloud, cloud);
00502       cloud.header.frame_id = m_worldFrameId;
00503       cloud.header.stamp = rostime;
00504       m_pointCloudPub.publish(cloud);
00505     }
00506 
00507     if (publishBinaryMap) {
00508       publishBinaryOctoMap(rostime);
00509     }
00510 
00511     if (publishFullMap) {
00512       publishFullOctoMap(rostime);
00513     }
00514 
00515     double totalElapsed = (ros::WallTime::now() - startTime).toSec();
00516     ROS_DEBUG("Map publishing in OctomapServer took %f sec", totalElapsed);
00517   }
00518 
00519   void OctomapServerContact::onInit(void)
00520   {
00521     DiagnosticNodelet::onInit();
00522     onInitPostProcess();
00523   }
00524 }
00525 
00526 #include <pluginlib/class_list_macros.h>
00527 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::OctomapServerContact, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:50