roseus_bridge.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 # Author: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>
00004 
00005 
00006 import rospy
00007 import os
00008 import sys
00009 import errno
00010 import subprocess
00011 import signal
00012 from threading import Thread
00013 try:
00014     from Queue import Queue, Empty
00015 except ImportError:
00016     from queue import Queue, Empty
00017 from roseus_remote.msg import RawCommand
00018 
00019 ON_POSIX = 'posix' in sys.builtin_module_names
00020 BUFF_SIZE = 1
00021 
00022 
00023 class ROSEUSBridgeNode:
00024     def __init__(self):
00025         self.pub = rospy.Publisher('output', RawCommand)
00026         self.sub = rospy.Subscriber('input', RawCommand, self.raw_command_cb)
00027         rospy.on_shutdown(self._on_node_shutdown)
00028         while not rospy.is_shutdown():
00029             self.launch_roseus()
00030             self.main_loop()
00031             rospy.loginfo("respawning...")
00032             self.kill_roseus()
00033 
00034     def get_output(self, out, queue):
00035         while self.roseus_process.poll() is None:
00036             data = out.read(BUFF_SIZE)
00037             if data:
00038                 queue.put(str(data))
00039         else:
00040             rospy.logerr("thread is dead")
00041 
00042     def launch_roseus(self):
00043         cmd = ['rosrun', 'roseus', 'roseus']
00044         self.roseus_process = subprocess.Popen(cmd,
00045                                                stdout=subprocess.PIPE,
00046                                                stderr=subprocess.PIPE,
00047                                                stdin=subprocess.PIPE,
00048                                                bufsize=BUFF_SIZE,
00049                                                close_fds=ON_POSIX,
00050                                                env=os.environ.copy(),
00051                                                preexec_fn=os.setpgrp)
00052         self.stdout_queue = Queue()
00053         self.stderr_queue = Queue()
00054         self.received_cmd = Queue()
00055 
00056         self.get_stdout_thread = Thread(target=self.get_output,
00057                                         args=(self.roseus_process.stdout,
00058                                               self.stdout_queue))
00059         self.get_stdout_thread.daemon = True
00060         self.get_stderr_thread = Thread(target=self.get_output,
00061                                         args=(self.roseus_process.stderr,
00062                                               self.stderr_queue))
00063         self.get_stderr_thread.daemon = True
00064         self.get_stdout_thread.start()
00065         self.get_stderr_thread.start()
00066 
00067     def kill_roseus(self):
00068         try:
00069             rospy.loginfo("send SIGTERM to roseus")
00070             self.roseus_process.terminate()
00071         except Exception as e:
00072             rospy.logwarn("escalated to kill")
00073             try:
00074                 self.roseus_process.kill()
00075             except Exception as e:
00076                 rospy.logerr('could not kill roseus: ' + str(e))
00077 
00078     def main_loop(self):
00079         while self.roseus_process.poll() is None:
00080             stdout = ""
00081             stderr = ""
00082             try:
00083                 cmd = self.received_cmd.get_nowait()
00084                 rospy.logdebug("write to stdin: " + cmd)
00085                 self.roseus_process.stdin.write(cmd)
00086                 self.roseus_process.stdin.flush()
00087             except IOError as e:
00088                 if e.errno == errno.EINTR:
00089                     continue
00090             except Empty as e:
00091                 pass
00092             except Exception as e:
00093                 rospy.logwarn('error: ' + str(e))
00094             try:
00095                 while not self.stdout_queue.empty():
00096                     stdout += self.stdout_queue.get_nowait()
00097             except Empty as e:
00098                 pass
00099             try:
00100                 while not self.stderr_queue.empty():
00101                     stderr += self.stderr_queue.get_nowait()
00102             except Empty as e:
00103                 pass
00104             except Exception as e:
00105                 rospy.logwarn('error: ' + str(e))
00106             if stdout != "":
00107                 self.pub.publish(stdout.strip())
00108                 rospy.logdebug("stdout: " + stdout)
00109             if stderr != "":
00110                 self.pub.publish(stderr.strip())
00111                 rospy.logdebug("stderr: " + stderr)
00112             rospy.sleep(0.1)
00113         else:
00114             rospy.logwarn("roseus process has been stopped.")
00115 
00116         if self.roseus_process.returncode != 0:
00117             rospy.logerr('roseus process exits abnormally with exit code: ' + str(self.roseus_process.returncode))
00118 
00119     def raw_command_cb(self, msg):
00120         cmd_str = str(msg.data).rstrip(' \t\r\n\0') + os.linesep
00121         self.received_cmd.put(cmd_str)
00122 
00123     def _on_node_shutdown(self):
00124         self.kill_roseus()
00125 
00126 if __name__ == '__main__':
00127     rospy.init_node('roseus_bridge', anonymous=True)
00128     node = ROSEUSBridgeNode()
00129     rospy.spin()


roseus_remote
Author(s): Yuki Furuta
autogenerated on Sat Jul 1 2017 02:43:50