LaserPublisher.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Willow Garage, Inc.
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 Willow Garage, Inc. 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 /* 
00031  * Author: Chad Rockey
00032  */
00033 
00034 #include <laser_proc/LaserPublisher.h>
00035 
00036 namespace laser_proc {
00037 
00038 typedef sensor_msgs::LaserScanPtr (*PublishFunction)(const sensor_msgs::MultiEchoLaserScan& msg);
00039 
00040 struct LaserPublisher::Impl
00041 {
00042   Impl()
00043     : unadvertised_(false)
00044   {
00045   }
00046 
00047   ~Impl()
00048   {
00049     shutdown();
00050   }
00051 
00052   bool isValid() const
00053   {
00054     return !unadvertised_;
00055   }
00056   
00057   void shutdown()
00058   {
00059     if (!unadvertised_) {
00060       unadvertised_ = true;
00061       for(size_t i = 0; i < pubs_.size(); i++){
00062         pubs_[i].shutdown();
00063       }
00064     }
00065   }
00066 
00067   ros::Publisher echo_pub_;
00068   std::vector<ros::Publisher> pubs_;
00069   std::vector<PublishFunction> functs_;
00070   bool unadvertised_;
00071 };
00072 
00074 LaserPublisher::LaserPublisher(ros::NodeHandle& nh, uint32_t queue_size,
00075                                  const ros::SubscriberStatusCallback& connect_cb,
00076                                  const ros::SubscriberStatusCallback& disconnect_cb,
00077                                  const ros::VoidPtr& tracked_object, bool latch, bool publish_echoes)
00078   : impl_(new Impl)
00079 {
00080   if(publish_echoes){
00081     impl_->echo_pub_ = nh.advertise<sensor_msgs::MultiEchoLaserScan>("echoes", queue_size, connect_cb, disconnect_cb, tracked_object, latch);
00082   }
00083   impl_->pubs_.push_back(nh.advertise<sensor_msgs::LaserScan>("first", queue_size, connect_cb, disconnect_cb, tracked_object, latch));
00084   impl_->functs_.push_back(laser_proc::LaserProc::getFirstScan);
00085   impl_->pubs_.push_back(nh.advertise<sensor_msgs::LaserScan>("last", queue_size, connect_cb, disconnect_cb, tracked_object, latch));
00086   impl_->functs_.push_back(laser_proc::LaserProc::getLastScan);
00087   impl_->pubs_.push_back(nh.advertise<sensor_msgs::LaserScan>("most_intense", queue_size, connect_cb, disconnect_cb, tracked_object, latch));
00088   impl_->functs_.push_back(laser_proc::LaserProc::getMostIntenseScan);
00089 }
00090 
00091 uint32_t LaserPublisher::getNumSubscribers() const
00092 {
00093   if (impl_ && impl_->isValid()){
00094     uint32_t num = impl_->echo_pub_.getNumSubscribers();
00095     for(size_t i = 0; i < impl_->pubs_.size(); i++){
00096       num = num + impl_->pubs_[i].getNumSubscribers();
00097     }
00098     return num;
00099   }
00100   return 0;
00101 }
00102 
00103 std::vector<std::string> LaserPublisher::getTopics() const
00104 {
00105   std::vector<std::string> topics;
00106   topics.push_back(impl_->echo_pub_.getTopic());
00107   if (impl_ && impl_->isValid()){
00108     for(size_t i = 0; i < impl_->pubs_.size(); i++){
00109       topics.push_back(impl_->pubs_[i].getTopic());
00110     }
00111   }
00112   return topics;
00113 }
00114 
00115 void LaserPublisher::publish(const sensor_msgs::MultiEchoLaserScan& msg) const
00116 {
00117   if (!impl_ || !impl_->isValid()) {
00118     ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::LaserPublisher");
00119     return;
00120   }
00121 
00122   // Publish original MultiEchoLaserScan
00123   if(impl_->echo_pub_){
00124     if(impl_->echo_pub_.getNumSubscribers() > 0){
00125       impl_->echo_pub_.publish(msg);
00126     }
00127   }
00128   
00129   // If needed, publish LaserScans
00130   for(size_t i = 0; i < impl_->pubs_.size(); i++){
00131     if(impl_->pubs_[i].getNumSubscribers() > 0){
00132        try{
00133         impl_->pubs_[i].publish(impl_->functs_[i](msg));
00134       } catch(std::runtime_error& e){
00135         ROS_ERROR_THROTTLE(1.0, "Could not publish to topic %s: %s", impl_->pubs_[i].getTopic().c_str(), e.what());
00136       }
00137     }
00138   }
00139 }
00140 
00141 void LaserPublisher::publish(const sensor_msgs::MultiEchoLaserScanConstPtr& msg) const
00142 {
00143   if (!impl_ || !impl_->isValid()) {
00144     ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::LaserPublisher");
00145     return;
00146   }
00147 
00148   // Publish original MultiEchoLaserScan
00149   if(impl_->echo_pub_){
00150     impl_->echo_pub_.publish(msg);
00151   }
00152   
00153   // If needed, publish LaserScans
00154   for(size_t i = 0; i < impl_->pubs_.size(); i++){
00155     if(impl_->pubs_[i].getNumSubscribers() > 0){
00156        try{
00157         impl_->pubs_[i].publish(impl_->functs_[i](*msg));
00158       } catch(std::runtime_error& e){
00159         ROS_ERROR_THROTTLE(1.0, "Could not publish to topic %s: %s", impl_->pubs_[i].getTopic().c_str(), e.what());
00160       }
00161     }
00162   }
00163 }
00164 
00165 void LaserPublisher::shutdown()
00166 {
00167   if (impl_) {
00168     impl_->shutdown();
00169     impl_.reset();
00170   }
00171 }
00172 
00173 LaserPublisher::operator void*() const
00174 {
00175   return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
00176 }
00177 
00178 } //namespace laser_proc
00179 


laser_proc
Author(s): Chad Rockey
autogenerated on Thu Aug 27 2015 13:47:25