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 #include "rosspawn/NodeAction.h"
00027 #include "rosspawn/ListAvailable.h"
00028 #include "rosspawn/ListLoaded.h"
00029 #include "rosspawn/NodeEvent.h"
00030
00031 #include <ros/ros.h>
00032 #include <ros/console.h>
00033 #include <rospack/rospack.h>
00034
00035 #include <map>
00036 #include <string>
00037 #include <utility>
00038 #include <fstream>
00039
00040 #include <boost/thread/mutex.hpp>
00041 #include <boost/thread/thread.hpp>
00042 #include <boost/thread/condition.hpp>
00043
00044 #include <sys/types.h>
00045 #include <sys/wait.h>
00046 #include <sys/stat.h>
00047 #include <dirent.h>
00048 #include <signal.h>
00049 #include <unistd.h>
00050 #include <regex.h>
00051
00052
00053
00054 const char *test_paths[] = { "%s/bin/%s" };
00055
00056 class RosSpawnMain
00057 {
00058 public:
00059 RosSpawnMain(ros::NodeHandle &n)
00060 : __n(n)
00061 {
00062 __children_wait_thread = boost::thread(boost::bind(&RosSpawnMain::wait_thread, this));
00063 if (regcomp(&__re_alnum, "^[[:alnum:]]+$", REG_EXTENDED) != 0) {
00064 throw ros::Exception("Failed to compile regex");
00065 }
00066
00067 __use_acceptable_modules_file = n.getParam("/rosspawn/acceptable_modules_file",
00068 __acceptable_modules_file);
00069
00070 rospack::ROSPack rospack;
00071
00072 if (__use_acceptable_modules_file) {
00073 ROS_INFO("Using acceptable modules file %s", __acceptable_modules_file.c_str());
00074 std::ifstream f(__acceptable_modules_file.c_str());
00075 while (! (f.fail() || f.eof()) ) {
00076 std::string mod;
00077 f >> mod;
00078 for (rospack::VecPkg::iterator i = rospack::Package::pkgs.begin();
00079 i != rospack::Package::pkgs.end(); ++i) {
00080 if ((*i)->name == mod) {
00081 __search_paths.push_back((*i)->path);
00082 }
00083 }
00084 }
00085 } else {
00086 std::string ros_root_bin = getenv("ROS_ROOT");
00087 if (ros_root_bin == "") {
00088 throw ros::Exception("Failed to read ROS_ROOT environment variable");
00089 }
00090 ros_root_bin += "/bin";
00091 __search_paths.push_back(ros_root_bin);
00092 for (rospack::VecPkg::iterator i = rospack::Package::pkgs.begin();
00093 i != rospack::Package::pkgs.end(); ++i) {
00094 __search_paths.push_back((*i)->path);
00095 }
00096 }
00097
00098 __srv_start = n.advertiseService("/rosspawn/start", &RosSpawnMain::start_node, this);
00099 __srv_stop = n.advertiseService("/rosspawn/stop", &RosSpawnMain::stop_node, this);
00100 __srv_pause = n.advertiseService("/rosspawn/pause", &RosSpawnMain::pause_node, this);
00101 __srv_continue = n.advertiseService("/rosspawn/continue", &RosSpawnMain::continue_node, this);
00102 __srv_list_loaded = n.advertiseService("/rosspawn/list_loaded",
00103 &RosSpawnMain::list_loaded, this);
00104 __srv_list_avail = n.advertiseService("/rosspawn/list_available",
00105 &RosSpawnMain::list_available, this);
00106
00107 __pub_node_events = n.advertise<rosspawn::NodeEvent>("rosspawn/node_events", 10);
00108 }
00109
00110 ~RosSpawnMain()
00111 {
00112
00113
00114
00115 void *dont_care;
00116 pthread_cancel(__children_wait_thread.native_handle());
00117 pthread_join(__children_wait_thread.native_handle(), &dont_care);
00118 }
00119
00120 std::string find_valid(std::string &progname)
00121 {
00122 if (regexec(&__re_alnum, progname.c_str(), 0, NULL, 0) == REG_NOMATCH) {
00123 throw ros::Exception("Invalid program name");
00124 }
00125
00126 for (ChildrenMap::iterator i = __children.begin(); i != __children.end(); ++i) {
00127 if (i->second.first == progname) {
00128 throw ros::Exception("Program is already running");
00129 }
00130 }
00131
00132 for (std::list<std::string>::iterator i = __search_paths.begin();
00133 i != __search_paths.end(); ++i) {
00134 for (unsigned int j = 0; (j < sizeof(test_paths) / sizeof(const char *)); ++j) {
00135 char *tmp;
00136 if (asprintf(&tmp, test_paths[j], i->c_str(), progname.c_str()) != -1) {
00137 ROS_DEBUG("Trying %s", tmp);
00138 std::string path = tmp;
00139 free(tmp);
00140 if (access(path.c_str(), X_OK) == 0) {
00141 return path;
00142 }
00143 }
00144 }
00145 }
00146
00147 throw ros::Exception("No program with the requested name found");
00148 }
00149
00150 bool fork_and_exec(std::string &progname)
00151 {
00152 std::string p;
00153 try {
00154 p = find_valid(progname);
00155 } catch (ros::Exception &e) {
00156 ROS_ERROR("Error starting %s: %s", progname.c_str(), e.what());
00157 return false;
00158 }
00159
00160 pid_t pid = fork();
00161 if (pid == -1) {
00162 return false;
00163 } else if (pid == 0) {
00164
00165 setsid();
00166 signal(SIGINT, SIG_IGN);
00167 ROS_INFO("Running %s from path %s", progname.c_str(), p.c_str());
00168 fclose(stdout);
00169 fclose(stdin);
00170 fclose(stderr);
00171 execl(p.c_str(), p.c_str(), NULL);
00172 } else {
00173 ROS_DEBUG("Child PID %i", pid);
00174 boost::mutex::scoped_lock lock(__children_mutex);
00175 __children[pid] = make_pair(progname, p);
00176 if (__children.size() == 1) {
00177 __children_cond.notify_all();
00178 }
00179 rosspawn::NodeEvent msg;
00180 msg.header.stamp.setNow(ros::Time::now());
00181 msg.event_type = rosspawn::NodeEvent::NODE_STARTED;
00182 msg.node_name = progname;
00183 __pub_node_events.publish(msg);
00184 }
00185
00186 return true;
00187 }
00188
00189 void wait_thread()
00190 {
00191 while (ros::ok()) {
00192 boost::unique_lock<boost::mutex> lock(__children_mutex);
00193 while (__children.empty()) {
00194 __children_cond.wait(lock);
00195 }
00196
00197 int status = 0;
00198 lock.unlock();
00199 pid_t pid = waitpid(-1, &status, WUNTRACED | WCONTINUED);
00200 if (pid == -1) continue;
00201 lock.lock();
00202
00203 rosspawn::NodeEvent msg;
00204 msg.event_type = rosspawn::NodeEvent::NODE_DIED;
00205 msg.node_name = __children[pid].first;
00206
00207
00208 if (WIFEXITED(status)) {
00209 ROS_INFO("%i/%s exited, status=%d", pid,
00210 __children[pid].first.c_str(), WEXITSTATUS(status));
00211 char *tmp;
00212 if (asprintf(&tmp, "%s (PID %i) exited, status=%d",
00213 __children[pid].first.c_str(), pid, WEXITSTATUS(status)) != -1) {
00214 msg.message = tmp;
00215 free(tmp);
00216 }
00217 } else if (WIFSIGNALED(status)) {
00218 ROS_INFO("%i/%s killed by signal %d", pid,
00219 __children[pid].first.c_str(), WTERMSIG(status));
00220 char *tmp;
00221 if (asprintf(&tmp, "%s (PID %i) killed by signal %d",
00222 __children[pid].first.c_str(), pid, WTERMSIG(status)) != -1) {
00223 msg.message = tmp;
00224 free(tmp);
00225 }
00226 } else if (WIFSTOPPED(status)) {
00227 ROS_INFO("%i/%s stopped by signal %d", pid,
00228 __children[pid].first.c_str(), WSTOPSIG(status));
00229 char *tmp;
00230 msg.event_type = rosspawn::NodeEvent::NODE_PAUSED;
00231 if (asprintf(&tmp, "%s (PID %i) stopped by signal %d",
00232 __children[pid].first.c_str(), pid, WSTOPSIG(status)) != -1) {
00233 msg.message = tmp;
00234 free(tmp);
00235 }
00236 } else if (WIFCONTINUED(status)) {
00237 ROS_INFO("%i/%s continued", pid, __children[pid].first.c_str());
00238 char *tmp;
00239 msg.event_type = rosspawn::NodeEvent::NODE_CONTINUED;
00240 if (asprintf(&tmp, "%s (PID %i) continued",
00241 __children[pid].first.c_str(), pid) != -1) {
00242 msg.message = tmp;
00243 free(tmp);
00244 }
00245 }
00246
00247 if (WIFEXITED(status) || WIFSIGNALED(status)) {
00248 if (WIFSIGNALED(status)) {
00249 int sig = WTERMSIG(status);
00250 if (sig == SIGSEGV) {
00251
00252 ROS_WARN("Program %s (%s) died with segfault", __children[pid].first.c_str(),
00253 __children[pid].second.c_str());
00254
00255 char *tmp;
00256 msg.event_type |= rosspawn::NodeEvent::NODE_SEGFAULT;
00257 if (asprintf(&tmp, "%s (PID %i) died with segfault",
00258 __children[pid].first.c_str(), pid) != -1) {
00259 msg.message = tmp;
00260 free(tmp);
00261 }
00262 }
00263 }
00264 __children.erase(pid);
00265 }
00266
00267 __pub_node_events.publish(msg);
00268 }
00269 }
00270
00271 std::string get_process_state(pid_t pid)
00272 {
00273 char *procpath;
00274 if (asprintf(&procpath, "/proc/%i/stat", pid) != -1) {
00275 FILE *f = fopen(procpath, "r");
00276 if (f) {
00277 int pid;
00278 char *program;
00279 char state[2]; state[1] = 0;
00280 if (fscanf(f, "%d %as %c", &pid, &program, state) == 3) {
00281 free(program);
00282 return state;
00283 }
00284 fclose(f);
00285 }
00286 free(procpath);
00287 }
00288
00289 return "?";
00290 }
00291
00292 pid_t get_pid(std::string &node_file_name)
00293 {
00294 for (ChildrenMap::iterator i = __children.begin(); i != __children.end(); ++i) {
00295 if (i->second.first == node_file_name) {
00296 return i->first;
00297 }
00298 }
00299 return 0;
00300 }
00301
00302 bool start_node(rosspawn::NodeAction::Request &req,
00303 rosspawn::NodeAction::Response &resp)
00304 {
00305 return fork_and_exec(req.node_file_name);
00306 }
00307
00308 bool send_signal(std::string &node_file_name, int signum)
00309 {
00310 pid_t pid = get_pid(node_file_name);
00311 if (pid != 0) {
00312 ROS_INFO("Sending signal %s (%i) to %s (PID %i)", strsignal(signum), signum,
00313 __children[pid].first.c_str(), pid);
00314 ::kill(pid, signum);
00315 return true;
00316 } else {
00317 return false;
00318 }
00319 }
00320
00321 bool pause_node(rosspawn::NodeAction::Request &req,
00322 rosspawn::NodeAction::Response &resp)
00323 {
00324 return send_signal(req.node_file_name, SIGSTOP);
00325 }
00326
00327 bool continue_node(rosspawn::NodeAction::Request &req,
00328 rosspawn::NodeAction::Response &resp)
00329 {
00330 return send_signal(req.node_file_name, SIGCONT);
00331 }
00332
00333 bool stop_node(rosspawn::NodeAction::Request &req,
00334 rosspawn::NodeAction::Response &resp)
00335 {
00336 pid_t pid = get_pid(req.node_file_name);
00337 if (pid != 0) {
00338 std::string state = get_process_state(pid);
00339 ROS_INFO("Sending signal %s (%i) to %s (PID %i)", strsignal(SIGINT), SIGINT,
00340 __children[pid].first.c_str(), pid);
00341 ::kill(pid, SIGINT);
00342
00343 if (state == "T") {
00344 ROS_INFO("Process %s (PID %i) was stopped, sending %s (%i)",
00345 __children[pid].first.c_str(), pid, strsignal(SIGCONT), SIGCONT);
00346 ::kill(pid, SIGCONT);
00347 }
00348 return true;
00349 } else {
00350 return false;
00351 }
00352 }
00353
00354 bool list_loaded(rosspawn::ListLoaded::Request &req,
00355 rosspawn::ListLoaded::Response &resp)
00356 {
00357 for (ChildrenMap::iterator i = __children.begin(); i != __children.end(); ++i) {
00358 resp.nodes.clear();
00359 resp.nodes.push_back(i->second.first);
00360 }
00361 return true;
00362 }
00363
00364 bool list_available(rosspawn::ListAvailable::Request &req,
00365 rosspawn::ListAvailable::Response &resp)
00366 {
00367 resp.bin_files.clear();
00368 for (std::list<std::string>::iterator i = __search_paths.begin();
00369 i != __search_paths.end(); ++i) {
00370 for (unsigned int j = 0; (j < sizeof(test_paths) / sizeof(const char *)); ++j) {
00371 char *tmp;
00372 if (asprintf(&tmp, test_paths[j], i->c_str(), "") != -1) {
00373 struct stat s;
00374 if (stat(tmp, &s) == 0) {
00375 if (S_ISDIR(s.st_mode) && (access(tmp, X_OK) == 0)) {
00376
00377 DIR *d = opendir(tmp);
00378 if (d != NULL) {
00379 struct dirent de, *deres;
00380 if ((readdir_r(d, &de, &deres) == 0) && (deres != NULL)) {
00381 do {
00382 char *tmp2;
00383 if (asprintf(&tmp2, test_paths[j], i->c_str(), de.d_name) != -1) {
00384 struct stat filestat;
00385 if (stat(tmp2, &filestat) == 0) {
00386 if (S_ISREG(filestat.st_mode) && (access(tmp2, X_OK) == 0)) {
00387 resp.bin_files.push_back(de.d_name);
00388 }
00389 }
00390 free(tmp2);
00391 }
00392 } while ((readdir_r(d, &de, &deres) == 0) && (deres != NULL));
00393 }
00394 closedir(d);
00395 }
00396 }
00397 }
00398 free(tmp);
00399 }
00400 }
00401 }
00402 return true;
00403 }
00404
00405
00406
00407 private:
00408 ros::NodeHandle &__n;
00409 ros::Publisher __pub_node_events;
00410 ros::ServiceServer __srv_start;
00411 ros::ServiceServer __srv_stop;
00412 ros::ServiceServer __srv_pause;
00413 ros::ServiceServer __srv_continue;
00414 ros::ServiceServer __srv_list_loaded;
00415 ros::ServiceServer __srv_list_avail;
00416
00417 std::list<std::string> __search_paths;
00418
00419 typedef std::map<int, std::pair<std::string, std::string> > ChildrenMap;
00420 ChildrenMap __children;
00421 boost::mutex __children_mutex;
00422 boost::condition_variable __children_cond;
00423 boost::thread __children_wait_thread;
00424
00425 bool __use_acceptable_modules_file;
00426 std::string __acceptable_modules_file;
00427 std::list<std::string> __acceptable_modules;
00428
00429 regex_t __re_alnum;
00430 };
00431
00432 int
00433 main(int argc, char **argv)
00434 {
00435 ros::init(argc, argv, "rosspawn");
00436 ros::NodeHandle n;
00437
00438 RosSpawnMain rosspawn(n);
00439
00440 ROS_INFO("Ready to roll");
00441 ros::spin();
00442 }
