RobotNet/RobotNet.RobotManager/Services/Simulation/ForkliftNavigationSevice.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 ForkliftNavigationSevice(VisualizationService visualization, RobotSimulationModel model, IServiceProvider ServiceProvider) : NavigationService(visualization, model, ServiceProvider)
{
    public override MessageResult Rotate(double angle)
    {
        return new(false, "Robot không có chức năng này");
    }

    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.15).WithPath(path);

        UpdateGoal(NavigationPath[^1]);
        FinalGoal = NavigationPath[^1];
        NavStart();
        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.2).WithPath(path);

        NavStart();
        CheckingStart();
        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 pathSplit = PathPlanner.PathSplit(nodes, 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ệ");
        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]);

        NavStart();
        CheckingStart();
        return new(true);
    }

    protected override void NavigationHandler()
    {
        try
        {
            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 DistanceToCheckingNode = 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.1;
                    if (DistanceToCheckingNode > deviation)
                    {
                        NavigationState = NavigationStateType.Moving;
                        double SpeedTarget = MovePID.PID_step(DistanceToCheckingNode, 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}");
        }
    }
}