340 lines
14 KiB
C#
340 lines
14 KiB
C#
using RobotNet.VDA5050.Order;
|
|
using RobotNet10.RobotApp.Detection;
|
|
using RobotNet10.RobotApp.Interfaces;
|
|
using RobotNet10.RobotApp.Services.ConfigManager;
|
|
using RobotNet10.RobotApp.Services.Exceptions;
|
|
using RobotNet10.RobotApp.Services.Simulation;
|
|
using RobotNet10.RobotApp.Shared.Enums;
|
|
|
|
namespace RobotNet10.RobotApp.Services.Robot.Modules;
|
|
|
|
public class RobotNavigation(
|
|
IRobotConfiguration robotConfiguration,
|
|
IServiceProvider serviceProvider,
|
|
RobotNet10.RobotApp.Navigation.NavigationIntegrationService navigationIntegrationService,
|
|
ILogger<RobotNavigation> logger) : INavigation
|
|
{
|
|
public bool IsReady { get; private set; }
|
|
private bool _navResultSubscribed;
|
|
|
|
public bool Driving
|
|
{
|
|
get
|
|
{
|
|
if (IsSimulation)
|
|
return SimNavigation?.Driving ?? false;
|
|
|
|
var feedback = navigationIntegrationService.GetFeedback();
|
|
if (feedback == null) return false;
|
|
return feedback.NavigationState is RobotNet10.RobotApp.Navigation.NavigationState.Controlling;
|
|
// return feedback.NavigationState is RobotNet10.RobotApp.Navigation.NavigationState.Active
|
|
// or RobotNet10.RobotApp.Navigation.NavigationState.Planning
|
|
// or RobotNet10.RobotApp.Navigation.NavigationState.Controlling;
|
|
}
|
|
}
|
|
|
|
public double VelocityX => IsSimulation ? (SimNavigation?.VelocityX ?? 0) : (navigationIntegrationService.GetTwist()?.x ?? 0);
|
|
public double VelocityY => IsSimulation ? (SimNavigation?.VelocityY ?? 0) : (navigationIntegrationService.GetTwist()?.y ?? 0);
|
|
public double Omega => IsSimulation ? (SimNavigation?.Omega ?? 0) : (navigationIntegrationService.GetTwist()?.theta ?? 0);
|
|
public RobotNet10.RobotApp.Interfaces.NavigationState State => _lastFinishedState ?? (IsSimulation ? (SimNavigation?.State ?? RobotNet10.RobotApp.Interfaces.NavigationState.Idle) : MapNavigationState(navigationIntegrationService.GetFeedback()?.NavigationState));
|
|
|
|
public IReadOnlyList<NavigationNode>? CurrentPath
|
|
{
|
|
get
|
|
{
|
|
if (IsSimulation)
|
|
return null;
|
|
|
|
var globalPath = navigationIntegrationService.GetGlobalPathData();
|
|
if (globalPath == null || globalPath.Points.Count == 0)
|
|
return null;
|
|
|
|
return globalPath.Points.Select(p => new NavigationNode
|
|
{
|
|
Id = Guid.NewGuid(),
|
|
X = p.X,
|
|
Y = p.Y,
|
|
Theta = p.Theta
|
|
}).ToList();
|
|
}
|
|
}
|
|
|
|
// C API navigation currently does not expose these dock monitoring values.
|
|
public bool IsDockingActive => false;
|
|
public NavigationNode? DockGoal => null;
|
|
public string DockPhase => string.Empty;
|
|
public string DockDirection => string.Empty;
|
|
public int DockRetryCount => 0;
|
|
public int DockMaxRetries => 0;
|
|
public int DockWaypointCount => 0;
|
|
public NavigationNode? DockStartNode => null;
|
|
public IReadOnlyList<NavigationNode>? DockWaypoints => null;
|
|
|
|
private volatile SimulationNavigation? SimNavigation;
|
|
private RobotNet10.RobotApp.Interfaces.NavigationState? _lastFinishedState;
|
|
private bool IsSimulation => robotConfiguration.GetSimulationConfig().IsEnable;
|
|
|
|
public event Action<RobotNet10.RobotApp.Interfaces.NavigationState>? OnNavigationFinished;
|
|
|
|
public void CancelMovement()
|
|
{
|
|
if (IsSimulation)
|
|
{
|
|
SimNavigation?.CancelMovement();
|
|
return;
|
|
}
|
|
|
|
navigationIntegrationService.Cancel();
|
|
}
|
|
|
|
public void Move(OrderMsg order, bool hasLoad = false)
|
|
{
|
|
_lastFinishedState = null;
|
|
var nodes = order.Nodes;
|
|
var edges = order.Edges;
|
|
|
|
if (IsSimulation)
|
|
{
|
|
if (SimNavigation is not null) throw new NavigationException("The Sim Navigation module is called during operation.");
|
|
SimNavigation = SimulationNavigationManager.GetNavigation(robotConfiguration.GetRobotPhysicalConfig().NavigationType, serviceProvider);
|
|
SimNavigation.OnNavigationFinished += NavigationFinished;
|
|
SimNavigation.Move(order, hasLoad);
|
|
return;
|
|
}
|
|
|
|
if (nodes.Length == 0)
|
|
throw new NavigationException("Move failed: nodes list is empty.");
|
|
|
|
var target = nodes[^1];
|
|
var (targetX, targetY, theta) = GetNodePose(target, nodes);
|
|
var (qz, qw) = ToYawQuaternion(theta);
|
|
|
|
// Convert VDA5050 order (from MQTT server) to OrderData and run full graph navigation
|
|
var orderData = RobotNet10.RobotApp.Navigation.VDA5050ToOrderDataConverter.ToOrderData(nodes, edges, orderMsg: order);
|
|
if (!navigationIntegrationService.MoveToOrder(orderData, targetX, targetY, 0.0, 0.0, 0.0, qz, qw))
|
|
throw new NavigationException("Move failed: Navigation C API service is not ready or rejected goal.");
|
|
}
|
|
|
|
public void MoveStraight(double x, double y, bool hasLoad = false, RobotDirection? direction = null)
|
|
{
|
|
if (IsSimulation)
|
|
{
|
|
if (SimNavigation is not null) throw new NavigationException("The Sim Navigation module is called during operation.");
|
|
SimNavigation = SimulationNavigationManager.GetNavigation(robotConfiguration.GetRobotPhysicalConfig().NavigationType, serviceProvider);
|
|
SimNavigation.OnNavigationFinished += NavigationFinished;
|
|
SimNavigation.MoveStraight(x, y, hasLoad, direction);
|
|
return;
|
|
}
|
|
|
|
var current = navigationIntegrationService.GetRobotPose2D();
|
|
var currentX = current?.x ?? 0.0;
|
|
var currentY = current?.y ?? 0.0;
|
|
var heading = Math.Atan2(y - currentY, x - currentX);
|
|
var (qz, qw) = ToYawQuaternion(heading);
|
|
|
|
if (!navigationIntegrationService.MoveTo(x, y, 0.0, 0.0, 0.0, qz, qw))
|
|
throw new NavigationException("MoveStraight failed: Navigation C API service is not ready or rejected goal.");
|
|
}
|
|
|
|
public void Pause()
|
|
{
|
|
if (IsSimulation) SimNavigation?.Pause();
|
|
else navigationIntegrationService.Pause();
|
|
}
|
|
|
|
public void Resume()
|
|
{
|
|
if (IsSimulation) SimNavigation?.Resume();
|
|
else navigationIntegrationService.Resume();
|
|
}
|
|
|
|
public void Rotate(double angle)
|
|
{
|
|
_lastFinishedState = null;
|
|
|
|
if (IsSimulation)
|
|
{
|
|
if (SimNavigation is not null) throw new NavigationException("The Sim Navigation module is called during operation.");
|
|
SimNavigation = SimulationNavigationManager.GetNavigation(robotConfiguration.GetRobotPhysicalConfig().NavigationType, serviceProvider);
|
|
SimNavigation.OnNavigationFinished += NavigationFinished;
|
|
SimNavigation.Rotate(angle * 180 / Math.PI);
|
|
return;
|
|
}
|
|
|
|
var pose = navigationIntegrationService.GetRobotPose2D();
|
|
var x = pose?.x ?? 0.0;
|
|
var y = pose?.y ?? 0.0;
|
|
var (qz, qw) = ToYawQuaternion(angle);
|
|
|
|
if (!navigationIntegrationService.RotateTo(x, y, 0.0, 0.0, 0.0, qz, qw))
|
|
throw new NavigationException("Rotate failed: Navigation C API service is not ready or rejected goal.");
|
|
}
|
|
|
|
public void DockTo(IDetectSession session, bool hasLoad = false, RobotDirection? direction = null)
|
|
{
|
|
_lastFinishedState = null;
|
|
|
|
if (IsSimulation)
|
|
{
|
|
if (SimNavigation is not null) throw new NavigationException("The Sim Navigation module is called during operation.");
|
|
SimNavigation = SimulationNavigationManager.GetNavigation(robotConfiguration.GetRobotPhysicalConfig().NavigationType, serviceProvider);
|
|
SimNavigation.OnNavigationFinished += NavigationFinished;
|
|
SimNavigation.DockTo(session, hasLoad, direction);
|
|
return;
|
|
}
|
|
|
|
var goal = session.Goal ?? throw new NavigationException("DockTo failed: session goal is missing.");
|
|
var markerName = "dock-marker";
|
|
var p = goal.Pose.Position;
|
|
var o = goal.Pose.Orientation;
|
|
|
|
if (!navigationIntegrationService.DockTo(markerName, p.X, p.Y, p.Z, o.X, o.Y, o.Z, o.W))
|
|
throw new NavigationException("DockTo failed: Navigation C API service is not ready or rejected goal.");
|
|
}
|
|
|
|
public void RefreshOrder(Node[] nodes, Edge[] edges)
|
|
{
|
|
logger.LogWarning("RefreshOrder is not yet implemented for C API navigation path.");
|
|
}
|
|
|
|
public void UpdateOrder(string lastBaseNodeId)
|
|
{
|
|
if (IsSimulation)
|
|
{
|
|
SimNavigation?.UpdateOrder(lastBaseNodeId);
|
|
return;
|
|
}
|
|
|
|
logger.LogDebug("UpdateOrder called in C API mode with lastBaseNodeId={LastBaseNodeId}.", lastBaseNodeId);
|
|
}
|
|
|
|
public void SafetyStop()
|
|
{
|
|
if (IsSimulation) SimNavigation?.SafetyStop();
|
|
else navigationIntegrationService.Cancel();
|
|
}
|
|
|
|
public void Refresh()
|
|
{
|
|
if (IsSimulation) SimNavigation?.Refresh();
|
|
}
|
|
|
|
private void NavigationFinished(RobotNet10.RobotApp.Interfaces.NavigationState state)
|
|
{
|
|
_lastFinishedState = state;
|
|
OnNavigationFinished?.Invoke(state);
|
|
|
|
if (IsSimulation) SimNavigation?.OnNavigationFinished -= NavigationFinished;
|
|
SimNavigation = null;
|
|
}
|
|
|
|
public void SetSpeed(double speed)
|
|
{
|
|
if (IsSimulation) SimNavigation?.SetSpeed(speed);
|
|
else
|
|
{
|
|
logger.LogInformation("SetSpeed called with speed={Speed}", speed);
|
|
if (!navigationIntegrationService.SetTwistLinear(speed, 0.0, 0.0))
|
|
throw new NavigationException($"SetSpeed failed: unable to set linear velocity to {speed} via Navigation C API.");
|
|
}
|
|
}
|
|
|
|
public void Start()
|
|
{
|
|
IsReady = IsSimulation || navigationIntegrationService.IsInitialized;
|
|
if (!IsSimulation && !_navResultSubscribed)
|
|
{
|
|
navigationIntegrationService.OnNavigationResult += OnNavigationResultReceived;
|
|
_navResultSubscribed = true;
|
|
}
|
|
}
|
|
|
|
private void OnNavigationResultReceived(RobotNet10.RobotApp.Navigation.NavigationState state)
|
|
{
|
|
var mapped = MapNavigationState(state);
|
|
NavigationFinished(mapped);
|
|
}
|
|
|
|
public void Stop()
|
|
{
|
|
if (SimNavigation is not null)
|
|
{
|
|
SimNavigation.CancelMovement();
|
|
}
|
|
else
|
|
{
|
|
navigationIntegrationService.Cancel();
|
|
}
|
|
}
|
|
|
|
private static (double qz, double qw) ToYawQuaternion(double yaw)
|
|
{
|
|
var half = yaw / 2.0;
|
|
return (Math.Sin(half), Math.Cos(half));
|
|
}
|
|
|
|
private static (double x, double y, double theta) GetNodePose(Node target, Node[] allNodes)
|
|
{
|
|
var (x, y, thetaOpt) = ExtractNodePosition(target);
|
|
if (thetaOpt.HasValue)
|
|
return (x, y, thetaOpt.Value);
|
|
|
|
if (allNodes.Length >= 2)
|
|
{
|
|
var (prevX, prevY, _) = ExtractNodePosition(allNodes[^2]);
|
|
return (x, y, Math.Atan2(y - prevY, x - prevX));
|
|
}
|
|
|
|
return (x, y, 0.0);
|
|
}
|
|
|
|
private static (double x, double y, double? theta) ExtractNodePosition(Node node)
|
|
{
|
|
// VDA5050 Node may store coordinates in NodePosition, while legacy models may use X/Y/Theta directly.
|
|
var nodeType = node.GetType();
|
|
|
|
var nodePosProp = nodeType.GetProperty("NodePosition");
|
|
if (nodePosProp?.GetValue(node) is object nodePos)
|
|
{
|
|
var posType = nodePos.GetType();
|
|
var xObj = posType.GetProperty("X")?.GetValue(nodePos);
|
|
var yObj = posType.GetProperty("Y")?.GetValue(nodePos);
|
|
var thetaObj = posType.GetProperty("Theta")?.GetValue(nodePos);
|
|
|
|
return (
|
|
xObj is null ? 0.0 : Convert.ToDouble(xObj),
|
|
yObj is null ? 0.0 : Convert.ToDouble(yObj),
|
|
thetaObj is null ? null : Convert.ToDouble(thetaObj));
|
|
}
|
|
|
|
var xLegacy = nodeType.GetProperty("X")?.GetValue(node);
|
|
var yLegacy = nodeType.GetProperty("Y")?.GetValue(node);
|
|
var thetaLegacy = nodeType.GetProperty("Theta")?.GetValue(node);
|
|
|
|
return (
|
|
xLegacy is null ? 0.0 : Convert.ToDouble(xLegacy),
|
|
yLegacy is null ? 0.0 : Convert.ToDouble(yLegacy),
|
|
thetaLegacy is null ? null : Convert.ToDouble(thetaLegacy));
|
|
}
|
|
|
|
private static RobotNet10.RobotApp.Interfaces.NavigationState MapNavigationState(RobotNet10.RobotApp.Navigation.NavigationState? state)
|
|
{
|
|
return state switch
|
|
{
|
|
RobotNet10.RobotApp.Navigation.NavigationState.Pending => RobotNet10.RobotApp.Interfaces.NavigationState.Waiting,
|
|
RobotNet10.RobotApp.Navigation.NavigationState.Planning => RobotNet10.RobotApp.Interfaces.NavigationState.Initializing,
|
|
RobotNet10.RobotApp.Navigation.NavigationState.Active => RobotNet10.RobotApp.Interfaces.NavigationState.Moving,
|
|
RobotNet10.RobotApp.Navigation.NavigationState.Controlling => RobotNet10.RobotApp.Interfaces.NavigationState.Moving,
|
|
RobotNet10.RobotApp.Navigation.NavigationState.Clearing => RobotNet10.RobotApp.Interfaces.NavigationState.Moving,
|
|
RobotNet10.RobotApp.Navigation.NavigationState.Succeeded => RobotNet10.RobotApp.Interfaces.NavigationState.Completed,
|
|
RobotNet10.RobotApp.Navigation.NavigationState.Paused => RobotNet10.RobotApp.Interfaces.NavigationState.Paused,
|
|
RobotNet10.RobotApp.Navigation.NavigationState.Preempted => RobotNet10.RobotApp.Interfaces.NavigationState.Canceled,
|
|
RobotNet10.RobotApp.Navigation.NavigationState.Recalled => RobotNet10.RobotApp.Interfaces.NavigationState.Canceled,
|
|
RobotNet10.RobotApp.Navigation.NavigationState.Rejected => RobotNet10.RobotApp.Interfaces.NavigationState.Error,
|
|
RobotNet10.RobotApp.Navigation.NavigationState.Aborted => RobotNet10.RobotApp.Interfaces.NavigationState.Error,
|
|
RobotNet10.RobotApp.Navigation.NavigationState.Lost => RobotNet10.RobotApp.Interfaces.NavigationState.Error,
|
|
_ => RobotNet10.RobotApp.Interfaces.NavigationState.Idle
|
|
};
|
|
}
|
|
}
|