diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs index cd27e9e..b5d93db 100644 --- a/MultiWheel/MultiWheelC/PilotConfig.cs +++ b/MultiWheel/MultiWheelC/PilotConfig.cs @@ -184,6 +184,9 @@ public class PilotConfig : MultiWheelPilotConfig [FieldMember(desc = "轮胎跟踪:离车时切换至盲走距离")] public float TireFollowingLeaveCarWalkBlindSwitchingDistance = 1200f; [FieldMember(desc = "轮胎跟踪:测试钻轮胎数量")] public int TireFollowingTireNum = 1; + [FieldMember(desc = "轮胎跟踪:过近距离")] public float TireFollowingCloseDistance = 1400; [FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f; + [FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5; + #endregion } diff --git a/MultiWheel/MultiWheelC/TireFollowing.cs b/MultiWheel/MultiWheelC/TireFollowing.cs index 12302a1..4998ef7 100644 --- a/MultiWheel/MultiWheelC/TireFollowing.cs +++ b/MultiWheel/MultiWheelC/TireFollowing.cs @@ -122,6 +122,7 @@ namespace MultiWheelC private Painter _painter; private List _remainDistanceList = new List(); private List _remainAngleList = new List(); + private List _targetYList = new List(); private List SetFilters(float guessCenterX, float guessCenterY) { @@ -350,7 +351,7 @@ namespace MultiWheelC var targetPos = new Vector2((target.Src.X + target.Dst.X) / 2f, (target.Src.Y + target.Dst.Y) / 2f); var dis2target = (float)Math.Sqrt(Math.Pow(targetPos.X, 2) + Math.Pow(targetPos.Y, 2)); //距离较近以后角度容易跳变 - if (dis2target < 1400 && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr) + if (dis2target < PilotDefinition.Conf.TireFollowingCloseDistance && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr) { yield return true; continue; @@ -375,9 +376,22 @@ namespace MultiWheelC { lastDetectX = ((target.Src + target.Dst) / 2f).X; lastDetectY = ((target.Src + target.Dst) / 2f).Y; - _painter.DrawLine(Color.GreenYellow, path.CarPath.Src, path.CarPath.Dst, endArrow: true); - var rd = (float)LessMath.PerpendicularPosition(0, 0, path.CarPath.Dst.X, path.CarPath.Dst.Y, + var currentY = path.CarPath.Dst.Y; + if (Math.Abs(targetAngle) < PilotDefinition.Conf.TireFollowingAngleIgnoreThr && + dis2target < PilotDefinition.Conf.TireFollowingCloseDistance) + { + _targetYList.Add(currentY); + while (_targetYList.Count > PilotDefinition.Conf.TireFollowingYAverageFrameCount) _targetYList.RemoveAt(0); + } + + var trackDstY = _targetYList.Count > 0 ? _targetYList.Average() : currentY; + Hedingben.ToastText($"target Y:{_targetYList.Count} {trackDstY}", "target Y"); + + var trackDst = new Vector2(path.CarPath.Dst.X, trackDstY); + _painter.DrawLine(Color.GreenYellow, path.CarPath.Src, trackDst, endArrow: true); + + var rd = (float)LessMath.PerpendicularPosition(0, 0, trackDst.X, trackDst.Y, path.CarPath.Src.X, path.CarPath.Src.Y); _remainDistanceList.Add(rd); while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0); @@ -395,8 +409,8 @@ namespace MultiWheelC detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId); DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing"); } - WalkBlindCarPathDstX = path.CarPath.Dst.X; - WalkBlindCarPathDstY = path.CarPath.Dst.Y; + WalkBlindCarPathDstX = trackDst.X; + WalkBlindCarPathDstY = trackDst.Y; WalkBlindCarPathDstTh = angle2target + WalkBlindTh; DLog.Log($"切换至第一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " + $"{WalkBlindCarPathDstY:F2} " + @@ -410,6 +424,7 @@ namespace MultiWheelC _lastRRLEncoder = PilotDefinition.Self.RRLActualPos; _lastRRREncoder = PilotDefinition.Self.RRRActualPos; _remainDistanceList.Clear(); + _targetYList.Clear(); lastDetectX = detectors[detectorIndex + 1].StartGuessingX; lastDetectY = detectors[detectorIndex + 1].StartGuessingY; continue; @@ -420,8 +435,8 @@ namespace MultiWheelC if (detectors[detectorIndex].SwitchWalkBlindCondition(rd)) { WalkBlindStage2 = true; - WalkBlindCarPathDstX = path.CarPath.Dst.X; - WalkBlindCarPathDstY = path.CarPath.Dst.Y; + WalkBlindCarPathDstX = trackDst.X; + WalkBlindCarPathDstY = trackDst.Y; WalkBlindCarPathDstTh = angle2target + WalkBlindTh; DLog.Log($"切换至最后一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " + $"{WalkBlindCarPathDstY:F2} " + @@ -444,11 +459,12 @@ namespace MultiWheelC _lastRRLEncoder = PilotDefinition.Self.RRLActualPos; _lastRRREncoder = PilotDefinition.Self.RRRActualPos; _remainDistanceList.Clear(); + _targetYList.Clear(); continue; } } DLog.Log($"投影点剩余距离:{rd:F2}", "TireFollowing"); - var track = new LineTrack(path.CarPath.Src, path.CarPath.Dst); + var track = new LineTrack(path.CarPath.Src, trackDst); track.CarDirectionBias = CarDirection; controller.UpdateTracks(new List { track }); NoTarget = false;