00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
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
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
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
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
00155 struct ManagedNodelet : boost::noncopyable
00156 {
00157 detail::CallbackQueuePtr st_queue;
00158 detail::CallbackQueuePtr mt_queue;
00159 NodeletPtr nodelet;
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
00170
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_;
00188
00189 typedef boost::ptr_map<std::string, ManagedNodelet> M_stringToNodelet;
00190 M_stringToNodelet nodelets_;
00191
00192 Impl()
00193 {
00194
00195 typedef pluginlib::ClassLoader<Nodelet> Loader;
00196 boost::shared_ptr<Loader> loader(new Loader("nodelet", "nodelet::Nodelet"));
00197
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);
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 }
00310