plugin_mux.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #ifndef NAV_2D_UTILS_PLUGIN_MUX_H
36 #define NAV_2D_UTILS_PLUGIN_MUX_H
37 
38 #include <ros/ros.h>
39 #include <pluginlib/class_loader.h>
40 #include <std_msgs/String.h>
41 #include <nav_2d_msgs/SwitchPlugin.h>
42 #include <map>
43 #include <string>
44 #include <vector>
45 
46 namespace nav_2d_utils
47 {
58 template<class T>
59 class PluginMux
60 {
61 public:
72  PluginMux(const std::string& plugin_package, const std::string& plugin_class,
73  const std::string& parameter_name, const std::string& default_value,
74  const std::string& ros_name = "current_plugin", const std::string& switch_service_name = "switch_plugin");
75 
81  void addPlugin(const std::string& plugin_name, const std::string& plugin_class_name);
82 
89  bool usePlugin(const std::string& name)
90  {
91  // If plugin with given name doesn't exist, return false
92  if (plugins_.count(name) == 0) return false;
93 
94  if (switch_callback_)
95  {
97  }
98 
99  // Switch Mux
100  current_plugin_ = name;
101 
102  // Update ROS info
103  std_msgs::String str_msg;
104  str_msg.data = current_plugin_;
105  current_plugin_pub_.publish(str_msg);
107 
108  return true;
109  }
110 
115  std::string getCurrentPluginName() const
116  {
117  return current_plugin_;
118  }
119 
125  bool hasPlugin(const std::string& name) const
126  {
127  return plugins_.find(name) != plugins_.end();
128  }
129 
135  T& getPlugin(const std::string& name)
136  {
137  if (!hasPlugin(name))
138  throw std::invalid_argument("Plugin not found");
139  return *plugins_[name];
140  }
141 
147  {
148  return getPlugin(current_plugin_);
149  }
150 
154  std::vector<std::string> getPluginNames() const
155  {
156  std::vector<std::string> names;
157  for (auto& kv : plugins_)
158  {
159  names.push_back(kv.first);
160  }
161  return names;
162  }
163 
170  using SwitchCallback = std::function<void(const std::string&, const std::string&)>;
171 
180  void setSwitchCallback(SwitchCallback switch_callback) { switch_callback_ = switch_callback; }
181 
182 protected:
186  bool switchPluginService(nav_2d_msgs::SwitchPlugin::Request &req, nav_2d_msgs::SwitchPlugin::Response &resp)
187  {
188  std::string name = req.new_plugin;
189  if (usePlugin(name))
190  {
191  resp.success = true;
192  resp.message = "Loaded plugin namespace " + name + ".";
193  }
194  else
195  {
196  resp.success = false;
197  resp.message = "Namespace " + name + " not configured!";
198  }
199  return true;
200  }
201 
202  // Plugin Management
204  std::map<std::string, boost::shared_ptr<T>> plugins_;
205  std::string current_plugin_;
206 
207  // ROS Interface
211  std::string ros_name_;
212 
213  // Switch Callback
215 };
216 
217 // *********************************************************************************************************************
218 // ****************** Implementation of Templated Methods **************************************************************
219 // *********************************************************************************************************************
220 template<class T>
221 PluginMux<T>::PluginMux(const std::string& plugin_package, const std::string& plugin_class,
222  const std::string& parameter_name, const std::string& default_value,
223  const std::string& ros_name, const std::string& switch_service_name)
224  : plugin_loader_(plugin_package, plugin_class), private_nh_("~"), ros_name_(ros_name), switch_callback_(nullptr)
225 {
226  // Create the latched publisher
227  current_plugin_pub_ = private_nh_.advertise<std_msgs::String>(ros_name_, 1, true);
228 
229  // Load Plugins
230  std::string plugin_class_name;
231  std::vector<std::string> plugin_namespaces;
232  private_nh_.getParam(parameter_name, plugin_namespaces);
233  if (plugin_namespaces.size() == 0)
234  {
235  // If no namespaces are listed, use the name of the default class as the singular namespace.
236  std::string plugin_name = plugin_loader_.getName(default_value);
237  plugin_namespaces.push_back(plugin_name);
238  }
239 
240  for (const std::string& the_namespace : plugin_namespaces)
241  {
242  // Load the class name from namespace/plugin_class, or use default value
243  private_nh_.param(std::string(the_namespace + "/plugin_class"), plugin_class_name, default_value);
244  addPlugin(the_namespace, plugin_class_name);
245  }
246 
247  // By default, use the first one as current
248  usePlugin(plugin_namespaces[0]);
249 
250  // Now that the plugins are created, advertise the service if there are multiple
251  if (plugin_namespaces.size() > 1)
252  {
254  }
255 }
256 
257 template<class T>
258 void PluginMux<T>::addPlugin(const std::string& plugin_name, const std::string& plugin_class_name)
259 {
260  try
261  {
262  plugins_[plugin_name] = plugin_loader_.createInstance(plugin_class_name);
263  }
264  catch (const pluginlib::PluginlibException& ex)
265  {
266  ROS_FATAL_NAMED("PluginMux",
267  "Failed to load the plugin: %s. Exception: %s", plugin_name.c_str(), ex.what());
268  exit(EXIT_FAILURE);
269  }
270 }
271 
272 } // namespace nav_2d_utils
273 
274 #endif // NAV_2D_UTILS_PLUGIN_MUX_H
void addPlugin(const std::string &plugin_name, const std::string &plugin_class_name)
Create an instance of the given plugin_class_name and save it with the given plugin_name.
Definition: plugin_mux.h:258
bool hasPlugin(const std::string &name) const
Check to see if a plugin exists.
Definition: plugin_mux.h:125
void publish(const boost::shared_ptr< M > &message) const
pluginlib::ClassLoader< T > plugin_loader_
Definition: plugin_mux.h:203
bool switchPluginService(nav_2d_msgs::SwitchPlugin::Request &req, nav_2d_msgs::SwitchPlugin::Response &resp)
ROS Interface for Switching Plugins.
Definition: plugin_mux.h:186
ros::NodeHandle private_nh_
Definition: plugin_mux.h:210
ros::Publisher current_plugin_pub_
Definition: plugin_mux.h:209
An organizer for switching between multiple different plugins of the same type.
Definition: plugin_mux.h:59
T & getCurrentPlugin()
Get the Current Plugin.
Definition: plugin_mux.h:146
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
T & getPlugin(const std::string &name)
Get the Specified Plugin.
Definition: plugin_mux.h:135
bool usePlugin(const std::string &name)
C++ Interface for switching to a given plugin.
Definition: plugin_mux.h:89
bool param(const std::string &param_name, T &param_val, const T &default_val) const
std::string getCurrentPluginName() const
Get the Current Plugin Name.
Definition: plugin_mux.h:115
def default_value(type)
PluginMux(const std::string &plugin_package, const std::string &plugin_class, const std::string &parameter_name, const std::string &default_value, const std::string &ros_name="current_plugin", const std::string &switch_service_name="switch_plugin")
Main Constructor - Loads plugin(s) and sets up ROS interfaces.
Definition: plugin_mux.h:221
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void setSwitchCallback(SwitchCallback switch_callback)
Set the callback fired when the plugin switches.
Definition: plugin_mux.h:180
bool getParam(const std::string &key, std::string &s) const
ros::ServiceServer switch_plugin_srv_
Definition: plugin_mux.h:208
#define ROS_FATAL_NAMED(name,...)
std::string current_plugin_
Definition: plugin_mux.h:205
std::map< std::string, boost::shared_ptr< T > > plugins_
Definition: plugin_mux.h:204
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
SwitchCallback switch_callback_
Definition: plugin_mux.h:214
std::function< void(const std::string &, const std::string &)> SwitchCallback
alias for the function-type of the callback fired when the plugin switches.
Definition: plugin_mux.h:170
std::vector< std::string > getPluginNames() const
Get the current list of plugin names.
Definition: plugin_mux.h:154


nav_2d_utils
Author(s):
autogenerated on Wed Jun 26 2019 20:06:11