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
00034
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
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
00165 if (!ros::service::exists (service_name, true))
00166 {
00167
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
00175
00176
00177 nodelet::NodeletUnload srv;
00178 srv.request.name = name;
00179 if (!client.call (srv))
00180 {
00181
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
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
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
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
00260
00261
00262
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;
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
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
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
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