269 lines
9.5 KiB
C#
269 lines
9.5 KiB
C#
using RobotNet.VDA5050.Order;
|
|
using RobotNet10.Common;
|
|
using RobotNet10.RobotApp.Detection;
|
|
using RobotNet10.RobotApp.Interfaces;
|
|
using RobotNet10.RobotApp.Services.ConfigManager;
|
|
using RobotNet10.RobotApp.Services.Robot;
|
|
using RobotNet10.RobotApp.Services.Robot.Models;
|
|
using RobotNet10.RobotApp.Services.Simulation.Algorithm;
|
|
using RobotNet10.RobotApp.Shared.Enums;
|
|
|
|
namespace RobotNet10.RobotApp.Services.Simulation;
|
|
|
|
public class SimulationNavigation : INavigation, IDisposable
|
|
{
|
|
public NavigationState State => NavState;
|
|
public bool Driving => NavDriving;
|
|
public bool IsReady => true;
|
|
public double VelocityX => Visualization.Vx;
|
|
public double VelocityY => Visualization.Vy;
|
|
public double Omega => Visualization.Omega;
|
|
|
|
public event Action<NavigationState>? OnNavigationFinished;
|
|
|
|
protected NavigationState NavState = NavigationState.Idle;
|
|
protected bool NavDriving = false;
|
|
|
|
protected const int CycleHandlerMilliseconds = 50;
|
|
private WatchThread<SimulationNavigation>? NavigationTimer;
|
|
|
|
protected double TargetAngle = 0;
|
|
protected PID? RotatePID;
|
|
protected readonly double AngularVelocity;
|
|
|
|
protected PID? MovePID;
|
|
protected FuzzyLogic? MoveFuzzy;
|
|
protected PurePursuit? MovePurePursuit;
|
|
|
|
protected readonly SimulationVisualization Visualization;
|
|
protected readonly SimulationVelocity VelocityController;
|
|
protected readonly IRobotConfiguration RobotConfiguration;
|
|
|
|
protected OrderNode? GoalRotate;
|
|
protected OrderNode? CurrentBaseNode;
|
|
protected HashSet<string> ProcessedRotations = [];
|
|
protected NavigationState ResumeState = NavigationState.Idle;
|
|
|
|
protected SimulationConfig SimCog;
|
|
protected RobotPhysicalConfig PhysicalCog;
|
|
protected double MaxVelocity;
|
|
|
|
private readonly ILogger<SimulationNavigation> Logger;
|
|
|
|
public SimulationNavigation(IServiceProvider ServiceProvider)
|
|
{
|
|
using var scope = ServiceProvider.CreateScope();
|
|
Logger = scope.ServiceProvider.GetRequiredService<ILogger<SimulationNavigation>>();
|
|
Visualization = scope.ServiceProvider.GetRequiredService<SimulationVisualization>();
|
|
RobotConfiguration = scope.ServiceProvider.GetRequiredService<IRobotConfiguration>();
|
|
SimCog = RobotConfiguration.GetSimulationConfig();
|
|
MaxVelocity = SimCog.MaxVelocity;
|
|
PhysicalCog = RobotConfiguration.GetRobotPhysicalConfig();
|
|
VelocityController = new(Visualization, SimCog);
|
|
Visualization.SetPhysical(PhysicalCog.WheelRadius, PhysicalCog.Width);
|
|
AngularVelocity = SimCog.MaxAngularVelocity * PhysicalCog.Width / 2 / 2 / PhysicalCog.WheelRadius;
|
|
}
|
|
|
|
protected void HandleNavigationStart()
|
|
{
|
|
SimCog = RobotConfiguration.GetSimulationConfig();
|
|
PhysicalCog = RobotConfiguration.GetRobotPhysicalConfig();
|
|
Visualization.SetPhysical(PhysicalCog.WheelRadius, PhysicalCog.Width);
|
|
NavigationTimer = new(CycleHandlerMilliseconds, NavigationHandler, Logger);
|
|
NavigationTimer.Start();
|
|
}
|
|
|
|
protected void HandleNavigationStop()
|
|
{
|
|
NavigationTimer?.Dispose();
|
|
NavigationTimer = null;
|
|
}
|
|
|
|
protected virtual void NavigationHandler() { }
|
|
|
|
public void SafetyStop()
|
|
{
|
|
NavState = NavigationState.SafetyStop;
|
|
}
|
|
|
|
public void CancelMovement()
|
|
{
|
|
NavState = NavigationState.Canceled;
|
|
Dispose();
|
|
}
|
|
|
|
public void Move(RobotNet.VDA5050.Order.OrderMsg order, bool hasLoad = false)
|
|
{
|
|
var nodes = order.Nodes;
|
|
var edges = order.Edges;
|
|
NavState = NavigationState.Initializing;
|
|
|
|
MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
|
|
MoveFuzzy = new FuzzyLogic();
|
|
MovePurePursuit = new PurePursuit()
|
|
.WithLookheadDistance(0.35)
|
|
.WithPath(nodes, edges, Visualization.Theta * Math.PI / 180);
|
|
|
|
(NavigationNode node, int index) = MovePurePursuit.GetOnNavNode(Visualization.X, Visualization.Y);
|
|
if (index >= MovePurePursuit.Waypoints_Value.Count - 1) return;
|
|
|
|
double angleFoward = Math.Atan2(MovePurePursuit.Waypoints_Value[index + 1].Y - node.Y, MovePurePursuit.Waypoints_Value[index + 1].X - node.X) * 180 / Math.PI;
|
|
double angleBacward = Math.Atan2(node.Y - MovePurePursuit.Waypoints_Value[index + 1].Y, node.X - MovePurePursuit.Waypoints_Value[index + 1].X) * 180 / Math.PI;
|
|
Rotate(node.Direction == RobotDirection.FORWARD ? angleFoward : angleBacward);
|
|
}
|
|
|
|
public void MoveStraight(double x, double y, bool hasLoad = false, RobotDirection? direction = null)
|
|
{
|
|
//var headRobotNode = new NavigationNode()
|
|
//{
|
|
// X = Visualization.X * Math.Acos(Visualization.Theta * Math.PI / 180),
|
|
// Y = Visualization.Y * Math.Asin(Visualization.Theta * Math.PI / 180),
|
|
//};
|
|
//var goalNode = new NavigationNode()
|
|
//{
|
|
// NodeId = "RobotGoal",
|
|
// X = x,
|
|
// Y = y,
|
|
//};
|
|
//var currentRobotNode = new NavigationNode()
|
|
//{
|
|
// NodeId = "RobotCurrentNode",
|
|
// X = Visualization.X,
|
|
// Y = Visualization.Y,
|
|
//};
|
|
//goalNode.Theta = SpaceCompute.GetVectorAngle(currentRobotNode.X, currentRobotNode.Y, headRobotNode.X, headRobotNode.Y, goalNode.X, goalNode.Y) > 90 ?
|
|
// Math.Atan2(currentRobotNode.Y - goalNode.Y, currentRobotNode.X - goalNode.X) :
|
|
// Math.Atan2(goalNode.Y - currentRobotNode.Y, goalNode.X - currentRobotNode.X);
|
|
//currentRobotNode.Theta = goalNode.Theta;
|
|
|
|
//MovePID = new PID().WithKp(1.5).WithKi(0.0001).WithKd(0.8);
|
|
//MoveFuzzy = new FuzzyLogic();
|
|
//MovePurePursuit = new PurePursuit()
|
|
// .WithLookheadDistance(0.25)
|
|
// .WithPath([currentRobotNode, goalNode], [new Edge()
|
|
//{
|
|
// EdgeId = "Straight edge",
|
|
// Trajectory = new Trajectory()
|
|
// {
|
|
// Degree = 1,
|
|
// ControlPoints = []
|
|
// },
|
|
// StartNodeId = currentRobotNode.NodeId,
|
|
// EndNodeId = goalNode.NodeId,
|
|
//}], Visualization.Theta);
|
|
|
|
//double Angle = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X);
|
|
//Rotate(Angle * 180 / Math.PI);
|
|
//UpdateOrder(goalNode.NodeId);
|
|
}
|
|
|
|
public void DockTo(IDetectSession session, bool hasLoad = false, RobotDirection? direction = null)
|
|
{
|
|
}
|
|
|
|
public void Pause()
|
|
{
|
|
ResumeState = NavState;
|
|
NavState = NavigationState.Paused;
|
|
}
|
|
|
|
public void Resume()
|
|
{
|
|
NavState = ResumeState;
|
|
}
|
|
|
|
public void Rotate(double angle)
|
|
{
|
|
RotatePID = new PID().WithKp(10).WithKi(0.01).WithKd(0.1);
|
|
TargetAngle = SpaceCompute.NormalizeDegreeAngle(angle);
|
|
NavState = NavigationState.Rotating;
|
|
HandleNavigationStart();
|
|
}
|
|
|
|
protected void UpdateGoal(string goalId)
|
|
{
|
|
MovePurePursuit?.UpdateGoal(goalId);
|
|
}
|
|
|
|
public void UpdateOrder(string newBaseNodeId)
|
|
{
|
|
var newBaseNode = MovePurePursuit?.OrderNodes.FirstOrDefault(n => n.NodeId == newBaseNodeId);
|
|
if (newBaseNode is not null && newBaseNode.NodeId != CurrentBaseNode?.NodeId)
|
|
{
|
|
CurrentBaseNode = newBaseNode;
|
|
var newGoalRotate = FindNextRotateGoal();
|
|
if (newGoalRotate is not null && newGoalRotate.NodeId != GoalRotate?.NodeId)
|
|
{
|
|
GoalRotate = newGoalRotate;
|
|
UpdateGoal(newGoalRotate.NodeId);
|
|
}
|
|
}
|
|
}
|
|
|
|
public void RefreshOrder(Node[] nodes, Edge[] edges)
|
|
{
|
|
return;
|
|
}
|
|
|
|
public void SetSpeed(double speed)
|
|
{
|
|
MaxVelocity = speed;
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
HandleNavigationStop();
|
|
VelocityController.Stop();
|
|
CurrentBaseNode = null;
|
|
MovePurePursuit = null;
|
|
MovePID = null;
|
|
RotatePID = null;
|
|
NavDriving = false;
|
|
OnNavigationFinished?.Invoke(NavState);
|
|
NavState = NavigationState.Idle;
|
|
GC.SuppressFinalize(this);
|
|
}
|
|
|
|
public void Refresh()
|
|
{
|
|
NavState = NavigationState.Idle;
|
|
}
|
|
|
|
public void Start()
|
|
{
|
|
return;
|
|
}
|
|
|
|
public void Stop()
|
|
{
|
|
Dispose();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Tìm node có IsWaitingRotate đầu tiên trong path từ currentNode đến currentGoal
|
|
/// </summary>
|
|
protected OrderNode? FindNextRotateGoal()
|
|
{
|
|
if (CurrentBaseNode == null || MovePurePursuit is null || MovePurePursuit.OrderNodes.Length == 0) return null;
|
|
|
|
int goalIndex = Array.FindIndex(MovePurePursuit.OrderNodes, n => n.NodeId == CurrentBaseNode.NodeId);
|
|
if (goalIndex == -1) return null;
|
|
|
|
int lastNodeIdx = Array.FindIndex(MovePurePursuit.OrderNodes, n => n.NodeId == GoalRotate?.NodeId);
|
|
lastNodeIdx = lastNodeIdx == -1 ? 0 : lastNodeIdx + 1;
|
|
|
|
// Tìm từ node hiện tại đến goal
|
|
for (int i = lastNodeIdx; i <= goalIndex; i++)
|
|
{
|
|
var node = MovePurePursuit.OrderNodes[i];
|
|
|
|
// Tìm node có IsWaitingRotate và chưa xử lý
|
|
if (node.IsWaitRotating && !ProcessedRotations.Contains(node.NodeId))
|
|
{
|
|
return node;
|
|
}
|
|
}
|
|
return CurrentBaseNode;
|
|
}
|
|
}
|