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 #include <uuid/uuid.h>
00047 
00048 #include <ros/ros.h>
00049 #include <ros/xmlrpc_manager.h>
00050 #include <bondcpp/bond.h>
00051 #include "nodelet/loader.h"
00052 #include "nodelet/NodeletList.h"
00053 #include "nodelet/NodeletLoad.h"
00054 #include "nodelet/NodeletUnload.h"
00055 
00056 std::string genId()
00057 {
00058   uuid_t uuid;
00059   uuid_generate_random(uuid);
00060   char uuid_str[40];
00061   uuid_unparse(uuid, uuid_str);
00062   return std::string(uuid_str);
00063 }
00064 
00065 class NodeletArgumentParsing
00066 {
00067   private:
00068     std::string command_;
00069     std::string type_;
00070     std::string name_;
00071     std::string default_name_;
00072     std::string manager_;
00073     std::vector<std::string> local_args_;
00074     bool is_bond_enabled_;
00075   
00076   public:
00077     //NodeletArgumentParsing() { };
00078     bool
00079       parseArgs(int argc, char** argv)
00080     {
00081       is_bond_enabled_ = true;
00082       std::vector<std::string> non_ros_args;
00083       ros::removeROSArgs (argc, argv, non_ros_args);
00084       size_t used_args = 0;
00085 
00086       if (non_ros_args.size() > 1)
00087         command_ = non_ros_args[1];
00088       else
00089         return false;
00090 
00091       if (command_ == "load" && non_ros_args.size() > 3)
00092       {
00093         type_ = non_ros_args[2];
00094         manager_ = non_ros_args[3];
00095         used_args = 4;
00096 
00097         if (non_ros_args.size() > used_args)
00098         {
00099           if (non_ros_args[used_args] == "--no-bond")
00100           {
00101             is_bond_enabled_ = false;
00102             ++used_args;
00103           }
00104         }
00105       }
00106 
00107       if (non_ros_args.size() > 2)
00108       {
00109         if (command_ == "unload")
00110         {
00111           name_    = non_ros_args[2];
00112           manager_ = non_ros_args[3];
00113           used_args = 4;
00114         }
00115         else if (command_ == "standalone")
00116         {
00117           type_ = non_ros_args[2];
00118           printf("type is %s\n", type_.c_str());
00119           used_args = 3;
00120         }
00121       }
00122 
00123       if (command_ == "manager")
00124         used_args = 2;
00125 
00126       for (size_t i = used_args; i < non_ros_args.size(); i++)
00127         local_args_.push_back(non_ros_args[i]);
00128 
00129 
00130       if (used_args > 0) return true;
00131       else return false;
00132 
00133     };
00134 
00135 
00136     std::string getCommand () const { return (command_); }
00137     std::string getType () const    { return (type_);    }
00138     std::string getName () const    { return (name_);    }
00139     std::string getManager() const  { return (manager_); }
00140     bool isBondEnabled() const { return is_bond_enabled_; }
00141 
00142     std::vector<std::string> getMyArgv () const {return local_args_;};
00143     std::string getDefaultName()
00144     {
00145       std::string s = type_;
00146       replace(s.begin(), s.end(), '/', '_');
00147       return s;
00148     };
00149 
00150 };
00151 
00152 
00153 class NodeletInterface
00154 {
00155   public:
00157 
00158     bool
00159       unloadNodelet (const std::string &name, const std::string &manager)
00160     {
00161       ROS_INFO_STREAM ("Unloading nodelet " << name << " from manager " << manager);
00162 
00163       std::string service_name = manager + "/unload_nodelet";
00164       // Check if the service exists and is available
00165       if (!ros::service::exists (service_name, true))
00166       {
00167         // Probably the manager has shut down already, which is fine
00168         ROS_WARN("Couldn't find service %s, perhaps the manager is already shut down",
00169                  service_name.c_str());
00170         return (false);
00171       }
00172 
00173       ros::ServiceClient client = n_.serviceClient<nodelet::NodeletUnload> (service_name);
00174       //client.waitForExistence ();
00175 
00176       // Call the service
00177       nodelet::NodeletUnload srv;
00178       srv.request.name = name;
00179       if (!client.call (srv))
00180       {
00181         // Maybe service shut down in the meantime, which isn't an error
00182         if (ros::service::exists(service_name, false))
00183           ROS_FATAL("Service call failed!");
00184         return (false);
00185       }
00186       return (true);
00187     }
00188 
00190 
00191     bool
00192       loadNodelet (const std::string &name, const std::string &type,
00193                    const std::string &manager, const std::vector<std::string> &args,
00194                    const std::string &bond_id)
00195     {
00196       ros::M_string remappings = ros::names::getRemappings ();
00197       std::vector<std::string> sources (remappings.size ()), targets (remappings.size ());
00198       ROS_INFO_STREAM ("Loading nodelet " << name << " of type " << type << " to manager " << manager << " with the following remappings:");
00199       int i = 0;
00200       for (ros::M_string::iterator it = remappings.begin (); it != remappings.end (); ++it, ++i)
00201       {
00202         sources[i] = (*it).first;
00203         targets[i] = (*it).second;
00204         ROS_INFO_STREAM (sources[i] << " -> " << targets[i]);
00205       }
00206 
00207       // Get and set the parameters
00208       XmlRpc::XmlRpcValue param;
00209       std::string node_name = ros::this_node::getName ();
00210       n_.getParam (node_name, param);
00211       n_.setParam (name, param);
00212 
00213       std::string service_name = std::string (manager) + "/load_nodelet";
00214 
00215       // Wait until the service is advertised
00216       ROS_DEBUG ("Waiting for service %s to be available...", service_name.c_str ());
00217       ros::ServiceClient client = n_.serviceClient<nodelet::NodeletLoad> (service_name);
00218       client.waitForExistence ();
00219 
00220       // Call the service
00221       nodelet::NodeletLoad srv;
00222       srv.request.name = std::string (name);
00223       srv.request.type = std::string (type);
00224       srv.request.remap_source_args = sources;
00225       srv.request.remap_target_args = targets;
00226       srv.request.my_argv = args;
00227       srv.request.bond_id = bond_id;
00228       if (!client.call (srv))
00229       {
00230         ROS_FATAL("Service call failed!");
00231         return false;
00232       }
00233       return true;
00234     }
00235   private:
00236     ros::NodeHandle n_;
00237 };
00238 
00239 void print_usage(int argc, char** argv)
00240 {
00241   printf("Your usage: \n");
00242   for (int i = 0; i < argc; i++)
00243     printf("%s ", argv[i]);
00244   printf("\nnodelet usage:\n");
00245   printf("nodelet load pkg/Type manager [--no-bond]  - Launch a nodelet of type pkg/Type on manager manager\n");
00246   printf("nodelet standalone pkg/Type   - Launch a nodelet of type pkg/Type in a standalone node\n");
00247   printf("nodelet unload name manager   - Unload a nodelet a nodelet by name from manager\n");
00248   printf("nodelet manager               - Launch a nodelet manager node\n");
00249 
00250 };
00251 
00252 sig_atomic_t volatile request_shutdown = 0;
00253 
00254 void nodeletLoaderSigIntHandler(int sig)
00255 {
00256   request_shutdown = 1;
00257 }
00258 
00259 // Shutdown can be triggered externally by an XML-RPC call, this is how "rosnode kill"
00260 // works. When shutting down a "nodelet load" we always want to unload the nodelet
00261 // before shutting down our ROS comm channels, so we override the default roscpp
00262 // handler for a "shutdown" XML-RPC call.
00263 void shutdownCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
00264 {
00265   int num_params = 0;
00266   if (params.getType() == XmlRpc::XmlRpcValue::TypeArray)
00267     num_params = params.size();
00268   if (num_params > 1)
00269   {
00270     std::string reason = params[1];
00271     ROS_WARN("Shutdown request received. Reason: [%s]", reason.c_str());
00272     request_shutdown = 1;
00273   }
00274 
00275   result = ros::xmlrpc::responseInt(1, "", 0);
00276 }
00277 
00278 /* ---[ */
00279 int
00280   main (int argc, char** argv)
00281 {
00282   NodeletArgumentParsing arg_parser;
00283 
00284   if (!arg_parser.parseArgs(argc, argv))
00285   {
00286     print_usage(argc, argv);
00287     return (-1);
00288   }
00289   std::string command = arg_parser.getCommand();
00290 
00291 
00292   if (command == "manager")
00293   {
00294     ros::init (argc, argv, "manager");
00295     nodelet::Loader n;
00296     ros::spin ();
00297   }
00298   else if (command == "standalone")
00299   {
00300     ros::init (argc, argv, arg_parser.getDefaultName());
00301 
00302     ros::NodeHandle nh;
00303     nodelet::Loader n(false);
00304     ros::M_string remappings; //Remappings are already applied by ROS no need to generate them.
00305     std::string nodelet_name = ros::this_node::getName ();
00306     std::string nodelet_type = arg_parser.getType();
00307     n.load(nodelet_name, nodelet_type, remappings, arg_parser.getMyArgv());
00308     ROS_DEBUG("Successfully loaded nodelet of type '%s' into name '%s'\n", nodelet_name.c_str(), nodelet_name.c_str());
00309 
00310     ros::spin();
00311   }
00312   else if (command == "load")
00313   {
00314     ros::init (argc, argv, arg_parser.getDefaultName (), ros::init_options::NoSigintHandler);
00315     NodeletInterface ni;
00316     ros::NodeHandle nh;
00317     std::string name = ros::this_node::getName ();
00318     std::string type = arg_parser.getType();
00319     std::string manager = arg_parser.getManager();
00320     std::string bond_id;
00321     if (arg_parser.isBondEnabled())
00322       bond_id = name + "_" + genId();
00323     bond::Bond bond(manager + "/bond", bond_id);
00324     if (!ni.loadNodelet(name, type, manager, arg_parser.getMyArgv(), bond_id))
00325       return -1;
00326 
00327     // Override default exit handlers for roscpp
00328     signal(SIGINT, nodeletLoaderSigIntHandler);
00329     ros::XMLRPCManager::instance()->unbind("shutdown");
00330     ros::XMLRPCManager::instance()->bind("shutdown", shutdownCallback);
00331     
00332     if (arg_parser.isBondEnabled())
00333       bond.start();
00334     // Spin our own loop
00335     while (!request_shutdown)
00336     {
00337       ros::spinOnce();
00338       if (arg_parser.isBondEnabled() && bond.isBroken())
00339       {
00340         ROS_INFO("Bond broken, exiting");
00341         goto shutdown;
00342       }
00343       usleep(100000);
00344     }
00345     // Attempt to unload the nodelet before shutting down ROS
00346     ni.unloadNodelet(name, manager);
00347     if (arg_parser.isBondEnabled())
00348       bond.breakBond();
00349   shutdown:
00350     ros::shutdown();
00351   }
00352   else if (command == "unload")
00353   {
00354     ros::init (argc, argv, arg_parser.getDefaultName ());
00355     std::string name = arg_parser.getName ();
00356     std::string manager = arg_parser.getManager();
00357     NodeletInterface ni;
00358     if (!ni.unloadNodelet(name, manager))
00359       return -1;
00360   }
00361   else
00362   {
00363     ros::init(argc, argv, "nodelet");
00364     ROS_ERROR("Command %s unknown", command.c_str());
00365     return -1;
00366   }
00367 
00368   return (0);
00369 }
00370 /* ]--- */


nodelet
Author(s): Tully Foote, Radu Bogdan Rusu
autogenerated on Mon Oct 6 2014 02:52:26