diff --git a/selfdrive/carrot/amap_navi.py b/selfdrive/carrot/amap_navi.py index 5136afe3..d9439b33 100644 --- a/selfdrive/carrot/amap_navi.py +++ b/selfdrive/carrot/amap_navi.py @@ -206,11 +206,15 @@ class AmapNaviServ: self.rightFrontTarget = RadarSpeedEstimator() self.rightBehindTarget = RadarSpeedEstimator() - self.min_drel_vego_time = 1.0 - self.min_vrel_vego_time = 1.0 - self.sideBsdDelayTime = 2. - self.sideRelDistTime = 1. - self.sidevRelDistTime = 1. + self.min_front_drel_vego_time = 3.0 + self.min_front_vrel_vego_time = 3.0 + self.min_behind_drel_vego_time = 3.0 + self.min_behind_vrel_vego_time = 3.0 + self.lidarBsdDelayTime = 1. + self.lidarFrontVDistTime = 3. + self.lidarFrontVRelDistTime = 3. + self.lidarBehindVDistTime = 3. + self.lidarBehindVRelDistTime = 3. self.disableBlindSpot = False self.dynamicBlindRange = 0 self.dynamicBlindDistance = 0 @@ -275,12 +279,16 @@ class AmapNaviServ: def update_param(self): if self.frame % 100 == 0: - self.sideBsdDelayTime = self.params.get_int("SideBsdDelayTime") * 0.1 - self.sideRelDistTime = self.params.get_int("SideRelDistTime") * 0.1 - self.sidevRelDistTime = self.params.get_int("SidevRelDistTime") * 0.1 - self.min_drel_vego_time = self.sideRelDistTime - self.min_vrel_vego_time = self.sidevRelDistTime - self.min_object_detected_count_thr = int(-1 * self.sideBsdDelayTime / DT_BROADCAST) + self.lidarBsdDelayTime = self.params.get_int("LidarBsdDelayTime") * 0.1 + self.lidarFrontVDistTime = self.params.get_int("LidarFrontVDistTime") * 0.1 + self.lidarFrontVRelDistTime = self.params.get_int("LidarFrontVRelDistTime") * 0.1 + self.lidarBehindVDistTime = self.params.get_int("LidarBehindVDistTime") * 0.1 + self.lidarBehindVRelDistTime = self.params.get_int("LidarBehindVRelDistTime") * 0.1 + self.min_front_drel_vego_time = self.lidarFrontVDistTime + self.min_front_vrel_vego_time = self.lidarFrontVRelDistTime + 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.disableBlindSpot = self.params.get_bool("DisableBlindSpot") self.dynamicBlindRange = self.params.get_int("DynamicBlindRange") self.dynamicBlindDistance = self.params.get_int("DynamicBlindDistance") @@ -896,9 +904,9 @@ class AmapNaviServ: self.shared_data.lb_vrel = self.leftFrontTarget.update(lb_dreltmp, dist_timems) #动态时距盲区判断 self.lf_object_detected = self.is_side_object_risky(lf_dreltmp, self.shared_data.lf_vrel, self.shared_data.v_ego_m, - self.min_vrel_vego_time, self.min_drel_vego_time) + self.min_front_vrel_vego_time, self.min_front_drel_vego_time) self.lb_object_detected = self.is_side_object_risky(lb_dreltmp, self.shared_data.lb_vrel, self.shared_data.v_ego_m, - self.min_vrel_vego_time, self.min_drel_vego_time) + self.min_behind_vrel_vego_time, self.min_behind_drel_vego_time) if detect_side & 2: # 右前方 if rf_drel is None: rf_drel = old_info.get("rf_drel", None) # 距离数据消抖 @@ -924,13 +932,13 @@ class AmapNaviServ: self.shared_data.rb_vrel = self.rightBehindTarget.update(rb_dreltmp, dist_timems) #动态时距盲区判断 self.rf_object_detected = self.is_side_object_risky(rf_dreltmp, self.shared_data.rf_vrel, self.shared_data.v_ego_m, - self.min_vrel_vego_time, self.min_drel_vego_time) + self.min_front_vrel_vego_time, self.min_front_drel_vego_time) self.rb_object_detected = self.is_side_object_risky(rb_dreltmp, self.shared_data.rb_vrel, self.shared_data.v_ego_m, - self.min_vrel_vego_time, self.min_drel_vego_time) + self.min_behind_vrel_vego_time, self.min_behind_drel_vego_time) #self.rb_object_detected = self.is_side_object_risky_debug(rb_drel, self.shared_data.rb_vrel, # self.shared_data.v_ego_m, - # self.min_vrel_vego_time, - # self.min_drel_vego_time, "RB") + # self.min_front_vrel_vego_time, + # self.min_front_drel_vego_time, "RB") # 通讯时间检查 now = time.time() @@ -1091,7 +1099,7 @@ class AmapNaviServ: danger_dist = max(v_ego_mps * min_drel_scale, 15) # 未来距离预测 - future_dist = drel - closing_speed * time_horizon * 3 + future_dist = drel - closing_speed * time_horizon #* 3 # 判定规则： # 1) 未来距离过小（可调阈值 3~5m，我设成 4m) diff --git a/selfdrive/carrot/config.py b/selfdrive/carrot/config.py index 9880c3f4..abcad250 100644 --- a/selfdrive/carrot/config.py +++ b/selfdrive/carrot/config.py @@ -156,6 +156,13 @@ class UnifiedParams: "SideRelDistTime": 10, "SidevRelDistTime": 10, "SideRadarMinDist": 0, + + "LidarBsdDelayTime": 10, + "LidarFrontVDistTime": 10, + "LidarFrontvRelDistTime": 30, + "LidarBehindVDistTime": 10, + "LidarBehindvRelDistTime": 30, + "AutoTurnInNotRoadEdge": 1, "ContinuousLaneChange": 1, "ContinuousLaneChangeCnt": 4, diff --git a/selfdrive/carrot/nav_params.html b/selfdrive/carrot/nav_params.html index b572cfd8..d3b177bb 100644 --- a/selfdrive/carrot/nav_params.html +++ b/selfdrive/carrot/nav_params.html @@ -561,34 +561,34 @@ category: "公共参数设置" }, "BsdDelayTime": { - chineseName: "后盲区有车延时(20x0.1s)", + chineseName: "原车雷达后盲区有车延时(x0.1s)", defaultValue: 20, - description: "当后盲区有车信号消失后，经过延时的秒数后允许变道", + description: "当原车雷达后盲区有车信号消失后，经过延时的秒数后允许变道", inputRange: [0, 100], step: 1, uiType: "number", category: "公共参数设置" }, "SideBsdDelayTime": { - chineseName: "侧前方有车延时(20x0.1s)", + chineseName: "原车雷达侧前方有车延时(x0.1s)", defaultValue: 20, - description: "当侧前方有车信号消失后，经过延时的秒数后允许变道", + description: "当原车雷达侧前方有车信号消失后，经过延时的秒数后允许变道", inputRange: [0, 100], step: 1, uiType: "number", category: "公共参数设置" }, "SideRelDistTime": { - chineseName: "侧前方有车变道相对距离", + chineseName: "原车雷达侧前方有车变道绝对速度时距(x0.1s)", defaultValue: 10, - description: "当与侧前方车辆相对距离小于本车速度x时间时不允许变道，单位0.1秒", + description: "当与原车雷达侧前方车辆的距离大于'本车速度x时间'时允许变道，单位0.1秒", inputRange: [0, 50], step: 1, uiType: "number", category: "公共参数设置" }, "SidevRelDistTime": { - chineseName: "侧前方有车变道等效距离", + chineseName: "原车雷达侧前方有车允许变道相对速度时距(x0.1s)", defaultValue: 10, description: "侧前方车辆速度x3+相对距离小于本车速度x(时间+3)时，不允许变道，单位0.1秒", inputRange: [0, 50], @@ -597,14 +597,59 @@ category: "公共参数设置" }, "SideRadarMinDist": { - chineseName: "侧面最小雷达距离(0m)", + chineseName: "原车雷达侧前方最小距离过滤(0m)", defaultValue: 0, - description: "在左右两侧的车道上，忽略小于此雷达探测距离的车辆，单位为0.1m", + description: "在原车雷达探测左右车道侧前方车辆时，小于设定距离的目标会被忽略，单位为0.1m", inputRange: [-50, 100], step: 1, uiType: "number", category: "公共参数设置" }, + "LidarBsdDelayTime": { + chineseName: "激光雷达侧面车道有车延时(x0.1s)", + defaultValue: 20, + description: "当激光雷达探测侧面车道有车信号消失后，经过延时的秒数后允许变道", + inputRange: [0, 100], + step: 1, + uiType: "number", + category: "公共参数设置" + }, + "LidarFrontVDistTime": { + chineseName: "激光雷达侧前方有车允许变道绝对速度时距(x0.1s)", + defaultValue: 10, + description: "当与激光雷达侧前方车辆的距离大于'本车速度x时间'时允许变道，单位0.1秒", + inputRange: [0, 50], + step: 1, + uiType: "number", + category: "公共参数设置" + }, + "LidarFrontvRelDistTime": { + chineseName: "激光雷达侧前方有车允许变道相对速度时距(x0.1s)", + defaultValue: 30, + description: "当'本车相对于侧前方车辆的速度差x时间'小于两车距离时，允许变道，单位0.1秒", + inputRange: [0, 200], + step: 1, + uiType: "number", + category: "公共参数设置" + }, + "LidarBehindVDistTime": { + chineseName: "激光雷达侧后方有车允许变道的绝对速度时距(x0.1s)", + defaultValue: 10, + description: "当与激光雷达侧后方车辆的距离大于'对方速度x时间'时允许变道，单位0.1秒", + inputRange: [0, 50], + step: 1, + uiType: "number", + category: "公共参数设置" + }, + "LidarBehindvRelDistTime": { + chineseName: "激光雷达侧后方有车允许变道的相对速度时距(x0.1s)", + defaultValue: 30, + description: "当'侧后方车辆的相对于本车的速度差x时间'小于两车距离时，允许变道，单位0.1秒", + inputRange: [0, 200], + step: 1, + uiType: "number", + category: "公共参数设置" + }, "DynamicExperimentalSpeed": { chineseName: "实验模式:条件实验模式速度(-5)", defaultValue: -5,