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()
