TrackingOctomapServer.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, D. Kuhner, P. Ruchti, University of Freiburg
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the University of Freiburg nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <octomap_server/TrackingOctomapServer.h>
00031 #include <string>
00032 
00033 using namespace octomap;
00034 
00035 namespace octomap_server {
00036 
00037 TrackingOctomapServer::TrackingOctomapServer(const std::string& filename) :
00038             OctomapServer()
00039 {
00040   //read tree if necessary
00041   if (filename != "") {
00042     if (m_octree->readBinary(filename)) {
00043       ROS_INFO("Octomap file %s loaded (%zu nodes).", filename.c_str(), m_octree->size());
00044       m_treeDepth = m_octree->getTreeDepth();
00045       m_res = m_octree->getResolution();
00046       m_gridmap.info.resolution = m_res;
00047 
00048       publishAll();
00049     } else {
00050       ROS_ERROR("Could not open requested file %s, exiting.", filename.c_str());
00051       exit(-1);
00052     }
00053   }
00054 
00055   ros::NodeHandle private_nh("~");
00056 
00057   std::string changeSetTopic = "changes";
00058 
00059   private_nh.param("topic_changes", changeSetTopic, changeSetTopic);
00060   private_nh.param("track_changes", track_changes, false);
00061   private_nh.param("listen_changes", listen_changes, false);
00062 
00063   if (track_changes && listen_changes) {
00064     ROS_WARN("OctoMapServer: It might not be useful to publish changes and at the same time listen to them."
00065         "Setting 'track_changes' to false!");
00066     track_changes = false;
00067   }
00068 
00069   if (track_changes) {
00070     ROS_INFO("starting server");
00071     pubChangeSet = private_nh.advertise<sensor_msgs::PointCloud2>(
00072         changeSetTopic, 1);
00073     m_octree->enableChangeDetection(true);
00074   }
00075 
00076   if (listen_changes) {
00077     ROS_INFO("starting client");
00078     subChangeSet = private_nh.subscribe(changeSetTopic, 1,
00079                                         &TrackingOctomapServer::trackCallback, this);
00080   }
00081 }
00082 
00083 TrackingOctomapServer::~TrackingOctomapServer() {
00084 }
00085 
00086 void TrackingOctomapServer::insertScan(const tf::Point & sensorOrigin, const PCLPointCloud & ground, const PCLPointCloud & nonground) {
00087   OctomapServer::insertScan(sensorOrigin, ground, nonground);
00088 
00089   if (track_changes) {
00090     trackChanges();
00091   }
00092 }
00093 
00094 void TrackingOctomapServer::trackChanges() {
00095   KeyBoolMap::const_iterator startPnt = m_octree->changedKeysBegin();
00096   KeyBoolMap::const_iterator endPnt = m_octree->changedKeysEnd();
00097 
00098   pcl::PointCloud<pcl::PointXYZI> changedCells = pcl::PointCloud<pcl::PointXYZI>();
00099 
00100   int c = 0;
00101   for (KeyBoolMap::const_iterator iter = startPnt; iter != endPnt; ++iter) {
00102     c++;
00103     OcTreeNode* node = m_octree->search(iter->first);
00104 
00105     bool occupied = m_octree->isNodeOccupied(node);
00106 
00107     pcl::PointXYZI pnt;
00108     pnt.x = iter->first.k[0];
00109     pnt.y = iter->first.k[1];
00110     pnt.z = iter->first.k[2];
00111 
00112     if (occupied) {
00113       pnt.intensity = 1000;
00114     }
00115     else {
00116       pnt.intensity = -1000;
00117     }
00118 
00119     changedCells.push_back(pnt);
00120   }
00121 
00122   sensor_msgs::PointCloud2 changed;
00123   pcl::toROSMsg(changedCells, changed);
00124   changed.header.frame_id = "/talker/changes";
00125   changed.header.stamp = ros::Time().now();
00126   pubChangeSet.publish(changed);
00127   ROS_DEBUG("[server] sending %d changed entries", (int)changedCells.size());
00128 
00129   m_octree->resetChangeDetection();
00130   ROS_DEBUG("[server] octomap size after updating: %d", (int)m_octree->calcNumNodes());
00131 }
00132 
00133 void TrackingOctomapServer::trackCallback(sensor_msgs::PointCloud2Ptr cloud) {
00134   pcl::PointCloud<pcl::PointXYZI> cells;
00135   pcl::fromROSMsg(*cloud, cells);
00136   ROS_DEBUG("[client] size of newly occupied cloud: %i", (int)cells.points.size());
00137 
00138   for (size_t i = 0; i < cells.points.size(); i++) {
00139     pcl::PointXYZI& pnt = cells.points[i];
00140     m_octree->updateNode(OcTreeKey(pnt.x, pnt.y, pnt.z), pnt.intensity, false);
00141   }
00142 
00143   m_octree->updateInnerOccupancy();
00144   ROS_DEBUG("[client] octomap size after updating: %d", (int)m_octree->calcNumNodes());
00145 }
00146 
00147 } /* namespace octomap */


octomap_server
Author(s): Armin Hornung
autogenerated on Thu Aug 27 2015 14:14:07