nodelet.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id$
00035  *
00036  */
00037 
00045 #include <signal.h>
00046 
00047 #include <ros/ros.h>
00048 #include <ros/xmlrpc_manager.h>
00049 #include <bondcpp/bond.h>
00050 #include "nodelet/loader.h"
00051 #include "nodelet/NodeletList.h"
00052 #include "nodelet/NodeletLoad.h"
00053 #include "nodelet/NodeletUnload.h"
00054 
00055 #ifndef _WIN32
00056 # include <uuid/uuid.h>
00057 #else
00058 # include <rpc.h>
00059 #endif
00060 
00061 std::string genId()
00062 {
00063 #ifndef _WIN32
00064   uuid_t uuid;
00065   uuid_generate_random(uuid);
00066   char uuid_str[40];
00067   uuid_unparse(uuid, uuid_str);
00068   return std::string(uuid_str);
00069 #else
00070   UUID uuid;
00071   UuidCreate(&uuid);
00072   RPC_CSTR str;
00073   UuidToStringA(&uuid, &str);
00074   std::string return_string(reinterpret_cast<char *>(str));
00075   RpcStringFreeA(&str);
00076   return return_string;
00077 #endif
00078 }
00079 
00080 class NodeletArgumentParsing
00081 {
00082   private:
00083     std::string command_;
00084     std::string type_;
00085     std::string name_;
00086     std::string default_name_;
00087     std::string manager_;
00088     std::vector<std::string> local_args_;
00089     bool is_bond_enabled_;
00090 
00091   public:
00092     //NodeletArgumentParsing() { };
00093     bool
00094       parseArgs(int argc, char** argv)
00095     {
00096       is_bond_enabled_ = true;
00097       std::vector<std::string> non_ros_args;
00098       ros::removeROSArgs (argc, argv, non_ros_args);
00099       size_t used_args = 0;
00100 
00101       if (non_ros_args.size() > 1)
00102         command_ = non_ros_args[1];
00103       else
00104         return false;
00105 
00106       if (command_ == "load" && non_ros_args.size() > 3)
00107       {
00108         type_ = non_ros_args[2];
00109         manager_ = non_ros_args[3];
00110         used_args = 4;
00111 
00112         if (non_ros_args.size() > used_args)
00113         {
00114           if (non_ros_args[used_args] == "--no-bond")
00115           {
00116             is_bond_enabled_ = false;
00117             ++used_args;
00118           }
00119         }
00120       }
00121 
00122       else if (command_ == "unload" && non_ros_args.size() > 3)
00123       {
00124         type_    = "nodelet_unloader";
00125         name_    = non_ros_args[2];
00126         manager_ = non_ros_args[3];
00127         used_args = 4;
00128       }
00129       else if (command_ == "standalone" && non_ros_args.size() > 2)
00130       {
00131         type_ = non_ros_args[2];
00132         printf("type is %s\n", type_.c_str());
00133         used_args = 3;
00134       }
00135 
00136       if (command_ == "manager")
00137         used_args = 2;
00138 
00139       for (size_t i = used_args; i < non_ros_args.size(); i++)
00140         local_args_.push_back(non_ros_args[i]);
00141 
00142 
00143       if (used_args > 0) return true;
00144       else return false;
00145 
00146     };
00147 
00148 
00149     std::string getCommand () const { return (command_); }
00150     std::string getType () const    { return (type_);    }
00151     std::string getName () const    { return (name_);    }
00152     std::string getManager() const  { return (manager_); }
00153     bool isBondEnabled() const { return is_bond_enabled_; }
00154 
00155     std::vector<std::string> getMyArgv () const {return local_args_;};
00156     std::string getDefaultName()
00157     {
00158       std::string s = type_;
00159       replace(s.begin(), s.end(), '/', '_');
00160       return s;
00161     };
00162 
00163 };
00164 
00165 
00166 class NodeletInterface
00167 {
00168   public:
00170 
00171     bool
00172       unloadNodelet (const std::string &name, const std::string &manager)
00173     {
00174       ROS_INFO_STREAM ("Unloading nodelet " << name << " from manager " << manager);
00175 
00176       std::string service_name = manager + "/unload_nodelet";
00177       // Check if the service exists and is available
00178       if (!ros::service::exists (service_name, true))
00179       {
00180         // Probably the manager has shut down already, which is fine
00181         ROS_WARN("Couldn't find service %s, perhaps the manager is already shut down",
00182                  service_name.c_str());
00183         return (false);
00184       }
00185 
00186       ros::ServiceClient client = n_.serviceClient<nodelet::NodeletUnload> (service_name);
00187       //client.waitForExistence ();
00188 
00189       // Call the service
00190       nodelet::NodeletUnload srv;
00191       srv.request.name = name;
00192       if (!client.call (srv))
00193       {
00194         // Maybe service shut down in the meantime, which isn't an error
00195         if (ros::service::exists(service_name, false))
00196           ROS_FATAL_STREAM("Failed to unload nodelet '" << name << "` from manager `" << manager << "'");
00197         return (false);
00198       }
00199       return (true);
00200     }
00201 
00203 
00204     bool
00205       loadNodelet (const std::string &name, const std::string &type,
00206                    const std::string &manager, const std::vector<std::string> &args,
00207                    const std::string &bond_id)
00208     {
00209       ros::M_string remappings = ros::names::getRemappings ();
00210       std::vector<std::string> sources (remappings.size ()), targets (remappings.size ());
00211       ROS_INFO_STREAM ("Loading nodelet " << name << " of type " << type << " to manager " << manager << " with the following remappings:");
00212       int i = 0;
00213       for (ros::M_string::iterator it = remappings.begin (); it != remappings.end (); ++it, ++i)
00214       {
00215         sources[i] = (*it).first;
00216         targets[i] = (*it).second;
00217         ROS_INFO_STREAM (sources[i] << " -> " << targets[i]);
00218       }
00219 
00220       // Get and set the parameters
00221       XmlRpc::XmlRpcValue param;
00222       std::string node_name = ros::this_node::getName ();
00223       n_.getParam (node_name, param);
00224       n_.setParam (name, param);
00225 
00226       std::string service_name = std::string (manager) + "/load_nodelet";
00227 
00228       // Wait until the service is advertised
00229       ROS_DEBUG ("Waiting for service %s to be available...", service_name.c_str ());
00230       ros::ServiceClient client = n_.serviceClient<nodelet::NodeletLoad> (service_name);
00231       client.waitForExistence ();
00232 
00233       // Call the service
00234       nodelet::NodeletLoad srv;
00235       srv.request.name = std::string (name);
00236       srv.request.type = std::string (type);
00237       srv.request.remap_source_args = sources;
00238       srv.request.remap_target_args = targets;
00239       srv.request.my_argv = args;
00240       srv.request.bond_id = bond_id;
00241       if (!client.call (srv))
00242       {
00243         ROS_FATAL_STREAM("Failed to load nodelet '" << name << "` of type `" << type << "` to manager `" << manager << "'");
00244         return false;
00245       }
00246       return true;
00247     }
00248   private:
00249     ros::NodeHandle n_;
00250 };
00251 
00252 void print_usage(int argc, char** argv)
00253 {
00254   printf("Your usage: \n");
00255   for (int i = 0; i < argc; i++)
00256     printf("%s ", argv[i]);
00257   printf("\nnodelet usage:\n");
00258   printf("nodelet load pkg/Type manager [--no-bond]  - Launch a nodelet of type pkg/Type on manager manager\n");
00259   printf("nodelet standalone pkg/Type   - Launch a nodelet of type pkg/Type in a standalone node\n");
00260   printf("nodelet unload name manager   - Unload a nodelet by name from manager\n");
00261   printf("nodelet manager               - Launch a nodelet manager node\n");
00262 
00263 };
00264 
00265 sig_atomic_t volatile request_shutdown = 0;
00266 
00267 void nodeletLoaderSigIntHandler(int)
00268 {
00269   request_shutdown = 1;
00270 }
00271 
00272 // Shutdown can be triggered externally by an XML-RPC call, this is how "rosnode kill"
00273 // works. When shutting down a "nodelet load" we always want to unload the nodelet
00274 // before shutting down our ROS comm channels, so we override the default roscpp
00275 // handler for a "shutdown" XML-RPC call.
00276 void shutdownCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
00277 {
00278   int num_params = 0;
00279   if (params.getType() == XmlRpc::XmlRpcValue::TypeArray)
00280     num_params = params.size();
00281   if (num_params > 1)
00282   {
00283     std::string reason = params[1];
00284     ROS_WARN("Shutdown request received. Reason: [%s]", reason.c_str());
00285     request_shutdown = 1;
00286   }
00287 
00288   result = ros::xmlrpc::responseInt(1, "", 0);
00289 }
00290 
00291 /* ---[ */
00292 int
00293   main (int argc, char** argv)
00294 {
00295   NodeletArgumentParsing arg_parser;
00296 
00297   if (!arg_parser.parseArgs(argc, argv))
00298   {
00299     print_usage(argc, argv);
00300     return (-1);
00301   }
00302   std::string command = arg_parser.getCommand();
00303 
00304 
00305   if (command == "manager")
00306   {
00307     ros::init (argc, argv, "manager");
00308     nodelet::Loader n;
00309     ros::spin ();
00310   }
00311   else if (command == "standalone")
00312   {
00313     ros::init (argc, argv, arg_parser.getDefaultName());
00314 
00315     ros::NodeHandle nh;
00316     nodelet::Loader n(false);
00317     ros::M_string remappings; //Remappings are already applied by ROS no need to generate them.
00318     std::string nodelet_name = ros::this_node::getName ();
00319     std::string nodelet_type = arg_parser.getType();
00320     if(!n.load(nodelet_name, nodelet_type, remappings, arg_parser.getMyArgv()))
00321       return -1;
00322 
00323     ROS_DEBUG("Successfully loaded nodelet of type '%s' into name '%s'\n", nodelet_name.c_str(), nodelet_name.c_str());
00324 
00325     ros::spin();
00326   }
00327   else if (command == "load")
00328   {
00329     ros::init (argc, argv, arg_parser.getDefaultName (), ros::init_options::NoSigintHandler);
00330     NodeletInterface ni;
00331     ros::NodeHandle nh;
00332     std::string name = ros::this_node::getName ();
00333     std::string type = arg_parser.getType();
00334     std::string manager = arg_parser.getManager();
00335     std::string bond_id;
00336     if (arg_parser.isBondEnabled())
00337       bond_id = name + "_" + genId();
00338     bond::Bond bond(manager + "/bond", bond_id);
00339     if (!ni.loadNodelet(name, type, manager, arg_parser.getMyArgv(), bond_id))
00340       return -1;
00341 
00342     // Override default exit handlers for roscpp
00343     signal(SIGINT, nodeletLoaderSigIntHandler);
00344     ros::XMLRPCManager::instance()->unbind("shutdown");
00345     ros::XMLRPCManager::instance()->bind("shutdown", shutdownCallback);
00346 
00347     if (arg_parser.isBondEnabled())
00348       bond.start();
00349     // Spin our own loop
00350     ros::AsyncSpinner spinner(1);
00351     spinner.start();
00352     while (!request_shutdown)
00353     {
00354       if (arg_parser.isBondEnabled() && bond.isBroken())
00355       {
00356         ROS_INFO("Bond broken, exiting");
00357         goto shutdown;
00358       }
00359 #ifndef _WIN32
00360       usleep(100000);
00361 #else
00362       Sleep(100);
00363 #endif
00364     }
00365     // Attempt to unload the nodelet before shutting down ROS
00366     ni.unloadNodelet(name, manager);
00367     if (arg_parser.isBondEnabled())
00368       bond.breakBond();
00369   shutdown:
00370     ros::shutdown();
00371   }
00372   else if (command == "unload")
00373   {
00374     ros::init (argc, argv, arg_parser.getDefaultName ());
00375     std::string name = arg_parser.getName ();
00376     std::string manager = arg_parser.getManager();
00377     NodeletInterface ni;
00378     if (!ni.unloadNodelet(name, manager))
00379       return -1;
00380   }
00381   else
00382   {
00383     ros::init(argc, argv, "nodelet");
00384     ROS_ERROR("Command %s unknown", command.c_str());
00385     return -1;
00386   }
00387 
00388   return (0);
00389 }
00390 /* ]--- */


nodelet
Author(s): Tully Foote, Radu Bogdan Rusu
autogenerated on Sun Feb 17 2019 03:43:51