nodelet.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
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 Willow Garage, Inc. 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 OWNER 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  * $Id$
35  *
36  */
37 
45 #include <signal.h>
46 
47 #include <ros/ros.h>
48 #include <ros/xmlrpc_manager.h>
49 #include <bondcpp/bond.h>
50 #include "nodelet/loader.h"
51 #include "nodelet/NodeletList.h"
52 #include "nodelet/NodeletLoad.h"
53 #include "nodelet/NodeletUnload.h"
54 
55 #ifndef _WIN32
56 # include <uuid/uuid.h>
57 #else
58 # include <rpc.h>
59 #endif
60 
61 std::string genId()
62 {
63 #ifndef _WIN32
64  uuid_t uuid;
65  uuid_generate_random(uuid);
66  char uuid_str[40];
67  uuid_unparse(uuid, uuid_str);
68  return std::string(uuid_str);
69 #else
70  UUID uuid;
71  UuidCreate(&uuid);
72  RPC_CSTR str;
73  UuidToStringA(&uuid, &str);
74  std::string return_string(reinterpret_cast<char *>(str));
75  RpcStringFreeA(&str);
76  return return_string;
77 #endif
78 }
79 
81 {
82  private:
83  std::string command_;
84  std::string type_;
85  std::string name_;
86  std::string default_name_;
87  std::string manager_;
88  std::vector<std::string> local_args_;
90 
91  public:
92  //NodeletArgumentParsing() { };
93  bool
94  parseArgs(int argc, char** argv)
95  {
96  is_bond_enabled_ = true;
97  std::vector<std::string> non_ros_args;
98  ros::removeROSArgs (argc, argv, non_ros_args);
99  size_t used_args = 0;
100 
101  if (non_ros_args.size() > 1)
102  command_ = non_ros_args[1];
103  else
104  return false;
105 
106  if (command_ == "load" && non_ros_args.size() > 3)
107  {
108  type_ = non_ros_args[2];
109  manager_ = non_ros_args[3];
110  used_args = 4;
111 
112  if (non_ros_args.size() > used_args)
113  {
114  if (non_ros_args[used_args] == "--no-bond")
115  {
116  is_bond_enabled_ = false;
117  ++used_args;
118  }
119  }
120  }
121 
122  else if (command_ == "unload" && non_ros_args.size() > 3)
123  {
124  type_ = "nodelet_unloader";
125  name_ = non_ros_args[2];
126  manager_ = non_ros_args[3];
127  used_args = 4;
128  }
129  else if (command_ == "standalone" && non_ros_args.size() > 2)
130  {
131  type_ = non_ros_args[2];
132  printf("type is %s\n", type_.c_str());
133  used_args = 3;
134  }
135 
136  if (command_ == "manager")
137  used_args = 2;
138 
139  for (size_t i = used_args; i < non_ros_args.size(); i++)
140  local_args_.push_back(non_ros_args[i]);
141 
142 
143  if (used_args > 0) return true;
144  else return false;
145 
146  };
147 
148 
149  std::string getCommand () const { return (command_); }
150  std::string getType () const { return (type_); }
151  std::string getName () const { return (name_); }
152  std::string getManager() const { return (manager_); }
153  bool isBondEnabled() const { return is_bond_enabled_; }
154 
155  std::vector<std::string> getMyArgv () const {return local_args_;};
156  std::string getDefaultName()
157  {
158  std::string s = type_;
159  replace(s.begin(), s.end(), '/', '_');
160  return s;
161  };
162 
163 };
164 
165 
167 {
168  public:
170 
171  bool
172  unloadNodelet (const std::string &name, const std::string &manager)
173  {
174  ROS_INFO_STREAM ("Unloading nodelet " << name << " from manager " << manager);
175 
176  std::string service_name = manager + "/unload_nodelet";
177  // Check if the service exists and is available
178  if (!ros::service::exists (service_name, true))
179  {
180  // Probably the manager has shut down already, which is fine
181  ROS_WARN("Couldn't find service %s, perhaps the manager is already shut down",
182  service_name.c_str());
183  return (false);
184  }
185 
186  ros::ServiceClient client = n_.serviceClient<nodelet::NodeletUnload> (service_name);
187  //client.waitForExistence ();
188 
189  // Call the service
190  nodelet::NodeletUnload srv;
191  srv.request.name = name;
192  if (!client.call (srv))
193  {
194  // Maybe service shut down in the meantime, which isn't an error
195  if (ros::service::exists(service_name, false))
196  ROS_FATAL_STREAM("Failed to unload nodelet '" << name << "` from manager `" << manager << "'");
197  return (false);
198  }
199  return (true);
200  }
201 
203 
204  bool
205  loadNodelet (const std::string &name, const std::string &type,
206  const std::string &manager, const std::vector<std::string> &args,
207  const std::string &bond_id)
208  {
210  std::vector<std::string> sources (remappings.size ()), targets (remappings.size ());
211  ROS_INFO_STREAM ("Loading nodelet " << name << " of type " << type << " to manager " << manager << " with the following remappings:");
212  int i = 0;
213  for (ros::M_string::iterator it = remappings.begin (); it != remappings.end (); ++it, ++i)
214  {
215  sources[i] = (*it).first;
216  targets[i] = (*it).second;
217  ROS_INFO_STREAM (sources[i] << " -> " << targets[i]);
218  }
219 
220  // Get and set the parameters
222  std::string node_name = ros::this_node::getName ();
223  n_.getParam (node_name, param);
224  n_.setParam (name, param);
225 
226  std::string service_name = std::string (manager) + "/load_nodelet";
227 
228  // Wait until the service is advertised
229  ROS_DEBUG ("Waiting for service %s to be available...", service_name.c_str ());
230  ros::ServiceClient client = n_.serviceClient<nodelet::NodeletLoad> (service_name);
231  client.waitForExistence ();
232 
233  // Call the service
234  nodelet::NodeletLoad srv;
235  srv.request.name = std::string (name);
236  srv.request.type = std::string (type);
237  srv.request.remap_source_args = sources;
238  srv.request.remap_target_args = targets;
239  srv.request.my_argv = args;
240  srv.request.bond_id = bond_id;
241  if (!client.call (srv))
242  {
243  ROS_FATAL_STREAM("Failed to load nodelet '" << name << "` of type `" << type << "` to manager `" << manager << "'");
244  return false;
245  }
246  return true;
247  }
248  private:
250 };
251 
252 void print_usage(int argc, char** argv)
253 {
254  printf("Your usage: \n");
255  for (int i = 0; i < argc; i++)
256  printf("%s ", argv[i]);
257  printf("\nnodelet usage:\n");
258  printf("nodelet load pkg/Type manager [--no-bond] - Launch a nodelet of type pkg/Type on manager manager\n");
259  printf("nodelet standalone pkg/Type - Launch a nodelet of type pkg/Type in a standalone node\n");
260  printf("nodelet unload name manager - Unload a nodelet by name from manager\n");
261  printf("nodelet manager - Launch a nodelet manager node\n");
262 
263 };
264 
265 sig_atomic_t volatile request_shutdown = 0;
266 
268 {
269  request_shutdown = 1;
270 }
271 
272 // Shutdown can be triggered externally by an XML-RPC call, this is how "rosnode kill"
273 // works. When shutting down a "nodelet load" we always want to unload the nodelet
274 // before shutting down our ROS comm channels, so we override the default roscpp
275 // handler for a "shutdown" XML-RPC call.
277 {
278  int num_params = 0;
279  if (params.getType() == XmlRpc::XmlRpcValue::TypeArray)
280  num_params = params.size();
281  if (num_params > 1)
282  {
283  std::string reason = params[1];
284  ROS_WARN("Shutdown request received. Reason: [%s]", reason.c_str());
285  request_shutdown = 1;
286  }
287 
288  result = ros::xmlrpc::responseInt(1, "", 0);
289 }
290 
291 /* ---[ */
292 int
293  main (int argc, char** argv)
294 {
295  NodeletArgumentParsing arg_parser;
296 
297  if (!arg_parser.parseArgs(argc, argv))
298  {
299  print_usage(argc, argv);
300  return (-1);
301  }
302  std::string command = arg_parser.getCommand();
303 
304 
305  if (command == "manager")
306  {
307  ros::init (argc, argv, "manager");
308  nodelet::Loader n;
309  ros::spin ();
310  }
311  else if (command == "standalone")
312  {
313  ros::init (argc, argv, arg_parser.getDefaultName());
314 
315  ros::NodeHandle nh;
316  nodelet::Loader n(false);
317  ros::M_string remappings; //Remappings are already applied by ROS no need to generate them.
318  std::string nodelet_name = ros::this_node::getName ();
319  std::string nodelet_type = arg_parser.getType();
320  if(!n.load(nodelet_name, nodelet_type, remappings, arg_parser.getMyArgv()))
321  return -1;
322 
323  ROS_DEBUG("Successfully loaded nodelet of type '%s' into name '%s'\n", nodelet_name.c_str(), nodelet_name.c_str());
324 
325  ros::spin();
326  }
327  else if (command == "load")
328  {
329  ros::init (argc, argv, arg_parser.getDefaultName (), ros::init_options::NoSigintHandler);
330  NodeletInterface ni;
331  ros::NodeHandle nh;
332  std::string name = ros::this_node::getName ();
333  std::string type = arg_parser.getType();
334  std::string manager = arg_parser.getManager();
335  std::string bond_id;
336  if (arg_parser.isBondEnabled())
337  bond_id = name + "_" + genId();
338  bond::Bond bond(manager + "/bond", bond_id);
339  if (!ni.loadNodelet(name, type, manager, arg_parser.getMyArgv(), bond_id))
340  return -1;
341 
342  // Override default exit handlers for roscpp
343  signal(SIGINT, nodeletLoaderSigIntHandler);
344  ros::XMLRPCManager::instance()->unbind("shutdown");
345  ros::XMLRPCManager::instance()->bind("shutdown", shutdownCallback);
346 
347  if (arg_parser.isBondEnabled())
348  bond.start();
349  // Spin our own loop
350  ros::AsyncSpinner spinner(1);
351  spinner.start();
352  while (!request_shutdown)
353  {
354  if (arg_parser.isBondEnabled() && bond.isBroken())
355  {
356  ROS_INFO("Bond broken, exiting");
357  goto shutdown;
358  }
359 #ifndef _WIN32
360  usleep(100000);
361 #else
362  Sleep(100);
363 #endif
364  }
365  // Attempt to unload the nodelet before shutting down ROS
366  ni.unloadNodelet(name, manager);
367  if (arg_parser.isBondEnabled())
368  bond.breakBond();
369  shutdown:
370  ros::shutdown();
371  }
372  else if (command == "unload")
373  {
374  ros::init (argc, argv, arg_parser.getDefaultName ());
375  std::string name = arg_parser.getName ();
376  std::string manager = arg_parser.getManager();
377  NodeletInterface ni;
378  if (!ni.unloadNodelet(name, manager))
379  return -1;
380  }
381  else
382  {
383  ros::init(argc, argv, "nodelet");
384  ROS_ERROR("Command %s unknown", command.c_str());
385  return -1;
386  }
387 
388  return (0);
389 }
390 /* ]--- */
bool parseArgs(int argc, char **argv)
Definition: nodelet.cpp:94
std::string getType() const
Definition: nodelet.cpp:150
bool param(const std::string &param_name, T &param_val, const T &default_val)
void breakBond()
bool loadNodelet(const std::string &name, const std::string &type, const std::string &manager, const std::vector< std::string > &args, const std::string &bond_id)
Load the nodelet.
Definition: nodelet.cpp:205
std::string getDefaultName()
Definition: nodelet.cpp:156
XmlRpc::XmlRpcValue responseInt(int code, const std::string &msg, int response)
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
int size() const
std::string default_name_
Definition: nodelet.cpp:86
XmlRpcServer s
std::string name_
Definition: nodelet.cpp:85
std::string getName() const
Definition: nodelet.cpp:151
ros::NodeHandle n_
Definition: nodelet.cpp:249
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
ROSCPP_DECL const std::string & getName()
void shutdownCallback(XmlRpc::XmlRpcValue &params, XmlRpc::XmlRpcValue &result)
Definition: nodelet.cpp:276
Type const & getType() const
#define ROS_WARN(...)
int main(int argc, char **argv)
Definition: nodelet.cpp:293
ROSCPP_DECL const M_string & getRemappings()
std::map< std::string, std::string > M_string
void start()
std::string genId()
Definition: nodelet.cpp:61
ROSLIB_DECL std::string command(const std::string &cmd)
#define ROS_FATAL_STREAM(args)
bool unloadNodelet(const std::string &name, const std::string &manager)
Unload the nodelet.
Definition: nodelet.cpp:172
#define ROS_INFO(...)
sig_atomic_t volatile request_shutdown
Definition: nodelet.cpp:265
bool isBondEnabled() const
Definition: nodelet.cpp:153
ROSCPP_DECL void spin()
void print_usage(int argc, char **argv)
Definition: nodelet.cpp:252
void nodeletLoaderSigIntHandler(int)
Definition: nodelet.cpp:267
bool isBroken()
std::string type_
Definition: nodelet.cpp:84
#define ROS_INFO_STREAM(args)
ROSCONSOLE_DECL void shutdown()
A class which will construct and sequentially call Nodelets according to xml This is the primary way ...
Definition: loader.h:62
std::string command_
Definition: nodelet.cpp:83
std::string manager_
Definition: nodelet.cpp:87
static const XMLRPCManagerPtr & instance()
ROSCPP_DECL void shutdown()
ROSCPP_DECL void removeROSArgs(int argc, const char *const *argv, V_string &args_out)
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
std::vector< std::string > local_args_
Definition: nodelet.cpp:88
std::string getManager() const
Definition: nodelet.cpp:152
#define ROS_ERROR(...)
std::string getCommand() const
Definition: nodelet.cpp:149
#define ROS_DEBUG(...)
std::vector< std::string > getMyArgv() const
Definition: nodelet.cpp:155


nodelet
Author(s): Tully Foote, Radu Bogdan Rusu
autogenerated on Sat Jul 18 2020 03:17:53