first commit -push
This commit is contained in:
dungtt
@@ -0,0 +1,252 @@
|
||||
namespace RobotNet.RobotManager.Services.Simulation.Algorithm;
|
||||
|
||||
public class FuzzyLogic
|
||||
{
|
||||
private double Gain_P = 0.5;
|
||||
private double Gain_I = 0.01;
|
||||
private double DiscreteTimeIntegrator_DSTATE;
|
||||
|
||||
public FuzzyLogic WithGainP(double gainP)
|
||||
{
|
||||
Gain_P = gainP;
|
||||
return this;
|
||||
}
|
||||
|
||||
public FuzzyLogic WithGainI(double gainI)
|
||||
{
|
||||
Gain_I = gainI;
|
||||
return this;
|
||||
}
|
||||
|
||||
private static double Fuzzy_trapmf(double x, double[] parame)
|
||||
{
|
||||
double b_y1;
|
||||
double y2;
|
||||
b_y1 = 0.0;
|
||||
y2 = 0.0;
|
||||
if (x >= parame[1])
|
||||
{
|
||||
b_y1 = 1.0;
|
||||
}
|
||||
if (x < parame[0])
|
||||
{
|
||||
b_y1 = 0.0;
|
||||
}
|
||||
if (parame[0] <= x && x < parame[1] && parame[0] != parame[1])
|
||||
{
|
||||
b_y1 = 1.0 / (parame[1] - parame[0]) * (x - parame[0]);
|
||||
}
|
||||
if (x <= parame[2])
|
||||
{
|
||||
y2 = 1.0;
|
||||
}
|
||||
if (x > parame[3])
|
||||
{
|
||||
y2 = 0.0;
|
||||
}
|
||||
if (parame[2] < x && x <= parame[3] && parame[2] != parame[3])
|
||||
{
|
||||
y2 = 1.0 / (parame[3] - parame[2]) * (parame[3] - x);
|
||||
}
|
||||
return b_y1 < y2 ? b_y1 : y2;
|
||||
}
|
||||
|
||||
private static double Fuzzy_trimf(double x, double[] parame)
|
||||
{
|
||||
double y;
|
||||
y = 0.0;
|
||||
if (parame[0] != parame[1] && parame[0] < x && x < parame[1])
|
||||
{
|
||||
y = 1.0 / (parame[1] - parame[0]) * (x - parame[0]);
|
||||
}
|
||||
if (parame[1] != parame[2] && parame[1] < x && x < parame[2])
|
||||
{
|
||||
y = 1.0 / (parame[2] - parame[1]) * (parame[2] - x);
|
||||
}
|
||||
if (x == parame[1])
|
||||
{
|
||||
y = 1.0;
|
||||
}
|
||||
return y;
|
||||
}
|
||||
|
||||
public (double wl, double wr) Fuzzy_step(double v, double w, double TimeSample)
|
||||
{
|
||||
(double wl, double wr) result = new();
|
||||
double[] inputMFCache = new double[10];
|
||||
double[] outputMFCache = new double[5];
|
||||
double[] outputMFCache_0 = new double[5];
|
||||
double[] tmp = new double[3];
|
||||
double aggregatedOutputs;
|
||||
double rtb_TmpSignalConversionAtSFun_0;
|
||||
double rtb_antecedentOutputs_e;
|
||||
double sumAntecedentOutputs;
|
||||
int ruleID;
|
||||
double[] f = [-1.0E+10, -1.0E+10, -1.0, -0.5];
|
||||
double[] e = [0.5, 1.0, 1.0E+10, 1.0E+10];
|
||||
double[] d = [0.75, 1.0, 1.0E+9, 1.0E+9];
|
||||
double[] c = [-1.0E+9, -1.0E+9, 0.0, 0.25];
|
||||
byte[] b = [ 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4,
|
||||
4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 1, 2, 3,
|
||||
4, 5, 1, 2, 3, 4, 5, 3, 4, 5, 1, 2, 1, 2, 3, 4, 5 ];
|
||||
byte[] b_0 = [1, 1, 2, 1, 1, 2, 3, 5, 1, 4, 5, 5, 5, 5, 5, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5];
|
||||
byte[] b_1 = [ 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4,
|
||||
4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 4, 1,
|
||||
2, 3, 5, 3, 1, 2, 4, 5, 1, 2, 3, 4, 5, 1, 2, 4, 5, 3 ];
|
||||
byte[] b_2 = [5, 5, 5, 5, 5, 1, 2, 3, 5, 4, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5, 1, 1, 1, 1, 2];
|
||||
double inputMFCache_tmp;
|
||||
double inputMFCache_tmp_0;
|
||||
double inputMFCache_tmp_1;
|
||||
double inputMFCache_tmp_2;
|
||||
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller1' */
|
||||
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller' */
|
||||
/* SignalConversion generated from: '<S4>/ SFunction ' incorporates:
|
||||
* Constant: '<Root>/w'
|
||||
* DiscreteIntegrator: '<Root>/Discrete-Time Integrator'
|
||||
* Gain: '<Root>/Gain1'
|
||||
* MATLAB Function: '<S1>/Evaluate Rule Antecedents'
|
||||
* MATLAB Function: '<S2>/Evaluate Rule Antecedents'
|
||||
* SignalConversion generated from: '<S7>/ SFunction '
|
||||
* Sum: '<Root>/Sum'
|
||||
*/
|
||||
DiscreteTimeIntegrator_DSTATE += Gain_I * w * TimeSample;
|
||||
rtb_TmpSignalConversionAtSFun_0 = Gain_P * w + DiscreteTimeIntegrator_DSTATE;
|
||||
/* End of Outputs for SubSystem: '<Root>/Fuzzy Logic Controller1' */
|
||||
/* MATLAB Function: '<S1>/Evaluate Rule Antecedents' incorporates:
|
||||
* Constant: '<Root>/v'
|
||||
* MATLAB Function: '<S2>/Evaluate Rule Antecedents'
|
||||
* SignalConversion generated from: '<S4>/ SFunction '
|
||||
*/
|
||||
sumAntecedentOutputs = 0.0;
|
||||
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller1' */
|
||||
inputMFCache_tmp = Fuzzy_trapmf(rtb_TmpSignalConversionAtSFun_0, f);
|
||||
/* End of Outputs for SubSystem: '<Root>/Fuzzy Logic Controller1' */
|
||||
inputMFCache[0] = inputMFCache_tmp;
|
||||
tmp[0] = -0.5;
|
||||
tmp[1] = 0.0;
|
||||
tmp[2] = 0.5;
|
||||
inputMFCache[1] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
|
||||
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller1' */
|
||||
inputMFCache_tmp_0 = Fuzzy_trapmf(rtb_TmpSignalConversionAtSFun_0, e);
|
||||
/* End of Outputs for SubSystem: '<Root>/Fuzzy Logic Controller1' */
|
||||
inputMFCache[2] = inputMFCache_tmp_0;
|
||||
tmp[0] = -1.0;
|
||||
tmp[1] = -0.5;
|
||||
tmp[2] = 0.0;
|
||||
inputMFCache[3] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
|
||||
tmp[0] = 0.0;
|
||||
tmp[1] = 0.5;
|
||||
tmp[2] = 1.0;
|
||||
inputMFCache[4] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
|
||||
tmp[0] = 0.0;
|
||||
tmp[1] = 0.25;
|
||||
tmp[2] = 0.5;
|
||||
inputMFCache[5] = Fuzzy_trimf(v, tmp);
|
||||
tmp[0] = 0.25;
|
||||
tmp[1] = 0.5;
|
||||
tmp[2] = 0.75;
|
||||
inputMFCache[6] = Fuzzy_trimf(v, tmp);
|
||||
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller1' */
|
||||
inputMFCache_tmp_1 = Fuzzy_trapmf(v, d);
|
||||
/* End of Outputs for SubSystem: '<Root>/Fuzzy Logic Controller1' */
|
||||
inputMFCache[7] = inputMFCache_tmp_1;
|
||||
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller1' */
|
||||
inputMFCache_tmp_2 = Fuzzy_trapmf(v, c);
|
||||
/* End of Outputs for SubSystem: '<Root>/Fuzzy Logic Controller1' */
|
||||
inputMFCache[8] = inputMFCache_tmp_2;
|
||||
tmp[0] = 0.5;
|
||||
tmp[1] = 0.75;
|
||||
tmp[2] = 1.0;
|
||||
inputMFCache[9] = Fuzzy_trimf(v, tmp);
|
||||
/* MATLAB Function: '<S1>/Evaluate Rule Consequents' */
|
||||
aggregatedOutputs = 0.0;
|
||||
outputMFCache[0] = 0.0;
|
||||
outputMFCache[1] = 0.25;
|
||||
outputMFCache[2] = 0.5;
|
||||
outputMFCache[3] = 0.75;
|
||||
outputMFCache[4] = 1.0;
|
||||
for (ruleID = 0; ruleID < 25; ruleID++)
|
||||
{
|
||||
/* MATLAB Function: '<S1>/Evaluate Rule Antecedents' */
|
||||
rtb_antecedentOutputs_e = inputMFCache[b[ruleID + 25] + 4] * inputMFCache[b[ruleID] - 1];
|
||||
sumAntecedentOutputs += rtb_antecedentOutputs_e;
|
||||
/* MATLAB Function: '<S1>/Evaluate Rule Consequents' */
|
||||
aggregatedOutputs += outputMFCache[b_0[ruleID] - 1] * rtb_antecedentOutputs_e;
|
||||
}
|
||||
/* MATLAB Function: '<S1>/Defuzzify Outputs' incorporates:
|
||||
* MATLAB Function: '<S1>/Evaluate Rule Antecedents'
|
||||
* MATLAB Function: '<S1>/Evaluate Rule Consequents'
|
||||
*/
|
||||
if (sumAntecedentOutputs == 0.0)
|
||||
{
|
||||
result.wr = 0.5;
|
||||
}
|
||||
else
|
||||
{
|
||||
result.wr = 1.0 / sumAntecedentOutputs * aggregatedOutputs;
|
||||
}
|
||||
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller1' */
|
||||
/* MATLAB Function: '<S2>/Evaluate Rule Antecedents' incorporates:
|
||||
* Constant: '<Root>/v'
|
||||
* SignalConversion generated from: '<S7>/ SFunction '
|
||||
*/
|
||||
sumAntecedentOutputs = 0.0;
|
||||
inputMFCache[0] = inputMFCache_tmp;
|
||||
tmp[0] = -0.5;
|
||||
tmp[1] = 0.0;
|
||||
tmp[2] = 0.5;
|
||||
inputMFCache[1] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
|
||||
inputMFCache[2] = inputMFCache_tmp_0;
|
||||
tmp[0] = -1.0;
|
||||
tmp[1] = -0.5;
|
||||
tmp[2] = 0.0;
|
||||
inputMFCache[3] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
|
||||
tmp[0] = 0.0;
|
||||
tmp[1] = 0.5;
|
||||
tmp[2] = 1.0;
|
||||
inputMFCache[4] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
|
||||
tmp[0] = 0.0;
|
||||
tmp[1] = 0.25;
|
||||
tmp[2] = 0.5;
|
||||
inputMFCache[5] = Fuzzy_trimf(v, tmp);
|
||||
tmp[0] = 0.25;
|
||||
tmp[1] = 0.5;
|
||||
tmp[2] = 0.75;
|
||||
inputMFCache[6] = Fuzzy_trimf(v, tmp);
|
||||
inputMFCache[7] = inputMFCache_tmp_1;
|
||||
inputMFCache[8] = inputMFCache_tmp_2;
|
||||
tmp[0] = 0.5;
|
||||
tmp[1] = 0.75;
|
||||
tmp[2] = 1.0;
|
||||
inputMFCache[9] = Fuzzy_trimf(v, tmp);
|
||||
/* MATLAB Function: '<S2>/Evaluate Rule Consequents' */
|
||||
aggregatedOutputs = 0.0;
|
||||
outputMFCache_0[0] = 0.0;
|
||||
outputMFCache_0[1] = 0.25;
|
||||
outputMFCache_0[2] = 0.5;
|
||||
outputMFCache_0[3] = 0.75;
|
||||
outputMFCache_0[4] = 1.0;
|
||||
for (ruleID = 0; ruleID < 25; ruleID++)
|
||||
{
|
||||
/* MATLAB Function: '<S2>/Evaluate Rule Antecedents' */
|
||||
rtb_antecedentOutputs_e = inputMFCache[b_1[ruleID + 25] + 4] * inputMFCache[b_1[ruleID] - 1];
|
||||
sumAntecedentOutputs += rtb_antecedentOutputs_e;
|
||||
/* MATLAB Function: '<S2>/Evaluate Rule Consequents' */
|
||||
aggregatedOutputs += outputMFCache_0[b_2[ruleID] - 1] *
|
||||
rtb_antecedentOutputs_e;
|
||||
}
|
||||
/* MATLAB Function: '<S2>/Defuzzify Outputs' incorporates:
|
||||
* MATLAB Function: '<S2>/Evaluate Rule Antecedents'
|
||||
* MATLAB Function: '<S2>/Evaluate Rule Consequents'
|
||||
*/
|
||||
if (sumAntecedentOutputs == 0.0)
|
||||
{
|
||||
result.wl = 0.5;
|
||||
}
|
||||
else
|
||||
{
|
||||
result.wl = 1.0 / sumAntecedentOutputs * aggregatedOutputs;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
namespace RobotNet.RobotManager.Services.Simulation.Algorithm;
|
||||
|
||||
public class MathExtension
|
||||
{
|
||||
public static double ToRad => Math.PI / 180;
|
||||
public static double ToDegree => 180 / Math.PI;
|
||||
public static double CheckLimit(double value, double Max, double Min) => value < Max ? value > Min ? value : Min : Max;
|
||||
}
|
||||
42
RobotNet.RobotManager/Services/Simulation/Algorithm/PID.cs
Normal file
42
RobotNet.RobotManager/Services/Simulation/Algorithm/PID.cs
Normal file
@@ -0,0 +1,42 @@
|
||||
namespace RobotNet.RobotManager.Services.Simulation.Algorithm;
|
||||
|
||||
public class PID
|
||||
{
|
||||
private double Kp = 0.3;
|
||||
private double Ki = 0.0001;
|
||||
private double Kd = 0.01;
|
||||
private double Pre_Error;
|
||||
private double Pre_Pre_Error;
|
||||
private double Pre_Out;
|
||||
|
||||
public PID WithKp(double kp)
|
||||
{
|
||||
Kp = kp;
|
||||
return this;
|
||||
}
|
||||
|
||||
public PID WithKi(double ki)
|
||||
{
|
||||
Ki = ki;
|
||||
return this;
|
||||
}
|
||||
|
||||
public PID WithKd(double kd)
|
||||
{
|
||||
Kd = kd;
|
||||
return this;
|
||||
}
|
||||
|
||||
public double PID_step(double Error, double LimitMax, double LimitMin, double TimeSample)
|
||||
{
|
||||
double P_part = Kp * (Error - Pre_Error);
|
||||
double I_part = 0.5 * Ki * TimeSample * (Error + Pre_Error);
|
||||
double D_part = Kd / TimeSample * (Error - 2 * Pre_Error + Pre_Pre_Error);
|
||||
double Out = Pre_Out + P_part + I_part + D_part;
|
||||
Pre_Pre_Error = Pre_Error;
|
||||
Pre_Error = Error;
|
||||
Pre_Out = Out;
|
||||
Out = MathExtension.CheckLimit(Out, LimitMax, LimitMin);
|
||||
return Out;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,159 @@
|
||||
using RobotNet.RobotManager.Services.Simulation.Models;
|
||||
using RobotNet.RobotShares.Enums;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Simulation.Algorithm;
|
||||
public class PurePursuit
|
||||
{
|
||||
private double MaxAngularVelocity = 1.5;
|
||||
private double LookaheadDistance = 0.5;
|
||||
private List<NavigationNode> Waypoints_Value = [];
|
||||
public int OnNodeIndex = 0;
|
||||
public int GoalIndex = 0;
|
||||
public NavigationNode? Goal;
|
||||
|
||||
public PurePursuit WithLookheadDistance(double distance)
|
||||
{
|
||||
LookaheadDistance = distance;
|
||||
return this;
|
||||
}
|
||||
|
||||
public PurePursuit WithMaxAngularVelocity(double vel)
|
||||
{
|
||||
MaxAngularVelocity = vel;
|
||||
return this;
|
||||
}
|
||||
|
||||
public PurePursuit WithPath(NavigationNode[] path)
|
||||
{
|
||||
Waypoints_Value = [.. path];
|
||||
return this;
|
||||
}
|
||||
|
||||
public void UpdatePath(NavigationNode[] path)
|
||||
{
|
||||
Waypoints_Value = [.. path];
|
||||
}
|
||||
|
||||
public void UpdateGoal(NavigationNode goal)
|
||||
{
|
||||
Goal = goal;
|
||||
GoalIndex = Waypoints_Value.IndexOf(Goal);
|
||||
}
|
||||
|
||||
public (NavigationNode node, int index) GetOnNode(double x, double y)
|
||||
{
|
||||
double minDistance = double.MaxValue;
|
||||
NavigationNode onNode = Waypoints_Value[0];
|
||||
int index = 0;
|
||||
for (int i = 1; i < Waypoints_Value.Count; i++)
|
||||
{
|
||||
var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2));
|
||||
if (distance < minDistance)
|
||||
{
|
||||
onNode = Waypoints_Value[i];
|
||||
minDistance = distance;
|
||||
index = i;
|
||||
}
|
||||
}
|
||||
return (onNode, index);
|
||||
}
|
||||
|
||||
private (NavigationNode? node, int index) OnNode(double x, double y)
|
||||
{
|
||||
if (Waypoints_Value is null || Waypoints_Value.Count == 0) return (null, 0);
|
||||
double minDistance = double.MaxValue;
|
||||
int index = 0;
|
||||
NavigationNode? onNode = null;
|
||||
if(Goal is null) return (onNode, index);
|
||||
for (int i = OnNodeIndex; i < Waypoints_Value.IndexOf(Goal); i ++)
|
||||
{
|
||||
var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2));
|
||||
if (distance < minDistance)
|
||||
{
|
||||
onNode = Waypoints_Value[i];
|
||||
minDistance = distance;
|
||||
index = i;
|
||||
}
|
||||
}
|
||||
return (onNode, index);
|
||||
}
|
||||
|
||||
public double PurePursuit_step(double X_Ref, double Y_Ref, double Angle_Ref)
|
||||
{
|
||||
if (Waypoints_Value is null || Waypoints_Value.Count < 2) return 0;
|
||||
NavigationNode? lookaheadStartPt = null;
|
||||
var (onNode, index) = OnNode(X_Ref, Y_Ref);
|
||||
if (onNode is null || Goal is null) return 0;
|
||||
OnNodeIndex = index;
|
||||
double lookDistance = 0;
|
||||
for (int i = OnNodeIndex + 1; i < Waypoints_Value.IndexOf(Goal); i++)
|
||||
{
|
||||
lookDistance += Math.Sqrt(Math.Pow(Waypoints_Value[i - 1].X - Waypoints_Value[i].X, 2) + Math.Pow(Waypoints_Value[i - 1].Y - Waypoints_Value[i].Y, 2));
|
||||
if (lookDistance >= LookaheadDistance || Waypoints_Value[i].Direction != onNode.Direction)
|
||||
{
|
||||
lookaheadStartPt = Waypoints_Value[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
lookaheadStartPt ??= Goal;
|
||||
if (onNode.Direction == RobotDirection.BACKWARD)
|
||||
{
|
||||
if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
|
||||
else if (Angle_Ref < -Math.PI) Angle_Ref += Math.PI * 2;
|
||||
Angle_Ref += Math.PI;
|
||||
if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
|
||||
}
|
||||
var distance = Math.Atan2(lookaheadStartPt.Y - Y_Ref, lookaheadStartPt.X - X_Ref) - Angle_Ref;
|
||||
|
||||
if (Math.Abs(distance) > Math.PI)
|
||||
{
|
||||
double minDistance;
|
||||
if (distance + Math.PI == 0.0) minDistance = 0.0;
|
||||
else
|
||||
{
|
||||
double data = (distance + Math.PI) / (2 * Math.PI);
|
||||
if (data < 0) data = Math.Round(data + 0.5);
|
||||
else data = Math.Round(data - 0.5);
|
||||
minDistance = distance + Math.PI - data * (2 * Math.PI);
|
||||
double checker = 0;
|
||||
if (minDistance != 0.0)
|
||||
{
|
||||
checker = Math.Abs((distance + Math.PI) / (2 * Math.PI));
|
||||
}
|
||||
if (!(Math.Abs(checker - Math.Floor(checker + 0.5)) > 2.2204460492503131E-16 * checker))
|
||||
{
|
||||
minDistance = 0.0;
|
||||
}
|
||||
else if (distance + Math.PI < 0.0)
|
||||
{
|
||||
minDistance += Math.PI * 2;
|
||||
}
|
||||
}
|
||||
if (minDistance == 0.0 && distance + Math.PI > 0.0)
|
||||
{
|
||||
minDistance = Math.PI * 2;
|
||||
}
|
||||
distance = minDistance - Math.PI;
|
||||
}
|
||||
|
||||
var AngularVelocity = 2.0 * 0.5 * Math.Sin(distance) / LookaheadDistance;
|
||||
if (Math.Abs(AngularVelocity) > MaxAngularVelocity)
|
||||
{
|
||||
if (AngularVelocity < 0.0)
|
||||
{
|
||||
AngularVelocity = -1.0;
|
||||
}
|
||||
else if (AngularVelocity > 0.0)
|
||||
{
|
||||
AngularVelocity = 1.0;
|
||||
}
|
||||
else if (AngularVelocity == 0.0)
|
||||
{
|
||||
AngularVelocity = 0.0;
|
||||
}
|
||||
AngularVelocity *= MaxAngularVelocity;
|
||||
}
|
||||
return AngularVelocity;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,169 @@
|
||||
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 DifferentialNavigationService(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 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]);
|
||||
|
||||
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 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);
|
||||
|
||||
//AngularVelocityLeft /= RobotModel.RadiusWheel;
|
||||
//AngularVelocityRight = AngularVelocityRight / RobotModel.RadiusWheel * -1;
|
||||
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}");
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,140 @@
|
||||
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}");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,172 @@
|
||||
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}");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.RobotManager.Services.Simulation.Models;
|
||||
using RobotNet.Shares;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Simulation;
|
||||
|
||||
public interface INavigationService : IRobotOrder, IDisposable
|
||||
{
|
||||
Guid MapId { get; set; }
|
||||
NavigationNode? InNode { get; }
|
||||
List<ZoneDto> CurrentZones { get; set; }
|
||||
NavigationStateType NavigationState { get; }
|
||||
MessageResult Rotate(double angle);
|
||||
MessageResult Move(NavigationNode[] path);
|
||||
MessageResult Move(NodeDto[] nodes, EdgeDto[] edges);
|
||||
MessageResult MoveStraight(NavigationNode[] path);
|
||||
MessageResult Cancel();
|
||||
}
|
||||
@@ -0,0 +1,9 @@
|
||||
namespace RobotNet.RobotManager.Services.Simulation.Models;
|
||||
|
||||
public enum NavigationAction
|
||||
{
|
||||
Rotate,
|
||||
Move,
|
||||
Start,
|
||||
Stop,
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.RobotManager.Services.Planner.Space;
|
||||
using RobotNet.RobotShares.Enums;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Simulation.Models;
|
||||
|
||||
public class NavigationNode
|
||||
{
|
||||
public Guid Id { get; set; }
|
||||
public double X { get; set; }
|
||||
public double Y { get; set; }
|
||||
public double Theta { get; set; }
|
||||
public RobotDirection Direction { get; set; }
|
||||
public string Actions { get; set; } = string.Empty;
|
||||
|
||||
public override bool Equals(object? obj)
|
||||
{
|
||||
if (obj is NavigationNode other)
|
||||
return Id == other.Id;
|
||||
return false;
|
||||
}
|
||||
|
||||
public override int GetHashCode()
|
||||
{
|
||||
return HashCode.Combine(Id);
|
||||
}
|
||||
|
||||
public NodeDto ToNodeDto()
|
||||
{
|
||||
return new NodeDto
|
||||
{
|
||||
Id = Id,
|
||||
X = X,
|
||||
Y = Y,
|
||||
Theta = Theta,
|
||||
Direction = MapCompute.GetNodeDirection(Direction),
|
||||
Actions = Actions
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
namespace RobotNet.RobotManager.Services.Simulation.Models;
|
||||
|
||||
public enum NavigationStateType
|
||||
{
|
||||
None,
|
||||
Ready,
|
||||
Moving,
|
||||
Stop,
|
||||
Error,
|
||||
Rotating,
|
||||
Waitting,
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
using RobotNet.RobotShares.Enums;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Simulation.Models;
|
||||
|
||||
public class RobotSimulationModel
|
||||
{
|
||||
public string RobotId { get; set; } = string.Empty;
|
||||
public double RadiusWheel { get; set; }
|
||||
public double Width { get; set; }
|
||||
public double Length { get; set; }
|
||||
public double MaxVelocity { get; set; }
|
||||
public double MaxAngularVelocity { get; set; }
|
||||
public double Acceleration { get; set; }
|
||||
public double Deceleration { get; set; }
|
||||
public NavigationType NavigationType { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
using RobotNet.RobotManager.Services.Simulation.Models;
|
||||
using RobotNet.RobotManager.Services.Traffic;
|
||||
using RobotNet.RobotShares.Enums;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Simulation;
|
||||
|
||||
public class NavigationManager
|
||||
{
|
||||
public static INavigationService GetNavigation(VisualizationService Visualization, RobotSimulationModel RobotModel, IServiceProvider ServiceProvider)
|
||||
{
|
||||
if (RobotModel.NavigationType == NavigationType.Forklift) return new ForkliftNavigationSevice(Visualization, RobotModel, ServiceProvider);
|
||||
else if(RobotModel.NavigationType == NavigationType.GridDifferential) return new GridDifferentialNavigationService(Visualization, RobotModel, ServiceProvider);
|
||||
return new DifferentialNavigationService(Visualization, RobotModel, ServiceProvider);
|
||||
}
|
||||
}
|
||||
655
RobotNet.RobotManager/Services/Simulation/NavigationService.cs
Normal file
655
RobotNet.RobotManager/Services/Simulation/NavigationService.cs
Normal file
@@ -0,0 +1,655 @@
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.RobotManager.Services.OpenACS;
|
||||
using RobotNet.RobotManager.Services.Planner.Space;
|
||||
using RobotNet.RobotManager.Services.Simulation.Algorithm;
|
||||
using RobotNet.RobotManager.Services.Simulation.Models;
|
||||
using RobotNet.RobotManager.Services.Traffic;
|
||||
using RobotNet.RobotShares.Dtos;
|
||||
using RobotNet.RobotShares.Enums;
|
||||
using RobotNet.Shares;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Simulation;
|
||||
|
||||
public abstract class NavigationService : INavigationService, IDisposable
|
||||
{
|
||||
protected readonly VisualizationService Visualization;
|
||||
protected readonly VelocityController VelocityController;
|
||||
protected readonly RobotSimulationModel RobotModel;
|
||||
protected readonly TrafficManager TrafficManager;
|
||||
protected readonly TrafficACS TrafficACS;
|
||||
protected readonly PathPlanner PathPlanner;
|
||||
protected readonly MapManager MapManager;
|
||||
protected readonly LoggerController<NavigationService> Logger;
|
||||
|
||||
private WatchTimer<NavigationService>? navTimer;
|
||||
protected const int SampleTimeMilliseconds = 50;
|
||||
|
||||
private WatchTimerAsync<NavigationService>? checkingTimer;
|
||||
private const int CheckingTimeMilliseconds = 1000;
|
||||
|
||||
protected double TargetAngle = 0;
|
||||
protected PID? RotatePID;
|
||||
protected readonly double AngularVelocity;
|
||||
|
||||
protected PID? MovePID;
|
||||
protected FuzzyLogic? MoveFuzzy;
|
||||
protected PurePursuit? MovePurePursuit;
|
||||
|
||||
private Guid TrafficManagerGoalId = Guid.Empty;
|
||||
private Guid TrafficACSGoalId = Guid.Empty;
|
||||
protected NavigationNode? NavigationGoal;
|
||||
protected NavigationNode? FinalGoal;
|
||||
protected List<NavigationNode>? NavigationPath;
|
||||
protected List<NavigationNode> CheckingNodes = [];
|
||||
private List<NavigationNode> RefreshPath = [];
|
||||
private List<NavigationNode> BaseNodes = [];
|
||||
private bool IsWaittingStop;
|
||||
private double RotateAngle;
|
||||
protected Dictionary<Guid, ZoneDto[]> Zones = [];
|
||||
|
||||
protected NavigationAction Action = NavigationAction.Stop;
|
||||
|
||||
public NavigationNode? InNode { get; set; } = null;
|
||||
public NavigationStateType NavigationState { get; set; } = NavigationStateType.Ready;
|
||||
public TrafficSolutionState TrafficSolutionState { get; private set; } = TrafficSolutionState.None;
|
||||
public bool IsError { get; private set; }
|
||||
public bool IsCompleted { get; private set; } = true;
|
||||
public bool IsProcessing { get; private set; }
|
||||
public bool IsCanceled { get; private set; }
|
||||
public string[] Errors => [];
|
||||
public NavigationPathEdge[] FullPath => GetFullPath();
|
||||
public NavigationPathEdge[] BasePath => GetBasePath();
|
||||
public Guid MapId { get; set; } = Guid.Empty;
|
||||
public List<ZoneDto> CurrentZones { get; set; } = [];
|
||||
|
||||
public NavigationService(VisualizationService visualization, RobotSimulationModel model, IServiceProvider ServiceProvider)
|
||||
{
|
||||
Visualization = visualization;
|
||||
VelocityController = new VelocityController(Visualization).WithDeceleration(model.Deceleration).WithAcceleration(model.Acceleration).WithSampleTime(SampleTimeMilliseconds / 1000.0);
|
||||
RobotModel = model;
|
||||
AngularVelocity = MathExtension.CheckLimit(RobotModel.MaxAngularVelocity * RobotModel.Width / RobotModel.RadiusWheel, 2 * Math.PI, 0);
|
||||
TrafficManager = ServiceProvider.GetRequiredService<TrafficManager>();
|
||||
TrafficACS = ServiceProvider.GetRequiredService<TrafficACS>();
|
||||
PathPlanner = ServiceProvider.GetRequiredService<PathPlanner>();
|
||||
MapManager = ServiceProvider.GetRequiredService<MapManager>();
|
||||
Logger = ServiceProvider.GetRequiredService<LoggerController<NavigationService>>();
|
||||
}
|
||||
|
||||
public virtual 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 virtual 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);
|
||||
return new(true);
|
||||
}
|
||||
|
||||
public virtual 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 virtual 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);
|
||||
}
|
||||
|
||||
public MessageResult Cancel()
|
||||
{
|
||||
Dispose();
|
||||
VelocityController.Stop();
|
||||
NavigationState = NavigationStateType.Ready;
|
||||
IsCanceled = true;
|
||||
return new(true);
|
||||
}
|
||||
|
||||
protected virtual void NavigationHandler()
|
||||
{
|
||||
}
|
||||
|
||||
private void HandleRefreshPath(TrafficSolution trafficSolution)
|
||||
{
|
||||
var edgesDto = trafficSolution.Edges.Select(e => new EdgeDto
|
||||
{
|
||||
Id = e.Id,
|
||||
StartNodeId = e.StartNodeId,
|
||||
EndNodeId = e.EndNodeId,
|
||||
TrajectoryDegree = e.TrajectoryDegree,
|
||||
ControlPoint1X = e.ControlPoint1X,
|
||||
ControlPoint1Y = e.ControlPoint1Y,
|
||||
ControlPoint2X = e.ControlPoint2X,
|
||||
ControlPoint2Y = e.ControlPoint2Y,
|
||||
});
|
||||
List<NodeDto> nodesDto = [..trafficSolution.Nodes.Select(n => new NodeDto
|
||||
{
|
||||
Id = n.Id,
|
||||
X = n.X,
|
||||
Y = n.Y,
|
||||
Name = n.Name,
|
||||
Direction = MapCompute.GetNodeDirection(n.Direction),
|
||||
})];
|
||||
var splitPath = PathPlanner.PathSplit([.. nodesDto], [.. edgesDto]);
|
||||
|
||||
if (splitPath.IsSuccess)
|
||||
{
|
||||
if (splitPath.Data != null)
|
||||
{
|
||||
RefreshPath = [..splitPath.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,
|
||||
})];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void HandleCompleteState(TrafficSolution trafficSolution)
|
||||
{
|
||||
if (trafficSolution.State == TrafficSolutionState.Complete && RefreshPath.Count > 0)
|
||||
{
|
||||
NavigationPath = RefreshPath;
|
||||
CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
|
||||
if (MovePurePursuit is not null)
|
||||
{
|
||||
MovePurePursuit.UpdatePath([.. NavigationPath]);
|
||||
MovePurePursuit.OnNodeIndex = MovePurePursuit.GetOnNode(Visualization.X, Visualization.Y).index;
|
||||
}
|
||||
RefreshPath = [];
|
||||
|
||||
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;
|
||||
TargetAngle = MathExtension.CheckLimit(CheckingNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward, 180, -180);
|
||||
Action = NavigationAction.Rotate;
|
||||
}
|
||||
}
|
||||
|
||||
private void HandleGivewayState(TrafficSolution trafficSolution)
|
||||
{
|
||||
if (trafficSolution.GivewayNodes[^1].Id == trafficSolution.ReleaseNode.Id && NavigationGoal?.Id != trafficSolution.ReleaseNode.Id)
|
||||
{
|
||||
var splitPath = PathPlanner.PathSplit([..trafficSolution.GivewayNodes.Select(n => new NodeDto
|
||||
{
|
||||
Id = n.Id,
|
||||
X = n.X,
|
||||
Y = n.Y,
|
||||
Name = n.Name,
|
||||
Direction = MapCompute.GetNodeDirection(n.Direction),
|
||||
})], [.. trafficSolution.GivewayEdges.Select(e => new EdgeDto
|
||||
{
|
||||
Id = e.Id,
|
||||
StartNodeId = e.StartNodeId,
|
||||
EndNodeId = e.EndNodeId,
|
||||
TrajectoryDegree = e.TrajectoryDegree,
|
||||
ControlPoint1X = e.ControlPoint1X,
|
||||
ControlPoint1Y = e.ControlPoint1Y,
|
||||
ControlPoint2X = e.ControlPoint2X,
|
||||
ControlPoint2Y = e.ControlPoint2Y,
|
||||
})]);
|
||||
if (splitPath.IsSuccess && splitPath.Data != null && MovePurePursuit != null)
|
||||
{
|
||||
NavigationPath = [..splitPath.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")];
|
||||
MovePurePursuit.UpdatePath([.. NavigationPath]);
|
||||
MovePurePursuit.OnNodeIndex = 0;
|
||||
|
||||
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;
|
||||
TargetAngle = MathExtension.CheckLimit(CheckingNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward, 180, -180);
|
||||
Action = NavigationAction.Rotate;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private async Task<Guid> RequestInACS(Dictionary<Guid, ZoneDto[]> newZones)
|
||||
{
|
||||
Guid trafficACSrelaseNodeId = Guid.Empty;
|
||||
foreach (var zone in newZones)
|
||||
{
|
||||
if (zone.Value.Length == 0) trafficACSrelaseNodeId = zone.Key;
|
||||
else
|
||||
{
|
||||
bool requestSuccess = true;
|
||||
foreach (var zoneACS in zone.Value)
|
||||
{
|
||||
if (string.IsNullOrEmpty(zoneACS.Name)) continue;
|
||||
if (CurrentZones.Any(z => z.Id == zoneACS.Id)) continue;
|
||||
|
||||
var getInTrafficACS = await TrafficACS.RequestIn(RobotModel.RobotId, zoneACS.Name);
|
||||
if (getInTrafficACS.IsSuccess && getInTrafficACS.Data) CurrentZones.Add(zoneACS);
|
||||
else
|
||||
{
|
||||
requestSuccess = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (requestSuccess) trafficACSrelaseNodeId = zone.Key;
|
||||
else break;
|
||||
}
|
||||
}
|
||||
return trafficACSrelaseNodeId;
|
||||
}
|
||||
|
||||
private void UpdateTraffic()
|
||||
{
|
||||
var trafficManagerGoalIndex = CheckingNodes.FindIndex(n => n.Id == TrafficManagerGoalId);
|
||||
var trafficACSGoalIndex = CheckingNodes.FindIndex(n => n.Id == TrafficACSGoalId);
|
||||
if (trafficManagerGoalIndex != -1 && trafficACSGoalIndex != -1)
|
||||
{
|
||||
var goalIndex = Math.Min(trafficManagerGoalIndex, trafficACSGoalIndex);
|
||||
NavigationNode goal = CheckingNodes[goalIndex];
|
||||
var inNodeIndex = CheckingNodes.FindIndex(n => n.Id == (InNode is null ? Guid.Empty : InNode.Id));
|
||||
inNodeIndex = inNodeIndex != -1 ? inNodeIndex : 0;
|
||||
if (BaseNodes.Count == 0 || BaseNodes[^1].Id != goal.Id) BaseNodes = CheckingNodes.GetRange(inNodeIndex, goalIndex + 1 - inNodeIndex);
|
||||
|
||||
if (IsWaittingStop)
|
||||
{
|
||||
if (NavigationState == NavigationStateType.Waitting)
|
||||
{
|
||||
TargetAngle = MathExtension.CheckLimit(RotateAngle, 180, -180);
|
||||
Action = NavigationAction.Rotate;
|
||||
IsWaittingStop = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int i = inNodeIndex + 1; i <= goalIndex; i++)
|
||||
{
|
||||
if (Math.Abs(CheckingNodes[i].Theta - CheckingNodes[i - 1].Theta) > 30)
|
||||
{
|
||||
goal = CheckingNodes[i];
|
||||
IsWaittingStop = true;
|
||||
RotateAngle = CheckingNodes[i].Theta;
|
||||
break;
|
||||
}
|
||||
}
|
||||
UpdateGoal(goal);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private async Task GoOutTrafficACS()
|
||||
{
|
||||
if (BaseNodes is null || BaseNodes.Count == 0 || InNode is null) return;
|
||||
var goalIndex = CheckingNodes.FindIndex(n => n.Id == BaseNodes[^1].Id);
|
||||
if (goalIndex == -1) return;
|
||||
var inNodeIndex = CheckingNodes.FindIndex(n => n.Id == InNode.Id);
|
||||
inNodeIndex = inNodeIndex != -1 ? inNodeIndex : 0;
|
||||
if (goalIndex <= inNodeIndex) return;
|
||||
var baseNodes = CheckingNodes.GetRange(inNodeIndex, goalIndex + 1 - inNodeIndex);
|
||||
var baseZones = PathPlanner.GetZones([..baseNodes.Select(n => n.ToNodeDto())], Zones);
|
||||
var outZones = CurrentZones.Where(z => !baseZones.Any(bz => bz.Id == z.Id)).ToList();
|
||||
foreach (var zoneACS in outZones)
|
||||
{
|
||||
if (string.IsNullOrEmpty(zoneACS.Name)) continue;
|
||||
var outTrafficACS = await TrafficACS.RequestOut(RobotModel.RobotId, zoneACS.Name);
|
||||
if (outTrafficACS.IsSuccess && outTrafficACS.Data) CurrentZones.RemoveAll(z => z.Id == zoneACS.Id);
|
||||
}
|
||||
}
|
||||
|
||||
protected virtual async Task CheckingHandler()
|
||||
{
|
||||
try
|
||||
{
|
||||
if (NavigationPath is null || InNode is null) throw new Exception("Đường đi không tồn tại");
|
||||
|
||||
await GoOutTrafficACS();
|
||||
|
||||
if (!TrafficManager.Enable)
|
||||
{
|
||||
if (TrafficManagerGoalId != CheckingNodes[^1].Id) TrafficManagerGoalId = CheckingNodes[^1].Id;
|
||||
}
|
||||
else
|
||||
{
|
||||
TrafficManager.UpdateInNode(RobotModel.RobotId, InNode.Id);
|
||||
var trafficSolution = TrafficManager.GetTrafficNode(RobotModel.RobotId);
|
||||
if (trafficSolution.State != TrafficSolutionState.RefreshPath && TrafficSolutionState == TrafficSolutionState.RefreshPath && trafficSolution.Edges.Length > 0 && trafficSolution.Nodes.Length > 1)
|
||||
{
|
||||
HandleRefreshPath(trafficSolution);
|
||||
}
|
||||
if (trafficSolution.State == TrafficSolutionState.Complete || trafficSolution.State == TrafficSolutionState.Waitting)
|
||||
{
|
||||
HandleCompleteState(trafficSolution);
|
||||
}
|
||||
else if (trafficSolution.State == TrafficSolutionState.GiveWay && trafficSolution.GivewayNodes.Count > 1 && trafficSolution.GivewayEdges.Count > 0)
|
||||
{
|
||||
HandleGivewayState(trafficSolution);
|
||||
}
|
||||
var goal = CheckingNodes.FirstOrDefault(n => n.Id == trafficSolution.ReleaseNode.Id);
|
||||
if (goal is not null && goal.Id != TrafficManagerGoalId) TrafficManagerGoalId = goal.Id;
|
||||
TrafficSolutionState = trafficSolution.State;
|
||||
}
|
||||
|
||||
if (!TrafficACS.Enable)
|
||||
{
|
||||
if (TrafficACSGoalId != CheckingNodes[^1].Id) TrafficACSGoalId = CheckingNodes[^1].Id;
|
||||
}
|
||||
else
|
||||
{
|
||||
var newZones = TrafficACS.GetZones(InNode?.Id ?? Guid.Empty, [.. CheckingNodes.Select(n => n.ToNodeDto())], Zones);
|
||||
var trafficACSrelaseNodeId = await RequestInACS(newZones);
|
||||
if (trafficACSrelaseNodeId != Guid.Empty && trafficACSrelaseNodeId != TrafficACSGoalId) TrafficACSGoalId = trafficACSrelaseNodeId;
|
||||
}
|
||||
UpdateTraffic();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Warning($"{RobotModel.RobotId} Checking ex: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
private NavigationPathEdge[] GetBasePath()
|
||||
{
|
||||
if (InNode is not null && BaseNodes is not null && BaseNodes.Count > 1)
|
||||
{
|
||||
var inNodeIndex = BaseNodes.FindIndex(n => n.Id == InNode.Id);
|
||||
if (inNodeIndex != -1 && inNodeIndex < BaseNodes.Count - 1)
|
||||
{
|
||||
var nodes = BaseNodes.GetRange(inNodeIndex, BaseNodes.Count - inNodeIndex);
|
||||
var edges = MapManager.GetEdges(MapId, [..nodes.Select(n => new NodeDto
|
||||
{
|
||||
Id = n.Id,
|
||||
})]);
|
||||
if (edges.Length == nodes.Count - 1 && nodes.Count > 1)
|
||||
{
|
||||
List<NavigationPathEdge> pathEdges = [];
|
||||
var edge = MapManager.GetEdge(nodes[0].Id, nodes[1].Id, MapId);
|
||||
if (edge is null)
|
||||
{
|
||||
pathEdges.Add(new()
|
||||
{
|
||||
StartX = Visualization.X,
|
||||
StartY = Visualization.Y,
|
||||
EndX = nodes[1].X,
|
||||
EndY = nodes[1].Y,
|
||||
Degree = 1,
|
||||
});
|
||||
}
|
||||
else
|
||||
{
|
||||
var splitStartPath = PathPlanner.PathSplit([new NodeDto
|
||||
{
|
||||
Id = nodes[0].Id,
|
||||
X = nodes[0].X,
|
||||
Y = nodes[0].Y,
|
||||
}, new NodeDto
|
||||
{
|
||||
Id = nodes[1].Id,
|
||||
X = nodes[1].X,
|
||||
Y = nodes[1].Y,
|
||||
Direction = MapCompute.GetNodeDirection(nodes[0].Direction),
|
||||
}], [edge]);
|
||||
if (splitStartPath.IsSuccess && splitStartPath.Data is not null)
|
||||
{
|
||||
int index = 0;
|
||||
double minDistance = double.MaxValue;
|
||||
for (int i = 0; i < splitStartPath.Data.Length; i++)
|
||||
{
|
||||
var distance = Math.Sqrt(Math.Pow(Visualization.X - splitStartPath.Data[i].X, 2) + Math.Pow(Visualization.Y - splitStartPath.Data[i].Y, 2));
|
||||
if (distance < minDistance)
|
||||
{
|
||||
minDistance = distance;
|
||||
index = i;
|
||||
}
|
||||
}
|
||||
for (int i = index; i < splitStartPath.Data.Length - 1; i++)
|
||||
{
|
||||
pathEdges.Add(new()
|
||||
{
|
||||
StartX = splitStartPath.Data[i].X,
|
||||
StartY = splitStartPath.Data[i].Y,
|
||||
EndX = splitStartPath.Data[i + 1].X,
|
||||
EndY = splitStartPath.Data[i + 1].Y,
|
||||
Degree = 1,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int i = 1; i < nodes.Count - 1; i++)
|
||||
{
|
||||
pathEdges.Add(new()
|
||||
{
|
||||
StartX = nodes[i].X,
|
||||
StartY = nodes[i].Y,
|
||||
EndX = nodes[i + 1].X,
|
||||
EndY = nodes[i + 1].Y,
|
||||
ControlPoint1X = edges[i].ControlPoint1X,
|
||||
ControlPoint1Y = edges[i].ControlPoint1Y,
|
||||
ControlPoint2X = edges[i].ControlPoint2X,
|
||||
ControlPoint2Y = edges[i].ControlPoint2Y,
|
||||
Degree = edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.One ? 1 : edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Two ? 2 : 3,
|
||||
});
|
||||
}
|
||||
return [.. pathEdges];
|
||||
}
|
||||
}
|
||||
}
|
||||
return [];
|
||||
}
|
||||
|
||||
private NavigationPathEdge[] GetFullPath()
|
||||
{
|
||||
if (InNode is not null && CheckingNodes is not null && CheckingNodes.Count > 1)
|
||||
{
|
||||
var inNodeIndex = CheckingNodes.FindIndex(n => n.Id == InNode.Id);
|
||||
if (inNodeIndex != -1 && inNodeIndex < CheckingNodes.Count - 1)
|
||||
{
|
||||
var nodes = CheckingNodes.GetRange(inNodeIndex, CheckingNodes.Count - inNodeIndex);
|
||||
var edges = MapManager.GetEdges(MapId, [..nodes.Select(n => new NodeDto
|
||||
{
|
||||
Id = n.Id,
|
||||
})]);
|
||||
if (edges.Length == nodes.Count - 1 && nodes.Count > 1)
|
||||
{
|
||||
List<NavigationPathEdge> pathEdges = [];
|
||||
var edge = MapManager.GetEdge(nodes[0].Id, nodes[1].Id, MapId);
|
||||
if (edge is null)
|
||||
{
|
||||
pathEdges.Add(new()
|
||||
{
|
||||
StartX = Visualization.X,
|
||||
StartY = Visualization.Y,
|
||||
EndX = nodes[1].X,
|
||||
EndY = nodes[1].Y,
|
||||
Degree = 1,
|
||||
});
|
||||
}
|
||||
else
|
||||
{
|
||||
var splitStartPath = PathPlanner.PathSplit([new NodeDto
|
||||
{
|
||||
Id = nodes[0].Id,
|
||||
X = nodes[0].X,
|
||||
Y = nodes[0].Y,
|
||||
}, new NodeDto
|
||||
{
|
||||
Id = nodes[1].Id,
|
||||
X = nodes[1].X,
|
||||
Y = nodes[1].Y,
|
||||
Direction = MapCompute.GetNodeDirection(nodes[0].Direction),
|
||||
}], [edge]);
|
||||
if (splitStartPath.IsSuccess && splitStartPath.Data is not null)
|
||||
{
|
||||
int index = 0;
|
||||
double minDistance = double.MaxValue;
|
||||
for (int i = 0; i < splitStartPath.Data.Length; i++)
|
||||
{
|
||||
var distance = Math.Sqrt(Math.Pow(Visualization.X - splitStartPath.Data[i].X, 2) + Math.Pow(Visualization.Y - splitStartPath.Data[i].Y, 2));
|
||||
if (distance < minDistance)
|
||||
{
|
||||
minDistance = distance;
|
||||
index = i;
|
||||
}
|
||||
}
|
||||
for (int i = index; i < splitStartPath.Data.Length - 1; i++)
|
||||
{
|
||||
pathEdges.Add(new()
|
||||
{
|
||||
StartX = splitStartPath.Data[i].X,
|
||||
StartY = splitStartPath.Data[i].Y,
|
||||
EndX = splitStartPath.Data[i + 1].X,
|
||||
EndY = splitStartPath.Data[i + 1].Y,
|
||||
Degree = 1,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int i = 1; i < nodes.Count - 1; i++)
|
||||
{
|
||||
pathEdges.Add(new()
|
||||
{
|
||||
StartX = nodes[i].X,
|
||||
StartY = nodes[i].Y,
|
||||
EndX = nodes[i + 1].X,
|
||||
EndY = nodes[i + 1].Y,
|
||||
ControlPoint1X = edges[i].ControlPoint1X,
|
||||
ControlPoint1Y = edges[i].ControlPoint1Y,
|
||||
ControlPoint2X = edges[i].ControlPoint2X,
|
||||
ControlPoint2Y = edges[i].ControlPoint2Y,
|
||||
Degree = edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.One ? 1 : edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Two ? 2 : 3,
|
||||
});
|
||||
}
|
||||
return [.. pathEdges];
|
||||
}
|
||||
}
|
||||
}
|
||||
return [];
|
||||
}
|
||||
|
||||
public bool UpdateGoal(NavigationNode goal)
|
||||
{
|
||||
if (NavigationGoal is null || NavigationGoal.Id != goal.Id)
|
||||
{
|
||||
if (NavigationPath is not null && NavigationPath.Any(node => node.Id == goal.Id))
|
||||
{
|
||||
NavigationGoal = goal;
|
||||
MovePurePursuit?.UpdateGoal(goal);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private void ClearPath()
|
||||
{
|
||||
NavigationPath = null;
|
||||
NavigationGoal = null;
|
||||
InNode = null;
|
||||
CheckingNodes.Clear();
|
||||
TrafficManager.DeleteAgent(RobotModel.RobotId);
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
CheckingStop();
|
||||
NavStop();
|
||||
ClearPath();
|
||||
NavigationState = NavigationStateType.Ready;
|
||||
VelocityController.Stop();
|
||||
IsCompleted = true;
|
||||
IsProcessing = false;
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
|
||||
protected void NavStart()
|
||||
{
|
||||
navTimer = new(SampleTimeMilliseconds, NavigationHandler, Logger);
|
||||
navTimer.Start();
|
||||
IsCompleted = false;
|
||||
IsCanceled = false;
|
||||
IsError = false;
|
||||
IsProcessing = true;
|
||||
}
|
||||
|
||||
protected void NavStop()
|
||||
{
|
||||
navTimer?.Dispose();
|
||||
}
|
||||
|
||||
protected void CheckingStart()
|
||||
{
|
||||
checkingTimer = new(CheckingTimeMilliseconds, CheckingHandler, Logger);
|
||||
checkingTimer.Start();
|
||||
}
|
||||
|
||||
public void CreateComledted()
|
||||
{
|
||||
IsCompleted = true;
|
||||
IsCanceled = false;
|
||||
IsError = false;
|
||||
}
|
||||
|
||||
protected void CheckingStop()
|
||||
{
|
||||
checkingTimer?.Dispose();
|
||||
}
|
||||
}
|
||||
340
RobotNet.RobotManager/Services/Simulation/RobotSimulation.cs
Normal file
340
RobotNet.RobotManager/Services/Simulation/RobotSimulation.cs
Normal file
@@ -0,0 +1,340 @@
|
||||
using Microsoft.EntityFrameworkCore;
|
||||
using RobotNet.MapShares;
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.MapShares.Enums;
|
||||
using RobotNet.RobotManager.Data;
|
||||
using RobotNet.RobotManager.Services.Planner.Space;
|
||||
using RobotNet.RobotManager.Services.Robot;
|
||||
using RobotNet.RobotManager.Services.Simulation.Models;
|
||||
using RobotNet.RobotManager.Services.Traffic;
|
||||
using RobotNet.RobotShares.Dtos;
|
||||
using RobotNet.RobotShares.VDA5050.Factsheet;
|
||||
using RobotNet.RobotShares.VDA5050.FactsheetExtend;
|
||||
using RobotNet.RobotShares.VDA5050.State;
|
||||
using RobotNet.RobotShares.VDA5050.Visualization;
|
||||
using RobotNet.Shares;
|
||||
using Action = RobotNet.RobotShares.VDA5050.InstantAction.Action;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Simulation;
|
||||
|
||||
public class RobotSimulation : IRobotController, IDisposable
|
||||
{
|
||||
public string SerialNumber { get; }
|
||||
public bool IsOnline { get; set; } = true;
|
||||
public bool IsWorking => NavigationService is not null && NavigationService.IsProcessing;
|
||||
public string State { get; } = "Simulation";
|
||||
public AutoResetEvent RobotUpdated { get; set; } = new(false);
|
||||
public string[] CurrentZones => NavigationService is null ? [] : [.. NavigationService.CurrentZones.Select(z => z.Name)];
|
||||
public StateMsg StateMsg { get => GetState(); set { } }
|
||||
public VisualizationMsg VisualizationMsg { get => GetVisualization(); set { } }
|
||||
public FactSheetMsg FactSheetMsg { get; set; } = new();
|
||||
public FactsheetExtendMsg FactsheetExtendMsg { get; set; } = new();
|
||||
public VisualizationService Visualization { get; set; }
|
||||
public NavigationPathEdge[] BasePath => NavigationService is null ? [] : NavigationService.BasePath;
|
||||
public NavigationPathEdge[] FullPath => NavigationService is null ? [] : NavigationService.FullPath;
|
||||
public RobotOrderDto OrderState => GetOrderState();
|
||||
public RobotActionDto[] ActionStates => [];
|
||||
|
||||
private INavigationService? NavigationService;
|
||||
private readonly IServiceProvider ServiceProvider;
|
||||
private readonly LoggerController<RobotSimulation> Logger;
|
||||
private readonly RobotSimulationModel RobotSimulationModel;
|
||||
private CancellationTokenSource? CancelRandom;
|
||||
|
||||
public RobotSimulation(string robotid, RobotModel model, IServiceProvider serviceProvider)
|
||||
{
|
||||
SerialNumber = robotid;
|
||||
RobotSimulationModel = new RobotSimulationModel()
|
||||
{
|
||||
RobotId = robotid,
|
||||
Acceleration = 2,
|
||||
Deceleration = 1,
|
||||
Length = model.Length / 2,
|
||||
Width = model.Width,
|
||||
MaxAngularVelocity = 0.3,
|
||||
MaxVelocity = 1.5,
|
||||
RadiusWheel = 0.1,
|
||||
NavigationType = model.NavigationType,
|
||||
};
|
||||
Visualization = new VisualizationService().WithRadiusWheel(RobotSimulationModel.RadiusWheel).WithRadiusRobot(RobotSimulationModel.Width);
|
||||
ServiceProvider = serviceProvider;
|
||||
Logger = ServiceProvider.GetRequiredService<LoggerController<RobotSimulation>>();
|
||||
}
|
||||
|
||||
public void Initialize(double x, double y, double theta) => Visualization.LocalizationInitialize(x, y, theta);
|
||||
|
||||
public MessageResult Rotate(double angle)
|
||||
{
|
||||
if (NavigationService is not null && NavigationService.NavigationState != NavigationStateType.Ready) return new(false, "Robot chưa sẵn sàng thực hiện nhiệm vụ");
|
||||
var olderNavigationService = NavigationService;
|
||||
NavigationService = NavigationManager.GetNavigation(Visualization, RobotSimulationModel, ServiceProvider);
|
||||
if (NavigationService is not null)
|
||||
{
|
||||
NavigationService.CurrentZones = olderNavigationService is null ? [] : olderNavigationService.CurrentZones;
|
||||
return NavigationService.Rotate(angle);
|
||||
}
|
||||
return new(false, "Model chưa được thiết lập");
|
||||
}
|
||||
|
||||
public MessageResult MoveStraight(double x, double y)
|
||||
{
|
||||
try
|
||||
{
|
||||
var scope = ServiceProvider.CreateAsyncScope();
|
||||
var PathPlanningService = scope.ServiceProvider.GetRequiredService<PathPlanner>();
|
||||
var headRobotNode = new NodeDto()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
X = Visualization.X * Math.Acos(Visualization.Theta * Math.PI / 180),
|
||||
Y = Visualization.Y * Math.Asin(Visualization.Theta * Math.PI / 180),
|
||||
};
|
||||
var currentRobotNode = new NodeDto()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
X = Visualization.X,
|
||||
Y = Visualization.Y,
|
||||
};
|
||||
var goalNode = new NodeDto()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
X = x,
|
||||
Y = y,
|
||||
};
|
||||
goalNode.Theta = MapEditorHelper.GetAngle(currentRobotNode, headRobotNode, goalNode) > 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;
|
||||
NodeDto[] nodesplanning = [currentRobotNode, goalNode];
|
||||
EdgeDto[] edgesplanning = [new()
|
||||
{
|
||||
Id= Guid.NewGuid(),
|
||||
DirectionAllowed = DirectionAllowed.Both,
|
||||
TrajectoryDegree = TrajectoryDegree.One,
|
||||
StartNodeId = currentRobotNode.Id,
|
||||
EndNodeId = goalNode.Id
|
||||
}];
|
||||
var resultSplit = PathPlanningService.PathSplit(nodesplanning, edgesplanning);
|
||||
if (!resultSplit.IsSuccess) return new(false, resultSplit.Message);
|
||||
if (resultSplit.Data is null || resultSplit.Data.Length < 1) return new(false, "Không tìm thấy đường dẫn tới đích");
|
||||
if (resultSplit.Data.Length < 1) return new(false, "Dữ liệu truyền vào không đúng");
|
||||
var navigationPath = resultSplit.Data.Select(n => new NavigationNode()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
X = n.X,
|
||||
Y = n.Y,
|
||||
Theta = n.Theta,
|
||||
Direction = n.Direction == Direction.FORWARD ? RobotShares.Enums.RobotDirection.FORWARD : n.Direction == Direction.BACKWARD ? RobotShares.Enums.RobotDirection.BACKWARD : RobotShares.Enums.RobotDirection.NONE,
|
||||
Actions = n.Actions,
|
||||
}).ToArray();
|
||||
|
||||
if (NavigationService is not null && NavigationService.NavigationState != NavigationStateType.Ready) return new(false, "Robot chưa sẵn sàng thực hiện nhiệm vụ");
|
||||
var olderNavigationService = NavigationService;
|
||||
NavigationService = NavigationManager.GetNavigation(Visualization, RobotSimulationModel, ServiceProvider);
|
||||
if (NavigationService is not null)
|
||||
{
|
||||
NavigationService.CurrentZones = olderNavigationService is null ? [] : olderNavigationService.CurrentZones;
|
||||
var moveTask = NavigationService.MoveStraight(navigationPath);
|
||||
return moveTask;
|
||||
}
|
||||
return new(false, "Model chưa được thiết lập");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return new(false, ex.Message);
|
||||
}
|
||||
}
|
||||
|
||||
public Task<MessageResult> CancelOrder()
|
||||
{
|
||||
NavigationService?.Cancel();
|
||||
CancelRandom?.Cancel();
|
||||
return Task.FromResult<MessageResult>(new(true));
|
||||
}
|
||||
|
||||
public async Task<MessageResult> MoveToNode(string goalName, IDictionary<string, IEnumerable<Action>>? actions = null, double? lastAngle = null)
|
||||
{
|
||||
try
|
||||
{
|
||||
var scope = ServiceProvider.CreateAsyncScope();
|
||||
var PathPlanner = scope.ServiceProvider.GetRequiredService<PathPlanner>();
|
||||
var AppDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
|
||||
var TrafficManager = ServiceProvider.GetRequiredService<TrafficManager>();
|
||||
var robotDb = await AppDb.Robots.FirstOrDefaultAsync(r => r.RobotId == SerialNumber);
|
||||
if (robotDb is null) return new(false, "RobotId không tồn tại");
|
||||
var robotModelDb = await AppDb.RobotModels.FirstOrDefaultAsync(r => r.Id == robotDb.ModelId);
|
||||
if (robotModelDb is null) return new(false, "Robot Model không tồn tại");
|
||||
|
||||
var path = await PathPlanner.Planning(Visualization.X, Visualization.Y, Visualization.Theta, RobotSimulationModel.NavigationType, robotDb.MapId, goalName);
|
||||
if (!path.IsSuccess) return new(false, path.Message);
|
||||
if (path.IsSuccess && path.Data.Nodes.Length < 2)
|
||||
{
|
||||
NavigationService?.CreateComledted();
|
||||
return new(true, "");
|
||||
}
|
||||
if (path.Data.Nodes is null || path.Data.Edges is null) return new(false, $"Đường dẫn tới đích {goalName} từ [{Visualization.X} - {Visualization.Y} - {Visualization.Theta}] không tồn tại");
|
||||
|
||||
if (NavigationService is not null && NavigationService.NavigationState != NavigationStateType.Ready) return new(false, "Robot chưa sẵn sàng thực hiện nhiệm vụ");
|
||||
var olderNavigationService = NavigationService;
|
||||
NavigationService = NavigationManager.GetNavigation(Visualization, RobotSimulationModel, ServiceProvider);
|
||||
if (NavigationService is not null)
|
||||
{
|
||||
var nodes = path.Data.Nodes;
|
||||
var createAgent = TrafficManager.CreateAgent(robotDb.MapId, this, new()
|
||||
{
|
||||
NavigationType = robotModelDb.NavigationType,
|
||||
Length = robotModelDb.Length,
|
||||
Width = robotModelDb.Width,
|
||||
NavigationPointX = robotModelDb.OriginX,
|
||||
NavigationPointY = robotModelDb.OriginY,
|
||||
},
|
||||
[..nodes.Select(n => new TrafficNodeDto()
|
||||
{
|
||||
Id = n.Id,
|
||||
Name = n.Name,
|
||||
X = n.X,
|
||||
Y = n.Y,
|
||||
Theta = n.Theta,
|
||||
Direction = MapCompute.GetRobotDirection(n.Direction),
|
||||
})], [..path.Data.Edges.Select(n => new TrafficEdgeDto()
|
||||
{
|
||||
Id = n.Id,
|
||||
StartNodeId = n.StartNodeId,
|
||||
EndNodeId = n.EndNodeId,
|
||||
TrajectoryDegree = n.TrajectoryDegree,
|
||||
ControlPoint1X = n.ControlPoint1X,
|
||||
ControlPoint1Y = n.ControlPoint1Y,
|
||||
ControlPoint2X = n.ControlPoint2X,
|
||||
ControlPoint2Y = n.ControlPoint2Y,
|
||||
})]);
|
||||
if (!createAgent.IsSuccess)
|
||||
{
|
||||
Logger.Warning($"{SerialNumber} - Không thể tạo traffic agent: {createAgent.Message}");
|
||||
return new(false, $"Không thể tạo traffic agent: {createAgent.Message}");
|
||||
}
|
||||
NavigationService.MapId = robotDb.MapId;
|
||||
NavigationService.CurrentZones = olderNavigationService is null ? [] : olderNavigationService.CurrentZones;
|
||||
var moveTask = NavigationService.Move(nodes, path.Data.Edges);
|
||||
return moveTask;
|
||||
}
|
||||
return new(false, "Model chưa được thiết lập");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return new(false, ex.Message);
|
||||
}
|
||||
}
|
||||
|
||||
private VisualizationMsg GetVisualization()
|
||||
{
|
||||
var scope = ServiceProvider.CreateAsyncScope();
|
||||
var AppDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
|
||||
var robotDb = AppDb.Robots.FirstOrDefault(r => r.RobotId == SerialNumber);
|
||||
return new()
|
||||
{
|
||||
SerialNumber = SerialNumber,
|
||||
MapId = robotDb is null ? string.Empty : robotDb.MapId.ToString(),
|
||||
AgvPosition = new()
|
||||
{
|
||||
X = Visualization.X,
|
||||
Y = Visualization.Y,
|
||||
Theta = Visualization.Theta,
|
||||
PositionInitialized = true,
|
||||
LocalizationScore = 100,
|
||||
DeviationRange = 100,
|
||||
},
|
||||
Velocity = new()
|
||||
{
|
||||
Vx = Visualization.Vx,
|
||||
Vy = Visualization.Vy,
|
||||
Omega = Visualization.Omega,
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
private StateMsg GetState()
|
||||
{
|
||||
return new()
|
||||
{
|
||||
SerialNumber = SerialNumber,
|
||||
Timestamp = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
|
||||
NewBaseRequest = true,
|
||||
BatteryState = new()
|
||||
{
|
||||
BatteryHealth = 100,
|
||||
BatteryCharge = 0,
|
||||
BatteryVoltage = 24,
|
||||
Charging = false,
|
||||
},
|
||||
ActionStates = [],
|
||||
Errors = [],
|
||||
Information = [],
|
||||
NodeStates = [],
|
||||
Loads = [],
|
||||
};
|
||||
}
|
||||
|
||||
public Task<MessageResult<string>> InstantAction(Action action, bool waittingFeedback)
|
||||
{
|
||||
return Task.FromResult<MessageResult<string>>(new(true));
|
||||
}
|
||||
|
||||
public void Log(string message, LogLevel level = LogLevel.Information)
|
||||
{
|
||||
switch (level)
|
||||
{
|
||||
case LogLevel.Trace: Logger.Trace($"{SerialNumber} - {message}"); break;
|
||||
case LogLevel.Error: Logger.Error($"{SerialNumber} - {message}"); break;
|
||||
case LogLevel.Warning: Logger.Warning($"{SerialNumber} - {message}"); break;
|
||||
case LogLevel.Critical: Logger.Critical($"{SerialNumber} - {message}"); break;
|
||||
case LogLevel.Information: Logger.Info($"{SerialNumber} - {message}"); break;
|
||||
case LogLevel.Debug: Logger.Debug($"{SerialNumber} - {message}"); break;
|
||||
default: Logger.Debug($"{SerialNumber} - {message}"); break;
|
||||
}
|
||||
}
|
||||
|
||||
public Task<MessageResult> MoveRandom(List<string> nodes)
|
||||
{
|
||||
try
|
||||
{
|
||||
CancelRandom = new CancellationTokenSource();
|
||||
var random = new Random();
|
||||
var randomTask = Task.Run(async () =>
|
||||
{
|
||||
while (!CancelRandom.IsCancellationRequested)
|
||||
{
|
||||
var index = random.Next(nodes.Count);
|
||||
await MoveToNode(nodes[index]);
|
||||
while (!CancelRandom.IsCancellationRequested)
|
||||
{
|
||||
if (!IsWorking) break;
|
||||
await Task.Delay(1000);
|
||||
}
|
||||
await Task.Delay(2000);
|
||||
}
|
||||
}, cancellationToken: CancelRandom.Token);
|
||||
}
|
||||
catch { }
|
||||
return Task.FromResult<MessageResult>(new(true));
|
||||
}
|
||||
|
||||
public Task<MessageResult> CancelAction()
|
||||
{
|
||||
return Task.FromResult<MessageResult>(new(true, ""));
|
||||
}
|
||||
|
||||
public RobotOrderDto GetOrderState()
|
||||
{
|
||||
return new()
|
||||
{
|
||||
OrderId = StateMsg.OrderId,
|
||||
IsCompleted = NavigationService is not null && NavigationService.IsCompleted,
|
||||
IsError = NavigationService is not null && NavigationService.IsError,
|
||||
IsProcessing = NavigationService is not null && NavigationService.IsProcessing,
|
||||
IsCanceled = NavigationService is not null && NavigationService.IsCanceled,
|
||||
Errors = NavigationService is not null ? NavigationService.Errors : [],
|
||||
};
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,75 @@
|
||||
namespace RobotNet.RobotManager.Services.Simulation;
|
||||
|
||||
public class VelocityController(VisualizationService Visualization)
|
||||
{
|
||||
private double Acceleration = 2;
|
||||
private double Deceleration = 10;
|
||||
private double AngularVelLeft;
|
||||
private double AngularVelRight;
|
||||
private double SampleTime = 0.05;
|
||||
|
||||
public VelocityController WithAcceleration(double acc)
|
||||
{
|
||||
Acceleration = acc;
|
||||
return this;
|
||||
}
|
||||
|
||||
public VelocityController WithDeceleration(double dec)
|
||||
{
|
||||
Deceleration = dec;
|
||||
return this;
|
||||
}
|
||||
|
||||
public VelocityController WithSampleTime(double time)
|
||||
{
|
||||
SampleTime = time;
|
||||
return this;
|
||||
}
|
||||
|
||||
private (double angularVelLeft, double angularVelRight) AccelerationCalculator(double wL, double wR, double wL_Current, double wR_Current)
|
||||
{
|
||||
var angularVelLeft = wL_Current;
|
||||
var angularVelRight = wR_Current;
|
||||
if (wL_Current == 0 || wL / wL_Current < 0)
|
||||
{
|
||||
if (wL != 0) angularVelLeft += wL / Math.Abs(wL) * Acceleration;
|
||||
else angularVelLeft = wL;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (Math.Abs(wL) - Math.Abs(wL_Current) > Acceleration) angularVelLeft += Acceleration * wL_Current / Math.Abs(wL_Current);
|
||||
else if (Math.Abs(wL_Current) - Math.Abs(wL) > Deceleration) angularVelLeft -= Deceleration * wL_Current / Math.Abs(wL_Current);
|
||||
else angularVelLeft = wL;
|
||||
}
|
||||
|
||||
if (wR_Current == 0 || wR / wR_Current < 0)
|
||||
{
|
||||
if (wR != 0) angularVelRight += wR / Math.Abs(wR) * Acceleration;
|
||||
else angularVelRight = wR;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (Math.Abs(wR) - Math.Abs(wR_Current) > Acceleration) angularVelRight += Acceleration * wR_Current / Math.Abs(wR_Current);
|
||||
else if (Math.Abs(wR_Current) - Math.Abs(wR) > Deceleration) angularVelRight -= Deceleration * wR_Current / Math.Abs(wR_Current);
|
||||
else angularVelRight = wR;
|
||||
}
|
||||
|
||||
if (Math.Abs(angularVelLeft) > Math.Abs(wL)) angularVelLeft = wL;
|
||||
if (Math.Abs(angularVelRight) > Math.Abs(wR)) angularVelRight = wR;
|
||||
return (angularVelLeft, angularVelRight);
|
||||
}
|
||||
|
||||
public bool SetSpeed(double angularVelLeft, double angularVelRight)
|
||||
{
|
||||
(AngularVelLeft, AngularVelRight) = AccelerationCalculator(angularVelLeft, angularVelRight, AngularVelLeft, AngularVelRight);
|
||||
//Console.WriteLine($"AngularVelLeft = {AngularVelLeft:0.####}, AngularVelRight = {AngularVelRight:0.####}");
|
||||
_ = Visualization.UpdatePosition(AngularVelLeft, AngularVelRight, SampleTime);
|
||||
return true;
|
||||
}
|
||||
|
||||
public void Stop()
|
||||
{
|
||||
(AngularVelLeft, AngularVelRight) = (0, 0);
|
||||
_ = Visualization.UpdatePosition(AngularVelLeft, AngularVelRight, SampleTime);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
using RobotNet.RobotManager.Services.Simulation.Algorithm;
|
||||
using RobotNet.RobotManager.Services.Simulation.Models;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Simulation;
|
||||
|
||||
public class VisualizationService
|
||||
{
|
||||
public double X { get; private set; }
|
||||
public double Y { get; private set; }
|
||||
public double Theta { get; private set; }
|
||||
public double Vx { get; private set; }
|
||||
public double Vy { get; private set; }
|
||||
public double Omega { get; private set; }
|
||||
|
||||
private double RadiusWheel;
|
||||
private double RadiusRobot;
|
||||
|
||||
public VisualizationService WithRadiusWheel(double radiusWheel)
|
||||
{
|
||||
RadiusWheel = radiusWheel;
|
||||
return this;
|
||||
}
|
||||
|
||||
public VisualizationService WithRadiusRobot(double radiusRobot)
|
||||
{
|
||||
RadiusRobot = radiusRobot;
|
||||
return this;
|
||||
}
|
||||
|
||||
public (double x, double y, double angle) UpdatePosition(double wL, double wR, double time)
|
||||
{
|
||||
Theta = (Theta + time * (-wR - wL) * RadiusWheel / RadiusRobot * MathExtension.ToDegree) % 360;
|
||||
X += time * (-wR + wL) * RadiusWheel * Math.Cos(Theta * MathExtension.ToRad) / 2;
|
||||
Y += time * (-wR + wL) * RadiusWheel * Math.Sin(Theta * MathExtension.ToRad) / 2;
|
||||
_ = UpdateVelocity(wL, wR);
|
||||
if (Theta > 180) Theta -= 360;
|
||||
else if (Theta < -180) Theta += 360;
|
||||
return (X, Y, Theta);
|
||||
}
|
||||
|
||||
public (double vx, double vy, double omega) UpdateVelocity(double wL, double wR)
|
||||
{
|
||||
Vx = (-wR + wL) * RadiusWheel / 2;
|
||||
Omega = (-wR - wL) * RadiusWheel / RadiusRobot;
|
||||
return (Vx, 0, Omega);
|
||||
}
|
||||
|
||||
public void LocalizationInitialize(double x, double y, double theta)
|
||||
{
|
||||
X = x;
|
||||
Y = y;
|
||||
Theta = theta;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user