def handleTimeRequest(self, data): """ Respond to device with system time. """ t = Time() t.data = rospy.Time.now() data_buffer = StringIO.StringIO() t.serialize(data_buffer) self.send( TopicInfo.ID_TIME, data_buffer.getvalue() ) self.lastsync = rospy.Time.now()
def handleTimeRequest(self, data): """ Respond to device with system time. """ t = Time() t.data = rospy.Time.now() data_buffer = StringIO.StringIO() t.serialize(data_buffer) self.send(TopicInfo.ID_TIME, data_buffer.getvalue()) self.lastsync = rospy.Time.now()
def handleTimeRequest(self, data): """ Respond to device with system time. """ t = Time() t.deserialize(data) if t.data.secs == 0 and t.data.nsecs == 0: t = Time() t.data = rospy.Time.now() data_buffer = StringIO.StringIO() t.serialize(data_buffer) self.send( TopicInfo.ID_TIME, data_buffer.getvalue() ) self.lastsync = rospy.Time.now() else: # TODO sync the time somehow self.lastsync = rospy.Time.now() pass
def run(self): """ Forward recieved messages to appropriate publisher. """ data = '' while not rospy.is_shutdown(): if (rospy.Time.now() - self.lastsync).to_sec() > (self.timeout * 3): rospy.logerr("Lost sync with device, restarting...") self.requestTopics() self.lastsync = rospy.Time.now() flag = [0,0] flag[0] = self.port.read(1) if (flag[0] != '\xff'): continue flag[1] = self.port.read(1) if ( flag[1] != '\xff'): rospy.loginfo("Failed Packet Flags ") continue # topic id (2 bytes) header = self.port.read(4) if (len(header) != 4): #self.port.flushInput() continue topic_id, msg_length = struct.unpack("<hh", header) msg = self.port.read(msg_length) if (len(msg) != msg_length): rospy.loginfo("Packet Failed : Failed to read msg data") #self.port.flushInput() continue chk = self.port.read(1) checksum = sum(map(ord,header) ) + sum(map(ord, msg)) + ord(chk) if checksum%256 == 255: if topic_id == TopicInfo.ID_PUBLISHER: try: m = TopicInfo() m.deserialize(msg) self.senders[m.topic_id] = Publisher(m.topic_name, m.message_type) rospy.loginfo("Setup Publisher on %s [%s]" % (m.topic_name, m.message_type) ) except Exception as e: rospy.logerr("Failed to parse publisher: %s", e) elif topic_id == TopicInfo.ID_SUBSCRIBER: try: m = TopicInfo() m.deserialize(msg) self.receivers[m.topic_name] = [m.topic_id, Subscriber(m.topic_name, m.message_type, self)] rospy.loginfo("Setup Subscriber on %s [%s]" % (m.topic_name, m.message_type)) except Exception as e: rospy.logerr("Failed to parse subscriber. %s"%e) elif topic_id == TopicInfo.ID_SERVICE_SERVER: try: m = TopicInfo() m.deserialize(msg) self.senders[m.topic_id]=ServiceServer(m.topic_name, m.message_type, self) rospy.loginfo("Setup ServiceServer on %s [%s]"%(m.topic_name, m.message_type) ) except: rospy.logerr("Failed to parse service server") elif topic_id == TopicInfo.ID_SERVICE_CLIENT: pass elif topic_id == TopicInfo.ID_PARAMETER_REQUEST: self.handleParameterRequest(msg) elif topic_id == TopicInfo.ID_LOG: self.handleLogging(msg) elif topic_id == TopicInfo.ID_TIME: t = Time() t.data = rospy.Time.now() data_buffer = StringIO.StringIO() t.serialize(data_buffer) self.send( TopicInfo.ID_TIME, data_buffer.getvalue() ) self.lastsync = rospy.Time.now() elif topic_id >= 100: # TOPIC try: self.senders[topic_id].handlePacket(msg) except KeyError: rospy.logerr("Tried to publish before configured, topic id %d" % topic_id) else: rospy.logerr("Unrecognized command topic!") rospy.sleep(0.001)
def requestSyncTime(self): t = Time() data_buffer = StringIO.StringIO() t.serialize(data_buffer) self.send(TopicInfo.ID_TIME, data_buffer.getvalue())
def run(self): """ Forward recieved messages to appropriate publisher. """ data = '' while not rospy.is_shutdown(): if (rospy.Time.now() - self.lastsync).to_sec() > (self.timeout * 3): rospy.logerr("Lost sync with device, restarting...") self.requestTopics() self.lastsync = rospy.Time.now() flag = [0,0] flag[0] = self.port.read(1) if (flag[0] != '\xff'): continue flag[1] = self.port.read(1) if ( flag[1] != '\xff'): rospy.loginfo("Failed Packet Flags ") continue # topic id (2 bytes) header = self.port.read(4) if (len(header) != 4): #self.port.flushInput() continue topic_id, msg_length = struct.unpack("<hh", header) msg = self.port.read(msg_length) if (len(msg) != msg_length): rospy.loginfo("Packet Failed : Failed to read msg data") #self.port.flushInput() continue chk = self.port.read(1) checksum = sum(map(ord,header) ) + sum(map(ord, msg)) + ord(chk) if checksum%256 == 255: if topic_id == TopicInfo.ID_PUBLISHER: try: m = TopicInfo() m.deserialize(msg) self.senders[m.topic_id] = Publisher(m.topic_name, m.message_type) rospy.loginfo("Setup Publisher on %s [%s]" % (m.topic_name, m.message_type) ) except Exception as e: rospy.logerr("Failed to parse publisher: %s", e) elif topic_id == TopicInfo.ID_SUBSCRIBER: try: m = TopicInfo() m.deserialize(msg) self.receivers[m.topic_name] = [m.topic_id, Subscriber(m.topic_name, m.message_type, self)] rospy.loginfo("Setup Subscriber on %s [%s]" % (m.topic_name, m.message_type)) except Exception as e: rospy.logerr("Failed to parse subscriber. %s"%e) elif topic_id == TopicInfo.ID_SERVICE_SERVER: try: m = TopicInfo() m.deserialize(msg) service = ServiceServer(m.topic_name, m.message_type, self) self.receivers[m.topic_name] = [m.topic_id, service] self.senders[m.topic_id] = service rospy.loginfo("Setup ServiceServer on %s [%s]"%(m.topic_name, m.message_type) ) except: rospy.logerr("Failed to parse service server") elif topic_id == TopicInfo.ID_SERVICE_CLIENT: pass elif topic_id == TopicInfo.ID_PARAMETER_REQUEST: self.handleParameterRequest(msg) elif topic_id == TopicInfo.ID_LOG: self.handleLogging(msg) elif topic_id == TopicInfo.ID_TIME: t = Time() t.data = rospy.Time.now() data_buffer = StringIO.StringIO() t.serialize(data_buffer) self.send( TopicInfo.ID_TIME, data_buffer.getvalue() ) self.lastsync = rospy.Time.now() elif topic_id >= 100: # TOPIC try: self.senders[topic_id].handlePacket(msg) except KeyError: rospy.logerr("Tried to publish before configured, topic id %d" % topic_id) else: rospy.logerr("Unrecognized command topic!") rospy.sleep(0.001)