nodelet_lazy.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2014-2016, JSK Lab, University of Tokyo.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/o2r other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the JSK Lab nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ryohei Ueda, Kentaro Wada <www.kentaro.wada@gmail.com>
00036  */
00037 
00038 #ifndef NODELET_LAZY_H_
00039 #define NODELET_LAZY_H_
00040 
00041 #include <ros/ros.h>
00042 #include <nodelet/nodelet.h>
00043 #include <boost/thread.hpp>
00044 #include <string>
00045 #include <vector>
00046 
00047 namespace nodelet_topic_tools
00048 {
00049 
00053 enum ConnectionStatus
00054 {
00055   NOT_INITIALIZED,
00056   NOT_SUBSCRIBED,
00057   SUBSCRIBED
00058 };
00059 
00070 class NodeletLazy: public nodelet::Nodelet
00071 {
00072 public:
00073   NodeletLazy() {}
00074 
00075 protected:
00080   virtual void onInit()
00081   {
00082     connection_status_ = NOT_SUBSCRIBED;
00083     bool use_multithread;
00084     ros::param::param<bool>("~use_multithread_callback", use_multithread, true);
00085     if (use_multithread)
00086     {
00087       NODELET_DEBUG("Using multithread callback");
00088       nh_.reset (new ros::NodeHandle(getMTNodeHandle()));
00089       pnh_.reset (new ros::NodeHandle(getMTPrivateNodeHandle()));
00090     }
00091     else
00092     {
00093       NODELET_DEBUG("Using singlethread callback");
00094       nh_.reset(new ros::NodeHandle(getNodeHandle()));
00095       pnh_.reset(new ros::NodeHandle(getPrivateNodeHandle()));
00096     }
00097     // option to use lazy transport
00098     pnh_->param("lazy", lazy_, true);
00099     // option for logging about being subscribed
00100     pnh_->param("verbose_connection", verbose_connection_, false);
00101     if (!verbose_connection_)
00102     {
00103       nh_->param("verbose_connection", verbose_connection_, false);
00104     }
00105     // timer to warn when no connection in the specified seconds
00106     ever_subscribed_ = false;
00107     double duration_to_warn_no_connection;
00108     pnh_->param("duration_to_warn_no_connection",
00109                 duration_to_warn_no_connection, 5.0);
00110     if (duration_to_warn_no_connection > 0)
00111     {
00112       timer_ever_subscribed_ = nh_->createWallTimer(
00113         ros::WallDuration(duration_to_warn_no_connection),
00114         &NodeletLazy::warnNeverSubscribedCallback,
00115         this,
00116         /*oneshot=*/true);
00117     }
00118   }
00119 
00125   virtual void onInitPostProcess()
00126   {
00127     if (!lazy_)
00128     {
00129       boost::mutex::scoped_lock lock(connection_mutex_);
00130       subscribe();
00131       ever_subscribed_ = true;
00132     }
00133   }
00134 
00138   virtual void connectionCallback(const ros::SingleSubscriberPublisher& pub)
00139   {
00140     if (verbose_connection_)
00141     {
00142       NODELET_INFO("New connection or disconnection is detected");
00143     }
00144     if (lazy_)
00145     {
00146       boost::mutex::scoped_lock lock(connection_mutex_);
00147       for (size_t i = 0; i < publishers_.size(); i++)
00148       {
00149         ros::Publisher pub = publishers_[i];
00150         if (pub.getNumSubscribers() > 0)
00151         {
00152           if (connection_status_ != SUBSCRIBED)
00153           {
00154             if (verbose_connection_)
00155             {
00156               NODELET_INFO("Subscribe input topics");
00157             }
00158             subscribe();
00159             connection_status_ = SUBSCRIBED;
00160           }
00161           if (!ever_subscribed_)
00162           {
00163             ever_subscribed_ = true;
00164           }
00165           return;
00166         }
00167       }
00168       if (connection_status_ == SUBSCRIBED)
00169       {
00170         if (verbose_connection_)
00171         {
00172           NODELET_INFO("Unsubscribe input topics");
00173         }
00174         unsubscribe();
00175         connection_status_ = NOT_SUBSCRIBED;
00176       }
00177     }
00178   }
00179 
00184   virtual void warnNeverSubscribedCallback(const ros::WallTimerEvent& event)
00185   {
00186     if (!ever_subscribed_)
00187     {
00188       NODELET_WARN("This node/nodelet subscribes topics only when subscribed.");
00189     }
00190   }
00191 
00192 
00198   virtual void subscribe() = 0;
00199 
00205   virtual void unsubscribe() = 0;
00206 
00218   template<class T> ros::Publisher
00219   advertise(ros::NodeHandle& nh,
00220             std::string topic, int queue_size, bool latch=false)
00221   {
00222     boost::mutex::scoped_lock lock(connection_mutex_);
00223     ros::SubscriberStatusCallback connect_cb
00224       = boost::bind(&NodeletLazy::connectionCallback, this, _1);
00225     ros::SubscriberStatusCallback disconnect_cb
00226       = boost::bind(&NodeletLazy::connectionCallback, this, _1);
00227     ros::Publisher pub = nh.advertise<T>(topic, queue_size,
00228                                           connect_cb,
00229                                           disconnect_cb,
00230                                           ros::VoidConstPtr(),
00231                                           latch);
00232     publishers_.push_back(pub);
00233     return pub;
00234   }
00235 
00240   boost::mutex connection_mutex_;
00241 
00245   boost::shared_ptr<ros::NodeHandle> nh_;
00246 
00250   boost::shared_ptr<ros::NodeHandle> pnh_;
00251 
00255   std::vector<ros::Publisher> publishers_;
00256 
00260   ros::WallTimer timer_ever_subscribed_;
00261 
00266   bool ever_subscribed_;
00267 
00272   bool lazy_;
00273 
00277   ConnectionStatus connection_status_;
00278 
00282   bool verbose_connection_;
00283 
00284 private:
00285 };
00286 
00287 }  // namespace nodelet_topic_tools
00288 
00289 #endif  // NODELET_LAZY_H_


nodelet_topic_tools
Author(s): Radu Bogdan Rusu, Tully Foote
autogenerated on Sun Aug 6 2017 02:24:03