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 try:
00021 incoming_dict = decoder.decode(incoming)
00022 msg_type = type_lookup[incoming_dict['receiver']]
00023 except:
00024 continue
00025 outgoing_dict = dict()
00026 outgoing_dict['receiver'] = incoming_dict['receiver']
00027 outgoing_dict['msg'] = incoming_dict['msg']
00028 outgoing_dict['type'] = msg_type
00029 outgoing = encoder.encode(outgoing_dict)
00030 outgoing = '\x00' + outgoing + '\xff'
00031 destination_socket.send(outgoing)
00032
00033 if __name__ == "__main__":
00034
00035 rospy.init_node('ros2ros')
00036 rospy.sleep(5)
00037
00038 host1_address = rospy.get_param('/brown/ros2ros/host1_address', '127.0.0.1')
00039 host1_port = rospy.get_param('/brown/ros2ros/host1_port', 9090)
00040 host2_address = rospy.get_param('/brown/ros2ros/host2_address', '127.0.0.1')
00041 host2_port = rospy.get_param('/brown/ros2ros/host2_port', 9090)
00042 host1_topics = rospy.get_param('/brown/ros2ros/host1_topics', [])
00043 host2_topics = rospy.get_param('/brown/ros2ros/host2_topics', [])
00044 host1_topic_types = rospy.get_param('/brown/ros2ros/host1_topic_types', [])
00045 host2_topic_types = rospy.get_param('/brown/ros2ros/host2_topic_types', [])
00046
00047 host1_type_lookup = dict(zip(host1_topics, host1_topic_types))
00048 host2_type_lookup = dict(zip(host2_topics, host2_topic_types))
00049
00050 host1_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
00051 host1_sock.connect((host1_address, host1_port))
00052 host1_sock.send('raw\r\n\r\n')
00053
00054 host2_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
00055 host2_sock.connect((host2_address, host2_port))
00056 host2_sock.send('raw\r\n\r\n')
00057
00058 rospy.sleep(5)
00059
00060 thread.start_new_thread(serve,(host2_sock, host1_sock, host2_type_lookup))
00061 thread.start_new_thread(serve,(host1_sock, host2_sock, host1_type_lookup))
00062
00063 while not rospy.is_shutdown():
00064 rospy.spin()
