|
|
@ -362,10 +362,6 @@ def get_car_params_callback(rc, pm, msgs, fingerprint): |
|
|
|
params.put("CarParams", CP.to_bytes()) |
|
|
|
params.put("CarParams", CP.to_bytes()) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def selfdrived_rcv_callback(msg, cfg, frame): |
|
|
|
|
|
|
|
return (frame - 1) == 0 or msg.which() == 'carState' |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def card_rcv_callback(msg, cfg, frame): |
|
|
|
def card_rcv_callback(msg, cfg, frame): |
|
|
|
# no sendcan until card is initialized |
|
|
|
# no sendcan until card is initialized |
|
|
|
if msg.which() != "can": |
|
|
|
if msg.which() != "can": |
|
|
@ -380,21 +376,6 @@ def card_rcv_callback(msg, cfg, frame): |
|
|
|
return len(socks) > 0 |
|
|
|
return len(socks) > 0 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def calibration_rcv_callback(msg, cfg, frame): |
|
|
|
|
|
|
|
# calibrationd publishes 1 calibrationData every 5 cameraOdometry packets. |
|
|
|
|
|
|
|
# should_recv always true to increment frame |
|
|
|
|
|
|
|
return (frame - 1) == 0 or msg.which() == 'cameraOdometry' |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def torqued_rcv_callback(msg, cfg, frame): |
|
|
|
|
|
|
|
# should_recv always true to increment frame |
|
|
|
|
|
|
|
return (frame - 1) == 0 or msg.which() == 'livePose' |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def dmonitoringmodeld_rcv_callback(msg, cfg, frame): |
|
|
|
|
|
|
|
return msg.which() == "driverCameraState" |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class ModeldCameraSyncRcvCallback: |
|
|
|
class ModeldCameraSyncRcvCallback: |
|
|
|
def __init__(self): |
|
|
|
def __init__(self): |
|
|
|
self.road_present = False |
|
|
|
self.road_present = False |
|
|
@ -419,11 +400,13 @@ class ModeldCameraSyncRcvCallback: |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class MessageBasedRcvCallback: |
|
|
|
class MessageBasedRcvCallback: |
|
|
|
def __init__(self, trigger_msg_type): |
|
|
|
def __init__(self, trigger_msg_type: str, first_frame: bool): |
|
|
|
self.trigger_msg_type = trigger_msg_type |
|
|
|
self.trigger_msg_type = trigger_msg_type |
|
|
|
|
|
|
|
self.first_frame = first_frame |
|
|
|
|
|
|
|
|
|
|
|
def __call__(self, msg, cfg, frame): |
|
|
|
def __call__(self, msg, cfg, frame): |
|
|
|
return msg.which() == self.trigger_msg_type |
|
|
|
# publish on first frame or trigger msg |
|
|
|
|
|
|
|
return ((frame - 1) == 0 and self.first_frame) or msg.which() == self.trigger_msg_type |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class FrequencyBasedRcvCallback: |
|
|
|
class FrequencyBasedRcvCallback: |
|
|
@ -462,7 +445,7 @@ CONFIGS = [ |
|
|
|
ignore=["logMonoTime"], |
|
|
|
ignore=["logMonoTime"], |
|
|
|
config_callback=selfdrived_config_callback, |
|
|
|
config_callback=selfdrived_config_callback, |
|
|
|
init_callback=get_car_params_callback, |
|
|
|
init_callback=get_car_params_callback, |
|
|
|
should_recv_callback=selfdrived_rcv_callback, |
|
|
|
should_recv_callback=MessageBasedRcvCallback("carState", True), |
|
|
|
tolerance=NUMPY_TOLERANCE, |
|
|
|
tolerance=NUMPY_TOLERANCE, |
|
|
|
processing_time=0.004, |
|
|
|
processing_time=0.004, |
|
|
|
main_pub="carState", |
|
|
|
main_pub="carState", |
|
|
@ -475,7 +458,7 @@ CONFIGS = [ |
|
|
|
subs=["carControl", "controlsState"], |
|
|
|
subs=["carControl", "controlsState"], |
|
|
|
ignore=["logMonoTime", ], |
|
|
|
ignore=["logMonoTime", ], |
|
|
|
init_callback=get_car_params_callback, |
|
|
|
init_callback=get_car_params_callback, |
|
|
|
should_recv_callback=MessageBasedRcvCallback("selfdriveState"), |
|
|
|
should_recv_callback=MessageBasedRcvCallback("selfdriveState", False), |
|
|
|
tolerance=NUMPY_TOLERANCE, |
|
|
|
tolerance=NUMPY_TOLERANCE, |
|
|
|
), |
|
|
|
), |
|
|
|
ProcessConfig( |
|
|
|
ProcessConfig( |
|
|
@ -513,7 +496,7 @@ CONFIGS = [ |
|
|
|
subs=["liveCalibration"], |
|
|
|
subs=["liveCalibration"], |
|
|
|
ignore=["logMonoTime"], |
|
|
|
ignore=["logMonoTime"], |
|
|
|
init_callback=get_car_params_callback, |
|
|
|
init_callback=get_car_params_callback, |
|
|
|
should_recv_callback=calibration_rcv_callback, |
|
|
|
should_recv_callback=MessageBasedRcvCallback("cameraOdometry", True), |
|
|
|
), |
|
|
|
), |
|
|
|
ProcessConfig( |
|
|
|
ProcessConfig( |
|
|
|
proc_name="dmonitoringd", |
|
|
|
proc_name="dmonitoringd", |
|
|
@ -530,7 +513,7 @@ CONFIGS = [ |
|
|
|
], |
|
|
|
], |
|
|
|
subs=["livePose"], |
|
|
|
subs=["livePose"], |
|
|
|
ignore=["logMonoTime"], |
|
|
|
ignore=["logMonoTime"], |
|
|
|
should_recv_callback=MessageBasedRcvCallback("cameraOdometry"), |
|
|
|
should_recv_callback=MessageBasedRcvCallback("cameraOdometry", False), |
|
|
|
tolerance=NUMPY_TOLERANCE, |
|
|
|
tolerance=NUMPY_TOLERANCE, |
|
|
|
unlocked_pubs=["accelerometer", "gyroscope"], |
|
|
|
unlocked_pubs=["accelerometer", "gyroscope"], |
|
|
|
), |
|
|
|
), |
|
|
@ -550,7 +533,7 @@ CONFIGS = [ |
|
|
|
subs=["liveDelay"], |
|
|
|
subs=["liveDelay"], |
|
|
|
ignore=["logMonoTime"], |
|
|
|
ignore=["logMonoTime"], |
|
|
|
init_callback=get_car_params_callback, |
|
|
|
init_callback=get_car_params_callback, |
|
|
|
should_recv_callback=MessageBasedRcvCallback("livePose"), |
|
|
|
should_recv_callback=MessageBasedRcvCallback("livePose", False), |
|
|
|
tolerance=NUMPY_TOLERANCE, |
|
|
|
tolerance=NUMPY_TOLERANCE, |
|
|
|
), |
|
|
|
), |
|
|
|
ProcessConfig( |
|
|
|
ProcessConfig( |
|
|
@ -565,7 +548,7 @@ CONFIGS = [ |
|
|
|
subs=["liveTorqueParameters"], |
|
|
|
subs=["liveTorqueParameters"], |
|
|
|
ignore=["logMonoTime"], |
|
|
|
ignore=["logMonoTime"], |
|
|
|
init_callback=get_car_params_callback, |
|
|
|
init_callback=get_car_params_callback, |
|
|
|
should_recv_callback=torqued_rcv_callback, |
|
|
|
should_recv_callback=MessageBasedRcvCallback("livePose", True), |
|
|
|
tolerance=NUMPY_TOLERANCE, |
|
|
|
tolerance=NUMPY_TOLERANCE, |
|
|
|
), |
|
|
|
), |
|
|
|
ProcessConfig( |
|
|
|
ProcessConfig( |
|
|
@ -586,7 +569,7 @@ CONFIGS = [ |
|
|
|
pubs=["liveCalibration", "driverCameraState"], |
|
|
|
pubs=["liveCalibration", "driverCameraState"], |
|
|
|
subs=["driverStateV2"], |
|
|
|
subs=["driverStateV2"], |
|
|
|
ignore=["logMonoTime", "driverStateV2.modelExecutionTime", "driverStateV2.gpuExecutionTime"], |
|
|
|
ignore=["logMonoTime", "driverStateV2.modelExecutionTime", "driverStateV2.gpuExecutionTime"], |
|
|
|
should_recv_callback=dmonitoringmodeld_rcv_callback, |
|
|
|
should_recv_callback=MessageBasedRcvCallback("driverCameraState", False), |
|
|
|
tolerance=NUMPY_TOLERANCE, |
|
|
|
tolerance=NUMPY_TOLERANCE, |
|
|
|
processing_time=0.020, |
|
|
|
processing_time=0.020, |
|
|
|
main_pub=vipc_get_endpoint_name("camerad", meta_from_camera_state("driverCameraState").stream), |
|
|
|
main_pub=vipc_get_endpoint_name("camerad", meta_from_camera_state("driverCameraState").stream), |
|
|
|