update

2026-07-21 11:11:01 +08:00
commit a420e9ae74
124 changed files with 16517 additions and 0 deletions
@@ -0,0 +1,511 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Linq;
+using System.Numerics;
+using System.Reflection;
+using System.Text;
+using System.Threading;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using ClumsyCore.Utilities;
+using ClumsyDance.ClumsyWalk.Detectors;
+using CommonUsage.Chassis;
+using FundamentalLib;
+using MDCSToolBox;
+using MDCSToolBox.Clumsy.Calibration;
+using MDCSToolBox.Clumsy.MotionControllers;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+using MDCSToolBox.Clumsy.Tracks;
+using MDCSToolBox.Commons.Controllers;
+using static ClumsyCore.DTools.Painter;
+using LineSegment = ClumsyCore.Utilities.LineSegment;
+
+namespace MultiWheelC
+{
+    public class TireFollowing : MovementDefinition
+    {
+        public Func<AbstractGeometricController> GetController;
+
+        /// <summary>
+        /// 车辆方向
+        /// </summary>
+        public float CarDirection = 0;
+
+        /// <summary>
+        /// 停止距离
+        /// </summary>
+        //public float FinishDistance = 1000;
+
+        /// <summary>
+        /// 减速距离
+        /// </summary>
+        public float SlowDistance = 1000;
+
+        /// <summary>
+        /// 最大速度
+        /// </summary>
+        public float MaxSpeed = 0.3f;
+
+        // 末段衔接：接近盲走终点时给非零速度，供后续动作连续接管
+        public bool EnableHandover = false;
+        public float HandoverDistance = 200f; // mm
+        public float HandoverSpeed = 0.2f;    // m/s
+
+        /// <summary>
+        /// 钻轮胎数量
+        /// </summary>
+        public int TireNum = 1;
+
+        /// <summary>
+        /// 盲走角度偏移
+        /// </summary>
+        public float WalkBlindTh = -1f;
+
+        /// <summary>
+        /// 是否检测到目标
+        /// </summary>
+        public bool NoTarget = false;
+
+        public float GuessRangeX;
+
+        public float GuessRangeY;
+
+        /// <summary>
+        /// 检测器定义
+        /// </summary>
+        public class DetectorDefinition
+        {
+            /// <summary>
+            /// 开始检测距离
+            /// </summary>
+            public float StartGuessingX;
+
+            /// <summary>
+            /// 开始检测距离
+            /// </summary>
+            public float StartGuessingY;
+
+            /// <summary>
+            /// 检测函数
+            /// </summary>
+            public Func<float, float, List<DetectFilter>, LineSegment> DetectFunction = null;
+
+            public Action<int> LeaveSrcFunction = null;
+
+            public int SrcId = -1;
+            public int DstId = -1;
+
+            /// <summary>
+            /// 路径偏移
+            /// </summary>
+            public Tuple<float, float, float> PathTransformation = Tuple.Create(0f, 0f, 0f);
+
+            public float PathTransformationAnchorDistance = 0f;
+
+            /// <summary>
+            /// 切换条件
+            /// </summary>
+            public Func<float, bool> SwitchWalkBlindCondition = null;
+
+            /// <summary>
+            /// 盲走停止距离
+            /// </summary>
+            public Func<float, bool> FinishWalkBlindCondition = null;
+        }
+
+        public Func<bool> FinishCondition;
+
+        /// <summary>
+        /// 多个检测器列表
+        /// </summary>
+        public List<DetectorDefinition> detectors = null;
+
+        private Painter _painter;
+        private List<float> _remainDistanceList = new List<float>();
+        private List<float> _remainAngleList = new List<float>();
+        private List<float> _targetYList = new List<float>();
+
+        private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
+        {
+            var painter = UI.GetPainter("GeneralFollowing.SetFilters", false);
+            painter.Clear();
+            painter.Clear(3000);
+
+            var box = new Vector2[]
+            {
+                new (guessCenterX - GuessRangeX, guessCenterY - GuessRangeY),
+                new (guessCenterX + GuessRangeX, guessCenterY - GuessRangeY),
+                new (guessCenterX + GuessRangeX, guessCenterY + GuessRangeY),
+                new (guessCenterX - GuessRangeX, guessCenterY + GuessRangeY),
+            };
+
+            for (var i = 0; i < box.Length; ++i)
+                painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
+
+            // PC filter in car coordinate frame
+            return new List<DetectFilter>()
+            {
+                new(CoordinateSystem.Car2D,
+                    p => LessMath.IsPointInPolygon4(
+                        box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
+            };
+        }
+
+        public void Stop()
+        {
+            _dt?.Stop();
+        }
+
+        /// <summary>
+        ///计算车体中心的位移和角度增量
+        /// </summary>
+        /// <param name="a">a轮在车体坐标系下位置</param>
+        /// <param name="va">a轮在车体坐标系下位移增量</param>
+        /// <param name="b">b轮在车体坐标系下位置</param>
+        /// <param name="vb">b轮在车体坐标系下位移增量</param>
+        /// <returns></returns>
+        private static (float, float, float) CenterMoveFromPoints(Vector2 a,
+            Vector2 aDelta,
+            Vector2 b,
+            Vector2 bDelta)
+        {
+            float th_x = 0, th_y = 0, th = 0, x = 0, y = 0;
+            var eps = 0.0000001;
+            if (Math.Abs(a.Y - b.Y) > eps)
+            {
+                th_x = (aDelta.X - bDelta.X) / (b.Y - a.Y);
+            }
+            if (Math.Abs(a.X - b.X) > eps)
+            {
+                th_y = (aDelta.Y - bDelta.Y) / (a.X - b.X);
+            }
+            th = th_x == 0 ? th_y : th_x;
+
+            x = (aDelta.X + bDelta.X) / 2f - (a.Y - b.Y) / 2f * th;
+
+            y = (aDelta.Y + bDelta.Y) / 2f + (a.X - b.X) / 2f * th;
+
+            return (x, y, th);
+        }
+
+
+        public override IEnumerable<bool> Get()
+        {
+            _painter = UI.GetPainter("GeneralFollowing", false);
+            var lastDetectX = detectors[0].StartGuessingX;
+            var lastDetectY = detectors[0].StartGuessingY;
+            var detectorIndex = 0;
+
+            var controller = (MultiWheelGeometricController)GetController.Invoke();
+            controller.BaseSpeed = MaxSpeed;
+            controller.FinishDistance = float.MinValue;
+            controller.FirstThAccuracy = 999;
+            _dt = new DriveTask(controller.Track(true, CoordinateSystem.Car2D));
+            void HardStop()
+            {
+                _dt?.Stop();
+                ((MultiWheelChassis)PilotDefinition.Chassis).DriveStop();
+                DLog.Log($"Hard Stop!", "TireFollowing");
+            }
+            float WalkBlindCarPathDstX = -1f, WalkBlindCarPathDstY = -1f, WalkBlindCarPathDstTh = -1f;
+            bool WalkBlindStage1 = false, WalkBlindStage2 = false;
+            var angle2target = -1f;
+            float _lastLFLEncoder = -1, _lastLFREncoder = -1, _lastRFLEncoder = -1, _lastRFREncoder = -1;
+            float _lastLRLEncoder = -1, _lastLRREncoder = -1, _lastRRLEncoder = -1, _lastRRREncoder = -1;
+
+            (float, float, float) GetCurrentPos2Dst(float lastX, float lastY, float lastTh)
+            {
+                // Read current encoders
+                var curLFLEncoder = PilotDefinition.Self.LFLActualPos;
+                var curLFREncoder = PilotDefinition.Self.LFRActualPos;
+                var curRFLEncoder = PilotDefinition.Self.RFLActualPos;
+                var curRFREncoder = PilotDefinition.Self.RFRActualPos;
+                var curLRLEncoder = PilotDefinition.Self.LRLActualPos;
+                var curLRREncoder = PilotDefinition.Self.LRRActualPos;
+                var curRRLEncoder = PilotDefinition.Self.RRLActualPos;
+                var curRRREncoder = PilotDefinition.Self.RRRActualPos;
+
+                // Average delta per wheel pair (LF, LR, RF, RR)
+                var lfDelta = (curLFLEncoder - _lastLFLEncoder + curLFREncoder - _lastLFREncoder) / 2f;
+                var lrDelta = (curLRLEncoder - _lastLRLEncoder + curLRREncoder - _lastLRREncoder) / 2f;
+                var rfDelta = (curRFLEncoder - _lastRFLEncoder + curRFREncoder - _lastRFREncoder) / 2f;
+                var rrDelta = (curRRLEncoder - _lastRRLEncoder + curRRREncoder - _lastRRREncoder) / 2f;
+                var deltaList = new List<float> { lfDelta, lrDelta, rfDelta, rrDelta };
+
+                var xs = new List<float>();
+                var ys = new List<float>();
+                var ths = new List<float>();
+                var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
+                var steerWheels = chassis.GetSteerWheels();
+                for (var i = 0; i < steerWheels.Count; ++i)
+                {
+                    var sw1 = steerWheels[i];
+                    var a = sw1.Position;
+                    var tha = sw1.ReadAngle() / 180f * (float)Math.PI;
+                    var deltaa = deltaList[i];
+                    var va = new Vector2(deltaa * (float)Math.Cos(tha), deltaa * (float)Math.Sin(tha));
+                    for (var j = i + 1; j < steerWheels.Count; ++j)
+                    {
+                        var sw2 = steerWheels[j];
+                        var b = sw2.Position;
+                        var thb = sw2.ReadAngle() / 180f * (float)Math.PI;
+                        var deltab = deltaList[j];
+                        var vb = new Vector2(deltab * (float)Math.Cos(thb), deltab * (float)Math.Sin(thb));
+                        var (tempx, tempy, tempth) = CenterMoveFromPoints(a, va, b, vb);
+                        Hedingben.ToastText($"{tempx:f2} {tempy:f2} {tempth / Math.PI * 180f:f2} ", $"{i}_{j}");
+                        xs.Add(tempx);
+                        ys.Add(tempy);
+                        ths.Add(tempth);
+                    }
+                }
+                var x = xs.Average();
+                var y = ys.Average();
+                var Th = ths.Average() / (float)Math.PI * 180;
+                var moveTup = Tuple.Create(x, y, Th);
+                var moved = MathTools.SolveTransform2D(MathTools.SolveTransform2D(Tuple.Create(lastX, lastY, lastTh), moveTup), Tuple.Create(0f, 0f, 0f));
+
+                _lastLFLEncoder = curLFLEncoder;
+                _lastLFREncoder = curLFREncoder;
+                _lastRFLEncoder = curRFLEncoder;
+                _lastRFREncoder = curRFREncoder;
+                _lastLRLEncoder = curLRLEncoder;
+                _lastLRREncoder = curLRREncoder;
+                _lastRRLEncoder = curRRLEncoder;
+                _lastRRREncoder = curRRREncoder;
+                return (moved.Item1, moved.Item2, moved.Item3);
+            }
+
+            while (true)
+            {
+                if (detectorIndex > detectors.Count - 1)
+                    throw new Exception("detector index out of range!");
+
+                _painter.Clear();
+                if (WalkBlindStage1 || WalkBlindStage2)
+                {
+                    //第二次盲走时或只钻一个轮胎时
+                    if (WalkBlindStage2 || detectors.Count == 1 || TireNum == 1)
+                    {
+                        //controller.FinishDistance = 10f;
+                        controller.SlowDistance = SlowDistance;
+                        controller.SlowingPow = 0.7f;
+                    }
+                    if (EnableHandover)
+                    {
+                        controller.SlowDistance = float.MinValue;
+                        controller.FinishSpeed = 0.2f;
+                        controller.FinishDistance = 50;
+                    }
+                    (WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh) = GetCurrentPos2Dst(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
+                    var walkBlindPathEnd = Tuple.Create(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
+                    var walkBlindPathStart = LessMath.Transform2D(walkBlindPathEnd, Tuple.Create(CarDirection == 0 ? -3000f : 3000f, 0f, 0f));
+                    var walkBlindPathDst = new Vector2(WalkBlindCarPathDstX, WalkBlindCarPathDstY);
+                    var walkBlindPathSrc = new Vector2(walkBlindPathStart.Item1, walkBlindPathStart.Item2);
+                    var walkBlindPath = new LineSegment(walkBlindPathSrc, walkBlindPathDst);
+
+                    DLog.Log($"盲走目标点:{walkBlindPath.Src.X:F2} {walkBlindPath.Src.Y:F2} {walkBlindPath.Dst.X:F2} {walkBlindPath.Dst.Y:F2}", "TireFollowing");
+                    _painter.DrawDot(Color.Purple, walkBlindPathDst, sz: 3);
+                    _painter.DrawLine(Color.GreenYellow, walkBlindPath.Src, walkBlindPath.Dst, endArrow: true, width: 2);
+
+                    var track = new LineTrack(walkBlindPath.Src, walkBlindPath.Dst);
+                    track.CarDirectionBias = CarDirection;
+                    controller.UpdateTracks(new List<AbstractTrack> { track });
+                    var rd = (float)LessMath.PerpendicularPosition(0, 0, walkBlindPath.Dst.X, walkBlindPath.Dst.Y,
+                        walkBlindPath.Src.X, walkBlindPath.Src.Y);
+                    _remainDistanceList.Add(rd);
+                    while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
+                    rd = _remainDistanceList.Average();
+                    DLog.Log($"盲走投影点剩余距离:{rd:0.0} ", "TireFollowing");
+                    // 检查是否达到盲走结束条件
+                    if (detectors[detectorIndex].FinishWalkBlindCondition(rd))
+                    {
+                        if (WalkBlindStage1)
+                        {
+                            DLog.Log("达到第一次盲走停止距离，停下或开始钻第二对轮胎", "TireFollowing");
+                            //if (detectors[detectorIndex].DstId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
+                            //{
+                            //    detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].DstId);
+                            //    DLog.Log($"释放取车点{detectors[detectorIndex].DstId}", "TireFollowing");
+                            //}
+                            WalkBlindStage1 = false;
+                            _remainAngleList.Clear();
+                            _remainDistanceList.Clear();
+                            detectorIndex++;
+                            if ((detectors.Count == 1 || TireNum == 1) && !EnableHandover)
+                            {
+                                HardStop();
+                                yield return false;
+                            }
+                        }
+                        else if (WalkBlindStage2)
+                        {
+                            DLog.Log("达到第二对轮胎处，停止移动", "TireFollowing");
+                            if (!EnableHandover)
+                            {
+                                HardStop();
+                            }
+                            yield return false;
+                        }
+                    }
+                    yield return true;
+                    continue;
+                }
+
+                var target = detectors[detectorIndex].DetectFunction(CarDirection, lastDetectX,
+                        SetFilters(lastDetectX, lastDetectY));
+
+                if (target == null)
+                {
+                    DLog.Log("无目标，等待下一帧", "TireFollowing");
+                    controller.FirstRotateMaxSpeed = 0;
+                    yield return true;
+                    continue;
+                }
+                else controller.FirstRotateMaxSpeed = 5;
+
+                var targetAngle = CalculateAngle2YAxis(target.Src, target.Dst);
+                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 < PilotDefinition.Conf.TireFollowingCloseDistance && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr)
+                {
+                    yield return true;
+                    continue;
+                }
+                else _remainAngleList.Add(targetAngle);
+                while (_remainAngleList.Count > 10) _remainAngleList.RemoveAt(0);
+                angle2target = _remainAngleList.Average();
+                var distanceLabelPos = targetPos / 2f;
+                _painter.DrawLine(Color.Cyan, Vector2.Zero, targetPos, width: 2);
+                _painter.DrawText(Color.Yellow, $"{dis2target:F3}", distanceLabelPos.X, distanceLabelPos.Y);
+
+                var path = DetectorHelper.GetApproachPath(target, CoordinateSystem.Car2D, pathLen: 3000,
+                                                          bias: detectors[detectorIndex].PathTransformation,
+                                                          biasAnchorDistance: detectors[detectorIndex].PathTransformationAnchorDistance);
+
+                if (path == null)
+                {
+                    DLog.Log("no path!", "TireFollowing");
+                    NoTarget = true;
+                }
+                else
+                {
+                    lastDetectX = ((target.Src + target.Dst) / 2f).X;
+                    lastDetectY = ((target.Src + target.Dst) / 2f).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);
+                    rd = _remainDistanceList.Average();
+                    _painter.DrawText(Color.Green, $"{rd:F3}", distanceLabelPos.X, distanceLabelPos.Y - 200);
+                    if(rd < PilotDefinition.Conf.TireFollowingReleaseDistance)
+                    {
+                        if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
+                        {
+                            detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
+                            DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
+                        }
+                    }
+
+                    if (detectorIndex < detectors.Count - 1)
+                    {
+                        controller.SlowDistance = 1;
+                        if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
+                        {
+                            WalkBlindStage1 = true;
+                            //if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
+                            //{
+                            //    detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
+                            //    DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
+                            //}
+                            WalkBlindCarPathDstX = trackDst.X;
+                            WalkBlindCarPathDstY = trackDst.Y;
+                            WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
+                            DLog.Log($"切换至第一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
+                                                                $"{WalkBlindCarPathDstY:F2} " +
+                                                                $"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
+                            _lastLFLEncoder = PilotDefinition.Self.LFLActualPos;
+                            _lastLFREncoder = PilotDefinition.Self.LFRActualPos;
+                            _lastRFLEncoder = PilotDefinition.Self.RFLActualPos;
+                            _lastRFREncoder = PilotDefinition.Self.RFRActualPos;
+                            _lastLRLEncoder = PilotDefinition.Self.LRLActualPos;
+                            _lastLRREncoder = PilotDefinition.Self.LRRActualPos;
+                            _lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
+                            _lastRRREncoder = PilotDefinition.Self.RRRActualPos;
+                            _remainDistanceList.Clear();
+                            _targetYList.Clear();
+                            lastDetectX = detectors[detectorIndex + 1].StartGuessingX;
+                            lastDetectY = detectors[detectorIndex + 1].StartGuessingY;
+                            continue;
+                        }
+                    }
+                    else if (detectorIndex == detectors.Count - 1)
+                    {
+                        if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
+                        {
+                            WalkBlindStage2 = true;
+                            WalkBlindCarPathDstX = trackDst.X;
+                            WalkBlindCarPathDstY = trackDst.Y;
+                            WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
+                            DLog.Log($"切换至最后一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
+                                                                $"{WalkBlindCarPathDstY:F2} " +
+                                                                $"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
+                            if (detectors.Count == 1)
+                            {
+                                if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
+                                {
+                                    detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
+                                    DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
+                                }
+                            }
+
+                            _lastLFLEncoder = PilotDefinition.Self.LFLActualPos;
+                            _lastLFREncoder = PilotDefinition.Self.LFRActualPos;
+                            _lastRFLEncoder = PilotDefinition.Self.RFLActualPos;
+                            _lastRFREncoder = PilotDefinition.Self.RFRActualPos;
+                            _lastLRLEncoder = PilotDefinition.Self.LRLActualPos;
+                            _lastLRREncoder = PilotDefinition.Self.LRRActualPos;
+                            _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, trackDst);
+                    track.CarDirectionBias = CarDirection;
+                    controller.UpdateTracks(new List<AbstractTrack> { track });
+                    NoTarget = false;
+                }
+                yield return true;
+            }
+        }
+
+        private static float CalculateAngle2YAxis(Vector2 point1, Vector2 point2)
+        {
+            return -(float)(Math.Atan((point1.X - point2.X) / (point1.Y - point2.Y)) * 180 / Math.PI);
+        }
+
+        private DriveTask _dt;
+    }
+}