Initial commit
This commit is contained in:
@@ -0,0 +1,268 @@
|
||||
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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user