mux_nodelet.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, 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/mux_nodelet.h"
00036 #include <std_msgs/String.h>
00037 #include "jsk_topic_tools/rosparam_utils.h"
00038 
00039 namespace jsk_topic_tools
00040 {
00041 
00042   const static std::string g_none_topic = "__none";
00043   
00044   void MUX::onInit()
00045   {
00046     advertised_ = false;
00047     pnh_ = getPrivateNodeHandle();
00048     readVectorParameter(pnh_, "topics", topics_);
00049     if (topics_.size() < 1) {
00050       NODELET_FATAL("need to specify at least one topic in ~topics");
00051       return;
00052     }
00053     pub_selected_ = pnh_.advertise<std_msgs::String>("selected", 1, true);
00054     // in original mux node, it subscribes all the topics first, however
00055     // in our version, we never subscribe topic which are not selected.
00056     // ros::SubscriberStatusCallback connect_cb
00057     //     = boost::bind( &Mux::connectCb, this);
00058     selected_topic_ = topics_[0];
00059     subscribeSelectedTopic();
00060     // service advertise: _select, select, add, list, delete
00061     ss_select_ = pnh_.advertiseService("select", &MUX::selectTopicCallback, this);
00062     ss_add_ = pnh_.advertiseService("add", &MUX::addTopicCallback, this);
00063     ss_list_ = pnh_.advertiseService("list_topics", &MUX::listTopicCallback, this);
00064     ss_del_ = pnh_.advertiseService("delete", &MUX::deleteTopicCallback, this);
00065     
00066   }
00067 
00068   void MUX::connectCb(const ros::SingleSubscriberPublisher& pub)
00069   {
00070     // new subscriber come or
00071     if (pub_.getNumSubscribers() > 0) {
00072       // subscribe topic again
00073       if (!subscribing_) {
00074         //NODELET_INFO("subscribe");
00075         sub_.reset(new ros::Subscriber(
00076                      pnh_.subscribe<topic_tools::ShapeShifter>(
00077                        selected_topic_, 10,
00078                        &MUX::inputCallback, this, th_)));
00079         subscribing_ = true;
00080       }
00081     }
00082     else {
00083       if (subscribing_) {
00084         //NODELET_INFO("unsubscribe");
00085         sub_->shutdown();
00086         subscribing_ = false;
00087       }
00088     }
00089   }
00090 
00091   
00092   
00093   bool MUX::selectTopicCallback(topic_tools::MuxSelect::Request  &req,
00094                                 topic_tools::MuxSelect::Response &res)
00095   {
00096     res.prev_topic = selected_topic_;
00097     if (selected_topic_ != g_none_topic) {
00098       //NODELET_INFO("unsubscribe");
00099       sub_->shutdown();            // unsubscribe first
00100     }
00101 
00102     if (req.topic == g_none_topic) { // same topic
00103       selected_topic_ = g_none_topic;
00104       return true;
00105     }
00106     for (size_t i = 0; i < topics_.size(); i++) {
00107       if (pnh_.resolveName(topics_[i]) == pnh_.resolveName(req.topic)) {
00108         // subscribe the topic
00109         selected_topic_ = topics_[i];
00110         subscribeSelectedTopic();
00111         return true;
00112       }
00113     }
00114 
00115     NODELET_WARN("%s is not provided in topic list", req.topic.c_str());
00116     return false;
00117   }
00118 
00119   bool MUX::addTopicCallback(topic_tools::MuxAdd::Request& req,
00120                         topic_tools::MuxAdd::Response& res)
00121   {
00122     NODELET_INFO("trying to add %s to mux", req.topic.c_str());
00123     if (req.topic == g_none_topic) {
00124       NODELET_WARN("failed to add topic %s to mux, because it's reserved for special use",
00125                    req.topic.c_str());
00126       return false;
00127     }
00128     
00129     for (size_t i = 0; i < topics_.size(); i++) {
00130       if (pnh_.resolveName(topics_[i]) == pnh_.resolveName(req.topic)) {
00131         NODELET_WARN("tried to add a topic that mux was already listening to: [%s]", 
00132                      topics_[i].c_str());
00133         return false;
00134       }
00135     }
00136 
00137     // in original mux, it subscribes the topic immediately after adds topic.
00138     // in this version, we postpone the subscription until selected.
00139     
00140     topics_.push_back(ros::names::resolve(req.topic));
00141     return true;
00142   }
00143 
00144   bool MUX::deleteTopicCallback(topic_tools::MuxDelete::Request& req,
00145                                 topic_tools::MuxDelete::Response& res)
00146   {
00147     // cannot delete the topic now selected
00148     for (size_t i = 0; i < topics_.size(); i++) {
00149       if (pnh_.resolveName(topics_[i]) == pnh_.resolveName(req.topic)) {
00150         if (pnh_.resolveName(req.topic) == pnh_.resolveName(selected_topic_)) {
00151           NODELET_WARN("tried to delete currently selected topic %s from mux",
00152                        req.topic.c_str());
00153           return false;
00154         }
00155         topics_.erase(topics_.begin() + i);
00156         return true;
00157       }
00158     }
00159     NODELET_WARN("cannot find the topics %s in the list of mux",
00160                  req.topic.c_str());
00161     return false;
00162   }
00163 
00164   bool MUX::listTopicCallback(topic_tools::MuxList::Request& req,
00165                               topic_tools::MuxList::Response& res)
00166   {
00167     for (size_t i = 0; i < topics_.size(); i++) {
00168       res.topics.push_back(pnh_.resolveName(topics_[i]));
00169     }
00170     return true;
00171   }
00172   
00173   void MUX::subscribeSelectedTopic()
00174   {
00175     // subscribe topic in order to "advertise"
00176     advertised_ = false;
00177     subscribing_ = false;
00178     // assume that selected_topic_ is already set correctly
00179     if (selected_topic_ == g_none_topic) {
00180       NODELET_WARN("none topic is selected");
00181       return;
00182     }
00183     //NODELET_INFO("subscribe");
00184     sub_.reset(new ros::Subscriber(
00185                  pnh_.subscribe<topic_tools::ShapeShifter>(
00186                    selected_topic_, 10,
00187                    &MUX::inputCallback, this, th_)));
00188     std_msgs::String msg;
00189     msg.data = selected_topic_;
00190     pub_selected_.publish(msg);
00191   }
00192 
00193   void MUX::inputCallback(const boost::shared_ptr<topic_tools::ShapeShifter const>& msg)
00194   {
00195     if (!advertised_) {         // first time
00196       ros::SubscriberStatusCallback connect_cb
00197         = boost::bind(&MUX::connectCb, this, _1);
00198       ros::AdvertiseOptions opts("output", 1,
00199                                  msg->getMD5Sum(),
00200                                  msg->getDataType(),
00201                                  msg->getMessageDefinition(),
00202                                  connect_cb,
00203                                  connect_cb);
00204       pub_ = pnh_.advertise(opts);
00205       advertised_ = true;
00206       sub_->shutdown();
00207       //NODELET_INFO("unsubscribe");
00208     }
00209     pub_.publish(msg);
00210   }
00211   
00212 }
00213 
00214 typedef jsk_topic_tools::MUX MUX;
00215 PLUGINLIB_EXPORT_CLASS(MUX, nodelet::Nodelet)
00216 


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