octree_stamped_pa_node.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002 *                                                                             *
00003 * octree_stamped_pa_node.cpp                                                  *
00004 * ==========================                                                  *
00005 *                                                                             *
00006 *******************************************************************************
00007 *                                                                             *
00008 * github repository                                                           *
00009 *   https://github.com/TUC-ProAut/ros_octomap                                 *
00010 *                                                                             *
00011 * Chair of Automation Technology, Technische Universität Chemnitz             *
00012 *   https://www.tu-chemnitz.de/etit/proaut                                    *
00013 *                                                                             *
00014 *******************************************************************************
00015 *                                                                             *
00016 * New BSD License                                                             *
00017 *                                                                             *
00018 * Copyright (c) 2015-2018, Peter Weissig, Technische Universität Chemnitz     *
00019 * All rights reserved.                                                        *
00020 *                                                                             *
00021 * Redistribution and use in source and binary forms, with or without          *
00022 * modification, are permitted provided that the following conditions are met: *
00023 *     * Redistributions of source code must retain the above copyright        *
00024 *       notice, this list of conditions and the following disclaimer.         *
00025 *     * Redistributions in binary form must reproduce the above copyright     *
00026 *       notice, this list of conditions and the following disclaimer in the   *
00027 *       documentation and/or other materials provided with the distribution.  *
00028 *     * Neither the name of the Technische Universität Chemnitz nor the       *
00029 *       names of its contributors may be used to endorse or promote products  *
00030 *       derived from this software without specific prior written permission. *
00031 *                                                                             *
00032 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" *
00033 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE   *
00034 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE  *
00035 * ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY      *
00036 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES  *
00037 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR          *
00038 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER  *
00039 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT          *
00040 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY   *
00041 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH *
00042 * DAMAGE.                                                                     *
00043 *                                                                             *
00044 ******************************************************************************/
00045 
00046 // local headers
00047 #include "octomap_pa/octree_stamped_pa_node.h"
00048 
00049 // ros headers
00050 #include <parameter_pa/parameter_pa_ros.h>
00051 
00052 // standard headers
00053 #include <string>
00054 
00055 //**************************[main]*********************************************
00056 int main(int argc, char **argv) {
00057 
00058     ros::init(argc, argv, "octree_stamped_pa_node");
00059     cOctreeStampedPaNode octomap;
00060 
00061     ros::spin();
00062 
00063     return 0;
00064 }
00065 
00066 //**************************[cOctreeStampedPaNode]*****************************
00067 cOctreeStampedPaNode::cOctreeStampedPaNode() : cOctreeStampedPaRos(0.1),
00068   tf_listener_(ros::Duration(20), true) {
00069 
00070     cParameterPaRos paramloader;
00071 
00072     paramloader.load("~/output_frame", rosparams_base_.output_frame_);
00073 
00074     // octomap parameter
00075     double temp;
00076     temp = 0.1 ;
00077     paramloader.load("~/map_resolution"    , temp); setResolution      (temp);
00078     temp = 0.5 ;
00079     paramloader.load("~/map_prob_threshold", temp); setOccupancyThres  (temp);
00080     temp = 0.12;
00081     paramloader.load("~/map_clamp_min"     , temp); setClampingThresMin(temp);
00082     temp = 0.97;
00083     paramloader.load("~/map_clamp_max"     , temp); setClampingThresMax(temp);
00084 
00085     // pointcloud insertion parameter
00086     paramloader.load("~/map_prob_hit"      , addparams_.map_prob_hit_      );
00087     paramloader.load("~/map_prob_miss"     , addparams_.map_prob_miss_     );
00088     paramloader.load("~/pcd_voxel_active"  , addparams_.pcd_voxel_active_  );
00089     paramloader.load("~/pcd_voxel_explicit", addparams_.pcd_voxel_explicit_);
00090     paramloader.load("~/pcd_voxel_explicit_relative_resolution",
00091       addparams_.pcd_voxel_explicit_relative_resolution_);
00092     paramloader.load("~/pcd_explicit_transform",
00093       addparams_.pcd_explicit_transform_);
00094 
00095     // degrading parameter
00096     paramloader.load("~/degrading_time"          , rosparams_.degrading_time_);
00097     paramloader.load("~/auto_degrading"          , rosparams_.auto_degrading_);
00098     paramloader.load("~/auto_degrading_intervall",
00099       rosparams_.auto_degrading_intervall_);
00100 
00101     // topics in
00102     paramloader.loadTopic("~/topic_in_cloud"    ,
00103       nodeparams_.topic_in_cloud_    );
00104     paramloader.loadTopic("~/topic_in_cloud_old",
00105       nodeparams_.topic_in_cloud_old_);
00106     paramloader.loadTopic("~/topic_in_laser"    ,
00107       nodeparams_.topic_in_laser_    );
00108 
00109     // topics out
00110     paramloader.loadTopic("~/topic_out_octomap"       ,
00111       nodeparams_.topic_out_octomap_);
00112     paramloader.loadTopic("~/topic_out_octomap_full"  ,
00113       nodeparams_.topic_out_octomap_full_);
00114     paramloader.loadTopic("~/topic_out_cloud_free"    ,
00115       nodeparams_.topic_out_cloud_free_);
00116     paramloader.loadTopic("~/topic_out_cloud_occupied",
00117       nodeparams_.topic_out_cloud_occupied_);
00118 
00119     // Subscriber for pointclouds
00120     if (nodeparams_.topic_in_cloud_ != "") {
00121         sub_cloud_ = nh_.subscribe<sensor_msgs::PointCloud2>(
00122           nodeparams_.topic_in_cloud_, 10,
00123           &cOctreeStampedPaNode::addPointcloudCallbackSub, this);
00124     }
00125 
00126     // Subscriber for pointclouds (old format)
00127     if (nodeparams_.topic_in_cloud_old_ != "") {
00128         sub_cloud_old_ = nh_.subscribe<sensor_msgs::PointCloud>(
00129           nodeparams_.topic_in_cloud_old_, 10,
00130           &cOctreeStampedPaNode::addPointcloudOldCallbackSub, this);
00131     }
00132     // Subscriber for laserscans
00133     if (nodeparams_.topic_in_laser_ != "") {
00134         sub_laser_ = nh_.subscribe<sensor_msgs::LaserScan>(
00135           nodeparams_.topic_in_laser_, 10,
00136           &cOctreeStampedPaNode::addLaserCallbackSub, this);
00137     }
00138 
00139 
00140     // puplisher for the octomap (binary data)
00141     pub_octomap_        = nh_.advertise<octomap_msgs::Octomap>(
00142       nodeparams_.topic_out_octomap_, 10, true);
00143     // puplisher for the octomap (full data)
00144     pub_octomap_full_   = nh_.advertise<octomap_msgs::Octomap>(
00145       nodeparams_.topic_out_octomap_full_, 10, true);
00146 
00147     // puplisher for free voxels as pointcloud
00148     pub_cloud_free_     = nh_.advertise<sensor_msgs::PointCloud2>(
00149       nodeparams_.topic_out_cloud_free_, 10, true);
00150     // puplisher for occupied voxels as pointcloud
00151     pub_cloud_occupied_ = nh_.advertise<sensor_msgs::PointCloud2>(
00152       nodeparams_.topic_out_cloud_occupied_, 10, true);
00153 
00154 
00155     std::string str_service("~/");
00156     str_service = paramloader.resolveRessourcename(str_service);
00157     // service for clearing the octomap
00158     srv_clear_ = nh_.advertiseService(str_service + "clear",
00159       &cOctreeStampedPaNode::clearCallbackSrv, this);
00160     // service for receiving the size of the octomap
00161     srv_getsize_ = nh_.advertiseService(str_service + "getsize",
00162       &cOctreeStampedPaNode::getSizeCallbackSrv, this);
00163     // service for saving the octomap
00164     srv_save_ = nh_.advertiseService(str_service + "save",
00165       &cOctreeStampedPaNode::saveCallbackSrv, this);
00166     // service for loading a octomap
00167     srv_load_ = nh_.advertiseService(str_service + "load",
00168       &cOctreeStampedPaNode::loadCallbackSrv, this);
00169 
00170 
00171     // count number of inserted pointclouds
00172     count_cloud_     = 0;
00173     count_cloud_old_ = 0;
00174     count_laser_     = 0;
00175 }
00176 
00177 //**************************[~cOctreeStampedPaNode]****************************
00178 cOctreeStampedPaNode::~cOctreeStampedPaNode() {
00179 
00180 }
00181 
00182 //**************************[publishOctomap]***********************************
00183 void cOctreeStampedPaNode::publishOctomap() {
00184 
00185     if (pub_octomap_.getNumSubscribers() > 0) {
00186         pub_octomap_.publish(getOctomap());
00187     }
00188     if (pub_octomap_full_.getNumSubscribers() > 0) {
00189         pub_octomap_.publish(getOctomapFull());
00190     }
00191 
00192     if (pub_cloud_occupied_.getNumSubscribers() > 0) {
00193         pub_cloud_occupied_.publish(getOctomapPcd());
00194     }
00195     if (pub_cloud_free_.getNumSubscribers() > 0) {
00196         pub_cloud_free_.publish(getOctomapPcdFree());
00197     }
00198 }
00199 
00200 //**************************[addPointcloudCallbackSub]*************************
00201 void cOctreeStampedPaNode::addPointcloudCallbackSub(
00202   const sensor_msgs::PointCloud2ConstPtr &msg) {
00203 
00204     if (!updateTime(msg->header.stamp)) {
00205         tf_listener_.clear();
00206         return;
00207     }
00208 
00209     tf::StampedTransform transform;
00210 
00211     try {
00212         tf_listener_.waitForTransform(rosparams_base_.output_frame_,
00213           msg->header.frame_id, msg->header.stamp, ros::Duration(0.2));
00214         tf_listener_.lookupTransform(rosparams_base_.output_frame_,
00215           msg->header.frame_id, msg->header.stamp, transform);
00216     } catch (tf::TransformException &ex){
00217         ROS_WARN("%s",ex.what());
00218         return;
00219     }
00220 
00221     if (addCloud(msg, addparams_, transform)) {
00222         count_cloud_++;
00223         checkDegrading();
00224         publishOctomap();
00225     }
00226 }
00227 
00228 //**************************[addPointcloudCallbackSub]*************************
00229 void cOctreeStampedPaNode::addPointcloudOldCallbackSub(
00230   const sensor_msgs::PointCloudConstPtr &msg) {
00231 
00232     if (!updateTime(msg->header.stamp)) {
00233         tf_listener_.clear();
00234         return;
00235     }
00236 
00237     tf::StampedTransform transform;
00238 
00239     try {
00240         tf_listener_.waitForTransform(rosparams_base_.output_frame_,
00241           msg->header.frame_id, msg->header.stamp, ros::Duration(0.2));
00242         tf_listener_.lookupTransform(rosparams_base_.output_frame_,
00243           msg->header.frame_id, msg->header.stamp, transform);
00244     } catch (tf::TransformException &ex){
00245         ROS_WARN("%s",ex.what());
00246         return;
00247     }
00248 
00249     if (addCloud(msg, addparams_, transform)) {
00250         count_cloud_old_++;
00251         checkDegrading();
00252         publishOctomap();
00253     }
00254 }
00255 
00256 //**************************[addLaserCallbackSub]******************************
00257 void cOctreeStampedPaNode::addLaserCallbackSub(
00258   const sensor_msgs::LaserScanConstPtr &msg) {
00259 
00260     if (!updateTime(msg->header.stamp)) {
00261         tf_listener_.clear();
00262         return;
00263     }
00264 
00265     tf::StampedTransform transform;
00266 
00267     try {
00268         tf_listener_.waitForTransform(rosparams_base_.output_frame_,
00269           msg->header.frame_id, msg->header.stamp, ros::Duration(0.2));
00270         tf_listener_.lookupTransform(rosparams_base_.output_frame_,
00271           msg->header.frame_id, msg->header.stamp, transform);
00272     } catch (tf::TransformException &ex){
00273         ROS_WARN("%s",ex.what());
00274         return;
00275     }
00276 
00277     updateTime(msg->header.stamp);
00278     if (addCloud(msg, addparams_, transform)) {
00279         count_laser_++;
00280         checkDegrading();
00281         publishOctomap();
00282     }
00283 }
00284 
00285 
00286 //**************************[clearCallbackSrv]*********************************
00287 bool cOctreeStampedPaNode::clearCallbackSrv(std_srvs::Empty::Request  &req,
00288   std_srvs::Empty::Response &res) {
00289 
00290     ROS_INFO("cOctreeStampedPaNode::clear()");
00291 
00292     count_cloud_     = 0;
00293     count_cloud_old_ = 0;
00294     count_laser_     = 0;
00295 
00296     clear();
00297     tf_listener_.clear();
00298 
00299     return true;
00300 }
00301 
00302 //**************************[getSizeCallbackSrv]*******************************
00303 bool cOctreeStampedPaNode::getSizeCallbackSrv (
00304   octomap_pa::OctomapPaGetSize::Request  &req,
00305   octomap_pa::OctomapPaGetSize::Response &res) {
00306 
00307     ROS_INFO("cOctreeStampedPaNode::getsize()");
00308 
00309     res.size = size();
00310     res.memoryusage = (int64_t) memoryUsage();
00311 
00312     res.count_cloud     = count_cloud_    ;
00313     res.count_cloud_old = count_cloud_old_;
00314     res.count_laser     = count_laser_    ;
00315 
00316     return true;
00317 }
00318 
00319 //**************************[saveCallbackSrv]**********************************
00320 bool cOctreeStampedPaNode::saveCallbackSrv(
00321   octomap_pa::OctomapPaFileName::Request  &req,
00322   octomap_pa::OctomapPaFileName::Response &res) {
00323 
00324     ROS_INFO_STREAM("cOctreeStampedPaNode::save(" << req.filename << ")");
00325 
00326     std::string filename;
00327     filename = req.filename;
00328     cParameterPaRos par;
00329     par.replaceFindpack(filename);
00330 
00331     //res.ok = writeBinary(filename);
00332     res.ok = this->write(filename);
00333 
00334     return res.ok;
00335 }
00336 
00337 //**************************[loadCallbackSrv]**********************************
00338 bool cOctreeStampedPaNode::loadCallbackSrv(
00339   octomap_pa::OctomapPaFileName::Request  &req,
00340   octomap_pa::OctomapPaFileName::Response &res) {
00341 
00342     ROS_INFO_STREAM("cOctreeStampedPaNode::load(" << req.filename << ")");
00343 
00344     std::string filename;
00345     filename = req.filename;
00346     cParameterPaRos par;
00347     par.replaceFindpack(filename);
00348 
00349     // res.ok = readBinary(filename);
00350     res.ok = readFull(filename);
00351 
00352     if (res.ok) { publishOctomap(); }
00353     return res.ok;
00354 }


octomap_pa
Author(s):
autogenerated on Thu Jun 6 2019 17:53:30