Skip to content

Commit

Permalink
Merge pull request #2 from arne182/066-clean
Browse files Browse the repository at this point in the history
066 clean
  • Loading branch information
sshane authored Dec 2, 2019
2 parents 3c81ac2 + a0241de commit bec9f51
Show file tree
Hide file tree
Showing 12 changed files with 189 additions and 41 deletions.
21 changes: 16 additions & 5 deletions selfdrive/boardd/boardd.cc
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,8 @@ bool loopback_can = false;
cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN;
bool is_pigeon = false;
const uint32_t NO_IGNITION_CNT_MAX = 2 * 60 * 60 * 24 * 1; // turn off charge after 1 days
const uint32_t VBATT_START_CHARGING = 12000;
const uint32_t VBATT_PAUSE_CHARGING = 10500;
uint32_t no_ignition_cnt = 0;
bool connected_once = false;
bool ignition_last = false;
Expand Down Expand Up @@ -353,11 +355,20 @@ void can_health(PubSocket *publisher) {
}

#ifndef __x86_64__
if ((no_ignition_cnt > NO_IGNITION_CNT_MAX) && (health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP)) || ((health.voltage < 10500) && !ignition)){
printf("TURN OFF CHARGING!\n");
pthread_mutex_lock(&usb_lock);
libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT);
pthread_mutex_unlock(&usb_lock);
bool cdp_mode = health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP);
bool no_ignition_exp = no_ignition_cnt > NO_IGNITION_CNT_MAX;
if ((no_ignition_exp || (health.voltage < VBATT_PAUSE_CHARGING)) && cdp_mode && !ignition) {
printf("TURN OFF CHARGING!\n");
pthread_mutex_lock(&usb_lock);
libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT);
pthread_mutex_unlock(&usb_lock);
}
if (!no_ignition_exp && (health.voltage > VBATT_START_CHARGING) && !cdp_mode) {

printf("TURN ON CHARGING!\n");
pthread_mutex_lock(&usb_lock);
libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CDP), 0, NULL, 0, TIMEOUT);
pthread_mutex_unlock(&usb_lock);
}
#endif

Expand Down
7 changes: 4 additions & 3 deletions selfdrive/car/toyota/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -424,10 +424,11 @@ def update(self, c, can_strings):
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))

# disable on pedals rising edge or when brake is pressed and speed isn't zero
if ((ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001))) and disengage_event:
if (((ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001))) and disengage_event) or (ret.brakePressed and not self.brake_pressed_prev and ret.vEgo < 0.1):
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))



if ret.gasPressed and disengage_event:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))

Expand Down
6 changes: 3 additions & 3 deletions selfdrive/car/toyota/values.py
Original file line number Diff line number Diff line change
Expand Up @@ -197,6 +197,6 @@ class ECU:
NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2] # no resume button press required
'''
# todo: I'm using stock 066 below, above this is from 065-clean. Arne, check this out and delete the above if it looks good
NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]
TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]
NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA] # no resume button press required
NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2]
TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2]
NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2] # no resume button press required
1 change: 1 addition & 0 deletions selfdrive/controls/lib/long_mpc.py
Original file line number Diff line number Diff line change
Expand Up @@ -120,6 +120,7 @@ def dynamic_follow(self): # in m/s
x = [4.4704, 6.7056] # smoothly ramp TR between 10 and 15 mph from 1.8s to defined TR above at 15mph
y = [1.8, interp(x[1], x_vel, y_mod)]
TR = interp(self.v_ego, x, y)
return round(TR, 3)

if self.lead_data['v_lead'] is not None: # if lead
x = [-15.6464, -9.8422, -6.0, -4.0, -2.68, -2.3, -1.8, -1.26, -0.61, 0, 0.61, 1.26, 2.1, 2.68] # relative velocity values
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/controls/lib/longcontrol.py
Original file line number Diff line number Diff line change
Expand Up @@ -184,10 +184,10 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_
self.pid.k_f=1.0
if gas_pressed or brake_pressed:
if not self.freeze:
self.pid.i = 0.0
self.freeze = True
else:
if self.freeze:
self.pid.i = 0.0
self.freeze = False

output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=(prevent_overshoot or gas_pressed or brake_pressed))
Expand Down
35 changes: 23 additions & 12 deletions selfdrive/controls/lib/planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -117,18 +117,28 @@ def __init__(self, CP):

self.params = Params()

def choose_solution(self, v_cruise_setpoint, enabled, yRel, dRel, steeringAngle):
def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAngle):
center_x = -2.5 # Wheel base 2.5m
lead1_check = True
lead2_check = True
if steeringAngle > 100: # only at high angles
center_y = -1+2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Left side considered in left hand turn
lead1_check = math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1.
lead2_check = math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1.
elif steeringAngle < -100: # only at high angles
center_y = +1-2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Right side considered in right hand turn
lead1_check = math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1.
lead2_check = math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1.
if enabled:
solutions = {'model': self.v_model, 'cruise': self.v_cruise}
if self.mpc1.prev_lead_status and (abs(math.atan2(yRel,dRel)*180/math.pi - steeringAngle/10) < yRel + max(math.atan2(1,dRel)*180/math.pi, 5.0)):
if self.mpc1.prev_lead_status and lead1_check:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
if self.mpc2.prev_lead_status and lead2_check:
solutions['mpc2'] = self.mpc2.v_mpc

slowest = min(solutions, key=solutions.get)

self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
Expand All @@ -142,7 +152,11 @@ def choose_solution(self, v_cruise_setpoint, enabled, yRel, dRel, steeringAngle)
self.v_acc = self.v_model
self.a_acc = self.a_model

self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
self.v_acc_future = v_cruise_setpoint
if lead1_check:
self.v_acc_future = min([self.mpc1.v_mpc_future, self.v_acc_future])
if lead2_check:
self.v_acc_future = min([self.mpc2.v_mpc_future, self.v_acc_future])

def update(self, sm, pm, CP, VM, PP):
self.arne_sm.update(0)
Expand Down Expand Up @@ -177,7 +191,7 @@ def update(self, sm, pm, CP, VM, PP):
lead_2 = sm['radarState'].leadTwo

enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
following = self.mpc1.prev_lead_status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0

v_speedlimit = NO_CURVATURE_SPEED
v_curvature_map = NO_CURVATURE_SPEED
Expand Down Expand Up @@ -302,7 +316,7 @@ def update(self, sm, pm, CP, VM, PP):
self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint)
self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint)

self.choose_solution(v_cruise_setpoint, enabled, lead_1.yRel, lead_1.dRel, sm['carState'].steeringAngle)
self.choose_solution(v_cruise_setpoint, enabled, lead_1, lead_2, sm['carState'].steeringAngle)

# determine fcw
if self.mpc1.new_lead:
Expand Down Expand Up @@ -340,10 +354,7 @@ def update(self, sm, pm, CP, VM, PP):
plan_send.plan.aStart = float(self.a_acc_start)
plan_send.plan.vTarget = float(self.v_acc)
plan_send.plan.aTarget = float(self.a_acc)
if (lead_1.status and lead_1.dRel < 4.0) or (lead_2.status and lead_2.dRel) < 4.0:
plan_send.plan.vTargetFuture = float(0.0)
else:
plan_send.plan.vTargetFuture = float(self.v_acc_future)
plan_send.plan.vTargetFuture = float(self.v_acc_future)
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource

Expand Down
2 changes: 2 additions & 0 deletions selfdrive/manager.py
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,7 @@ def unblock_stdout():
"gpsd": ("selfdrive/sensord", ["./start_gpsd.py"]),
"updated": "selfdrive.updated",
"mapd": ("selfdrive/mapd", ["./mapd.py"]),
#"traffic": ("selfdrive/traffic", ["./traffic.py"]),
}
daemon_processes = {
"manage_athenad": ("selfdrive.athena.manage_athenad", "AthenadPid"),
Expand Down Expand Up @@ -140,6 +141,7 @@ def get_running():
'gpsd',
'deleter',
'mapd',
#'traffic',
]

def register_managed_process(name, desc, car_started=False):
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/mapd/mapd.py
Original file line number Diff line number Diff line change
Expand Up @@ -245,7 +245,7 @@ def mapsd_thread():
circles = [circle_through_points(*p, direction=True) for p in zip(pnts, pnts[1:], pnts[2:])]
circles = np.asarray(circles)
radii = np.nan_to_num(circles[:, 2])
radii[radii < 15.] = 10000
radii[abs(radii) < 15.] = 10000

if cur_way.way.tags['highway'] == 'trunk':
radii = radii*1.6 # https://media.springernature.com/lw785/springer-static/image/chp%3A10.1007%2F978-3-658-01689-0_21/MediaObjects/298553_35_De_21_Fig65_HTML.gif
Expand Down
18 changes: 9 additions & 9 deletions selfdrive/mapd/mapd_helpers.py
Original file line number Diff line number Diff line change
Expand Up @@ -428,7 +428,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
if backwards and (n.tags['traffic_signals:direction']=='backward' or n.tags['traffic_signals:direction']=='both'):
print("backward")
if way_pts[count, 0] > 0:
speed_ahead_dist = max(0. , way_pts[count, 0] - 1.0)
speed_ahead_dist = max(0. , way_pts[count, 0] - 2.0)
print(speed_ahead_dist)
speed_ahead = 5/3.6
if n.tags['highway']=='traffic_signals':
Expand All @@ -438,7 +438,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
elif not backwards and (n.tags['traffic_signals:direction']=='forward' or n.tags['traffic_signals:direction']=='both'):
print("forward")
if way_pts[count, 0] > 0:
speed_ahead_dist = max(0. , way_pts[count, 0] - 1.0)
speed_ahead_dist = max(0. , way_pts[count, 0] - 2.0)
print(speed_ahead_dist)
speed_ahead = 5/3.6
if n.tags['highway']=='traffic_signals':
Expand All @@ -454,7 +454,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
if direction > 180:
direction = direction - 360
if abs(direction) > 135:
speed_ahead_dist = max(0. , way_pts[count, 0] - 1.0)
speed_ahead_dist = max(0. , way_pts[count, 0] - 2.0)
print(speed_ahead_dist)
speed_ahead = 5/3.6
if n.tags['highway']=='traffic_signals':
Expand Down Expand Up @@ -604,11 +604,11 @@ def next_way(self, heading):
except (KeyError, IndexError):
pass
try:
if ways[0].tags['oneway'] == 'yes':
if ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id:
if (ways[0].tags['oneway'] == 'yes') and (ways[1].tags['oneway'] == 'yes'):
if (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id) and not (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id):
way = Way(ways[0], self.query_results)
return way
elif ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id:
elif (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id) and not (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id):
way = Way(ways[1], self.query_results)
return way
except (KeyError, IndexError):
Expand Down Expand Up @@ -640,11 +640,11 @@ def next_way(self, heading):
except (KeyError, IndexError):
pass
try:
if ways[0].tags['oneway'] == 'yes':
if ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id:
if (ways[0].tags['oneway'] == 'yes') and (ways[1].tags['oneway'] == 'yes'):
if (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id) and not (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id):
way = Way(ways[0], self.query_results)
return way
elif ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id:
elif (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id) and not (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id):
way = Way(ways[1], self.query_results)
return way
except (KeyError, IndexError):
Expand Down
14 changes: 7 additions & 7 deletions selfdrive/thermald.py
Original file line number Diff line number Diff line change
Expand Up @@ -327,13 +327,6 @@ def thermald_thread():
elif usb_power and not usb_power_prev:
params.delete("Offroad_ChargeDisabled")

if msg.thermal.batteryVoltage < 11500:
alert_connectivity_prompt = copy.copy(OFFROAD_ALERTS["Offroad_VoltageLow"])
alert_connectivity_prompt["text"] += str(msg.thermal.batteryVoltage) + " Volts."
params.delete("Offroad_VoltageLow")
params.put("Offroad_VoltageLow", json.dumps(alert_connectivity_prompt))
else:
params.delete("Offroad_VoltageLow")

thermal_status_prev = thermal_status
usb_power_prev = usb_power
Expand All @@ -343,6 +336,13 @@ def thermald_thread():

# report to server once per minute
if (count % int(60. / DT_TRML)) == 0:
if msg.thermal.batteryVoltage < 11500:
alert_connectivity_prompt = copy.copy(OFFROAD_ALERTS["Offroad_VoltageLow"])
alert_connectivity_prompt["text"] += str(msg.thermal.batteryVoltage) + " Volts."
params.delete("Offroad_VoltageLow")
params.put("Offroad_VoltageLow", json.dumps(alert_connectivity_prompt))
else:
params.delete("Offroad_VoltageLow")
cloudlog.event("STATUS_PACKET",
count=count,
health=(health.to_dict() if health else None),
Expand Down
1 change: 1 addition & 0 deletions selfdrive/traffic/__init__.py
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@

121 changes: 121 additions & 0 deletions selfdrive/traffic/traffic.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,121 @@
#!/usr/bin/env python2
import os
import cv2
import numpy as np
import zmq

def detect(source):

font = cv2.FONT_HERSHEY_SIMPLEX
img = cv2.imread(source)

cimg = img
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)


lower_red1 = np.array([0,100,100])
upper_red1 = np.array([10,255,255])
lower_red2 = np.array([160,100,100])
upper_red2 = np.array([180,255,255])
lower_green = np.array([40,50,50])
upper_green = np.array([90,255,255])
lower_yellow = np.array([15,150,150])
upper_yellow = np.array([35,255,255])
mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
maskg = cv2.inRange(hsv, lower_green, upper_green)
masky = cv2.inRange(hsv, lower_yellow, upper_yellow)
maskr = cv2.add(mask1, mask2)

size = img.shape

r_circles = cv2.HoughCircles(maskr, cv2.HOUGH_GRADIENT, 1, 80,
param1=50, param2=10, minRadius=0, maxRadius=30)

g_circles = cv2.HoughCircles(maskg, cv2.HOUGH_GRADIENT, 1, 60,
param1=50, param2=10, minRadius=0, maxRadius=30)

y_circles = cv2.HoughCircles(masky, cv2.HOUGH_GRADIENT, 1, 30,
param1=50, param2=5, minRadius=0, maxRadius=30)

# traffic light detect
r = 5
bound = 4.0 / 10
if r_circles is not None:
r_circles = np.uint16(np.around(r_circles))

for i in r_circles[0, :]:
if i[0] > size[1] or i[1] > size[0]or i[1] > size[0]*bound:
continue

h, s = 0.0, 0.0
for m in range(-r, r):
for n in range(-r, r):

if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]:
continue
h += maskr[i[1]+m, i[0]+n]
s += 1
if h / s > 50:
cv2.circle(cimg, (i[0], i[1]), i[2]+10, (0, 255, 0), 2)
cv2.circle(maskr, (i[0], i[1]), i[2]+30, (255, 255, 255), 2)
#cv2.putText(cimg,'RED',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA)
print("RED")
if g_circles is not None:
g_circles = np.uint16(np.around(g_circles))

for i in g_circles[0, :]:
if i[0] > size[1] or i[1] > size[0] or i[1] > size[0]*bound:
continue

h, s = 0.0, 0.0
for m in range(-r, r):
for n in range(-r, r):

if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]:
continue
h += maskg[i[1]+m, i[0]+n]
s += 1
if h / s > 100:
cv2.circle(cimg, (i[0], i[1]), i[2]+10, (0, 255, 0), 2)
cv2.circle(maskg, (i[0], i[1]), i[2]+30, (255, 255, 255), 2)
#cv2.putText(cimg,'GREEN',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA)
print("GREEN")
if y_circles is not None:
y_circles = np.uint16(np.around(y_circles))

for i in y_circles[0, :]:
if i[0] > size[1] or i[1] > size[0] or i[1] > size[0]*bound:
continue

h, s = 0.0, 0.0
for m in range(-r, r):
for n in range(-r, r):

if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]:
continue
h += masky[i[1]+m, i[0]+n]
s += 1
if h / s > 50:
cv2.circle(cimg, (i[0], i[1]), i[2]+10, (0, 255, 0), 2)
cv2.circle(masky, (i[0], i[1]), i[2]+30, (255, 255, 255), 2)
#cv2.putText(cimg,'YELLOW',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA)
print("YELLOW")


return cimg
if __name__ == '__main__':


while True:
context = zmq.Context()
socket = context.socket(zmq.REP)
socket.bind("tcp://*:9000")
message = socket.recv()
print("Message is of type {}".format(type(message)))
print(message)
detectimg =detect(message)
sleep(0.05)
# This function either gives out "RED" "YELLOW" or "GREEN"


0 comments on commit bec9f51

Please sign in to comment.