00001
00002 import roslib; roslib.load_manifest('rosbridge')
00003 import rospy
00004 import socket, threading, SocketServer
00005 import json
00006 import thread, time
00007
00008 def serve(source_socket, destination_socket, type_lookup):
00009
00010 encoder = json.JSONEncoder()
00011 decoder = json.JSONDecoder()
00012
00013 for topic in type_lookup.keys():
00014 source_socket.send('\x00{"receiver":"/rosbridge/subscribe","msg":["' + topic + '",0,"' + type_lookup[topic] + '"]}\xff')
00015 destination_socket.send('\x00{"receiver":"/rosbridge/publish","msg":["' + topic + '","' + type_lookup[topic] + '"]}\xff')
00016
00017 while True:
00018 incoming = source_socket.recv(1024)
00019 incoming = incoming[1:len(incoming)-1]
00020
00021 try:
00022 incoming_dict = decoder.decode(incoming)
00023 msg_type = type_lookup[incoming_dict['receiver']]
00024 except:
00025 continue
00026 outgoing_dict = dict()
00027 outgoing_dict['receiver'] = incoming_dict['receiver']
00028 outgoing_dict['msg'] = incoming_dict['msg']
00029 outgoing_dict['type'] = msg_type
00030 outgoing = encoder.encode(outgoing_dict)
00031
00032 outgoing = '\x00' + outgoing + '\xff'
00033 destination_socket.send(outgoing)
00034
00035 if __name__ == "__main__":
00036
00037 rospy.init_node('ros2ros')
00038 rospy.sleep(5)
00039
00040 host1_address = rospy.get_param('/brown/ros2ros/host1_address', '127.0.0.1')
00041 host1_port = rospy.get_param('/brown/ros2ros/host1_port', 9090)
00042 host2_address = rospy.get_param('/brown/ros2ros/host2_address', '127.0.0.1')
00043 host2_port = rospy.get_param('/brown/ros2ros/host2_port', 9090)
00044 host1_topics = rospy.get_param('/brown/ros2ros/host1_topics', [])
00045 host2_topics = rospy.get_param('/brown/ros2ros/host2_topics', [])
00046 host1_topic_types = rospy.get_param('/brown/ros2ros/host1_topic_types', [])
00047 host2_topic_types = rospy.get_param('/brown/ros2ros/host2_topic_types', [])
00048
00049 host1_type_lookup = dict(zip(host1_topics, host1_topic_types))
00050 host2_type_lookup = dict(zip(host2_topics, host2_topic_types))
00051
00052 host1_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
00053 host1_sock.connect((host1_address, host1_port))
00054 host1_sock.send('raw\r\n\r\n')
00055
00056 host2_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
00057 host2_sock.connect((host2_address, host2_port))
00058 host2_sock.send('raw\r\n\r\n')
00059
00060 rospy.sleep(5)
00061
00062 thread.start_new_thread(serve,(host2_sock, host1_sock, host2_type_lookup))
00063 thread.start_new_thread(serve,(host1_sock, host2_sock, host1_type_lookup))
00064
00065 while not rospy.is_shutdown():
00066 rospy.spin()
