ros2ros.py
Go to the documentation of this file.
00001 #!/usr/bin/python
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()


rosbridge
Author(s): Graylin Trevor Jay
autogenerated on Fri Jan 3 2014 11:11:38