Tutorial/MultiWheel/MultiWheelC/TireFollowing.cs

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.FinishDistance = 20;
                    }
                    (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 (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;
    }
}