def run(self): while True: self.has_data.clear() data, addr = self.incomingUDP_PSO.recvfrom(1024) self.current_time = rospy.Time.now() # Do not continue if we are closing connections if self.closeConnections: break type = packets.read_packet_type(data) if type == 1: #self.status = packets.read_diagnostics_packet(data)[0] pass elif type == 0: self.x, self.y, self.heading, self.vel, self.omega, self.yaw_bias, self.x_var, self.y_var, self.heading_var, self.vel_var, self.omega_var, self.yaw_bias_var, self.sonar_ping_1, self.sonar_ping_2, self.sonar_ping_3, self.sonar_ping_4, self.sonar_ping_5 = packets.read_pose_packet( data) duration = ((self.current_time - self.last_time).to_sec()) self.has_data.set() self.last_time = self.current_time
def run(self): self.logger.debug("UDP Listener Started") while True: data, addr = self.incomingUDP_PSO.recvfrom(1024) # Do not continue if we are closing connections if self.closeConnections: break type = packets.read_packet_type(data) if type == packets.DIAGNOSTICS_t: self.status = packets.read_diagnostics_packet(data)[0] self.logger.debug("Received status. Was: %d" % (self.status, )) if type == packets.SYSINFO_t: self.voltage = packets.read_sysinfo_packet(data)[0] self.logger.debug("Received SysInfo packet, voltage = " % (self.voltage)) elif type == packets.POSE_t: self.x, self.y, self.heading, self.x_var, self.y_var, self.heading_var = packets.read_pose_packet(data) self.has_data.set() self.logger.debug('X: %f Y: %f Heading: %f' % (self.x, self.y, self.heading)) self.OnUpdate()
def run(self): while True: self.has_data.clear() data, addr = self.incomingUDP_PSO.recvfrom(1024) self.current_time = rospy.Time.now() # Do not continue if we are closing connections if self.closeConnections: break type = packets.read_packet_type(data) if type == 1: # self.status = packets.read_diagnostics_packet(data)[0] pass elif type == 0: self.x, self.y, self.heading, self.vel, self.omega, self.yaw_bias, self.x_var, self.y_var, self.heading_var, self.vel_var, self.omega_var, self.yaw_bias_var, self.sonar_ping_1, self.sonar_ping_2, self.sonar_ping_3, self.sonar_ping_4, self.sonar_ping_5 = packets.read_pose_packet( data ) duration = (self.current_time - self.last_time).to_sec() self.has_data.set() self.last_time = self.current_time