RobotNet/RobotNet.RobotManager/Services/Simulation/GridDifferentialNavigationService.cs

using RobotNet.MapShares.Dtos;
using RobotNet.RobotManager.Services.Planner.Space;
using RobotNet.RobotManager.Services.Simulation.Algorithm;
using RobotNet.RobotManager.Services.Simulation.Models;
using RobotNet.RobotShares.Enums;
using RobotNet.Shares;

namespace RobotNet.RobotManager.Services.Simulation;

public class GridDifferentialNavigationService(VisualizationService visualization, RobotSimulationModel model, IServiceProvider ServiceProvider) : NavigationService(visualization, model, ServiceProvider)
{
    public override MessageResult Rotate(double angle)
    {
        RotatePID = new PID().WithKp(10).WithKi(0.01).WithKd(0.1);
        TargetAngle = MathExtension.CheckLimit(angle, 180, -180);
        Action = NavigationAction.Rotate;
        NavStart();
        CheckingStart();
        return new(true);
    }

    public override MessageResult MoveStraight(NavigationNode[] path)
    {
        if (path.Length < 2) return new(false, "Đường dẫn không hợp lệ");
        NavigationPath = [.. path];
        CheckingNodes = [NavigationPath[0], NavigationPath[^1]];
        InNode = CheckingNodes[0];

        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath(path);
        double Angle = Math.Atan2(path[1].Y - path[0].Y, path[1].X - path[0].X);
        Rotate(Angle * 180 / Math.PI);
        UpdateGoal(NavigationPath[^1]);
        FinalGoal = NavigationPath[^1];
        return new(true);
    }

    public override MessageResult Move(NavigationNode[] path)
    {
        if (path.Length < 2) return new(false, "Đường dẫn không hợp lệ");
        NavigationPath = [.. path];
        CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
        if (CheckingNodes.Count < 2) return new(false, "Đường dẫn traffic không hợp lệ");
        InNode = CheckingNodes[0];

        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);
        double Angle = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X);
        Rotate(Angle * 180 / Math.PI);
        return new(true);
    }

    public override MessageResult Move(NodeDto[] nodes, EdgeDto[] edges)
    {
        if (nodes.Length < 2) return new(false, "Đường dẫn không hợp lệ");
        var pathNodes = PathPlanner.CalculatorDirection(MapCompute.GetRobotDirection(nodes[0].Direction), nodes, edges);
        var pathSplit = PathPlanner.PathSplit(pathNodes, edges);
        if (!pathSplit.IsSuccess) return new(false, pathSplit.Message);
        if (pathSplit.Data is null) return new(false, "Đường dẫn không hợp lệ");

        var getZones = PathPlanner.GetZones(MapId, nodes);
        getZones.Wait();
        Zones = getZones.Result.Data ?? [];

        NavigationPath = [..pathSplit.Data.Select(n => new NavigationNode()
        {
            Id = n.Id,
            X = n.X,
            Y = n.Y,
            Theta = n.Theta,
            Direction = MapCompute.GetRobotDirection(n.Direction),
            Actions = n.Actions,
        })];
        CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
        InNode = CheckingNodes[0];
        FinalGoal = CheckingNodes[^1];

        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);

        double angleFoward = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X) * 180 / Math.PI;
        double angleBacward = Math.Atan2(NavigationPath[0].Y - NavigationPath[1].Y, NavigationPath[0].X - NavigationPath[1].X) * 180 / Math.PI;
        Rotate(CheckingNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward);
        return new(true);
    }

    protected override void NavigationHandler()
    {
        try
        {
            if (Action == NavigationAction.Rotate)
            {
                if (RotatePID is not null)
                {
                    NavigationState = NavigationStateType.Rotating;
                    double Error = Visualization.Theta - TargetAngle;
                    if (Error > 180) Error -= 360;
                    else if (Error < -180) Error += 360;
                    if (Math.Abs(Error) < 1)
                    {
                        if (NavigationPath is not null) Action = NavigationAction.Move;
                        else Dispose();
                    }
                    else
                    {
                        var SpeedCal = RotatePID.PID_step(Error * MathExtension.ToRad, AngularVelocity, -AngularVelocity, SampleTimeMilliseconds / 1000.0);
                        VelocityController.SetSpeed(SpeedCal, SpeedCal);
                    }
                }
            }
            else if (Action == NavigationAction.Move)
            {
                if (NavigationPath is not null && NavigationGoal is not null)
                {
                    if (MovePID is not null && MoveFuzzy is not null && MovePurePursuit is not null && FinalGoal is not null)
                    {
                        var DistanceToGoal = Math.Sqrt(Math.Pow(Visualization.X - FinalGoal.X, 2) + Math.Pow(Visualization.Y - FinalGoal.Y, 2));
                        var DistanceToNavigationGoal = Math.Sqrt(Math.Pow(Visualization.X - NavigationGoal.X, 2) + Math.Pow(Visualization.Y - NavigationGoal.Y, 2));
                        var deviation = NavigationGoal.Id == FinalGoal.Id ? 0.02 : 0.05;
                        if (DistanceToNavigationGoal > deviation)
                        {
                            NavigationState = NavigationStateType.Moving;
                            double SpeedTarget = MovePID.PID_step(DistanceToNavigationGoal, RobotModel.MaxVelocity, 0, SampleTimeMilliseconds / 1000.0);
                            double AngularVel = MovePurePursuit.PurePursuit_step(Visualization.X, Visualization.Y, Visualization.Theta * Math.PI / 180);
                            AngularVel *= NavigationPath[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD ? 1 : -1;
                            (double AngularVelocityLeft, double AngularVelocityRight) = MoveFuzzy.Fuzzy_step(SpeedTarget, AngularVel, SampleTimeMilliseconds / 1000.0);

                            if (NavigationPath[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD)
                            {
                                AngularVelocityLeft /= RobotModel.RadiusWheel;
                                AngularVelocityRight = AngularVelocityRight / RobotModel.RadiusWheel * -1;
                            }
                            else
                            {
                                AngularVelocityLeft = AngularVelocityLeft / RobotModel.RadiusWheel * -1;
                                AngularVelocityRight /= RobotModel.RadiusWheel;
                            }
                            VelocityController.SetSpeed(AngularVelocityLeft, AngularVelocityRight);
                            if (MovePurePursuit.OnNodeIndex < NavigationPath.Count)
                            {
                                var inNode = NavigationPath[MovePurePursuit.OnNodeIndex];
                                if (CheckingNodes.Any(n => n.Id == inNode.Id)) InNode = inNode;
                                else
                                {
                                    var inNodeIndex = CheckingNodes.IndexOf(InNode ?? new());
                                    inNodeIndex = inNodeIndex < 0 ? 0 : inNodeIndex;
                                    for (int i = inNodeIndex; i < CheckingNodes.Count; i++)
                                    {
                                        if (Math.Sqrt(Math.Pow(CheckingNodes[i].X - inNode.X, 2) + Math.Pow(CheckingNodes[i].Y - inNode.Y, 2)) < 0.2)
                                        {
                                            InNode = CheckingNodes[i];
                                            break;
                                        }
                                    }
                                }
                            }
                        }
                        else if (DistanceToGoal < 0.02) Dispose();
                        else NavigationState = NavigationStateType.Waitting;
                    }
                }
            }
        }
        catch (Exception ex)
        {
            Logger.Warning($"{RobotModel.RobotId} Nav ex: {ex.Message}");
        }
    }
}