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)
