00001
00002
00003
00004
00005
00006
00007
00008
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
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
00068
00069
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
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
00164
00165
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
00192 for k, v in params.iteritems():
00193
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
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
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)
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()