diff --git a/selfdrive/carrot/amap_navi.py b/selfdrive/carrot/amap_navi.py index d9439b33..e417f690 100644 --- a/selfdrive/carrot/amap_navi.py +++ b/selfdrive/carrot/amap_navi.py @@ -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() diff --git a/selfdrive/carrot/config.py b/selfdrive/carrot/config.py index abcad250..89d948bc 100644 --- a/selfdrive/carrot/config.py +++ b/selfdrive/carrot/config.py @@ -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, diff --git a/selfdrive/carrot/nav_params.html b/selfdrive/carrot/nav_params.html index d3b177bb..7d89af52 100644 --- a/selfdrive/carrot/nav_params.html +++ b/selfdrive/carrot/nav_params.html @@ -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,