diff --git a/selfdrive/test/longitudinal_maneuvers/cleanup_plant_docker.sh b/selfdrive/test/longitudinal_maneuvers/cleanup_plant_docker.sh
deleted file mode 100755
index 69659940b2..0000000000
--- a/selfdrive/test/longitudinal_maneuvers/cleanup_plant_docker.sh
+++ /dev/null
@@ -1,2 +0,0 @@
-#!/bin/sh
-docker run -v $(pwd)/docker_out:/tmp/openpilot/selfdrive/test/out openpilot_ci /bin/bash -c "rm -Rf /tmp/openpilot/selfdrive/test/out/longitudinal"
diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py
index b6efd79796..42feadb460 100644
--- a/selfdrive/test/longitudinal_maneuvers/maneuver.py
+++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py
@@ -1,7 +1,5 @@
-from collections import defaultdict
-from selfdrive.test.longitudinal_maneuvers.maneuverplots import ManeuverPlot
-from selfdrive.test.longitudinal_maneuvers.plant import Plant
import numpy as np
+from selfdrive.test.longitudinal_maneuvers.plant import Plant
class Maneuver():
@@ -11,75 +9,32 @@ class Maneuver():
self.speed = kwargs.get("initial_speed", 0.0)
self.lead_relevancy = kwargs.get("lead_relevancy", 0)
- self.grade_values = kwargs.get("grade_values", [0.0, 0.0])
- self.grade_breakpoints = kwargs.get("grade_breakpoints", [0.0, duration])
self.speed_lead_values = kwargs.get("speed_lead_values", [0.0, 0.0])
self.speed_lead_breakpoints = kwargs.get("speed_lead_breakpoints", [0.0, duration])
- self.cruise_button_presses = kwargs.get("cruise_button_presses", [])
- self.checks = kwargs.get("checks", [])
-
self.duration = duration
self.title = title
def evaluate(self):
- """runs the plant sim and returns (score, run_data)"""
plant = Plant(
lead_relevancy=self.lead_relevancy,
speed=self.speed,
distance_lead=self.distance_lead
)
- logs = defaultdict(list)
- last_controls_state = None
- plot = ManeuverPlot(self.title)
-
- buttons_sorted = sorted(self.cruise_button_presses, key=lambda a: a[1])
- current_button = 0
+ valid = True
while plant.current_time() < self.duration:
- while buttons_sorted and plant.current_time() >= buttons_sorted[0][1]:
- current_button = buttons_sorted[0][0]
- buttons_sorted = buttons_sorted[1:]
- print("current button changed to {0}".format(current_button))
-
- grade = np.interp(plant.current_time(), self.grade_breakpoints, self.grade_values)
speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values)
-
- # distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state= plant.step(speed_lead, current_button, grade)
- log = plant.step(speed_lead, current_button, grade)
-
- if log['controls_state_msgs']:
- last_controls_state = log['controls_state_msgs'][-1]
+ log = plant.step(speed_lead)
d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200.
v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0.
log['d_rel'] = d_rel
log['v_rel'] = v_rel
- if last_controls_state:
- # print(last_controls_state)
- #develop plots
- plot.add_data(
- time=plant.current_time(),
- gas=log['gas'], brake=log['brake'], steer_torque=log['steer_torque'],
- distance=log['distance'], speed=log['speed'], acceleration=log['acceleration'],
- up_accel_cmd=last_controls_state.upAccelCmd, ui_accel_cmd=last_controls_state.uiAccelCmd,
- uf_accel_cmd=last_controls_state.ufAccelCmd,
- d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead,
- v_target_lead=last_controls_state.vTargetLead, pid_speed=last_controls_state.vPid,
- cruise_speed=last_controls_state.vCruise,
- a_target=last_controls_state.aTarget,
- fcw=log['fcw'])
-
- for k, v in log.items():
- logs[k].append(v)
-
- valid = True
- for check in self.checks:
- c = check(logs)
- if not c:
- print(check.__name__ + " not valid!")
- valid = valid and c
+ if d_rel < 1.0:
+ print("Crashed!!!!")
+ valid = False
print("maneuver end", valid)
- return (plot, valid)
+ return valid
diff --git a/selfdrive/test/longitudinal_maneuvers/maneuverplots.py b/selfdrive/test/longitudinal_maneuvers/maneuverplots.py
deleted file mode 100644
index c1c1f28e8f..0000000000
--- a/selfdrive/test/longitudinal_maneuvers/maneuverplots.py
+++ /dev/null
@@ -1,139 +0,0 @@
-import os
-
-import numpy as np
-import matplotlib.pyplot as plt
-import pylab
-
-from selfdrive.config import Conversions as CV
-
-class ManeuverPlot():
- def __init__(self, title=None):
- self.time_array = []
-
- self.gas_array = []
- self.brake_array = []
- self.steer_torque_array = []
-
- self.distance_array = []
- self.speed_array = []
- self.acceleration_array = []
-
- self.up_accel_cmd_array = []
- self.ui_accel_cmd_array = []
- self.uf_accel_cmd_array = []
-
- self.d_rel_array = []
- self.v_rel_array = []
- self.v_lead_array = []
- self.v_target_lead_array = []
- self.pid_speed_array = []
- self.cruise_speed_array = []
-
- self.a_target_array = []
-
- self.v_target_array = []
-
- self.fcw_array = []
-
- self.title = title
-
- def add_data(self, time, gas, brake, steer_torque, distance, speed,
- acceleration, up_accel_cmd, ui_accel_cmd, uf_accel_cmd, d_rel, v_rel,
- v_lead, v_target_lead, pid_speed, cruise_speed, a_target, fcw):
- self.time_array.append(time)
- self.gas_array.append(gas)
- self.brake_array.append(brake)
- self.steer_torque_array.append(steer_torque)
- self.distance_array.append(distance)
- self.speed_array.append(speed)
- self.acceleration_array.append(acceleration)
- self.up_accel_cmd_array.append(up_accel_cmd)
- self.ui_accel_cmd_array.append(ui_accel_cmd)
- self.uf_accel_cmd_array.append(uf_accel_cmd)
- self.d_rel_array.append(d_rel)
- self.v_rel_array.append(v_rel)
- self.v_lead_array.append(v_lead)
- self.v_target_lead_array.append(v_target_lead)
- self.pid_speed_array.append(pid_speed)
- self.cruise_speed_array.append(cruise_speed)
- self.a_target_array.append(a_target)
- self.fcw_array.append(fcw)
-
- def write_plot(self, path, maneuver_name):
- # title = self.title or maneuver_name
- # TODO: Missing plots from the old one:
- # long_control_state
- # proportional_gb, intergral_gb
- if not os.path.exists(path + "/" + maneuver_name):
- os.makedirs(path + "/" + maneuver_name)
- plt_num = 0
-
- # speed chart ===================
- plt_num += 1
- plt.figure(plt_num)
- plt.plot(
- np.array(self.time_array), np.array(self.speed_array) * CV.MS_TO_MPH, 'r',
- np.array(self.time_array), np.array(self.pid_speed_array) * CV.MS_TO_MPH, 'y--',
- np.array(self.time_array), np.array(self.v_target_lead_array) * CV.MS_TO_MPH, 'b',
- np.array(self.time_array), np.array(self.cruise_speed_array) * CV.KPH_TO_MPH, 'k',
- np.array(self.time_array), np.array(self.v_lead_array) * CV.MS_TO_MPH, 'm'
- )
- plt.xlabel('Time [s]')
- plt.ylabel('Speed [mph]')
- plt.legend(['speed', 'pid speed', 'Target (lead) speed', 'Cruise speed', 'Lead speed'], loc=0)
- plt.grid()
- pylab.savefig("/".join([path, maneuver_name, 'speeds.svg']), dpi=1000)
-
- # acceleration chart ============
- plt_num += 1
- plt.figure(plt_num)
- plt.plot(
- np.array(self.time_array), np.array(self.acceleration_array), 'g',
- np.array(self.time_array), np.array(self.a_target_array), 'k--',
- np.array(self.time_array), np.array(self.fcw_array), 'ro',
- )
- plt.xlabel('Time [s]')
- plt.ylabel('Acceleration [m/s^2]')
- plt.legend(['ego-plant', 'target', 'fcw'], loc=0)
- plt.grid()
- pylab.savefig("/".join([path, maneuver_name, 'acceleration.svg']), dpi=1000)
-
- # pedal chart ===================
- plt_num += 1
- plt.figure(plt_num)
- plt.plot(
- np.array(self.time_array), np.array(self.gas_array), 'g',
- np.array(self.time_array), np.array(self.brake_array), 'r',
- )
- plt.xlabel('Time [s]')
- plt.ylabel('Pedal []')
- plt.legend(['Gas pedal', 'Brake pedal'], loc=0)
- plt.grid()
- pylab.savefig("/".join([path, maneuver_name, 'pedals.svg']), dpi=1000)
-
- # pid chart ======================
- plt_num += 1
- plt.figure(plt_num)
- plt.plot(
- np.array(self.time_array), np.array(self.up_accel_cmd_array), 'g',
- np.array(self.time_array), np.array(self.ui_accel_cmd_array), 'b',
- np.array(self.time_array), np.array(self.uf_accel_cmd_array), 'r'
- )
- plt.xlabel("Time, [s]")
- plt.ylabel("Accel Cmd [m/s^2]")
- plt.grid()
- plt.legend(["Proportional", "Integral", "feedforward"], loc=0)
- pylab.savefig("/".join([path, maneuver_name, "pid.svg"]), dpi=1000)
-
- # relative distances chart =======
- plt_num += 1
- plt.figure(plt_num)
- plt.plot(
- np.array(self.time_array), np.array(self.d_rel_array), 'g',
- )
- plt.xlabel('Time [s]')
- plt.ylabel('Relative Distance [m]')
- plt.grid()
- pylab.savefig("/".join([path, maneuver_name, 'distance.svg']), dpi=1000)
-
- plt.close("all")
diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py
index 444ea7684e..7f2390858e 100755
--- a/selfdrive/test/longitudinal_maneuvers/plant.py
+++ b/selfdrive/test/longitudinal_maneuvers/plant.py
@@ -1,467 +1,137 @@
#!/usr/bin/env python3
-import binascii
-import os
-import struct
import time
-from collections import namedtuple
import numpy as np
-from opendbc import DBC_PATH
-
-from cereal import car, log
-from common.realtime import Ratekeeper
-from selfdrive.config import Conversions as CV
+from cereal import log
import cereal.messaging as messaging
-from selfdrive.car import crc8_pedal
-from selfdrive.car.honda.values import CAR
-from selfdrive.car.honda.carstate import get_can_signals
-from selfdrive.boardd.boardd import can_list_to_can_capnp
-
-from opendbc.can.parser import CANParser
-from selfdrive.car.honda.interface import CarInterface
-
-from opendbc.can.dbc import dbc
-honda = dbc(os.path.join(DBC_PATH, "honda_civic_touring_2016_can_generated.dbc"))
-
-# Trick: set 0x201 (interceptor) in fingerprints for gas is controlled like if there was an interceptor
-CP = CarInterface.get_params(CAR.CIVIC, {0: {0x201: 6}, 1: {}, 2: {}, 3: {}})
-
-# Honda checksum
-def can_cksum(mm):
- s = 0
- for c in mm:
- s += (c >> 4)
- s += c & 0xF
- s = 8-s
- s %= 0x10
- return s
-
-def fix(msg, addr):
- msg2 = msg[0:-1] + (msg[-1] | can_cksum(struct.pack("I", addr)+msg)).to_bytes(1, 'little')
- return msg2
-
-
-def car_plant(pos, speed, grade, gas, brake):
- # vehicle parameters
- mass = 1700
- aero_cd = 0.3
- force_peak = mass*3.
- force_brake_peak = -mass*10. # 1g
- power_peak = 100000 # 100kW
- speed_base = power_peak/force_peak
- rolling_res = 0.01
- gravity = 9.81
- frontal_area = 2.2
- air_density = 1.225
- gas_to_peak_linear_slope = 3.33
- brake_to_peak_linear_slope = 0.3
- creep_accel_v = [1., 0.]
- creep_accel_bp = [0., 1.5]
-
- #*** longitudinal model ***
- # find speed where peak torque meets peak power
- force_brake = brake * force_brake_peak * brake_to_peak_linear_slope
- if speed < speed_base: # torque control
- force_gas = gas * force_peak * gas_to_peak_linear_slope
- else: # power control
- force_gas = gas * power_peak / speed * gas_to_peak_linear_slope
-
- force_grade = - np.sin(np.arctan(grade)) * mass * gravity
+from common.realtime import Ratekeeper, DT_MDL
+from selfdrive.controls.lib.longcontrol import LongCtrlState
- creep_accel = np.interp(speed, creep_accel_bp, creep_accel_v)
- force_creep = creep_accel * mass
-
- force_resistance = -(rolling_res * mass * gravity + 0.5 * speed**2 * aero_cd * air_density * frontal_area)
- force = force_gas + force_brake + force_resistance + force_grade + force_creep
- acceleration = force / mass
-
- # TODO: lateral model
- return speed, acceleration
-
-def get_car_can_parser():
- dbc_f = 'honda_civic_touring_2016_can_generated'
- signals = [
- ("STEER_TORQUE", 0xe4, 0),
- ("STEER_TORQUE_REQUEST", 0xe4, 0),
- ("COMPUTER_BRAKE", 0x1fa, 0),
- ("COMPUTER_BRAKE_REQUEST", 0x1fa, 0),
- ("GAS_COMMAND", 0x200, 0),
- ]
- checks = [
- (0xe4, 100),
- (0x1fa, 50),
- (0x200, 50),
- ]
- return CANParser(dbc_f, signals, checks, 0)
-
-def to_3_byte(x):
- # Convert into 12 bit value
- s = struct.pack("!H", int(x))
- return binascii.hexlify(s)[1:]
-
-def to_3s_byte(x):
- s = struct.pack("!h", int(x))
- return binascii.hexlify(s)[1:]
class Plant():
messaging_initialized = False
- def __init__(self, lead_relevancy=False, rate=100, speed=0.0, distance_lead=2.0):
- self.rate = rate
+ def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0):
+ self.rate = 1. / DT_MDL
if not Plant.messaging_initialized:
- Plant.pm = messaging.PubMaster(['roadCameraState', 'driverCameraState', 'ubloxRaw'])
- Plant.logcan = messaging.pub_sock('can')
- Plant.sendcan = messaging.sub_sock('sendcan')
- Plant.model = messaging.pub_sock('modelV2')
- Plant.live_params = messaging.pub_sock('liveParameters')
- Plant.live_location_kalman = messaging.pub_sock('liveLocationKalman')
- Plant.pandaState = messaging.pub_sock('pandaState')
- Plant.deviceState = messaging.pub_sock('deviceState')
- Plant.driverMonitoringState = messaging.pub_sock('driverMonitoringState')
- Plant.cal = messaging.pub_sock('liveCalibration')
- Plant.controls_state = messaging.sub_sock('controlsState')
+ Plant.radar = messaging.pub_sock('radarState')
+ Plant.controls_state = messaging.pub_sock('controlsState')
+ Plant.car_state = messaging.pub_sock('carState')
Plant.plan = messaging.sub_sock('longitudinalPlan')
Plant.messaging_initialized = True
- self.frame = 0
- self.angle_steer = 0.
- self.gear_choice = 0
- self.speed, self.speed_prev = 0., 0.
+ self.v_lead_prev = 0.0
- self.esp_disabled = 0
- self.main_on = 1
- self.user_gas = 0
- self.computer_brake, self.user_brake = 0, 0
- self.brake_pressed = 0
- self.angle_steer_rate = 0
- self.distance, self.distance_prev = 0., 0.
- self.speed, self.speed_prev = speed, speed
- self.steer_error, self.brake_error, self.steer_not_allowed = 0, 0, 0
- self.gear_shifter = 8 # D gear
- self.pedal_gas = 0
- self.cruise_setting = 0
-
- self.seatbelt, self.door_all_closed = True, True
- self.steer_torque, self.v_cruise, self.acc_status = 0, 0, 0 # v_cruise is reported from can, not the one used for controls
-
- self.lead_relevancy = lead_relevancy
+ self.distance = 0.
+ self.speed = speed
+ self.acceleration = 0.0
# lead car
- self.distance_lead, self.distance_lead_prev = distance_lead , distance_lead
-
- self.rk = Ratekeeper(rate, print_delay_threshold=100.0)
- self.ts = 1./rate
-
- self.cp = get_car_can_parser()
- self.response_seen = False
+ self.distance_lead = distance_lead
+ self.lead_relevancy = lead_relevancy
+ self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
+ self.ts = 1. / self.rate
time.sleep(1)
- messaging.drain_sock(Plant.sendcan)
- messaging.drain_sock(Plant.controls_state)
-
- def close(self):
- Plant.logcan.close()
- Plant.model.close()
- Plant.live_params.close()
- Plant.live_location_kalman.close()
-
- def speed_sensor(self, speed):
- if speed < 0.3:
- return 0
- else:
- return speed * CV.MS_TO_KPH
+ self.sm = messaging.SubMaster(['longitudinalPlan'])
def current_time(self):
return float(self.rk.frame) / self.rate
- def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model=True):
- gen_signals, _ = get_can_signals(CP)
- sgs = [s[0] for s in gen_signals]
- msgs = [s[1] for s in gen_signals]
-
- # ******** get messages sent to the car ********
- can_strings = messaging.drain_sock_raw(Plant.sendcan, wait_for_one=self.response_seen)
+ def step(self, v_lead=0.0):
+ # ******** publish a fake model going straight and fake calibration ********
+ # note that this is worst case for MPC, since model will delay long mpc by one time step
+ radar = messaging.new_message('radarState')
+ control = messaging.new_message('controlsState')
+ car_state = messaging.new_message('carState')
+ a_lead = (v_lead - self.v_lead_prev)/self.ts
+ self.v_lead_prev = v_lead
- # After the first response the car is done fingerprinting, so we can run in lockstep with controlsd
- if can_strings:
- self.response_seen = True
+ if self.lead_relevancy:
+ d_rel = np.maximum(0., self.distance_lead - self.distance)
+ v_rel = v_lead - self.speed
+ prob = 1.0
+ else:
+ d_rel = 200.
+ v_rel = 0.
+ prob = 0.0
+
+ lead = log.RadarState.LeadData.new_message()
+ lead.dRel = float(d_rel)
+ lead.yRel = float(0.0)
+ lead.vRel = float(v_rel)
+ lead.aRel = float(a_lead - self.acceleration)
+ lead.vLead = float(v_lead)
+ lead.vLeadK = float(v_lead)
+ lead.aLeadK = float(a_lead)
+ lead.aLeadTau = float(1.5)
+ lead.status = True
+ lead.modelProb = prob
+ radar.radarState.leadOne = lead
+ radar.radarState.leadTwo = lead
+
+
+ control.controlsState.longControlState = LongCtrlState.pid
+ control.controlsState.vCruise = 130
+ car_state.carState.vEgo = self.speed
+ Plant.radar.send(radar.to_bytes())
+ Plant.controls_state.send(control.to_bytes())
+ Plant.car_state.send(car_state.to_bytes())
- self.cp.update_strings(can_strings, sendcan=True)
# ******** get controlsState messages for plotting ***
- controls_state_msgs = []
- for a in messaging.drain_sock(Plant.controls_state, wait_for_one=self.response_seen):
- controls_state_msgs.append(a.controlsState)
+ self.sm.update()
+ while True:
+ time.sleep(0.01)
+ if self.sm.updated['longitudinalPlan']:
+ plan = self.sm['longitudinalPlan']
+ self.acceleration = plan.aTarget
+ fcw = plan.fcw
+ break
+ self.speed += self.ts*self.acceleration
- fcw = None
- for a in messaging.drain_sock(Plant.plan):
- if a.longitudinalPlan.fcw:
- fcw = True
-
- if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
- brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE'] * 0.003906248
- else:
- brake = 0.0
-
- if self.cp.vl[0x200]['GAS_COMMAND'] > 0:
- gas = self.cp.vl[0x200]['GAS_COMMAND'] / 256.0
- else:
- gas = 0.0
-
- if self.cp.vl[0xe4]['STEER_TORQUE_REQUEST']:
- steer_torque = self.cp.vl[0xe4]['STEER_TORQUE']*1.0/0xf00
- else:
- steer_torque = 0.0
- distance_lead = self.distance_lead_prev + v_lead * self.ts
+ self.distance_lead = self.distance_lead + v_lead * self.ts
# ******** run the car ********
- speed, acceleration = car_plant(self.distance_prev, self.speed_prev, grade, gas, brake)
- distance = self.distance_prev + speed * self.ts
- speed = self.speed_prev + self.ts * acceleration
- if speed <= 0:
- speed = 0
- acceleration = 0
-
- # ******** lateral ********
- self.angle_steer -= (steer_torque/10.0) * self.ts
+ #print(self.distance, speed)
+ if self.speed <= 0:
+ self.speed = 0
+ self.acceleration = 0
+ self.distance = self.distance + self.speed * self.ts
# *** radar model ***
if self.lead_relevancy:
- d_rel = np.maximum(0., distance_lead - distance)
- v_rel = v_lead - speed
+ d_rel = np.maximum(0., self.distance_lead - self.distance)
+ v_rel = v_lead - self.speed
else:
d_rel = 200.
v_rel = 0.
- lateral_pos_rel = 0.
# print at 5hz
- if (self.frame % (self.rate//5)) == 0:
- print("%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s"
- % (distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel))
-
- # ******** publish the car ********
- vls_tuple = namedtuple('vls', [
- 'XMISSION_SPEED',
- 'WHEEL_SPEED_FL', 'WHEEL_SPEED_FR', 'WHEEL_SPEED_RL', 'WHEEL_SPEED_RR',
- 'STEER_ANGLE', 'STEER_ANGLE_RATE', 'STEER_TORQUE_SENSOR', 'STEER_TORQUE_MOTOR',
- 'LEFT_BLINKER', 'RIGHT_BLINKER',
- 'GEAR',
- 'WHEELS_MOVING',
- 'BRAKE_ERROR_1', 'BRAKE_ERROR_2',
- 'SEATBELT_DRIVER_LAMP', 'SEATBELT_DRIVER_LATCHED',
- 'BRAKE_PRESSED', 'BRAKE_SWITCH',
- 'CRUISE_BUTTONS',
- 'ESP_DISABLED',
- 'HUD_LEAD',
- 'USER_BRAKE',
- 'STEER_STATUS',
- 'GEAR_SHIFTER',
- 'PEDAL_GAS',
- 'CRUISE_SETTING',
- 'ACC_STATUS',
-
- 'CRUISE_SPEED_PCM',
- 'CRUISE_SPEED_OFFSET',
-
- 'DOOR_OPEN_FL', 'DOOR_OPEN_FR', 'DOOR_OPEN_RL', 'DOOR_OPEN_RR',
-
- 'CAR_GAS',
- 'MAIN_ON',
- 'EPB_STATE',
- 'BRAKE_HOLD_ACTIVE',
- 'INTERCEPTOR_GAS',
- 'INTERCEPTOR_GAS2',
- 'IMPERIAL_UNIT',
- 'MOTOR_TORQUE',
- ])
- vls = vls_tuple(
- self.speed_sensor(speed),
- self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed),
- self.angle_steer, self.angle_steer_rate, 0, 0, # Steer torque sensor
- 0, 0, # Blinkers
- self.gear_choice,
- speed != 0,
- self.brake_error, self.brake_error,
- not self.seatbelt, self.seatbelt, # Seatbelt
- self.brake_pressed, 0., # Brake pressed, Brake switch
- cruise_buttons,
- self.esp_disabled,
- 0, # HUD lead
- self.user_brake,
- self.steer_error,
- self.gear_shifter,
- self.pedal_gas,
- self.cruise_setting,
- self.acc_status,
-
- self.v_cruise,
- 0, # Cruise speed offset
-
- 0, 0, 0, 0, # Doors
-
- self.user_gas,
- self.main_on,
- 0, # EPB State
- 0, # Brake hold
- 0, # Interceptor feedback
- 0, # Interceptor 2 feedback
- False,
- 0,
- )
-
- # TODO: publish each message at proper frequency
- can_msgs = []
- for msg in set(msgs):
- msg_struct = {}
- indxs = [i for i, x in enumerate(msgs) if msg == msgs[i]]
- for i in indxs:
- msg_struct[sgs[i]] = getattr(vls, sgs[i])
-
- if "COUNTER" in honda.get_signals(msg):
- msg_struct["COUNTER"] = self.frame % 4
-
- if "COUNTER_PEDAL" in honda.get_signals(msg):
- msg_struct["COUNTER_PEDAL"] = self.frame % 0xf
-
- msg = honda.lookup_msg_id(msg)
- msg_data = honda.encode(msg, msg_struct)
-
- if "CHECKSUM" in honda.get_signals(msg):
- msg_data = fix(msg_data, msg)
-
- if "CHECKSUM_PEDAL" in honda.get_signals(msg):
- msg_struct["CHECKSUM_PEDAL"] = crc8_pedal(msg_data[:-1])
- msg_data = honda.encode(msg, msg_struct)
-
- can_msgs.append([msg, 0, msg_data, 0])
-
- # add the radar message
- # TODO: use the DBC
- if self.frame % 5 == 0:
- radar_state_msg = b'\x79\x00\x00\x00\x00\x00\x00\x00'
- radar_msg = to_3_byte(d_rel * 16.0) + \
- to_3_byte(int(lateral_pos_rel * 16.0) & 0x3ff) + \
- to_3s_byte(int(v_rel * 32.0)) + \
- b"0f00000"
-
- radar_msg = binascii.unhexlify(radar_msg)
- can_msgs.append([0x400, 0, radar_state_msg, 1])
- can_msgs.append([0x445, 0, radar_msg, 1])
-
- radar_messages = list(range(0x430, 0x43A)) + list(range(0x440, 0x445))
- for m in radar_messages:
- can_msgs.append([m, 0, b'\x00'*8, 1])
+ if (self.rk.frame % (self.rate // 5)) == 0:
+ print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s"
+ % (self.current_time(), self.distance, self.speed, self.acceleration, d_rel, v_rel))
- msg_struct["COUNTER"] = self.frame % 4
- camera_messages = [0x1FA, 0x30C, 0xE4]
- for m in camera_messages:
- msg = honda.lookup_msg_id(m)
- msg_data = honda.encode(msg, msg_struct)
- msg_data = fix(msg_data, m)
- can_msgs.append([m, 0, msg_data, 2])
-
- # Fake sockets that controlsd subscribes to
- live_parameters = messaging.new_message('liveParameters')
- live_parameters.liveParameters.valid = True
- live_parameters.liveParameters.sensorValid = True
- live_parameters.liveParameters.posenetValid = True
- live_parameters.liveParameters.steerRatio = CP.steerRatio
- live_parameters.liveParameters.stiffnessFactor = 1.0
- Plant.live_params.send(live_parameters.to_bytes())
-
- dmon_state = messaging.new_message('driverMonitoringState')
- dmon_state.driverMonitoringState.isDistracted = False
- Plant.driverMonitoringState.send(dmon_state.to_bytes())
-
- pandaState = messaging.new_message('pandaState')
- pandaState.pandaState.safetyModel = car.CarParams.SafetyModel.hondaNidec
- pandaState.pandaState.controlsAllowed = True
- Plant.pandaState.send(pandaState.to_bytes())
-
- deviceState = messaging.new_message('deviceState')
- deviceState.deviceState.freeSpacePercent = 100
- deviceState.deviceState.batteryPercent = 100
- Plant.deviceState.send(deviceState.to_bytes())
-
- live_location_kalman = messaging.new_message('liveLocationKalman')
- live_location_kalman.liveLocationKalman.inputsOK = True
- live_location_kalman.liveLocationKalman.gpsOK = True
- Plant.live_location_kalman.send(live_location_kalman.to_bytes())
-
- # ******** publish a fake model going straight and fake calibration ********
- # note that this is worst case for MPC, since model will delay long mpc by one time step
- if publish_model and self.frame % 5 == 0:
- md = messaging.new_message('modelV2')
- cal = messaging.new_message('liveCalibration')
- md.modelV2.frameId = 0
-
- if self.lead_relevancy:
- d_rel = np.maximum(0., distance_lead - distance)
- v_rel = v_lead - speed
- prob = 1.0
- else:
- d_rel = 200.
- v_rel = 0.
- prob = 0.0
-
- lead = log.ModelDataV2.LeadDataV2.new_message()
- lead.xyva = [float(d_rel), 0.0, float(v_rel), 0.0]
- lead.xyvaStd = [1.0, 1.0, 1.0, 1.0]
- lead.prob = prob
- md.modelV2.leads = [lead, lead]
-
- cal.liveCalibration.calStatus = 1
- cal.liveCalibration.calPerc = 100
- cal.liveCalibration.rpyCalib = [0.] * 3
- # fake values?
- Plant.model.send(md.to_bytes())
- Plant.cal.send(cal.to_bytes())
- for s in Plant.pm.sock.keys():
- try:
- Plant.pm.send(s, messaging.new_message(s))
- except Exception:
- Plant.pm.send(s, messaging.new_message(s, 1))
-
- Plant.logcan.send(can_list_to_can_capnp(can_msgs))
# ******** update prevs ********
- self.frame += 1
-
- if self.response_seen:
- self.rk.monitor_time()
-
- self.speed = speed
- self.distance = distance
- self.distance_lead = distance_lead
-
- self.speed_prev = speed
- self.distance_prev = distance
- self.distance_lead_prev = distance_lead
-
- else:
- # Don't advance time when controlsd is not yet ready
- self.rk.keep_time()
- self.rk._frame = 0
+ self.rk.monitor_time()
return {
- "distance": distance,
- "speed": speed,
- "acceleration": acceleration,
- "distance_lead": distance_lead,
- "brake": brake,
- "gas": gas,
- "steer_torque": steer_torque,
+ "distance": self.distance,
+ "speed": self.speed,
+ "acceleration": self.acceleration,
+ "distance_lead": self.distance_lead,
"fcw": fcw,
- "controls_state_msgs": controls_state_msgs,
}
# simple engage in standalone mode
-def plant_thread(rate=100):
- plant = Plant(rate)
+def plant_thread():
+ plant = Plant()
while 1:
plant.step()
+
if __name__ == "__main__":
plant_thread()
diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py
index 1a2edc63ea..3026510d9e 100755
--- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py
+++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py
@@ -1,90 +1,21 @@
#!/usr/bin/env python3
import os
-os.environ['NOCRASH'] = '1'
-
import unittest
-import matplotlib
-matplotlib.use('svg')
-from selfdrive.config import Conversions as CV
-from selfdrive.car.honda.values import CruiseButtons as CB
+from common.params import Params
from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
from selfdrive.manager.process_config import managed_processes
-from common.file_helpers import mkdirs_exists_ok
-from common.params import Params
-
-
-def check_no_collision(log):
- return min(log['d_rel']) > 0
-def check_fcw(log):
- return any(log['fcw'])
-
-
-def check_engaged(log):
- return log['controls_state_msgs'][-1][-1].active
+def put_default_car_params():
+ from selfdrive.car.honda.values import CAR
+ from selfdrive.car.honda.interface import CarInterface
+ cp = CarInterface.get_params(CAR.CIVIC)
+ Params().put("CarParams", cp.to_bytes())
+# TODO: make new FCW tests
maneuvers = [
- Maneuver(
- 'while cruising at 40 mph, change cruise speed to 50mph',
- duration=30.,
- initial_speed=40. * CV.MPH_TO_MS,
- cruise_button_presses=[(CB.DECEL_SET, 2.), (0, 2.3),
- (CB.RES_ACCEL, 10.), (0, 10.1),
- (CB.RES_ACCEL, 10.2), (0, 10.3)],
- checks=[check_engaged],
- ),
- Maneuver(
- 'while cruising at 60 mph, change cruise speed to 50mph',
- duration=30.,
- initial_speed=60. * CV.MPH_TO_MS,
- cruise_button_presses=[(CB.DECEL_SET, 2.), (0, 2.3),
- (CB.DECEL_SET, 10.), (0, 10.1),
- (CB.DECEL_SET, 10.2), (0, 10.3)],
- checks=[check_engaged],
- ),
- Maneuver(
- 'while cruising at 20mph, uphill grade of 10%',
- duration=25.,
- initial_speed=20. * CV.MPH_TO_MS,
- cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3)],
- grade_values=[0., 0., .1],
- grade_breakpoints=[0., 10., 11.],
- checks=[check_engaged],
- ),
- Maneuver(
- 'while cruising at 20mph, downhill grade of -10%',
- duration=25.,
- initial_speed=20. * CV.MPH_TO_MS,
- cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3)],
- grade_values=[0., 0., -.1],
- grade_breakpoints=[0., 10., 11.],
- checks=[check_engaged],
- ),
- Maneuver(
- 'approaching a 40mph car while cruising at 60mph from 100m away',
- duration=30.,
- initial_speed=60. * CV.MPH_TO_MS,
- lead_relevancy=True,
- initial_distance_lead=100.,
- speed_lead_values=[40. * CV.MPH_TO_MS, 40. * CV.MPH_TO_MS],
- speed_lead_breakpoints=[0., 100.],
- cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3)],
- checks=[check_engaged, check_no_collision],
- ),
- Maneuver(
- 'approaching a 0mph car while cruising at 40mph from 150m away',
- duration=30.,
- initial_speed=40. * CV.MPH_TO_MS,
- lead_relevancy=True,
- initial_distance_lead=150.,
- speed_lead_values=[0. * CV.MPH_TO_MS, 0. * CV.MPH_TO_MS],
- speed_lead_breakpoints=[0., 100.],
- cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3)],
- checks=[check_engaged, check_no_collision],
- ),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2',
duration=50.,
@@ -93,8 +24,6 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values=[20., 20., 0.],
speed_lead_breakpoints=[0., 15., 35.0],
- cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3)],
- checks=[check_engaged, check_no_collision],
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2',
@@ -104,8 +33,6 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values=[20., 20., 0.],
speed_lead_breakpoints=[0., 15., 25.0],
- cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3)],
- checks=[check_engaged, check_no_collision],
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2',
@@ -115,8 +42,6 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values=[20., 20., 0.],
speed_lead_breakpoints=[0., 15., 21.66],
- cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3)],
- checks=[check_engaged, check_fcw],
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 5m/s^2',
@@ -126,195 +51,19 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values=[20., 20., 0.],
speed_lead_breakpoints=[0., 15., 19.],
- cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3)],
- checks=[check_engaged, check_fcw],
- ),
- Maneuver(
- 'starting at 0mph, approaching a stopped car 100m away',
- duration=30.,
- initial_speed=0.,
- lead_relevancy=True,
- initial_distance_lead=100.,
- cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7),
- (CB.RES_ACCEL, 1.8), (0.0, 1.9)],
- checks=[check_engaged, check_no_collision],
- ),
- Maneuver(
- "following a car at 60mph, lead accel and decel at 0.5m/s^2 every 2s",
- duration=25.,
- initial_speed=30.,
- lead_relevancy=True,
- initial_distance_lead=49.,
- speed_lead_values=[30., 30., 29., 31., 29., 31., 29.],
- speed_lead_breakpoints=[0., 6., 8., 12., 16., 20., 24.],
- cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7)],
- checks=[check_engaged, check_no_collision],
- ),
- Maneuver(
- "following a car at 10mph, stop and go at 1m/s2 lead dece1 and accel",
- duration=70.,
- initial_speed=10.,
- lead_relevancy=True,
- initial_distance_lead=20.,
- speed_lead_values=[10., 0., 0., 10., 0., 10.],
- speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
- cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7)],
- checks=[check_engaged, check_no_collision],
),
Maneuver(
- "green light: stopped behind lead car, lead car accelerates at 1.5 m/s",
+ "approach stopped car at 20m/s",
duration=30.,
- initial_speed=0.,
- lead_relevancy=True,
- initial_distance_lead=4.,
- speed_lead_values=[0, 0, 45],
- speed_lead_breakpoints=[0, 10., 40.],
- cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7),
- (CB.RES_ACCEL, 1.8), (0.0, 1.9),
- (CB.RES_ACCEL, 2.0), (0.0, 2.1),
- (CB.RES_ACCEL, 2.2), (0.0, 2.3)],
- checks=[check_engaged, check_no_collision],
- ),
- Maneuver(
- "stop and go with 1m/s2 lead decel and accel, with full stops",
- duration=70.,
- initial_speed=0.,
- lead_relevancy=True,
- initial_distance_lead=20.,
- speed_lead_values=[10., 0., 0., 10., 0., 0.],
- speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
- cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7)],
- checks=[check_engaged, check_no_collision],
- ),
- Maneuver(
- "stop and go with 1.5m/s2 lead accel and 3.3m/s^2 lead decel, with full stops",
- duration=45.,
- initial_speed=0.,
- lead_relevancy=True,
- initial_distance_lead=20.,
- speed_lead_values=[10., 0., 0., 10., 0., 0.],
- speed_lead_breakpoints=[10., 13., 26., 33., 36., 45.],
- cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7)],
- checks=[check_engaged, check_no_collision],
- ),
- Maneuver(
- "accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2",
- duration=30.,
- initial_speed=10.,
- lead_relevancy=True,
- initial_distance_lead=10.,
- speed_lead_values=[20., 10.],
- speed_lead_breakpoints=[1., 11.],
- cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7),
- (CB.RES_ACCEL, 1.8), (0.0, 1.9),
- (CB.RES_ACCEL, 2.0), (0.0, 2.1),
- (CB.RES_ACCEL, 2.2), (0.0, 2.3)],
- checks=[check_engaged, check_no_collision],
- ),
- Maneuver(
- "accelerate from 20 while lead vehicle decelerates from 40 to 0 at 2m/s2",
- duration=30.,
- initial_speed=10.,
- lead_relevancy=True,
- initial_distance_lead=10.,
- speed_lead_values=[20., 0.],
- speed_lead_breakpoints=[1., 11.],
- cruise_button_presses=[(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7),
- (CB.RES_ACCEL, 1.8), (0.0, 1.9),
- (CB.RES_ACCEL, 2.0), (0.0, 2.1),
- (CB.RES_ACCEL, 2.2), (0.0, 2.3)],
- checks=[check_engaged, check_no_collision],
- ),
- Maneuver(
- "fcw: traveling at 30 m/s and approaching lead traveling at 20m/s",
- duration=15.,
- initial_speed=30.,
- lead_relevancy=True,
- initial_distance_lead=100.,
- speed_lead_values=[20.],
- speed_lead_breakpoints=[1.],
- cruise_button_presses=[],
- checks=[check_fcw],
- ),
- Maneuver(
- "fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 1m/s2",
- duration=18.,
initial_speed=20.,
lead_relevancy=True,
- initial_distance_lead=35.,
- speed_lead_values=[20., 0.],
- speed_lead_breakpoints=[3., 23.],
- cruise_button_presses=[],
- checks=[check_fcw],
- ),
- Maneuver(
- "fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 3m/s2",
- duration=13.,
- initial_speed=20.,
- lead_relevancy=True,
- initial_distance_lead=35.,
- speed_lead_values=[20., 0.],
- speed_lead_breakpoints=[3., 9.6],
- cruise_button_presses=[],
- checks=[check_fcw],
+ initial_distance_lead=50.,
+ speed_lead_values=[0., 0.],
+ speed_lead_breakpoints=[1., 11.],
),
- Maneuver(
- "fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 5m/s2",
- duration=8.,
- initial_speed=20.,
- lead_relevancy=True,
- initial_distance_lead=35.,
- speed_lead_values=[20., 0.],
- speed_lead_breakpoints=[3., 7.],
- cruise_button_presses=[],
- checks=[check_fcw],
- )
]
-def setup_output():
- output_dir = os.path.join(os.getcwd(), 'out/longitudinal')
- if not os.path.exists(os.path.join(output_dir, "index.html")):
- # write test output header
-
- css_style = """
- .maneuver_title {
- font-size: 24px;
- text-align: center;
- }
- .maneuver_graph {
- width: 100%;
- }
- """
-
- view_html = "" % (css_style,)
- for i, man in enumerate(maneuvers):
- view_html += "%s |
" % (man.title,)
- for c in ['distance.svg', 'speeds.svg', 'acceleration.svg', 'pedals.svg', 'pid.svg']:
- view_html += " | " % (os.path.join("maneuver" + str(i + 1).zfill(2), c), )
- view_html += "
"
-
- mkdirs_exists_ok(output_dir)
- with open(os.path.join(output_dir, "index.html"), "w") as f:
- f.write(view_html)
-
-
class LongitudinalControl(unittest.TestCase):
@classmethod
def setUpClass(cls):
@@ -322,8 +71,6 @@ class LongitudinalControl(unittest.TestCase):
os.environ['SKIP_FW_QUERY'] = "1"
os.environ['NO_CAN_TIMEOUT'] = "1"
- setup_output()
-
params = Params()
params.clear_all()
params.put_bool("Passive", bool(os.getenv("PASSIVE")))
@@ -336,35 +83,22 @@ class LongitudinalControl(unittest.TestCase):
def run_maneuver_worker(k):
- man = maneuvers[k]
- output_dir = os.path.join(os.getcwd(), 'out/longitudinal')
-
def run(self):
+ man = maneuvers[k]
print(man.title)
- valid = False
+ put_default_car_params()
+ managed_processes['plannerd'].start()
- for _ in range(3):
- managed_processes['radard'].start()
- managed_processes['controlsd'].start()
- managed_processes['plannerd'].start()
-
- plot, valid = man.evaluate()
- plot.write_plot(output_dir, "maneuver" + str(k + 1).zfill(2))
-
- managed_processes['radard'].stop()
- managed_processes['controlsd'].stop()
- managed_processes['plannerd'].stop()
-
- if valid:
- break
+ valid = man.evaluate()
+ managed_processes['plannerd'].stop()
self.assertTrue(valid)
-
return run
for k in range(len(maneuvers)):
- setattr(LongitudinalControl, "test_longitudinal_maneuvers_%d" % (k + 1), run_maneuver_worker(k))
+ setattr(LongitudinalControl, f"test_longitudinal_maneuvers_{k+1}",
+ run_maneuver_worker(k))
if __name__ == "__main__":
unittest.main(failfast=True)
diff --git a/tools/sim/lib/can.py b/tools/sim/lib/can.py
index a09725dfb4..96fd786337 100755
--- a/tools/sim/lib/can.py
+++ b/tools/sim/lib/can.py
@@ -1,16 +1,32 @@
#!/usr/bin/env python3
import cereal.messaging as messaging
from opendbc.can.packer import CANPacker
+from opendbc.can.parser import CANParser
from selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp # pylint: disable=no-name-in-module,import-error
from selfdrive.car.honda.values import FINGERPRINTS, CAR
from selfdrive.car import crc8_pedal
-from selfdrive.test.longitudinal_maneuvers.plant import get_car_can_parser
-cp = get_car_can_parser()
-
packer = CANPacker("honda_civic_touring_2016_can_generated")
rpacker = CANPacker("acura_ilx_2016_nidec")
+
+def get_car_can_parser():
+ dbc_f = 'honda_civic_touring_2016_can_generated'
+ signals = [
+ ("STEER_TORQUE", 0xe4, 0),
+ ("STEER_TORQUE_REQUEST", 0xe4, 0),
+ ("COMPUTER_BRAKE", 0x1fa, 0),
+ ("COMPUTER_BRAKE_REQUEST", 0x1fa, 0),
+ ("GAS_COMMAND", 0x200, 0),
+ ]
+ checks = [
+ (0xe4, 100),
+ (0x1fa, 50),
+ (0x200, 50),
+ ]
+ return CANParser(dbc_f, signals, checks, 0)
+cp = get_car_can_parser()
+
def can_function(pm, speed, angle, idx, cruise_button, is_engaged):
msg = []