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   std::string changeIdFrame = "/talker/changes";
00059 
00060   private_nh.param("topic_changes", changeSetTopic, changeSetTopic);
00061   private_nh.param("change_id_frame", change_id_frame, changeIdFrame);
00062   private_nh.param("track_changes", track_changes, false);
00063   private_nh.param("listen_changes", listen_changes, false);
00064   private_nh.param("min_change_pub", min_change_pub, 0);
00065 
00066   if (track_changes && listen_changes) {
00067     ROS_WARN("OctoMapServer: It might not be useful to publish changes and at the same time listen to them."
00068         "Setting 'track_changes' to false!");
00069     track_changes = false;
00070   }
00071 
00072   if (track_changes) {
00073     ROS_INFO("starting server");
00074     pubChangeSet = private_nh.advertise<sensor_msgs::PointCloud2>(
00075         changeSetTopic, 1);
00076     m_octree->enableChangeDetection(true);
00077   }
00078 
00079   if (listen_changes) {
00080     ROS_INFO("starting client");
00081     subChangeSet = private_nh.subscribe(changeSetTopic, 1,
00082                                         &TrackingOctomapServer::trackCallback, this);
00083   }
00084 }
00085 
00086 TrackingOctomapServer::~TrackingOctomapServer() {
00087 }
00088 
00089 void TrackingOctomapServer::insertScan(const tf::Point & sensorOrigin, const PCLPointCloud & ground, const PCLPointCloud & nonground) {
00090   OctomapServer::insertScan(sensorOrigin, ground, nonground);
00091 
00092   if (track_changes) {
00093     trackChanges();
00094   }
00095 }
00096 
00097 void TrackingOctomapServer::trackChanges() {
00098   KeyBoolMap::const_iterator startPnt = m_octree->changedKeysBegin();
00099   KeyBoolMap::const_iterator endPnt = m_octree->changedKeysEnd();
00100 
00101   pcl::PointCloud<pcl::PointXYZI> changedCells = pcl::PointCloud<pcl::PointXYZI>();
00102 
00103   int c = 0;
00104   for (KeyBoolMap::const_iterator iter = startPnt; iter != endPnt; ++iter) {
00105     ++c;
00106     OcTreeNode* node = m_octree->search(iter->first);
00107 
00108     bool occupied = m_octree->isNodeOccupied(node);
00109 
00110     point3d center = m_octree->keyToCoord(iter->first);
00111 
00112     pcl::PointXYZI pnt;
00113     pnt.x = center(0);
00114     pnt.y = center(1);
00115     pnt.z = center(2);
00116 
00117     if (occupied) {
00118       pnt.intensity = 1000;
00119     }
00120     else {
00121       pnt.intensity = -1000;
00122     }
00123 
00124     changedCells.push_back(pnt);
00125   }
00126 
00127   if (c > min_change_pub)
00128   {
00129     sensor_msgs::PointCloud2 changed;
00130     pcl::toROSMsg(changedCells, changed);
00131     changed.header.frame_id = change_id_frame;
00132     changed.header.stamp = ros::Time().now();
00133     pubChangeSet.publish(changed);
00134     ROS_DEBUG("[server] sending %d changed entries", (int)changedCells.size());
00135 
00136     m_octree->resetChangeDetection();
00137     ROS_DEBUG("[server] octomap size after updating: %d", (int)m_octree->calcNumNodes());
00138   }
00139 }
00140 
00141 void TrackingOctomapServer::trackCallback(sensor_msgs::PointCloud2Ptr cloud) {
00142   pcl::PointCloud<pcl::PointXYZI> cells;
00143   pcl::fromROSMsg(*cloud, cells);
00144   ROS_DEBUG("[client] size of newly occupied cloud: %i", (int)cells.points.size());
00145 
00146   for (size_t i = 0; i < cells.points.size(); i++) {
00147     pcl::PointXYZI& pnt = cells.points[i];
00148     m_octree->updateNode(m_octree->coordToKey(pnt.x, pnt.y, pnt.z), pnt.intensity, false);
00149   }
00150 
00151   m_octree->updateInnerOccupancy();
00152   ROS_DEBUG("[client] octomap size after updating: %d", (int)m_octree->calcNumNodes());
00153 }
00154 
00155 } /* namespace octomap */


octomap_server
Author(s): Armin Hornung
autogenerated on Sat Jun 8 2019 19:23:48