def setupSubscriber(self, data):
""" Register a new subscriber. """
try:
msg = TopicInfo()
msg.deserialize(data)
if not msg.topic_name in self.subscribers.keys():
sub = hSubscriber(msg, self)
self.subscribers[msg.topic_name] = sub
self.setSubscribeSize(msg.buffer_size)
rospy.loginfo("Setup hSubscriber on %s [%s]" % (msg.topic_name, msg.message_type) )
elif msg.message_type != self.subscribers[msg.topic_name].message._type:
old_message_type = self.subscribers[msg.topic_name].message._type
self.subscribers[msg.topic_name].unregister()
sub = hSubscriber(msg, self)
self.subscribers[msg.topic_name] = sub
self.setSubscribeSize(msg.buffer_size)
rospy.loginfo("Change the message type of subscriber on %s from [%s] to [%s]" % (msg.topic_name, old_message_type, msg.message_type) )
except Exception as e:
rospy.logerr("Creation of subscriber failed: %s", e)
def setupPublisher(self, data):
""" Register a new publisher. """
try:
msg = TopicInfo()
msg.deserialize(data)
pub = Publisher(msg)
self.publishers[msg.topic_id] = pub
self.callbacks[msg.topic_id] = pub.handlePacket
self.setPublishSize(msg.buffer_size)
rospy.loginfo("Setup publisher on %s [%s]" % (msg.topic_name, msg.message_type) )
except Exception as e:
rospy.logerr("Creation of publisher failed: %s", e)
def setupServiceClientSubscriber(self, data):
""" Register a new service client. """
try:
msg = TopicInfo()
msg.deserialize(data)
self.setSubscribeSize(msg.buffer_size)
try:
srv = self.services[msg.topic_name]
except KeyError:
srv = ServiceClient(msg, self)
rospy.loginfo("Setup service client on %s [%s]" % (msg.topic_name, msg.message_type) )
self.services[msg.topic_name] = srv
if srv.mres._md5sum == msg.md5sum:
srv.id = msg.topic_id
else:
raise Exception('Checksum does not match: ' + srv.mres._md5sum + ',' + msg.md5sum)
except Exception as e:
rospy.logerr("Creation of service client failed: %s", e)
def setupServiceServerPublisher(self, data):
""" Register a new service server. """
try:
msg = TopicInfo()
msg.deserialize(data)
self.setPublishSize(msg.buffer_size)
try:
srv = self.services[msg.topic_name]
except KeyError:
srv = ServiceServer(msg, self)
rospy.loginfo("Setup service server on %s [%s]" % (msg.topic_name, msg.message_type) )
self.services[msg.topic_name] = srv
if srv.mres._md5sum == msg.md5sum:
self.callbacks[msg.topic_id] = srv.handlePacket
else:
raise Exception('Checksum does not match: ' + srv.mres._md5sum + ',' + msg.md5sum)
except Exception as e:
rospy.logerr("Creation of service server failed: %s", e)
def initSerialNode(serial_node, node_data):
debug = rospy.get_param('~debug', False);
# To subscribe to atopic through rosserial means to create a local publisher that
# forwards rosserial data to the local ROS network
subscriber_list = rospy.get_param('~subscriber_list',[])
topic_idx = 0
for topic in subscriber_list:
# only create topic if we are explicitly subscribing to this node
# i.e topic['node_name'] == serial.node.node_data['node_id']
# or topic['node_name'] == 'broadcast'
if debug:
rospy.loginfo("Expected topic node_name [%s]. Discovered node name [%s]."%(topic['node_name'],node.node_data))
if topic['node_name'] != node_data['node_id'] or topic['node_name'] != 'broadcast':
pass
if debug:
rospy.loginfo("Trying to create serial subscriber")
topic_idx = topic_idx+1
msg = topic['type']
m = load_message(msg['package'],msg['name'])
ti = TopicInfo()
ti.topic_id=100+topic_idx
ti.topic_name = topic['topic']
ti.message_type = "%s/%s"%(msg['package'],msg['name'])
ti.md5sum = m._md5sum
ti.buffer_size = 512
_buffer = StringIO.StringIO()
ti.serialize(_buffer)
serial_node.setupPublisher(_buffer.getvalue())
# To publish to a topic through rosserial means to create a local subscriber that
# pushes ROS messages into the rosserial link
publisher_list = rospy.get_param('~publisher_list',[])
for topic in publisher_list:
# only create topic if we are explicitly publishing to this node
# i.e topic['node_name'] == serial.node.node_data['node_id']
# or topic['node_name'] == 'broadcast'
if debug:
rospy.loginfo("Expected topic node_name [%s]. Discovered node name [%s]."%(topic['node_name'],node.node_data))
if topic['node_name'] != node_data['node_id'] or topic['node_name'] != 'broadcast':
pass
if debug:
rospy.loginfo("Trying to create serial publisher")
topic_idx = topic_idx+1
msg = topic['type']
m = load_message(msg['package'],msg['name'])
ti = TopicInfo()
ti.topic_id=200+topic_idx
ti.topic_name = topic['topic']
ti.message_type = "%s/%s"%(msg['package'],msg['name'])
ti.md5sum = m._md5sum
ti.buffer_size = 512
_buffer = StringIO.StringIO()
ti.serialize(_buffer)
serial_node.setupSubscriber(_buffer.getvalue())
#service_client_list = rospy.get_param('~service_client_list',[])
#service_server_list = rospy.get_param('~service_server_list',[])
serial_node.negotiateTopics()
#-------------------------------------------------------------------------------
# MAIN FUNCTION
#-------------------------------------------------------------------------------
# Init Serial
ser = serial.Serial(port='/dev/ttyUSB0',
baudrate=115200,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
bytesize=serial.EIGHTBITS,
timeout=0)
# Setup Publisher Info
pub_topic_info = TopicInfo()
pub_topic_info.topic_id = 101
pub_topic_info.topic_name = "/marty/chatter"
pub_topic_info.message_type = "std_msgs/String"
pub_topic_info.md5sum = MD5_String
pub_topic_info.buffer_size = 256
# Initialise Pub Message
pub_msg = String()
pub_msg.data = "Testing!"
# Setup Subscriber Info
sub_topic_info = TopicInfo()
sub_topic_info.topic_id = 103
sub_topic_info.topic_name = "/marty/servo_array"
# sub_topic_info.message_type = "std_msgs/Int8"
# sub_topic_info.md5sum = MD5_Int8