loader.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2010, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <nodelet/loader.h>
31 #include <nodelet/nodelet.h>
35 #include <bondcpp/bond.h>
36 
37 #include <ros/ros.h>
38 #include <ros/callback_queue.h>
39 #include <nodelet/NodeletLoad.h>
40 #include <nodelet/NodeletList.h>
41 #include <nodelet/NodeletUnload.h>
42 
43 #include <boost/ptr_container/ptr_map.hpp>
44 #include <boost/utility.hpp>
45 
46 /*
47 Between Loader, Nodelet, CallbackQueue and CallbackQueueManager, who owns what?
48 
49 Loader contains the CallbackQueueManager. Loader and CallbackQueueManager share
50 ownership of the CallbackQueues. Loader is primary owner of the Nodelets, but
51 each CallbackQueue has weak ownership of its associated Nodelet.
52 
53 Loader ensures that the CallbackQueues associated with a Nodelet outlive that
54 Nodelet. CallbackQueueManager ensures that CallbackQueues continue to survive as
55 long as they have pending callbacks.
56 
57 CallbackQueue holds a weak_ptr to its associated Nodelet, which it attempts to
58 lock before invoking any callback. So any lingering callbacks processed after a
59 Nodelet is destroyed are safely discarded, and then the CallbackQueue expires.
60 
61 When Loader unloads a Nodelet, it calls CallbackQueueManager::removeQueue() for
62 the associated CallbackQueues, which prevents them from adding any more callbacks
63 to CallbackQueueManager. Otherwise a Nodelet that is continuously executing
64 callbacks might persist in a "zombie" state after being unloaded.
65  */
66 
67 namespace nodelet
68 {
69 
71 
73 class LoaderROS
74 {
75 public:
76  LoaderROS(Loader* parent, const ros::NodeHandle& nh)
77  : parent_(parent)
78  , nh_(nh)
80  {
81  load_server_ = nh_.advertiseService("load_nodelet", &LoaderROS::serviceLoad, this);
82  unload_server_ = nh_.advertiseService("unload_nodelet", &LoaderROS::serviceUnload, this);
84 
86  }
87 
88 private:
89  bool serviceLoad(nodelet::NodeletLoad::Request &req,
90  nodelet::NodeletLoad::Response &res)
91  {
92  boost::mutex::scoped_lock lock(lock_);
93 
94  // build map
95  M_string remappings;
96  if (req.remap_source_args.size() != req.remap_target_args.size())
97  {
98  ROS_ERROR("Bad remapppings provided, target and source of different length");
99  }
100  else
101  {
102  for (size_t i = 0; i < req.remap_source_args.size(); ++i)
103  {
104  remappings[ros::names::resolve(req.remap_source_args[i])] = ros::names::resolve(req.remap_target_args[i]);
105  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());
106  }
107  }
108 
109  res.success = parent_->load(req.name, req.type, remappings, req.my_argv);
110 
111  // If requested, create bond to sister process
112  if (res.success && !req.bond_id.empty())
113  {
114  bond::Bond* bond = new bond::Bond(nh_.getNamespace() + "/bond", req.bond_id);
115  bond_map_.insert(req.name, bond);
117  bond->setBrokenCallback(boost::bind(&LoaderROS::unload, this, req.name));
118  bond->start();
119  }
120  return res.success;
121  }
122 
123  bool serviceUnload(nodelet::NodeletUnload::Request &req,
124  nodelet::NodeletUnload::Response &res)
125  {
126  res.success = unload(req.name);
127  return res.success;
128  }
129 
130  bool unload(const std::string& name)
131  {
132  boost::mutex::scoped_lock lock(lock_);
133 
134  bool success = parent_->unload(name);
135  if (!success)
136  {
137  ROS_ERROR("Failed to find nodelet with name '%s' to unload.", name.c_str());
138  return success;
139  }
140 
141  // break the bond, if there is one
142  M_stringToBond::iterator it = bond_map_.find(name);
143  if (it != bond_map_.end()) {
144  // disable callback for broken bond, as we are breaking it intentially now
145  it->second->setBrokenCallback(boost::function<void(void)>());
146  // erase (and break) bond
147  bond_map_.erase(name);
148  }
149 
150  return success;
151  }
152 
153  bool serviceList(nodelet::NodeletList::Request &,
154  nodelet::NodeletList::Response &res)
155  {
156  res.nodelets = parent_->listLoadedNodelets();
157  return true;
158  }
159 
165 
166  boost::mutex lock_;
167 
170  typedef boost::ptr_map<std::string, bond::Bond> M_stringToBond;
171  M_stringToBond bond_map_;
172 };
173 
174 // Owns a Nodelet and its callback queues
175 struct ManagedNodelet : boost::noncopyable
176 {
179  NodeletPtr nodelet; // destroyed before the queues
181 
183  ManagedNodelet(const NodeletPtr& nodelet, detail::CallbackQueueManager* cqm)
184  : st_queue(new detail::CallbackQueue(cqm, nodelet))
185  , mt_queue(new detail::CallbackQueue(cqm, nodelet))
186  , nodelet(nodelet)
187  , callback_manager(cqm)
188  {
189  // NOTE: Can't do this in CallbackQueue constructor because the shared_ptr to
190  // it doesn't exist then.
191  callback_manager->addQueue(st_queue, false);
192  callback_manager->addQueue(mt_queue, true);
193  }
194 
196  {
197  callback_manager->removeQueue(st_queue);
198  callback_manager->removeQueue(mt_queue);
199  }
200 };
201 
203 {
205 
206  boost::function<boost::shared_ptr<Nodelet> (const std::string& lookup_name)> create_instance_;
207  boost::function<void ()> refresh_classes_;
209 
210  typedef boost::ptr_map<std::string, ManagedNodelet> M_stringToNodelet;
211  M_stringToNodelet nodelets_;
212 
214  {
215  // Under normal circumstances, we use pluginlib to load any registered nodelet
217  boost::shared_ptr<Loader> loader(new Loader("nodelet", "nodelet::Nodelet"));
218  // create_instance_ is self-contained; it owns a copy of the loader shared_ptr
219  create_instance_ = boost::bind(&Loader::createInstance, loader, _1);
220  refresh_classes_ = boost::bind(&Loader::refreshDeclaredClasses, loader);
221  }
222 
223  Impl(const boost::function<boost::shared_ptr<Nodelet> (const std::string& lookup_name)>& create_instance)
224  : create_instance_(create_instance)
225  {
226  }
227 
228  void advertiseRosApi(Loader* parent, const ros::NodeHandle& server_nh)
229  {
230  int num_threads_param;
231  server_nh.param("num_worker_threads", num_threads_param, 0);
232  callback_manager_.reset(new detail::CallbackQueueManager(num_threads_param));
233  ROS_INFO("Initializing nodelet with %d worker threads.", (int)callback_manager_->getNumWorkerThreads());
234 
235  services_.reset(new LoaderROS(parent, server_nh));
236  }
237 };
238 
240 Loader::Loader(bool provide_ros_api)
241  : impl_(new Impl)
242 {
243  if (provide_ros_api)
244  impl_->advertiseRosApi(this, ros::NodeHandle("~"));
245  else
246  impl_->callback_manager_.reset(new detail::CallbackQueueManager);
247 }
248 
250  : impl_(new Impl)
251 {
252  impl_->advertiseRosApi(this, server_nh);
253 }
254 
255 Loader::Loader(const boost::function<boost::shared_ptr<Nodelet> (const std::string& lookup_name)>& create_instance)
256  : impl_(new Impl(create_instance))
257 {
258  impl_->callback_manager_.reset(new detail::CallbackQueueManager);
259 }
260 
262 {
263 }
264 
265 bool Loader::load(const std::string &name, const std::string& type, const ros::M_string& remappings,
266  const std::vector<std::string> & my_argv)
267 {
268  boost::mutex::scoped_lock lock(lock_);
269  if (impl_->nodelets_.count(name) > 0)
270  {
271  ROS_ERROR("Cannot load nodelet %s for one exists with that name already", name.c_str());
272  return false;
273  }
274 
275  NodeletPtr p;
276  try
277  {
278  p = impl_->create_instance_(type);
279  }
280  catch (std::runtime_error& e)
281  {
282  // If we cannot refresh the nodelet cache, fail immediately
283  if(!impl_->refresh_classes_)
284  {
285  ROS_ERROR("Failed to load nodelet [%s] of type [%s]: %s", name.c_str(), type.c_str(), e.what());
286  return false;
287  }
288 
289  // otherwise, refresh the cache and try again.
290  try
291  {
292  impl_->refresh_classes_();
293  p = impl_->create_instance_(type);
294  }
295  catch (std::runtime_error& e2)
296  {
297  // dlopen() can return inconsistent results currently (see
298  // https://sourceware.org/bugzilla/show_bug.cgi?id=17833), so make sure
299  // that we display the messages of both exceptions to the user.
300  ROS_ERROR("Failed to load nodelet [%s] of type [%s] even after refreshing the cache: %s", name.c_str(), type.c_str(), e2.what());
301  ROS_ERROR("The error before refreshing the cache was: %s", e.what());
302  return false;
303  }
304  }
305 
306  if (!p)
307  {
308  return false;
309  }
310  ROS_DEBUG("Done loading nodelet %s", name.c_str());
311 
312  ManagedNodelet* mn = new ManagedNodelet(p, impl_->callback_manager_.get());
313  impl_->nodelets_.insert(const_cast<std::string&>(name), mn); // mn now owned by boost::ptr_map
314  try {
315  mn->st_queue->disable();
316  mn->mt_queue->disable();
317 
318  p->init(name, remappings, my_argv, mn->st_queue.get(), mn->mt_queue.get());
319 
320  mn->st_queue->enable();
321  mn->mt_queue->enable();
322 
323 
324  ROS_DEBUG("Done initing nodelet %s", name.c_str());
325  } catch(...) {
326  Impl::M_stringToNodelet::iterator it = impl_->nodelets_.find(name);
327  if (it != impl_->nodelets_.end())
328  {
329  impl_->nodelets_.erase(it);
330  ROS_DEBUG ("Failed to initialize nodelet %s", name.c_str ());
331  return (false);
332  }
333  }
334  return true;
335 }
336 
337 bool Loader::unload (const std::string & name)
338 {
339  boost::mutex::scoped_lock lock (lock_);
340  Impl::M_stringToNodelet::iterator it = impl_->nodelets_.find(name);
341  if (it != impl_->nodelets_.end())
342  {
343  impl_->nodelets_.erase(it);
344  ROS_DEBUG ("Done unloading nodelet %s", name.c_str ());
345  return (true);
346  }
347 
348  return (false);
349 }
350 
352 {
353  boost::mutex::scoped_lock lock(lock_);
354  impl_->nodelets_.clear();
355  return true;
356 };
357 
358 std::vector<std::string> Loader::listLoadedNodelets()
359 {
360  boost::mutex::scoped_lock lock (lock_);
361  std::vector<std::string> output;
362  Impl::M_stringToNodelet::iterator it = impl_->nodelets_.begin();
363  for (; it != impl_->nodelets_.end(); ++it)
364  {
365  output.push_back(it->first);
366  }
367  return output;
368 }
369 
370 } // namespace nodelet
371 
bool clear()
Clear all nodelets from this loader.
Definition: loader.cpp:351
detail::CallbackQueueManager * callback_manager
Definition: loader.cpp:180
void setBrokenCallback(boost::function< void(void)> on_broken)
bool serviceList(nodelet::NodeletList::Request &, nodelet::NodeletList::Response &res)
Definition: loader.cpp:153
LoaderROS(Loader *parent, const ros::NodeHandle &nh)
Definition: loader.cpp:76
void setCallbackQueue(ros::CallbackQueueInterface *queue)
void removeQueue(const CallbackQueuePtr &queue)
bool load(const std::string &name, const std::string &type, const M_string &remappings, const V_string &my_argv)
Load a nodelet.
Definition: loader.cpp:265
boost::mutex lock_
! Public methods must lock this to preserve internal integrity.
Definition: loader.h:91
ros::NodeHandle nh_
Definition: loader.cpp:161
detail::CallbackQueuePtr st_queue
Definition: loader.cpp:177
Loader(bool provide_ros_api=true)
Construct the nodelet loader with optional ros API at default location of NodeHandle("~") ...
Definition: loader.cpp:240
std::vector< std::string > listLoadedNodelets()
List the names of all loaded nodelets.
Definition: loader.cpp:358
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
boost::shared_ptr< LoaderROS > services_
Definition: loader.cpp:204
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool unload(const std::string &name)
Definition: loader.cpp:130
boost::shared_ptr< Nodelet > NodeletPtr
Definition: loader.cpp:70
M_stringToNodelet nodelets_
! A map of name to currently constructed nodelets
Definition: loader.cpp:211
ros::AsyncSpinner bond_spinner_
Definition: loader.cpp:169
Impl(const boost::function< boost::shared_ptr< Nodelet >(const std::string &lookup_name)> &create_instance)
Definition: loader.cpp:223
bool unload(const std::string &name)
Unload a nodelet.
Definition: loader.cpp:337
Loader * parent_
Definition: loader.cpp:160
boost::scoped_ptr< Impl > impl_
Definition: loader.h:92
void addQueue(const CallbackQueuePtr &queue, bool threaded)
std::map< std::string, std::string > M_string
void start()
bool param(const std::string &param_name, T &param_val, const T &default_val) const
boost::function< void()> refresh_classes_
Definition: loader.cpp:207
ros::ServiceServer list_server_
Definition: loader.cpp:164
#define ROS_INFO(...)
ros::CallbackQueue bond_callback_queue_
Definition: loader.cpp:168
ros::ServiceServer load_server_
Definition: loader.cpp:162
ManagedNodelet(const NodeletPtr &nodelet, detail::CallbackQueueManager *cqm)
Definition: loader.cpp:183
bool serviceLoad(nodelet::NodeletLoad::Request &req, nodelet::NodeletLoad::Response &res)
Definition: loader.cpp:89
boost::mutex lock_
Definition: loader.cpp:166
ros::ServiceServer unload_server_
Definition: loader.cpp:163
const std::string & getNamespace() const
A class which will construct and sequentially call Nodelets according to xml This is the primary way ...
Definition: loader.h:62
void advertiseRosApi(Loader *parent, const ros::NodeHandle &server_nh)
Definition: loader.cpp:228
boost::shared_ptr< detail::CallbackQueueManager > callback_manager_
Definition: loader.cpp:208
M_stringToBond bond_map_
Definition: loader.cpp:171
boost::ptr_map< std::string, bond::Bond > M_stringToBond
Definition: loader.cpp:170
detail::CallbackQueuePtr mt_queue
Definition: loader.cpp:178
#define ROS_ERROR(...)
bool serviceUnload(nodelet::NodeletUnload::Request &req, nodelet::NodeletUnload::Response &res)
Definition: loader.cpp:123
std::map< std::string, std::string > M_string
Definition: loader.h:55
boost::ptr_map< std::string, ManagedNodelet > M_stringToNodelet
Definition: loader.cpp:210
#define ROS_DEBUG(...)


nodelet
Author(s): Tully Foote, Radu Bogdan Rusu
autogenerated on Mon Feb 28 2022 22:57:12