mawei/onepilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

amap_navi.py:激光雷达可分别设置前后各个方向变道参数，实线可单独设置变道延迟参数。

Browse Source
This commit is contained in:
openpilot
2026-03-19 21:04:36 +08:00
parent daae207961
commit 91675a6c0d
3 changed files with 83 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/carrot/amap_navi.py
+65 -6
View File
@@ -98,6 +98,8 @@ class SharedData:
#盲区信号
self.left_lane = 0 #车道线类型
self.right_lane = 0
self.left_lane_blind = 0 #车道线阻止变道
self.right_lane_blind = 0
self.left_blind = False #摄像头盲区信号
self.right_blind = False
self.lidar_lblind = False #雷达盲区信号
@@ -236,6 +238,13 @@ class AmapNaviServ:
self.rf_side_object_detected = False
self.rb_side_object_detected = False
#实线消抖检测
self.left_solid_detected_count = 0
self.right_solid_detected_count = 0
self.min_solid_detected_count_thr = int(-2.0 / DT_BROADCAST) # 判断是否无实线的持续时间
self.left_solid_detected = False
self.right_solid_detected = False
self.model_event_type = 0
self.sec_count_down = 0
self.frame = 0
@@ -251,9 +260,9 @@ class AmapNaviServ:
def public_amap_navi(self):
msg = messaging.new_message('amapNavi')
msg.valid = True
msg.amapNavi.leftBlind = ((8 if self.shared_data.left_lane > 0 else 0) + (4 if self.shared_data.lidar_car_lblind else 0) +
msg.amapNavi.leftBlind = ((8 if self.shared_data.left_lane_blind else 0) + (4 if self.shared_data.lidar_car_lblind else 0) +
(2 if self.shared_data.left_blind else 0) + (1 if self.shared_data.lidar_lblind else 0))
msg.amapNavi.rightBlind = ((8 if self.shared_data.right_lane > 0 else 0) + (4 if self.shared_data.lidar_car_rblind else 0) +
msg.amapNavi.rightBlind = ((8 if self.shared_data.right_lane_blind else 0) + (4 if self.shared_data.lidar_car_rblind else 0) +
(2 if self.shared_data.right_blind else 0) + (1 if self.shared_data.lidar_rblind else 0))
msg.amapNavi.leftLine = self.shared_data.left_lane
msg.amapNavi.rightLine = self.shared_data.right_lane
@@ -261,9 +270,9 @@ class AmapNaviServ:
self.pm.send('amapNavi', msg)
def left_blindspot(self):
return self.shared_data.left_blind or self.shared_data.lidar_lblind or self.shared_data.left_lane > 0
return self.shared_data.left_blind or self.shared_data.lidar_lblind or self.shared_data.left_lane_blind
def right_blindspot(self):
return self.shared_data.right_blind or self.shared_data.lidar_rblind or self.shared_data.right_lane > 0
return self.shared_data.right_blind or self.shared_data.lidar_rblind or self.shared_data.right_lane_blind
def _capnp_list_to_list(self, capnp_list, max_items=None):
"""将capnp列表转换为Python列表"""
@@ -289,12 +298,53 @@ class AmapNaviServ:
self.min_behind_drel_vego_time = self.lidarBehindVDistTime
self.min_behind_vrel_vego_time = self.lidarBehindVRelDistTime
self.min_object_detected_count_thr = int(-1 * self.lidarBsdDelayTime / DT_BROADCAST)
self.min_solid_detected_count_thr = int(-0.1 * self.params.get_int("LaneLineDelayTime") / DT_BROADCAST)
self.disableBlindSpot = self.params.get_bool("DisableBlindSpot")
self.dynamicBlindRange = self.params.get_int("DynamicBlindRange")
self.dynamicBlindDistance = self.params.get_int("DynamicBlindDistance")
#new
self.frame += 1
#实线处理
def solid_line_blind(self):
#左侧实线
if self.shared_data.left_lane >= 1: #车道线为实线
self.left_solid_detected_count = 1
else:
self.left_solid_detected_count -= 1
if self.left_solid_detected_count < self.min_object_detected_count:
self.left_solid_detected_count = self.min_object_detected_count
if self.left_solid_detected:
if self.left_solid_detected_count <= self.min_object_detected_count_thr:
self.left_solid_detected = False
print("left_solid_detected False")
elif self.left_solid_detected_count > 0:
if not self.left_solid_detected:
print("left_solid_detected True")
self.left_solid_detected = True
self.shared_data.left_lane_blind = self.left_solid_detected
#右侧实线
if self.shared_data.right_lane >= 1: #车道线为实线
self.right_solid_detected_count = 1
else:
self.right_solid_detected_count -= 1
if self.right_solid_detected_count < self.min_object_detected_count:
self.right_solid_detected_count = self.min_object_detected_count
if self.right_solid_detected:
if self.right_solid_detected_count <= self.min_object_detected_count_thr:
self.right_solid_detected = False
print("right_solid_detected False")
elif self.right_solid_detected_count > 0:
if not self.right_solid_detected:
print("right_solid_detected True")
self.right_solid_detected = True
self.shared_data.right_lane_blind = self.right_solid_detected
# 动态盲区处理
def lidar_object_blind(self):
lf_blind_mask = False
@@ -494,6 +544,7 @@ class AmapNaviServ:
self.sm.update(0)
self.update_param() # 更新参数
self.lidar_object_blind()
self.solid_line_blind()
#拷贝客户端列表
with lock:
@@ -1092,11 +1143,17 @@ class AmapNaviServ:
if drel_mm > 0:
# 前方目标：风险来自我追它，所以 closing = max(v_ego - v_other, 0)
closing_speed = max(v_ego_mps - v_other, 0.0)
danger_dist = max(v_ego_mps * min_drel_scale, 10)
if min_drel_scale >= 0:
danger_dist = max(v_ego_mps * min_drel_scale, 0)
else:
danger_dist = abs(min_drel_scale)
else:
# 后方目标：风险来自它追我，所以 closing = max(v_other - v_ego, 0)
closing_speed = max(v_other - v_ego_mps, 0.0)
danger_dist = max(v_ego_mps * min_drel_scale, 15)
if min_drel_scale >= 0:
danger_dist = max(v_ego_mps * min_drel_scale, 0)
else:
danger_dist = abs(min_drel_scale)
# 未来距离预测
future_dist = drel - closing_speed * time_horizon #* 3
@@ -1111,6 +1168,7 @@ class AmapNaviServ:
return risk
'''
def is_side_object_risky_debug(self,
drel_mm,
vrel_mps,
@@ -1180,6 +1238,7 @@ class AmapNaviServ:
print("==============================")
return risk
'''
def camera_data_timeout(self, ip, info):
now = time.time()
selfdrive/carrot/config.py
+3 -2
View File
@@ -159,9 +159,10 @@ class UnifiedParams:
"LidarBsdDelayTime": 10,
"LidarFrontVDistTime": 10,
"LidarFrontvRelDistTime": 30,
"LidarFrontVRelDistTime": 30,
"LidarBehindVDistTime": 10,
"LidarBehindvRelDistTime": 30,
"LidarBehindVRelDistTime": 30,
"LaneLineDelayTime": 10,
"AutoTurnInNotRoadEdge": 1,
"ContinuousLaneChange": 1,
selfdrive/carrot/nav_params.html
+15 -6
View File
@@ -617,13 +617,13 @@
"LidarFrontVDistTime": {
chineseName: "激光雷达侧前方有车允许变道绝对速度时距(x0.1s)",
defaultValue: 10,
description: "当与激光雷达侧前方车辆的距离大于'本车速度x时间'时允许变道，单位0.1秒",
inputRange: [0, 50],
description: "当与激光雷达侧前方车辆的距离大于'本车速度x时间'时允许变道，单位0.1秒;如果设置为负值，表示绝对距离(单位为0.1m)，如-100，表示绝对距离大于10米时允许变道",
inputRange: [-500, 50],
step: 1,
uiType: "number",
category: "公共参数设置"
},
"LidarFrontvRelDistTime": {
"LidarFrontVRelDistTime": {
chineseName: "激光雷达侧前方有车允许变道相对速度时距(x0.1s)",
defaultValue: 30,
description: "当'本车相对于侧前方车辆的速度差x时间'小于两车距离时，允许变道，单位0.1秒",
@@ -635,13 +635,13 @@
"LidarBehindVDistTime": {
chineseName: "激光雷达侧后方有车允许变道的绝对速度时距(x0.1s)",
defaultValue: 10,
description: "当与激光雷达侧后方车辆的距离大于'对方速度x时间'时允许变道，单位0.1秒",
inputRange: [0, 50],
description: "当与激光雷达侧后方车辆的距离大于'对方速度x时间'时允许变道，单位0.1秒;如果设置为负值，表示绝对距离(单位为0.1m)，如-100，表示绝对距离大于10米时允许变道",
inputRange: [-500, 50],
step: 1,
uiType: "number",
category: "公共参数设置"
},
"LidarBehindvRelDistTime": {
"LidarBehindVRelDistTime": {
chineseName: "激光雷达侧后方有车允许变道的相对速度时距(x0.1s)",
defaultValue: 30,
description: "当'侧后方车辆的相对于本车的速度差x时间'小于两车距离时，允许变道，单位0.1秒",
@@ -650,6 +650,15 @@
uiType: "number",
category: "公共参数设置"
},
"LaneLineDelayTime": {
chineseName: "实线阻止变道延时(x0.1s)",
defaultValue: 20,
description: "当实线信号消失后，经过延时的秒数后允许变道",
inputRange: [0, 100],
step: 1,
uiType: "number",
category: "公共参数设置"
},
"DynamicExperimentalSpeed": {
chineseName: "实验模式:条件实验模式速度(-5)",
defaultValue: -5,