def setupSubscriber(self, data): """ Register a new subscriber. """ try: msg = TopicInfo() msg.deserialize(data) sub = Subscriber(msg, self) self.subscribers[msg.topic_name] = sub self.setSubscribeSize(msg.buffer_size) rospy.loginfo("Setup subscriber on %s [%s]" % (msg.topic_name, 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 handleLoggingRequest(self, data): """ Forward logging information from serial device into ROS. """ msg = Log() msg.deserialize(data) if (msg.level == Log.ROSDEBUG): rospy.logdebug(msg.msg) elif (msg.level == Log.INFO): rospy.loginfo(msg.msg) elif (msg.level == Log.WARN): rospy.logwarn(msg.msg) elif (msg.level == Log.ERROR): rospy.logerr(msg.msg) elif (msg.level == Log.FATAL): rospy.logfatal(msg.msg)
def handleLoggingRequest(self, data): """ Forward logging information from serial device into ROS. """ msg = Log() msg.deserialize(data) if (msg.level == Log.ROSDEBUG): rospy.logdebug(msg.msg) elif(msg.level== Log.INFO): rospy.loginfo(msg.msg) elif(msg.level== Log.WARN): rospy.logwarn(msg.msg) elif(msg.level== Log.ERROR): rospy.logerr(msg.msg) elif(msg.level==Log.FATAL): rospy.logfatal(msg.msg)
def setupPublisher(self, data): """ Register a new publisher. """ try: msg = TopicInfo() msg.deserialize(data) pub = Publisher(msg) # test if topic id already in use if msg.topic_id in self.publishers: rospy.logwarn("Setting up duplicate topic %d" % msg.topic_id) 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] as id %d" % (msg.topic_name, msg.message_type, msg.topic_id) ) 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: 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.res._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: 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.res._md5sum + ',' + msg.md5sum) except Exception as e: rospy.logerr("Creation of service server 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: 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: 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 setupSubscriber(self, data): """ Register a new subscriber. """ try: msg = TopicInfo() msg.deserialize(data) # test if topic id already registered if msg.topic_id in [sub.id for sub in self.subscribers.values()]: rospy.logwarn( "Ignored duplicate subscriber on %s [%s] as id %d" % (msg.topic_name, msg.message_type, msg.topic_id)) return sub = Subscriber(msg, self) self.subscribers[msg.topic_name] = sub self.setSubscribeSize(msg.buffer_size) rospy.loginfo("Setup subscriber on %s [%s] as id %d" % (msg.topic_name, msg.message_type, msg.topic_id)) except Exception as e: rospy.logerr("Creation of subscriber failed: %s", e)
def setupSubscriber(self, data): """ Register a new subscriber. """ try: msg = TopicInfo() msg.deserialize(data) if not msg.topic_name in self.subscribers.keys(): sub = Subscriber(msg, self) self.subscribers[msg.topic_name] = sub self.setSubscribeSize(msg.buffer_size) rospy.loginfo("Setup subscriber 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 = Subscriber(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): ''' Request to negotiate topics''' if len(data)==0: rospy.loginfo("Got request for topics!") self.requestSyncTime() self.negotiateTopics() return """ Register a new publisher. """ try: msg = TopicInfo() msg.deserialize(data) if msg.topic_id in self.publishers.keys(): rospy.loginfo("Publisher exists on %s [%s]" % (msg.topic_name, msg.message_type) ) return 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 setupSubscriber(self, data): """ Register a new subscriber. """ try: msg = TopicInfo() msg.deserialize(data) # test if topic id already registered if msg.topic_id in [sub.id for sub in self.subscribers.values()]: rospy.logwarn("Ignored duplicate subscriber on %s [%s] as id %d" % (msg.topic_name, msg.message_type, msg.topic_id) ) return if not msg.topic_name in self.subscribers.keys(): sub = Subscriber(msg, self) self.subscribers[msg.topic_name] = sub self.setSubscribeSize(msg.buffer_size) rospy.loginfo("Setup subscriber 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 = Subscriber(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)