Go to the documentation of this file.00001
00002
00003
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()