def check_msgs(self):
while self.serial.in_waiting >= self._nb_bytes_expected:
print("qsd")
if self._rcv_state == RcvState.START1: # wait for 0XFF
if ord(self.serial.read()) == 0xFF:
print("start1 ok")
self._rcv_state = RcvState.START2
self._nb_bytes_expected = 1
elif self._rcv_state == RcvState.START2:
if ord(self.serial.read()) == 0xFF:
self._rcv_state = RcvState.LEN
self._nb_bytes_expected = 1
else: # fallback to Idle
self._rcv_state = RcvState.START1
self._nb_bytes_expected = 1
elif self._rcv_state == RcvState.LEN:
self._nb_bytes_expected = ord(self.serial.read(
)) + 1 # expect 1 more byte for the checksum
self._rcv_state = RcvState.PAYLOAD_CHK
elif self._rcv_state == RcvState.PAYLOAD_CHK:
payload = self.serial.read(self._nb_bytes_expected -
1) # read message content
chk = ord(self.serial.read())
self._rcv_state = RcvState.START1
self._nb_bytes_expected = 1
if chk == self.compute_chk(payload):
msg = m.Message()
try:
msg.ParseFromString(payload)
return msg
except protobuf.message.DecodeError:
return None
else:
print(f"chgk failed: {chk} {self.compute_chk(payload)}")
return None
def send_pos_cmd(self, x, y, theta=None):
pos_cmd = m.Message()
pos_cmd.pos.x = x
pos_cmd.pos.y = y
if theta is not None:
pos_cmd.pos.theta = theta
pos_cmd.msg_type = m.Message.COMMAND
self.robot_manager.send_msg(self.robot_manager.current_rid, pos_cmd)
def __init__(self, *args, **kwargs):
QWidget.__init__(self, *args, **kwargs)
self.pix = QPixmap("data/map2022.png")
self.robot_manager = None # type: Optional[robots_manager.RobotsManager]
self.speed_cmd = m.Message()
self.speed_cmd.speed.vx = 0
self.speed_cmd.speed.vy = 0
self.speed_cmd.speed.vtheta = 0
self.speed_cmd.msg_type = m.Message.COMMAND
self.speed_timer = QTimer(self)
self.speed_timer.timeout.connect(self.send_speed_command)
self.speed_timer.start(200)
self.pix_rect = QRect(0, 0, 0, 0)
self.setMouseTracking(True)
self.press_pos = None
self.current_pos = None
def advance(self, data):
b = data[0]
if self._rcv_state == RcvState.START1: # wait for 0XFF
if b == 0xFF:
self._rcv_state = RcvState.START2
return 1, None
elif self._rcv_state == RcvState.START2:
if b == 0xFF:
self._rcv_state = RcvState.LEN
else: # fallback to Idle
self._rcv_state = RcvState.START1
return 1, None
elif self._rcv_state == RcvState.LEN:
self._nb_bytes_expected = b
self._rcv_state = RcvState.PAYLOAD
self.payload_buffer = b''
return 1, None
elif self._rcv_state == RcvState.PAYLOAD:
dlen = min(len(data), self._nb_bytes_expected)
self.payload_buffer += data[:dlen]
self._nb_bytes_expected -= dlen
if self._nb_bytes_expected == 0:
self._rcv_state = RcvState.CHK
return dlen, None
elif self._rcv_state == RcvState.CHK:
chk = b
self._rcv_state = RcvState.START1
if chk == self.compute_chk(self.payload_buffer):
msg = m.Message()
try:
msg.ParseFromString(self.payload_buffer)
return 1, msg
except protobuf.message.DecodeError:
return 1, None
else:
print(
f"chgk failed: {chk} {self.compute_chk(self.payload_buffer)}"
)
return 1, None
def send_speed_cmd(self, vx, vy, vtheta):
msg = m.Message()
msg.speed.vx = vx
msg.speed.vy = vy
msg.speed.vtheta = vtheta
self.send_msg(msg)
def __init__(self, rid):
self.rid = rid
self.pos = m.Message()
self.speed = m.Message()
self.bat = m.Message()