Go to the documentation of this file.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
00031
00032
00033 #include "nodelet_plugin_provider.h"
00034
00035 #include "roscpp_plugin_provider.h"
00036
00037 #include <ros/callback_queue.h>
00038 #include <ros/ros.h>
00039
00040 #include <stdexcept>
00041 #include <boost/bind.hpp>
00042
00043 namespace rqt_gui_cpp {
00044
00045 NodeletPluginProvider::NodeletPluginProvider(const QString& export_tag, const QString& base_class_type)
00046 : qt_gui_cpp::RosPluginlibPluginProvider<rqt_gui_cpp::Plugin>(export_tag, base_class_type)
00047 , loader_(0)
00048 , ros_spin_thread_(0)
00049 {}
00050
00051 NodeletPluginProvider::~NodeletPluginProvider()
00052 {
00053 if (loader_)
00054 {
00055 delete loader_;
00056 }
00057 }
00058
00059 void NodeletPluginProvider::unload(void* instance)
00060 {
00061 qDebug("NodeletPluginProvider::unload()");
00062 if (!instances_.contains(instance))
00063 {
00064 qCritical("NodeletPluginProvider::unload() instance not found");
00065 return;
00066 }
00067
00068 QString nodelet_name = instances_[instance];
00069 bool unloaded = loader_->unload(nodelet_name.toStdString());
00070 if (!unloaded)
00071 {
00072 qCritical("NodeletPluginProvider::unload() '%s' failed", nodelet_name.toStdString().c_str());
00073 }
00074
00075
00076 if (loader_->listLoadedNodelets().empty())
00077 {
00078 shutdown();
00079 }
00080
00081 qt_gui_cpp::RosPluginlibPluginProvider<rqt_gui_cpp::Plugin>::unload(instance);
00082 }
00083
00084 void NodeletPluginProvider::shutdown()
00085 {
00086 if (ros_spin_thread_ != 0)
00087 {
00088 ros_spin_thread_->abort = true;
00089 ros_spin_thread_->wait();
00090 ros_spin_thread_->deleteLater();
00091 ros_spin_thread_ = 0;
00092 }
00093 }
00094
00095 void NodeletPluginProvider::init_loader()
00096 {
00097
00098 if (loader_ == 0)
00099 {
00100 loader_ = new nodelet::Loader(boost::bind(&NodeletPluginProvider::create_instance, this, _1));
00101 }
00102
00103
00104 if (ros_spin_thread_ == 0)
00105 {
00106 ros_spin_thread_ = new RosSpinThread(this);
00107 ros_spin_thread_->start();
00108 }
00109 }
00110
00111 boost::shared_ptr<Plugin> NodeletPluginProvider::create_plugin(const std::string& lookup_name, qt_gui_cpp::PluginContext* plugin_context)
00112 {
00113 init_loader();
00114
00115 nodelet::M_string remappings;
00116 nodelet::V_string my_argv;
00117 std::string nodelet_name = lookup_name + "_" + QString::number(plugin_context->serialNumber()).toStdString();
00118 instance_.reset();
00119 qDebug("NodeletPluginProvider::create_plugin() load %s", lookup_name.c_str());
00120 bool loaded = loader_->load(nodelet_name, lookup_name, remappings, my_argv);
00121 if (loaded)
00122 {
00123 qDebug("NodeletPluginProvider::create_plugin() loaded");
00124 instances_[&*instance_] = nodelet_name.c_str();
00125 }
00126 boost::shared_ptr<rqt_gui_cpp::Plugin> instance = instance_;
00127 instance_.reset();
00128 return instance;
00129 }
00130
00131 boost::shared_ptr<nodelet::Nodelet> NodeletPluginProvider::create_instance(const std::string& lookup_name)
00132 {
00133 instance_ = qt_gui_cpp::RosPluginlibPluginProvider<rqt_gui_cpp::Plugin>::create_plugin(lookup_name);
00134 return instance_;
00135 }
00136
00137 void NodeletPluginProvider::init_plugin(const QString& plugin_id, qt_gui_cpp::PluginContext* plugin_context, qt_gui_cpp::Plugin* plugin)
00138 {
00139 qDebug("NodeletPluginProvider::init_plugin()");
00140
00141 rqt_gui_cpp::Plugin* nodelet = dynamic_cast<rqt_gui_cpp::Plugin*>(plugin);
00142 if (!nodelet)
00143 {
00144 throw std::runtime_error("plugin is not a nodelet");
00145 }
00146
00147 qt_gui_cpp::RosPluginlibPluginProvider<rqt_gui_cpp::Plugin>::init_plugin(plugin_id, plugin_context, plugin);
00148 }
00149
00150 NodeletPluginProvider::RosSpinThread::RosSpinThread(QObject* parent)
00151 : QThread(parent)
00152 , abort(false)
00153 {}
00154
00155 NodeletPluginProvider::RosSpinThread::~RosSpinThread()
00156 {}
00157
00158 void NodeletPluginProvider::RosSpinThread::run()
00159 {
00160 while (!abort)
00161 {
00162 ros::getGlobalCallbackQueue()->callOne(ros::WallDuration(0.1));
00163 }
00164 }
00165
00166 }