diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index eebf6798a2..abc2b88f53 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -1,3 +1,4 @@ +import copy import numpy as np from opendbc.can.packer import CANPacker @@ -75,7 +76,7 @@ class CarController(CarControllerBase): can_sends = [] can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r)) - new_actuators = CC.actuators.as_builder() + new_actuators = copy.deepcopy(CC.actuators) new_actuators.accel = torque_l new_actuators.steer = torque_r new_actuators.steerOutputCan = torque_r diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index a12c9091ee..2066e65b29 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -59,7 +59,7 @@ def can_comm_callbacks(logcan: messaging.SubSocket, sendcan: messaging.PubSocket return can_recv, can_send -def convert_to_capnp(struct: structs.CarParams | structs.CarState) -> capnp.lib.capnp._DynamicStructBuilder: +def convert_to_capnp(struct: structs.CarParams | structs.CarState | structs.CarControl.Actuators) -> capnp.lib.capnp._DynamicStructBuilder: struct_dict = dataclasses.asdict(struct) if isinstance(struct, structs.CarParams): @@ -71,8 +71,10 @@ def convert_to_capnp(struct: structs.CarParams | structs.CarState) -> capnp.lib. struct_capnp.lateralTuning.init(which) lateralTuning_dict = dataclasses.asdict(getattr(struct.lateralTuning, which)) setattr(struct_capnp.lateralTuning, which, lateralTuning_dict) - else: + elif isinstance(struct, structs.CarState): struct_capnp = car.CarState.new_message(**struct_dict) + else: + struct_capnp = car.CarControl.Actuators.new_message(**struct_dict) return struct_capnp @@ -82,12 +84,14 @@ def convert_carControl(struct: capnp.lib.capnp._DynamicStructReader) -> structs. def remove_deprecated(s: dict) -> dict: return {k: v for k, v in s.items() if not k.endswith('DEPRECATED')} - for field, value in struct.to_dict().items(): - if isinstance(value, dict): - name = field[0].upper() + field[1:] - struct[field] = getattr(structs.CarControl, name)(**remove_deprecated(value)) + struct_dict = struct.to_dict() + struct_dataclass = structs.CarControl(**remove_deprecated({k: v for k, v in struct_dict.items() if not isinstance(k, dict)})) + + struct_dataclass.actuators = structs.CarControl.Actuators(**remove_deprecated(struct_dict.get('actuators', {}))) + struct_dataclass.cruiseControl = structs.CarControl.CruiseControl(**remove_deprecated(struct_dict.get('cruiseControl', {}))) + struct_dataclass.hudControl = structs.CarControl.HUDControl(**remove_deprecated(struct_dict.get('hudControl', {}))) - return structs.CarControl(**remove_deprecated(struct)) + return struct_dataclass class Car: @@ -223,7 +227,7 @@ class Car: # publish new carOutput co_send = messaging.new_message('carOutput') co_send.valid = self.sm.all_checks(['carControl']) - co_send.carOutput.actuatorsOutput = self.last_actuators_output + co_send.carOutput.actuatorsOutput = convert_to_capnp(self.last_actuators_output) self.pm.send('carOutput', co_send) # kick off controlsd step while we actuate the latest carControl packet diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index c194488de9..37f878855b 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -1,3 +1,4 @@ +import copy from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import DT_CTRL, apply_meas_steer_torque_limits from openpilot.selfdrive.car.chrysler import chryslercan @@ -76,7 +77,7 @@ class CarController(CarControllerBase): self.frame += 1 - new_actuators = CC.actuators.as_builder() + new_actuators = copy.deepcopy(CC.actuators) new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 12e301c722..993c544b85 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -1,3 +1,4 @@ +import copy from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_std_steer_angle_limits, structs from openpilot.selfdrive.car.ford import fordcan @@ -110,7 +111,7 @@ class CarController(CarControllerBase): self.steer_alert_last = steer_alert self.lead_distance_bars_last = hud_control.leadDistanceBars - new_actuators = actuators.as_builder() + new_actuators = copy.deepcopy(actuators) new_actuators.curvature = self.apply_curvature_last self.frame += 1 diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index a33cf2a2a6..5d328367e6 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -1,3 +1,4 @@ +import copy from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits, structs from openpilot.selfdrive.car.gm import gmcan @@ -152,7 +153,7 @@ class CarController(CarControllerBase): if self.frame % 10 == 0: can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status)) - new_actuators = actuators.as_builder() + new_actuators = copy.deepcopy(actuators) new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last new_actuators.gas = self.apply_gas diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index e86a0aeb82..7901c061a7 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -1,3 +1,4 @@ +import copy from collections import namedtuple from opendbc.can.packer import CANPacker @@ -241,7 +242,7 @@ class CarController(CarControllerBase): self.speed = pcm_speed self.gas = pcm_accel / self.params.NIDEC_GAS_MAX - new_actuators = actuators.as_builder() + new_actuators = copy.deepcopy(actuators) new_actuators.speed = self.speed new_actuators.accel = self.accel new_actuators.gas = self.gas diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 5169247ca2..4101d25776 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -1,3 +1,4 @@ +import copy from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg, structs from openpilot.selfdrive.car.conversions import Conversions as CV @@ -161,7 +162,7 @@ class CarController(CarControllerBase): if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl: can_sends.append(hyundaican.create_frt_radar_opt(self.packer)) - new_actuators = actuators.as_builder() + new_actuators = copy.deepcopy(actuators) new_actuators.steer = apply_steer / self.params.STEER_MAX new_actuators.steerOutputCan = apply_steer new_actuators.accel = accel diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 13e7f8aa3d..9b801924ad 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -1,3 +1,4 @@ +import copy from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits, structs from openpilot.selfdrive.car.interfaces import CarControllerBase @@ -56,7 +57,7 @@ class CarController(CarControllerBase): can_sends.append(mazdacan.create_steering_control(self.packer, self.CP, self.frame, apply_steer, CS.cam_lkas)) - new_actuators = CC.actuators.as_builder() + new_actuators = copy.deepcopy(CC.actuators) new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX new_actuators.steerOutputCan = apply_steer diff --git a/selfdrive/car/mock/carcontroller.py b/selfdrive/car/mock/carcontroller.py index 0cd37c0369..b1f541f25d 100644 --- a/selfdrive/car/mock/carcontroller.py +++ b/selfdrive/car/mock/carcontroller.py @@ -1,5 +1,6 @@ +import copy from openpilot.selfdrive.car.interfaces import CarControllerBase class CarController(CarControllerBase): def update(self, CC, CS, now_nanos): - return CC.actuators.as_builder(), [] + return copy.deepcopy(CC.actuators), [] diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index ec9dc6e5c8..c127a5b181 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -1,3 +1,4 @@ +import copy from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_std_steer_angle_limits, structs from openpilot.selfdrive.car.interfaces import CarControllerBase @@ -73,7 +74,7 @@ class CarController(CarControllerBase): self.packer, CS.lkas_hud_info_msg, steer_hud_alert )) - new_actuators = actuators.as_builder() + new_actuators = copy.deepcopy(actuators) new_actuators.steeringAngleDeg = apply_angle self.frame += 1 diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index c8c2e7417b..493a754962 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,3 +1,4 @@ +import copy from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg from openpilot.selfdrive.car.helpers import clip, interp @@ -135,7 +136,7 @@ class CarController(CarControllerBase): if self.frame % 2 == 0: can_sends.append(subarucan.create_es_static_2(self.packer)) - new_actuators = actuators.as_builder() + new_actuators = copy.deepcopy(actuators) new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index ed81fd0c50..ded922ed08 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -1,3 +1,4 @@ +import copy from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_std_steer_angle_limits from openpilot.selfdrive.car.helpers import clip @@ -59,7 +60,7 @@ class CarController(CarControllerBase): # TODO: HUD control - new_actuators = actuators.as_builder() + new_actuators = copy.deepcopy(actuators) new_actuators.steeringAngleDeg = self.apply_angle_last self.frame += 1 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 51fa3e748c..b705c5dffb 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,3 +1,4 @@ +import copy from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_tester_present_msg, structs from openpilot.selfdrive.car.can_definitions import CanData from openpilot.selfdrive.car.helpers import clip @@ -167,7 +168,7 @@ class CarController(CarControllerBase): if self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: can_sends.append(make_tester_present_msg(0x750, 0, 0xF)) - new_actuators = actuators.as_builder() + new_actuators = copy.deepcopy(actuators) new_actuators.steer = apply_steer / self.params.STEER_MAX new_actuators.steerOutputCan = apply_steer new_actuators.steeringAngleDeg = self.last_angle diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index e782f14985..3086b35931 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -1,3 +1,4 @@ +import copy from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits, structs from openpilot.selfdrive.car.conversions import Conversions as CV @@ -109,7 +110,7 @@ class CarController(CarControllerBase): can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, self.ext_bus, CS.gra_stock_values, cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume)) - new_actuators = actuators.as_builder() + new_actuators = copy.deepcopy(actuators) new_actuators.steer = self.apply_steer_last / self.CCP.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last