def poller(): context = messaging.Context() p = messaging.Poller() sub = messaging.SubSocket() sub.connect(context, 'controlsState') p.registerSocket(sub) socks = p.poll(10000) r = [s.receive(non_blocking=True) for s in socks] return r
def steer_thread(): poller = messaging.Poller() logcan = messaging.sub_sock('can') health = messaging.sub_sock('health') joystick_sock = messaging.sub_sock('testJoystick', conflate=True, poller=poller) carstate = messaging.pub_sock('carState') carcontrol = messaging.pub_sock('carControl') sendcan = messaging.pub_sock('sendcan') button_1_last = 0 enabled = False # wait for health and CAN packets hw_type = messaging.recv_one(health).health.hwType has_relay = hw_type in [HwType.blackPanda, HwType.uno, HwType.dos] print("Waiting for CAN messages...") messaging.get_one_can(logcan) CI, CP = get_car(logcan, sendcan, has_relay) Params().put("CarParams", CP.to_bytes()) CC = car.CarControl.new_message() while True: # send joystick = messaging.recv_one(joystick_sock) can_strs = messaging.drain_sock_raw(logcan, wait_for_one=True) CS = CI.update(CC, can_strs) # Usually axis run in pairs, up/down for one, and left/right for # the other. actuators = car.CarControl.Actuators.new_message() if joystick is not None: axis_3 = clip(-joystick.testJoystick.axes[3] * 1.05, -1., 1.) # -1 to 1 actuators.steer = axis_3 actuators.steerAngle = axis_3 * 43. # deg axis_1 = clip(-joystick.testJoystick.axes[1] * 1.05, -1., 1.) # -1 to 1 actuators.gas = max(axis_1, 0.) actuators.brake = max(-axis_1, 0.) pcm_cancel_cmd = joystick.testJoystick.buttons[0] button_1 = joystick.testJoystick.buttons[1] if button_1 and not button_1_last: enabled = not enabled button_1_last = button_1 #print "enable", enabled, "steer", actuators.steer, "accel", actuators.gas - actuators.brake hud_alert = 0 if joystick.testJoystick.buttons[3]: hud_alert = "steerRequired" CC.actuators.gas = actuators.gas CC.actuators.brake = actuators.brake CC.actuators.steer = actuators.steer CC.actuators.steerAngle = actuators.steerAngle CC.hudControl.visualAlert = hud_alert CC.hudControl.setSpeed = 20 CC.cruiseControl.cancel = pcm_cancel_cmd CC.enabled = enabled can_sends = CI.apply(CC) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) # broadcast carState cs_send = messaging.new_message('carState') cs_send.carState = copy(CS) carstate.send(cs_send.to_bytes()) # broadcast carControl cc_send = messaging.new_message('carControl') cc_send.carControl = copy(CC) carcontrol.send(cc_send.to_bytes())
from cereal import log import cereal.messaging as messaging from helpers import draw_pose if __name__ == "__main__": os.environ["ZMQ"] = "1" parser = argparse.ArgumentParser(description='Sniff a communcation socket') parser.add_argument('--addr', default='192.168.5.11') args = parser.parse_args() messaging.context = messaging.Context() poller = messaging.Poller() m = 'driverMonitoring' messaging.sub_sock(m, poller, addr=args.addr) pygame.init() pygame.display.set_caption('livedm') screen = pygame.display.set_mode((320, 640), pygame.DOUBLEBUF) camera_surface = pygame.surface.Surface((160, 320), 0, 24).convert() while 1: polld = poller.poll(1000) for sock in polld: msg = sock.receive() evt = log.Event.from_bytes(msg)