relay_nodelet.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2014, JSK Lab
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 Willow Garage 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 #include <pluginlib/class_list_macros.h>
00035 #include "jsk_topic_tools/relay_nodelet.h"
00036 
00037 namespace jsk_topic_tools
00038 {
00039 
00040   void Relay::onInit()
00041   {
00042     advertised_ = false;
00043     subscribing_ = false;
00044     pnh_ = getPrivateNodeHandle();
00045     sub_ = pnh_.subscribe<topic_tools::ShapeShifter>("input", 1,
00046                                                      &Relay::inputCallback, this, th_);
00047   }
00048   
00049   void Relay::inputCallback(const boost::shared_ptr<topic_tools::ShapeShifter const>& msg)
00050   {
00051     boost::mutex::scoped_lock lock(mutex_);
00052     if (!advertised_) {
00053       // this block is called only once
00054       // in order to advertise topic.
00055       // we need to subscribe at least one message
00056       // in order to detect message definition.
00057       ros::SubscriberStatusCallback connect_cb
00058         = boost::bind( &Relay::connectCb, this);
00059       ros::SubscriberStatusCallback disconnect_cb
00060         = boost::bind( &Relay::disconnectCb, this);
00061       ros::AdvertiseOptions opts("output", 1,
00062                                  msg->getMD5Sum(),
00063                                  msg->getDataType(),
00064                                  msg->getMessageDefinition(),
00065                                  connect_cb,
00066                                  disconnect_cb);
00067       opts.latch = false;
00068       pub_ = pnh_.advertise(opts);
00069       advertised_ = true;
00070       // shutdown subscriber
00071       sub_.shutdown();
00072     }
00073     else if (pub_.getNumSubscribers() > 0) {
00074       pub_.publish(msg);
00075     }
00076   }
00077 
00078   void Relay::connectCb()
00079   {
00080     boost::mutex::scoped_lock lock(mutex_);
00081     NODELET_DEBUG("connectCB");
00082     if (advertised_) {
00083       if (pub_.getNumSubscribers() > 0) {
00084         if (!subscribing_) {
00085           NODELET_DEBUG("suscribe");
00086           sub_ = pnh_.subscribe<topic_tools::ShapeShifter>("input", 1,
00087                                                            &Relay::inputCallback, this, th_);
00088           subscribing_ = true;
00089         }
00090       }
00091     }
00092   }
00093 
00094   void Relay::disconnectCb()
00095   {
00096     boost::mutex::scoped_lock lock(mutex_);
00097     NODELET_DEBUG("disconnectCb");
00098     if (advertised_) {
00099       if (pub_.getNumSubscribers() == 0) {
00100         if (subscribing_) {
00101           NODELET_DEBUG("disconnect");
00102           sub_.shutdown();
00103           subscribing_ = false;
00104         }
00105       }
00106     }
00107   }
00108   
00109 }
00110 
00111 typedef jsk_topic_tools::Relay Relay;
00112 PLUGINLIB_EXPORT_CLASS(Relay, nodelet::Nodelet)


jsk_topic_tools
Author(s): Kei Okada , Yusuke Furuta
autogenerated on Sun Jan 25 2015 12:38:34