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     boost::mutex::scoped_lock lock(lock_);
00093 
00094     // build map
00095     M_string remappings;
00096     if (req.remap_source_args.size() != req.remap_target_args.size())
00097     {
00098       ROS_ERROR("Bad remapppings provided, target and source of different length");
00099     }
00100     else
00101     {
00102       for (size_t i = 0; i < req.remap_source_args.size(); ++i)
00103       {
00104         remappings[ros::names::resolve(req.remap_source_args[i])] = ros::names::resolve(req.remap_target_args[i]);
00105         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());
00106       }
00107     }
00108 
00109     res.success = parent_->load(req.name, req.type, remappings, req.my_argv);
00110 
00111     // If requested, create bond to sister process
00112     if (res.success && !req.bond_id.empty())
00113     {
00114       bond::Bond* bond = new bond::Bond(nh_.getNamespace() + "/bond", req.bond_id);
00115       bond_map_.insert(req.name, bond);
00116       bond->setCallbackQueue(&bond_callback_queue_);
00117       bond->setBrokenCallback(boost::bind(&LoaderROS::unload, this, req.name));
00118       bond->start();
00119     }
00120     return res.success;
00121   }
00122 
00123   bool serviceUnload(nodelet::NodeletUnload::Request &req,
00124                      nodelet::NodeletUnload::Response &res)
00125   {
00126     res.success = unload(req.name);
00127     return res.success;
00128   }
00129 
00130   bool unload(const std::string& name)
00131   {
00132     boost::mutex::scoped_lock lock(lock_);
00133 
00134     bool success = parent_->unload(name);
00135     if (!success)
00136     {
00137       ROS_ERROR("Failed to find nodelet with name '%s' to unload.", name.c_str());
00138       return success;
00139     }
00140 
00141     // break the bond, if there is one
00142     M_stringToBond::iterator it = bond_map_.find(name);
00143     if (it != bond_map_.end()) {
00144         // disable callback for broken bond, as we are breaking it intentially now
00145         it->second->setBrokenCallback(boost::function<void(void)>());
00146         // erase (and break) bond
00147         bond_map_.erase(name);
00148     }
00149 
00150     return success;
00151   }
00152 
00153   bool serviceList(nodelet::NodeletList::Request &,
00154                    nodelet::NodeletList::Response &res)
00155   {
00156     res.nodelets = parent_->listLoadedNodelets();
00157     return true;
00158   }
00159 
00160   Loader* parent_;
00161   ros::NodeHandle nh_;
00162   ros::ServiceServer load_server_;
00163   ros::ServiceServer unload_server_;
00164   ros::ServiceServer list_server_;
00165 
00166   boost::mutex lock_;
00167 
00168   ros::CallbackQueue bond_callback_queue_;
00169   ros::AsyncSpinner bond_spinner_;
00170   typedef boost::ptr_map<std::string, bond::Bond> M_stringToBond;
00171   M_stringToBond bond_map_;
00172 };
00173 
00174 // Owns a Nodelet and its callback queues
00175 struct ManagedNodelet : boost::noncopyable
00176 {
00177   detail::CallbackQueuePtr st_queue;
00178   detail::CallbackQueuePtr mt_queue;
00179   NodeletPtr nodelet; // destroyed before the queues
00180   detail::CallbackQueueManager* callback_manager;
00181 
00183   ManagedNodelet(const NodeletPtr& nodelet, detail::CallbackQueueManager* cqm)
00184     : st_queue(new detail::CallbackQueue(cqm, nodelet))
00185     , mt_queue(new detail::CallbackQueue(cqm, nodelet))
00186     , nodelet(nodelet)
00187     , callback_manager(cqm)
00188   {
00189     // NOTE: Can't do this in CallbackQueue constructor because the shared_ptr to
00190     // it doesn't exist then.
00191     callback_manager->addQueue(st_queue, false);
00192     callback_manager->addQueue(mt_queue, true);
00193   }
00194 
00195   ~ManagedNodelet()
00196   {
00197     callback_manager->removeQueue(st_queue);
00198     callback_manager->removeQueue(mt_queue);
00199   }
00200 };
00201 
00202 struct Loader::Impl
00203 {
00204   boost::shared_ptr<LoaderROS> services_;
00205 
00206   boost::function<boost::shared_ptr<Nodelet> (const std::string& lookup_name)> create_instance_;
00207   boost::function<void ()> refresh_classes_;
00208   boost::shared_ptr<detail::CallbackQueueManager> callback_manager_; // Must outlive nodelets_
00209 
00210   typedef boost::ptr_map<std::string, ManagedNodelet> M_stringToNodelet;
00211   M_stringToNodelet nodelets_; 
00212 
00213   Impl()
00214   {
00215     // Under normal circumstances, we use pluginlib to load any registered nodelet
00216     typedef pluginlib::ClassLoader<Nodelet> Loader;
00217     boost::shared_ptr<Loader> loader(new Loader("nodelet", "nodelet::Nodelet"));
00218     // create_instance_ is self-contained; it owns a copy of the loader shared_ptr
00219     create_instance_ = boost::bind(&Loader::createInstance, loader, _1);
00220     refresh_classes_ = boost::bind(&Loader::refreshDeclaredClasses, loader);
00221   }
00222 
00223   Impl(const boost::function<boost::shared_ptr<Nodelet> (const std::string& lookup_name)>& create_instance)
00224     : create_instance_(create_instance)
00225   {
00226   }
00227 
00228   void advertiseRosApi(Loader* parent, const ros::NodeHandle& server_nh)
00229   {
00230     int num_threads_param;
00231     server_nh.param("num_worker_threads", num_threads_param, 0);
00232     callback_manager_.reset(new detail::CallbackQueueManager(num_threads_param));
00233     ROS_INFO("Initializing nodelet with %d worker threads.", (int)callback_manager_->getNumWorkerThreads());
00234 
00235     services_.reset(new LoaderROS(parent, server_nh));
00236   }
00237 };
00238 
00240 Loader::Loader(bool provide_ros_api)
00241   : impl_(new Impl)
00242 {
00243   if (provide_ros_api)
00244     impl_->advertiseRosApi(this, ros::NodeHandle("~"));
00245   else
00246     impl_->callback_manager_.reset(new detail::CallbackQueueManager);
00247 }
00248 
00249 Loader::Loader(const ros::NodeHandle& server_nh)
00250   : impl_(new Impl)
00251 {
00252   impl_->advertiseRosApi(this, server_nh);
00253 }
00254 
00255 Loader::Loader(const boost::function<boost::shared_ptr<Nodelet> (const std::string& lookup_name)>& create_instance)
00256   : impl_(new Impl(create_instance))
00257 {
00258   impl_->callback_manager_.reset(new detail::CallbackQueueManager);
00259 }
00260 
00261 Loader::~Loader()
00262 {
00263 }
00264 
00265 bool Loader::load(const std::string &name, const std::string& type, const ros::M_string& remappings,
00266                   const std::vector<std::string> & my_argv)
00267 {
00268   boost::mutex::scoped_lock lock(lock_);
00269   if (impl_->nodelets_.count(name) > 0)
00270   {
00271     ROS_ERROR("Cannot load nodelet %s for one exists with that name already", name.c_str());
00272     return false;
00273   }
00274 
00275   NodeletPtr p;
00276   try
00277   {
00278     p = impl_->create_instance_(type);
00279   }
00280   catch (std::runtime_error& e)
00281   {
00282     // If we cannot refresh the nodelet cache, fail immediately
00283     if(!impl_->refresh_classes_)
00284     {
00285       ROS_ERROR("Failed to load nodelet [%s] of type [%s]: %s", name.c_str(), type.c_str(), e.what());
00286       return false;
00287     }
00288 
00289     // otherwise, refresh the cache and try again.
00290     try
00291     {
00292       impl_->refresh_classes_();
00293       p = impl_->create_instance_(type);
00294     }
00295     catch (std::runtime_error& e2)
00296     {
00297       // dlopen() can return inconsistent results currently (see
00298       // https://sourceware.org/bugzilla/show_bug.cgi?id=17833), so make sure
00299       // that we display the messages of both exceptions to the user.
00300       ROS_ERROR("Failed to load nodelet [%s] of type [%s] even after refreshing the cache: %s", name.c_str(), type.c_str(), e2.what());
00301       ROS_ERROR("The error before refreshing the cache was: %s", e.what());
00302       return false;
00303     }
00304   }
00305 
00306   if (!p)
00307   {
00308     return false;
00309   }
00310   ROS_DEBUG("Done loading nodelet %s", name.c_str());
00311 
00312   ManagedNodelet* mn = new ManagedNodelet(p, impl_->callback_manager_.get());
00313   impl_->nodelets_.insert(const_cast<std::string&>(name), mn); // mn now owned by boost::ptr_map
00314   try {
00315     p->init(name, remappings, my_argv, mn->st_queue.get(), mn->mt_queue.get());
00317 
00318     ROS_DEBUG("Done initing nodelet %s", name.c_str());
00319   } catch(...) {
00320     Impl::M_stringToNodelet::iterator it = impl_->nodelets_.find(name);
00321     if (it != impl_->nodelets_.end())
00322     {
00323       impl_->nodelets_.erase(it);
00324       ROS_DEBUG ("Failed to initialize nodelet %s", name.c_str ());
00325       return (false);
00326     }
00327   }
00328   return true;
00329 }
00330 
00331 bool Loader::unload (const std::string & name)
00332 {
00333   boost::mutex::scoped_lock lock (lock_);
00334   Impl::M_stringToNodelet::iterator it = impl_->nodelets_.find(name);
00335   if (it != impl_->nodelets_.end())
00336   {
00337     impl_->nodelets_.erase(it);
00338     ROS_DEBUG ("Done unloading nodelet %s", name.c_str ());
00339     return (true);
00340   }
00341 
00342   return (false);
00343 }
00344 
00345 bool Loader::clear ()
00346 {
00347   boost::mutex::scoped_lock lock(lock_);
00348   impl_->nodelets_.clear();
00349   return true;
00350 };
00351 
00352 std::vector<std::string> Loader::listLoadedNodelets()
00353 {
00354   boost::mutex::scoped_lock lock (lock_);
00355   std::vector<std::string> output;
00356   Impl::M_stringToNodelet::iterator it = impl_->nodelets_.begin();
00357   for (; it != impl_->nodelets_.end(); ++it)
00358   {
00359     output.push_back(it->first);
00360   }
00361   return output;
00362 }
00363 
00364 } // namespace nodelet
00365 


nodelet
Author(s): Tully Foote, Radu Bogdan Rusu
autogenerated on Sun Aug 6 2017 02:23:55