loader.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, 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 #include <nodelet/loader.h>
00031 #include <nodelet/nodelet.h>
00032 #include <nodelet/detail/callback_queue.h>
00033 #include <nodelet/detail/callback_queue_manager.h>
00034 #include <pluginlib/class_loader.h>
00035 #include <bondcpp/bond.h>
00036 
00037 #include <ros/ros.h>
00038 #include <ros/callback_queue.h>
00039 #include <nodelet/NodeletLoad.h>
00040 #include <nodelet/NodeletList.h>
00041 #include <nodelet/NodeletUnload.h>
00042 
00043 #include <boost/ptr_container/ptr_map.hpp>
00044 #include <boost/utility.hpp>
00045 
00046 /*
00047 Between Loader, Nodelet, CallbackQueue and CallbackQueueManager, who owns what?
00048 
00049 Loader contains the CallbackQueueManager. Loader and CallbackQueueManager share
00050 ownership of the CallbackQueues. Loader is primary owner of the Nodelets, but
00051 each CallbackQueue has weak ownership of its associated Nodelet.
00052 
00053 Loader ensures that the CallbackQueues associated with a Nodelet outlive that
00054 Nodelet. CallbackQueueManager ensures that CallbackQueues continue to survive as
00055 long as they have pending callbacks. 
00056 
00057 CallbackQueue holds a weak_ptr to its associated Nodelet, which it attempts to
00058 lock before invoking any callback. So any lingering callbacks processed after a
00059 Nodelet is destroyed are safely discarded, and then the CallbackQueue expires.
00060 
00061 When Loader unloads a Nodelet, it calls CallbackQueueManager::removeQueue() for
00062 the associated CallbackQueues, which prevents them from adding any more callbacks
00063 to CallbackQueueManager. Otherwise a Nodelet that is continuously executing
00064 callbacks might persist in a "zombie" state after being unloaded.
00065  */
00066 
00067 namespace nodelet
00068 {
00069 
00070 typedef boost::shared_ptr<Nodelet> NodeletPtr;
00071 
00073 class LoaderROS
00074 {
00075 public:
00076   LoaderROS(Loader* parent, const ros::NodeHandle& nh)
00077   : parent_(parent)
00078   , nh_(nh)
00079   , bond_spinner_(1, &bond_callback_queue_)
00080   {
00081     load_server_ = nh_.advertiseService("load_nodelet", &LoaderROS::serviceLoad, this);
00082     unload_server_ = nh_.advertiseService("unload_nodelet", &LoaderROS::serviceUnload, this);
00083     list_server_ = nh_.advertiseService("list", &LoaderROS::serviceList, this);
00084 
00085     bond_spinner_.start();
00086   }
00087 
00088 private:
00089   bool serviceLoad(nodelet::NodeletLoad::Request &req,
00090                    nodelet::NodeletLoad::Response &res)
00091   {
00092     // build map
00093     M_string remappings;
00094     if (req.remap_source_args.size() != req.remap_target_args.size())
00095     {
00096       ROS_ERROR("Bad remapppings provided, target and source of different length");
00097     }
00098     else
00099     {
00100       for (size_t i = 0; i < req.remap_source_args.size(); ++i)
00101       {
00102         remappings[ros::names::resolve(req.remap_source_args[i])] = ros::names::resolve(req.remap_target_args[i]);
00103         ROS_DEBUG("%s:%s\n", ros::names::resolve(req.remap_source_args[i]).c_str(), remappings[ros::names::resolve(req.remap_source_args[i])].c_str());
00104       }
00105     }
00106 
00107     res.success = parent_->load(req.name, req.type, remappings, req.my_argv);
00108 
00109     // If requested, create bond to sister process
00110     if (res.success && !req.bond_id.empty())
00111     {
00112       bond::Bond* bond = new bond::Bond(nh_.getNamespace() + "/bond", req.bond_id);
00113       bond_map_.insert(req.name, bond);
00114       bond->setCallbackQueue(&bond_callback_queue_);
00115       bond->setBrokenCallback(boost::bind(&Loader::unload, parent_, req.name));
00116       bond->start();
00117     }
00118     return res.success;
00119   }
00120 
00121   bool serviceUnload(nodelet::NodeletUnload::Request &req,
00122                      nodelet::NodeletUnload::Response &res)
00123   {
00124     res.success = parent_->unload(req.name);
00125     if (!res.success)
00126     {
00127       ROS_ERROR("Failed to find nodelet with name '%s' to unload.", req.name.c_str());
00128     }
00129     // Break the bond, if there is one
00130     bond_map_.erase(req.name);
00131 
00132     return res.success;
00133   }
00134 
00135   bool serviceList(nodelet::NodeletList::Request &req,
00136                    nodelet::NodeletList::Response &res)
00137   {
00138     res.nodelets = parent_->listLoadedNodelets();
00139     return true;
00140   }
00141 
00142   Loader* parent_;
00143   ros::NodeHandle nh_;
00144   ros::ServiceServer load_server_;
00145   ros::ServiceServer unload_server_;
00146   ros::ServiceServer list_server_;
00147   
00148   ros::CallbackQueue bond_callback_queue_;
00149   ros::AsyncSpinner bond_spinner_;
00150   typedef boost::ptr_map<std::string, bond::Bond> M_stringToBond;
00151   M_stringToBond bond_map_;
00152 };
00153 
00154 // Owns a Nodelet and its callback queues
00155 struct ManagedNodelet : boost::noncopyable
00156 {
00157   detail::CallbackQueuePtr st_queue;
00158   detail::CallbackQueuePtr mt_queue;
00159   NodeletPtr nodelet; // destroyed before the queues
00160   detail::CallbackQueueManager* callback_manager;
00161 
00163   ManagedNodelet(const NodeletPtr& nodelet, detail::CallbackQueueManager* cqm)
00164     : st_queue(new detail::CallbackQueue(cqm, nodelet))
00165     , mt_queue(new detail::CallbackQueue(cqm, nodelet))
00166     , nodelet(nodelet)
00167     , callback_manager(cqm)
00168   {
00169     // NOTE: Can't do this in CallbackQueue constructor because the shared_ptr to
00170     // it doesn't exist then.
00171     callback_manager->addQueue(st_queue, false);
00172     callback_manager->addQueue(mt_queue, true);
00173   }
00174 
00175   ~ManagedNodelet()
00176   {
00177     callback_manager->removeQueue(st_queue);
00178     callback_manager->removeQueue(mt_queue);
00179   }
00180 };
00181 
00182 struct Loader::Impl
00183 {
00184   boost::shared_ptr<LoaderROS> services_;
00185 
00186   boost::function<boost::shared_ptr<Nodelet> (const std::string& lookup_name)> create_instance_;
00187   boost::shared_ptr<detail::CallbackQueueManager> callback_manager_; // Must outlive nodelets_
00188 
00189   typedef boost::ptr_map<std::string, ManagedNodelet> M_stringToNodelet;
00190   M_stringToNodelet nodelets_; 
00191 
00192   Impl()
00193   {
00194     // Under normal circumstances, we use pluginlib to load any registered nodelet
00195     typedef pluginlib::ClassLoader<Nodelet> Loader;
00196     boost::shared_ptr<Loader> loader(new Loader("nodelet", "nodelet::Nodelet"));
00197     // create_instance_ is self-contained; it owns a copy of the loader shared_ptr
00198     create_instance_ = boost::bind(&Loader::createInstance, loader, _1);
00199   }
00200 
00201   Impl(const boost::function<boost::shared_ptr<Nodelet> (const std::string& lookup_name)>& create_instance)
00202     : create_instance_(create_instance)
00203   {
00204   }
00205 
00206   void advertiseRosApi(Loader* parent, const ros::NodeHandle& server_nh)
00207   {
00208     int num_threads_param;
00209     server_nh.param("num_worker_threads", num_threads_param, 0);
00210     callback_manager_.reset(new detail::CallbackQueueManager(num_threads_param));
00211     ROS_INFO("Initializing nodelet with %d worker threads.", (int)callback_manager_->getNumWorkerThreads());
00212   
00213     services_.reset(new LoaderROS(parent, server_nh));
00214   }
00215 };
00216 
00218 Loader::Loader(bool provide_ros_api)
00219   : impl_(new Impl)
00220 {
00221   if (provide_ros_api)
00222     impl_->advertiseRosApi(this, ros::NodeHandle("~"));
00223   else
00224     impl_->callback_manager_.reset(new detail::CallbackQueueManager);
00225 }
00226 
00227 Loader::Loader(const ros::NodeHandle& server_nh)
00228   : impl_(new Impl)
00229 {
00230   impl_->advertiseRosApi(this, server_nh);
00231 }
00232 
00233 Loader::Loader(const boost::function<boost::shared_ptr<Nodelet> (const std::string& lookup_name)>& create_instance)
00234   : impl_(new Impl(create_instance))
00235 {
00236   impl_->callback_manager_.reset(new detail::CallbackQueueManager);
00237 }
00238 
00239 Loader::~Loader()
00240 {
00241 }
00242 
00243 bool Loader::load(const std::string &name, const std::string& type, const ros::M_string& remappings,
00244                   const std::vector<std::string> & my_argv)
00245 {
00246   boost::mutex::scoped_lock lock(lock_);
00247   if (impl_->nodelets_.count(name) > 0)
00248   {
00249     ROS_ERROR("Cannot load nodelet %s for one exists with that name already", name.c_str());
00250     return false;
00251   }
00252 
00253   try
00254   {
00255     NodeletPtr p(impl_->create_instance_(type));
00256     if (!p)
00257       return false;
00258     ROS_DEBUG("Done loading nodelet %s", name.c_str());
00259 
00260     ManagedNodelet* mn = new ManagedNodelet(p, impl_->callback_manager_.get());
00261     impl_->nodelets_.insert(const_cast<std::string&>(name), mn); // mn now owned by boost::ptr_map
00262     p->init(name, remappings, my_argv, mn->st_queue.get(), mn->mt_queue.get());
00264 
00265     ROS_DEBUG("Done initing nodelet %s", name.c_str());
00266     return true;
00267   }
00268   catch (std::runtime_error& e)
00269   {
00270     ROS_ERROR("Failed to load nodelet [%s] of type [%s]: %s", name.c_str(), type.c_str(), e.what());
00271   }
00272 
00273   return false;
00274 }
00275 
00276 bool Loader::unload (const std::string & name)
00277 {
00278   boost::mutex::scoped_lock lock (lock_);
00279   Impl::M_stringToNodelet::iterator it = impl_->nodelets_.find(name);
00280   if (it != impl_->nodelets_.end())
00281   {
00282     impl_->nodelets_.erase(it);
00283     ROS_DEBUG ("Done unloading nodelet %s", name.c_str ());
00284     return (true);
00285   }
00286 
00287   return (false);
00288 }
00289 
00290 bool Loader::clear ()
00291 {
00292   boost::mutex::scoped_lock lock(lock_);
00293   impl_->nodelets_.clear();
00294   return true;
00295 };
00296 
00297 std::vector<std::string> Loader::listLoadedNodelets()
00298 {
00299   boost::mutex::scoped_lock lock (lock_);
00300   std::vector<std::string> output;
00301   Impl::M_stringToNodelet::iterator it = impl_->nodelets_.begin();
00302   for (; it != impl_->nodelets_.end(); ++it)
00303   {
00304     output.push_back(it->first);
00305   }
00306   return output;
00307 }
00308 
00309 } // namespace nodelet
00310 


nodelet
Author(s): Tully Foote, Radu Bogdan Rusu
autogenerated on Sat Dec 28 2013 17:14:39