ParkingRobot/CommonUsage-MultiVehicleSync/commonusage/Chassis/AbstractChassis.cs

using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Numerics;
using System.Security.Cryptography.X509Certificates;
using System.Text;
using FundamentalLib;
using Newtonsoft.Json;

namespace CommonUsage.Chassis
{
    public abstract class AbstractChassis
    {
	    protected AbstractChassis()
	    {
		    Valid = false;
	    }

	    public abstract void Initialize();

        public abstract void Visualize();

	    public abstract void AfterDirectionChanged();

        /// <summary>
        /// 当前行进方向。
        /// </summary>
        [Obsolete]
        public float DirectionAngle
        {
	        get => _originBiasTh;
	        set
	        {
                _originBiasTh = value;
				if (_originBiasTh != _lastDirectionAngle) AfterDirectionChanged();
                _lastDirectionAngle = _originBiasTh;
	        }
        }

        protected float _originBiasX = 0f, _originBiasY = 0f, _originBiasTh;

        public Vector3 GetOriginBias()
        {
            return new Vector3(_originBiasX, _originBiasY, _originBiasTh);
        }

        public class CarSpeed
        {
            public float Vx,Vy,Vw;
        }

        public abstract CarSpeed GetCarSpeed(bool isActual = false);
        public List<GeometricControlPoint> GetGeometricControlPoints()
        {
	        return GeometricControlPoints;
        }

        public void ComputeWheelsGeometrically(float speed)
        {
            // 打印调用位置信息
            var stackTrace = new StackTrace(true);
            var callerFrame = stackTrace.GetFrame(1); // 获取调用者的帧
            if (callerFrame != null)
            {
                var fileName = callerFrame.GetFileName();
                var lineNumber = callerFrame.GetFileLineNumber();
                DLog.Log($"s:{speed:0.000} from {fileName} ln.{lineNumber}", $"WheelComputeCaller");
            }

            DefineGeometricWheelComputation(speed);
        }

        protected abstract void DefineGeometricWheelComputation(float speed);

        public void DriveStop()
        {
            PredefinedDriveStop();
            CustomDriveStop?.Invoke();
        }

        public abstract void PredefinedDriveStop();

        public Action CustomDriveStop;

        public abstract bool ComputeRotateWheels(float rotSpeed);

        public abstract float CalculateTurningSpeedDecayFac(float turn);

        public enum ChassisState
        {
	        Standby,
            Running,
            AbnormalFeedback,
            ExceedMotionAbility,
        }

        protected ChassisState State;

        protected string StateDescription;

        public (ChassisState State, string Description) GetChassisState()
        {
	        return (State, StateDescription);
        }

        public bool Debug = false;
        public float AccPerSecond = 0.2f;
        public float DeAccPerSecond = 0.2f;
        public float MaxSpeed = 1; // m/s
        public float MinTurnSpeedFac = 0.5f;
        public float MaxTurnThreshold = 90f;

        public float GcpThetaPerSecond = 10f;

        public DateTime LastMoveTime = DateTime.MinValue;

		protected bool Valid = false;
		protected List<GeometricControlPoint> GeometricControlPoints = new();
		protected bool RotatingActive = false;
		protected bool GoingActive = false;
		protected bool GoingWheelAligned = false;

		private float _lastDirectionAngle = 0;
    }
}