event_launcher.py
Go to the documentation of this file.
00001 # -*- coding: utf-8 -*-
00002 # vim:set ts=4 sw=4 et:
00003 #
00004 # Copyright 2015 Vladimir Ermakov.
00005 #
00006 # This file is part of the mavros package and subject to the license terms
00007 # in the top-level LICENSE file of the mavros repository.
00008 # https://github.com/mavlink/mavros/tree/master/LICENSE.md
00009 
00010 from __future__ import print_function
00011 
00012 import os
00013 import sys
00014 import time
00015 import shlex
00016 import rospy
00017 import mavros
00018 import signal
00019 import argparse
00020 import threading
00021 import subprocess
00022 
00023 from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse
00024 from mavros_msgs.msg import State
00025 
00026 
00027 class EventHandler(object):
00028     """
00029     Base class for event handlers
00030     """
00031 
00032     __slots__ = [
00033         'name', 'events', 'actions', 'lock',
00034     ]
00035 
00036     def __init__(self, name, events=[], actions=[]):
00037         self.name = name
00038         self.events = events
00039         self.actions = actions
00040         self.lock = threading.RLock()
00041 
00042     def __str__(self):
00043         return self.name
00044 
00045     def __call__(self, event):
00046         if event not in self.events:
00047             return  # not our event
00048 
00049         idx = self.events.index(event)
00050         action = self.actions[idx]
00051 
00052         if hasattr(self, 'action_' + action):
00053             getattr(self, 'action_' + action)()
00054         else:
00055             rospy.logerr("Misconfiguration of %s, there no action '%s'",
00056                          self, action)
00057 
00058     def spin_once(self):
00059         raise NotImplementedError
00060 
00061 
00062 class ShellHandler(EventHandler):
00063     """
00064     Simple program runner
00065     """
00066 
00067     # XXX TODO:
00068     #   simple readlines() from Popen.stdout and .stderr simply don't work - it blocks
00069     #   so need to find a way for such logging, but for now i don't process process output.
00070 
00071     __slots__ = [
00072         'process', 'command', 'args', 'logfile',
00073     ]
00074 
00075     def __init__(self, name, command, args=[], logfile=None, events=[], actions=[]):
00076         super(ShellHandler, self).__init__(name, events, actions)
00077         self.process = None
00078         self.command = command
00079         self.args = args
00080         self.logfile = logfile
00081 
00082     def action_run(self):
00083         with self.lock:
00084             if self.process:
00085                 rospy.loginfo("%s: process still active, terminating before run", str(self))
00086                 self.action_stop()
00087 
00088             rospy.loginfo("%s: starting...", self)
00089 
00090             args = [self.command] + self.args
00091             if self.logfile:
00092                 child_stdout = open(self.logfile, 'a')
00093                 child_stderr = subprocess.STDOUT
00094             else:
00095                 child_stdout = subprocess.PIPE
00096                 child_stderr = subprocess.PIPE
00097 
00098             if hasattr(child_stdout, 'write'):
00099                 child_stdout.write("\n--- run cut: %s ---\n" % time.ctime())
00100 
00101             self.process = subprocess.Popen(args, stdout=child_stdout, stderr=child_stderr,
00102                                             close_fds=True, preexec_fn=os.setsid)
00103 
00104             poll_result = self.process.poll()
00105             if poll_result is None or poll_result == 0:
00106                 rospy.loginfo("%s: started, pid: %s", self, self.process.pid)
00107             else:
00108                 rospy.logerr("Failed to start '%s'", self)
00109                 self.process = None
00110 
00111     def action_stop(self):
00112         with self.lock:
00113             if not self.process:
00114                 return
00115 
00116             rospy.loginfo("%s: stoppig...", self)
00117 
00118             # code from roslaunch.nodeprocess _stop_unix
00119             pid = self.process.pid
00120             pgid = os.getpgid(pid)
00121 
00122             def poll_timeout(timeout):
00123                 timeout_t = time.time() + timeout
00124                 retcode = self.process.poll()
00125                 while time.time() < timeout_t and retcode is None:
00126                     time.sleep(0.1)
00127                     retcode = self.process.poll()
00128                 return retcode
00129 
00130             try:
00131                 rospy.loginfo("%s: sending SIGINT to pid [%s] pgid [%s]", self, pid, pgid)
00132                 os.killpg(pgid, signal.SIGINT)
00133 
00134                 retcode = poll_timeout(15.0)
00135                 if retcode is None:
00136                     rospy.logwarn("%s: escalating to SIGTERM", self)
00137                     os.killpg(pgid, signal.SIGTERM)
00138 
00139                     retcode = poll_timeout(2.0)
00140                     if retcode is None:
00141                         rospy.logerr("%s: escalating to SIGKILL, may still be running", self)
00142                         try:
00143                             os.killpg(pgid, signal.SIGKILL)
00144                         except OSError as ex:
00145                             rospy.logerr("%s: %s", self, ex)
00146 
00147                 if retcode is not None:
00148                     rospy.loginfo("%s: process (pid %s) terminated, exit code: %s",
00149                                   self, pid, retcode)
00150             finally:
00151                 self.process = None
00152 
00153     def spin_once(self):
00154         with self.lock:
00155             if self.process:
00156                 poll_result = self.process.poll()
00157                 if poll_result is not None:
00158                     rospy.loginfo("%s: process (pid %s) terminated, exit code: %s",
00159                                   self, self.process.pid, poll_result)
00160                     self.process = None
00161 
00162 
00163 # NOTE: here was RosrunHandler and RoslaunchHandler
00164 #       but roslaunch.scriptapi can't launch node from worker thread
00165 #       so i decided to leave only ShellHandler.
00166 
00167 
00168 class Launcher(object):
00169     __slots__ = [
00170         'handlers',
00171         'known_events',
00172         'triggers',
00173         'prev_armed',
00174         'state_sub',
00175     ]
00176 
00177     def __init__(self):
00178         self.handlers = []
00179         self.known_events = ['armed', 'disarmed']
00180         self.triggers = {}
00181         self.prev_armed = False
00182 
00183         try:
00184             params = rospy.get_param('~')
00185             if not isinstance(params, dict):
00186                 raise KeyError("bad configuration")
00187         except KeyError as e:
00188             rospy.logerr('Config error: %s', e)
00189             return
00190 
00191         # load configuration
00192         for k, v in params.iteritems():
00193             # TODO: add checks for mutually exclusive options
00194 
00195             if 'service' in v:
00196                 self._load_trigger(k, v)
00197             elif 'shell' in v:
00198                 self._load_shell(k, v)
00199             else:
00200                 rospy.logwarn("Skipping unknown section: %s", k)
00201 
00202         # check that events are known
00203         rospy.loginfo("Known events: %s", ', '.join(self.known_events))
00204         for h in self.handlers:
00205             for evt in h.events:
00206                 if evt not in self.known_events:
00207                     rospy.logwarn("%s: unknown event: %s", h.name, evt)
00208 
00209         # config loaded, we may subscribe
00210         self.state_sub = rospy.Subscriber(
00211             mavros.get_topic('state'),
00212             State,
00213             self.mavros_state_cb)
00214 
00215     def _load_trigger(self, name, params):
00216         rospy.logdebug("Loading trigger: %s", name)
00217 
00218         def gen_cb(event):
00219             def cb(req):
00220                 self(event)
00221                 return TriggerResponse(success=True)    # try later to check success
00222             return cb
00223 
00224         self.known_events.append(name)
00225         self.triggers[name] = rospy.Service(params['service'], Trigger, gen_cb(name))
00226         rospy.loginfo("Trigger: %s (%s)", name, params['service'])
00227 
00228     def _load_shell(self, name, params):
00229         rospy.logdebug("Loading shell: %s", name)
00230 
00231         events, actions = self._get_evt_act(params)
00232 
00233         def expandpath(p):
00234             return os.path.expanduser(os.path.expandvars(p))
00235 
00236         args = params['shell']
00237         if not isinstance(args, list):
00238             args = shlex.split(args)
00239 
00240         command = expandpath(args[0])
00241         args = args[1:]
00242 
00243         logfile = params.get('logfile')
00244         if logfile:
00245             logfile = expandpath(logfile)
00246 
00247         rospy.loginfo("Shell: %s (%s)", name, ' '.join([command] + [repr(v) for v in args]))
00248         if logfile:
00249             rospy.loginfo("Log: %s -> %s", name, logfile)
00250 
00251         handler = ShellHandler(name, command, args, logfile, events, actions)
00252         self.handlers.append(handler)
00253 
00254     def _get_evt_act(self, params):
00255         evt = self._param_to_list(params['event'])
00256         act = self._param_to_list(params['action'])
00257         if len(evt) != len(act):
00258             raise ValueError("event and action fileds has different size!")
00259         return evt, act
00260 
00261     def _param_to_list(self, str_or_list):
00262         if isinstance(str_or_list, list):
00263             return [it.strip() for it in str_or_list]
00264         else:
00265             ret = []
00266             for it in str_or_list.split():
00267                 ret.extend([v.strip() for v in it.split(',') if v])
00268             return ret
00269 
00270     def __call__(self, event):
00271         rospy.logdebug('Event: %s', event)
00272         for h in self.handlers:
00273             try:
00274                 h(event)
00275             except Exception as ex:
00276                 import traceback
00277                 rospy.logerr("Event %s -> %s exception: %s", event, h, ex)
00278                 rospy.logerr(traceback.format_exc())
00279 
00280     def spin(self):
00281         if not self.handlers:
00282             rospy.logwarn("No event handlers defined, terminating.")
00283             return
00284 
00285         rate = rospy.Rate(1.0)
00286         while not rospy.is_shutdown():
00287             for h in self.handlers:
00288                 h.spin_once()
00289 
00290             rate.sleep()
00291 
00292     def mavros_state_cb(self, msg):
00293         if msg.armed != self.prev_armed:
00294             self.prev_armed = msg.armed
00295             self('armed' if msg.armed else 'disarmed')
00296 
00297 
00298 def main():
00299     parser = argparse.ArgumentParser(
00300         description="This node launch/terminate processes on events.")
00301     parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE)
00302 
00303     args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:])
00304 
00305     rospy.init_node("event_launcher")
00306     mavros.set_namespace(args.mavros_ns)
00307 
00308     rospy.loginfo("Starting event launcher...")
00309 
00310     launcher = Launcher()
00311     launcher.spin()


mavros
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:19