Initial commit
This commit is contained in:
@@ -0,0 +1,89 @@
|
||||
using Microsoft.AspNetCore.SignalR;
|
||||
|
||||
namespace RobotNet10.FleetManager.Hubs;
|
||||
|
||||
/// <summary>
|
||||
/// SignalR Hub for real-time robot state and visualization updates.
|
||||
/// Manages client subscriptions to robot groups for receiving VDA5050 messages.
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// Clients subscribe to specific robots using SubscribeToRobot method.
|
||||
/// Messages are broadcast to groups named "robot:{robotId}".
|
||||
/// </remarks>
|
||||
public class RobotStateHub(
|
||||
Services.Logger<RobotStateHub> logger,
|
||||
RobotStateHubContext hubContext) : Hub
|
||||
{
|
||||
private readonly Services.Logger<RobotStateHub> _logger = logger;
|
||||
private readonly RobotStateHubContext _hubContext = hubContext;
|
||||
|
||||
/// <summary>
|
||||
/// Subscribe to receive updates for a specific robot
|
||||
/// </summary>
|
||||
/// <param name="robotId">Robot identifier (serialNumber)</param>
|
||||
public async Task SubscribeToRobot(string robotId)
|
||||
{
|
||||
var groupName = GetGroupName(robotId);
|
||||
await Groups.AddToGroupAsync(Context.ConnectionId, groupName);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Unsubscribe from updates for a specific robot
|
||||
/// </summary>
|
||||
/// <param name="robotId">Robot identifier (serialNumber)</param>
|
||||
public async Task UnsubscribeFromRobot(string robotId)
|
||||
{
|
||||
var groupName = GetGroupName(robotId);
|
||||
await Groups.RemoveFromGroupAsync(Context.ConnectionId, groupName);
|
||||
}
|
||||
|
||||
private static string GetGroupName(string robotId)
|
||||
{
|
||||
return $"robot:{robotId}";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Subscribe to receive monitor updates for a specific levelId
|
||||
/// Each connection can only subscribe to one levelId at a time.
|
||||
/// Maximum 5 connections per levelId (FIFO).
|
||||
/// </summary>
|
||||
/// <param name="levelId">Level identifier (LayoutLevel.Id)</param>
|
||||
public async Task SubscribeToLevelForMonitor(Guid levelId)
|
||||
{
|
||||
var evictedConnectionId = _hubContext.SubscribeToLevel(Context.ConnectionId, levelId);
|
||||
|
||||
// If a connection was evicted, notify it
|
||||
if (evictedConnectionId != null)
|
||||
{
|
||||
_logger.Info($"Connection {evictedConnectionId} evicted from level {levelId} (max 5 connections reached)");
|
||||
await Clients.Client(evictedConnectionId).SendAsync("OnMonitorDeactivated");
|
||||
}
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Unsubscribe from monitor updates for the current level
|
||||
/// </summary>
|
||||
public async Task UnsubscribeFromLevelForMonitor()
|
||||
{
|
||||
var removed = _hubContext.UnsubscribeFromLevel(Context.ConnectionId);
|
||||
if (removed)
|
||||
{
|
||||
_logger.Info($"Client {Context.ConnectionId} unsubscribed from level for monitor");
|
||||
}
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
public override async Task OnDisconnectedAsync(Exception? exception)
|
||||
{
|
||||
// Remove connection from subscription manager
|
||||
_hubContext.RemoveConnection(Context.ConnectionId);
|
||||
|
||||
if (exception != null)
|
||||
{
|
||||
_logger.Warning($"Client {Context.ConnectionId} disconnected with error: {exception.Message}");
|
||||
}
|
||||
await base.OnDisconnectedAsync(exception);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,680 @@
|
||||
using Microsoft.AspNetCore.SignalR;
|
||||
using RobotNet.VDA5050.Order;
|
||||
using RobotNet.VDA5050.State;
|
||||
using RobotNet.VDA5050.Visualization;
|
||||
using RobotNet10.Common;
|
||||
using RobotNet10.Common.Models;
|
||||
using RobotNet10.FleetManager.Services;
|
||||
using RobotNet10.FleetManager.Services.RobotManager;
|
||||
using RobotNet10.FleetManager.Services.TrafficControl;
|
||||
using RobotNet10.FleetManager.Services.TrafficControl.Models;
|
||||
using RobotNet10.FleetManager.Shared.DTOs.Robot;
|
||||
using System.Collections.Concurrent;
|
||||
|
||||
namespace RobotNet10.FleetManager.Hubs;
|
||||
|
||||
/// <summary>
|
||||
/// Hosted service for broadcasting robot state and visualization updates via SignalR.
|
||||
/// Broadcasts updates at 1Hz (every 1 second) to all subscribed clients.
|
||||
/// Also manages LevelId subscriptions for Monitor page.
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// This service runs as a BackgroundService and periodically broadcasts
|
||||
/// StateMsg and VisualizationMsg updates from RobotManagerService to subscribed clients.
|
||||
/// </remarks>
|
||||
public class RobotStateHubContext(
|
||||
IHubContext<RobotStateHub> hubContext,
|
||||
IRobotManagerService robotManagerService,
|
||||
IServiceScopeFactory serviceScopeFactory,
|
||||
Services.Logger<RobotStateHubContext> logger,
|
||||
IOrderControlService orderControlService,
|
||||
ILoggerFactory loggerFactory) : BackgroundService
|
||||
{
|
||||
private readonly IHubContext<RobotStateHub> _hubContext = hubContext;
|
||||
private readonly IRobotManagerService _robotManagerService = robotManagerService;
|
||||
private readonly IServiceScopeFactory _serviceScopeFactory = serviceScopeFactory;
|
||||
private readonly Services.Logger<RobotStateHubContext> _logger = logger;
|
||||
private readonly ILoggerFactory _loggerFactory = loggerFactory;
|
||||
private readonly IOrderControlService _orderControlService = orderControlService;
|
||||
private WatchTimerAsync<RobotStateHubContext>? _broadcastTimer;
|
||||
private const int BroadcastIntervalMs = 500; // 1Hz = 1000ms
|
||||
|
||||
// ===== MONITOR LEVEL SUBSCRIPTION MANAGEMENT =====
|
||||
private readonly ConcurrentDictionary<Guid, Queue<string>> _levelSubscriptions = new();
|
||||
private readonly ConcurrentDictionary<string, Guid> _connectionToLevel = new();
|
||||
private readonly Lock _subscriptionLock = new();
|
||||
private const int MaxConnectionsPerLevel = 5;
|
||||
|
||||
/// <summary>
|
||||
/// Subscribe a connection to a levelId for monitor updates.
|
||||
/// If connection already subscribed to another level, unsubscribe from old level first.
|
||||
/// If level has 5 connections, the oldest connection will be evicted.
|
||||
/// </summary>
|
||||
/// <returns>ConnectionId that was evicted (if any), null otherwise</returns>
|
||||
public string? SubscribeToLevel(string connectionId, Guid levelId)
|
||||
{
|
||||
lock (_subscriptionLock)
|
||||
{
|
||||
// If connection already subscribed to another level, unsubscribe first
|
||||
if (_connectionToLevel.TryGetValue(connectionId, out var oldLevelId))
|
||||
{
|
||||
if (oldLevelId == levelId)
|
||||
{
|
||||
// Already subscribed to this level, no change needed
|
||||
return null;
|
||||
}
|
||||
UnsubscribeFromLevelInternal(connectionId, oldLevelId);
|
||||
}
|
||||
|
||||
// Get or create queue for this level
|
||||
var queue = _levelSubscriptions.GetOrAdd(levelId, _ => new Queue<string>());
|
||||
|
||||
// If queue is full, evict oldest connection
|
||||
string? evictedConnectionId = null;
|
||||
if (queue.Count >= MaxConnectionsPerLevel)
|
||||
{
|
||||
evictedConnectionId = queue.Dequeue();
|
||||
_connectionToLevel.TryRemove(evictedConnectionId, out _);
|
||||
}
|
||||
|
||||
// Add new connection to queue
|
||||
queue.Enqueue(connectionId);
|
||||
_connectionToLevel[connectionId] = levelId;
|
||||
|
||||
return evictedConnectionId;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Unsubscribe a connection from its current level
|
||||
/// </summary>
|
||||
public bool UnsubscribeFromLevel(string connectionId)
|
||||
{
|
||||
lock (_subscriptionLock)
|
||||
{
|
||||
if (!_connectionToLevel.TryGetValue(connectionId, out var levelId))
|
||||
{
|
||||
return false; // Not subscribed
|
||||
}
|
||||
|
||||
return UnsubscribeFromLevelInternal(connectionId, levelId);
|
||||
}
|
||||
}
|
||||
|
||||
private bool UnsubscribeFromLevelInternal(string connectionId, Guid levelId)
|
||||
{
|
||||
if (!_levelSubscriptions.TryGetValue(levelId, out var queue))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Remove connection from queue
|
||||
var tempQueue = new Queue<string>();
|
||||
bool found = false;
|
||||
while (queue.Count > 0)
|
||||
{
|
||||
var id = queue.Dequeue();
|
||||
if (id != connectionId)
|
||||
{
|
||||
tempQueue.Enqueue(id);
|
||||
}
|
||||
else
|
||||
{
|
||||
found = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Replace queue
|
||||
while (tempQueue.Count > 0)
|
||||
{
|
||||
queue.Enqueue(tempQueue.Dequeue());
|
||||
}
|
||||
|
||||
// Remove connection mapping
|
||||
_connectionToLevel.TryRemove(connectionId, out _);
|
||||
|
||||
// If queue is empty, remove level entry
|
||||
if (queue.Count == 0)
|
||||
{
|
||||
_levelSubscriptions.TryRemove(levelId, out _);
|
||||
}
|
||||
|
||||
return found;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get all connectionIds subscribed to a levelId
|
||||
/// </summary>
|
||||
public IReadOnlyList<string> GetConnectionsForLevel(Guid levelId)
|
||||
{
|
||||
lock (_subscriptionLock)
|
||||
{
|
||||
if (!_levelSubscriptions.TryGetValue(levelId, out var queue))
|
||||
{
|
||||
return [];
|
||||
}
|
||||
|
||||
return [.. queue];
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get levelId that a connection is subscribed to
|
||||
/// </summary>
|
||||
public Guid? GetLevelForConnection(string connectionId)
|
||||
{
|
||||
_connectionToLevel.TryGetValue(connectionId, out var levelId);
|
||||
return levelId;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Remove connection (called on disconnect)
|
||||
/// </summary>
|
||||
public void RemoveConnection(string connectionId)
|
||||
{
|
||||
UnsubscribeFromLevel(connectionId);
|
||||
}
|
||||
|
||||
protected override async Task ExecuteAsync(CancellationToken stoppingToken)
|
||||
{
|
||||
await Task.Yield();
|
||||
|
||||
// Start broadcast timer at 1Hz
|
||||
_broadcastTimer = new WatchTimerAsync<RobotStateHubContext>(
|
||||
BroadcastIntervalMs,
|
||||
BroadcastAllRobots,
|
||||
_loggerFactory.CreateLogger<RobotStateHubContext>()
|
||||
);
|
||||
_broadcastTimer.Start();
|
||||
_logger.Info("Started robot state broadcast service at 1Hz");
|
||||
}
|
||||
|
||||
private async Task BroadcastAllRobots()
|
||||
{
|
||||
try
|
||||
{
|
||||
// Get all robot data from RobotManagerService
|
||||
var allRobotData = _robotManagerService.GetAllRobotData();
|
||||
|
||||
foreach (var (robotId, robotData) in allRobotData)
|
||||
{
|
||||
// Broadcast State if available
|
||||
if (robotData.State != null)
|
||||
{
|
||||
await BroadcastStateUpdate(robotId, robotData.State);
|
||||
}
|
||||
|
||||
// Broadcast Visualization if available
|
||||
if (robotData.Visualization != null && robotData.State != null)
|
||||
{
|
||||
await BroadcastMonitorDataUpdate(robotId, robotData.Visualization, robotData.State);
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"Error in broadcast cycle: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Broadcast state update to all clients subscribed to the robot
|
||||
/// </summary>
|
||||
/// <param name="robotId">Robot identifier (serialNumber)</param>
|
||||
/// <param name="state">VDA5050 State message</param>
|
||||
private async Task BroadcastStateUpdate(string robotId, StateMsg state)
|
||||
{
|
||||
try
|
||||
{
|
||||
var groupName = $"robot:{robotId}";
|
||||
await _hubContext.Clients.Group(groupName).SendAsync("OnStateUpdate", state);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"Error broadcasting state update for robot {robotId}: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Broadcast visualization update to clients subscribed to the robot's levelId (for Monitor)
|
||||
/// Only broadcasts to connections that have subscribed to the robot's levelId.
|
||||
/// </summary>
|
||||
/// <param name="robotId">Robot identifier (serialNumber)</param>
|
||||
/// <param name="visualization">VDA5050 Visualization message</param>
|
||||
/// <param name="state">VDA5050 State message</param>
|
||||
private async Task BroadcastMonitorDataUpdate(string robotId, VisualizationMsg? visualization, StateMsg? state)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (visualization is null || state is null) return;
|
||||
|
||||
// Get robot's MapId (LevelId) from database
|
||||
Guid? levelId = await GetRobotLevelIdAsync(robotId);
|
||||
if (!levelId.HasValue)
|
||||
{
|
||||
// Robot has no MapId assigned, skip broadcast
|
||||
return;
|
||||
}
|
||||
|
||||
// Get all connections subscribed to this levelId
|
||||
var connectionIds = GetConnectionsForLevel(levelId.Value);
|
||||
if (connectionIds.Count == 0)
|
||||
{
|
||||
// No connections subscribed to this levelId, skip broadcast
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// Get robot path
|
||||
var robotPath = GetRobotPath(robotId, visualization.AgvPosition.X, visualization.AgvPosition.Y, state.NodeStates, state.LastNodeId);
|
||||
|
||||
// Prepare broadcast data
|
||||
RobotMonitorBoardcastData data = new()
|
||||
{
|
||||
RobotId = robotId,
|
||||
AgvPosition = visualization.AgvPosition,
|
||||
AgvVelocity = visualization.Velocity,
|
||||
Battery = state.BatteryState,
|
||||
Errors = state.Errors,
|
||||
Infomations = state.Information,
|
||||
Loads = state.Loads,
|
||||
Path = robotPath,
|
||||
};
|
||||
|
||||
// Broadcast to specific connections (not groups)
|
||||
await _hubContext.Clients.Clients(connectionIds).SendAsync("OnMonitorUpdate", data);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"Error broadcasting visualization update for robot {robotId}: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get robot's LevelId (MapId) from database
|
||||
/// </summary>
|
||||
private async Task<Guid?> GetRobotLevelIdAsync(string robotId)
|
||||
{
|
||||
try
|
||||
{
|
||||
using var scope = _serviceScopeFactory.CreateScope();
|
||||
var robotService = scope.ServiceProvider.GetRequiredService<IRobotService>();
|
||||
var robot = await robotService.GetByRobotIdAsync(robotId);
|
||||
return robot?.MapId; // MapId is the LevelId
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"Error getting LevelId for robot {robotId}: {ex.Message}");
|
||||
return null;
|
||||
}
|
||||
}
|
||||
/// <summary>
|
||||
/// Split edge at robot's current position, returning remaining edge segment(s)
|
||||
/// </summary>
|
||||
private static NavigationPathEdge[] SplitChecking(double robotX, double robotY, Node lastNode, Node nearLastNode, Edge edge)
|
||||
{
|
||||
List<NavigationPathEdge> pathEdges = [];
|
||||
|
||||
var spaceEdge = new SpaceEdge
|
||||
{
|
||||
StartX = lastNode.NodePosition?.X ?? 0,
|
||||
StartY = lastNode.NodePosition?.Y ?? 0,
|
||||
EndX = nearLastNode.NodePosition?.X ?? 0,
|
||||
EndY = nearLastNode.NodePosition?.Y ?? 0,
|
||||
Degree = edge.Trajectory?.Degree ?? 1,
|
||||
ControlPoint1X = edge.Trajectory?.ControlPoints.Length > 1 ? edge.Trajectory.ControlPoints[1].X : 0,
|
||||
ControlPoint1Y = edge.Trajectory?.ControlPoints.Length > 1 ? edge.Trajectory.ControlPoints[1].Y : 0,
|
||||
ControlPoint2X = edge.Trajectory?.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[2].X : 0,
|
||||
ControlPoint2Y = edge.Trajectory?.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[2].Y : 0,
|
||||
};
|
||||
|
||||
// Get projection point on edge and the time parameter
|
||||
var (projX, projY, _, projTime) = SpaceCompute.GetProjectionOnEdge(robotX, robotY, spaceEdge);
|
||||
|
||||
// Clamp projTime to [0, 1]
|
||||
projTime = Math.Max(0, Math.Min(1, projTime));
|
||||
|
||||
// If edge is degree 1, create a single linear edge
|
||||
if (spaceEdge.Degree == 1)
|
||||
{
|
||||
var linearEdge = new NavigationPathEdge
|
||||
{
|
||||
StartX = projX,
|
||||
StartY = projY,
|
||||
EndX = nearLastNode.NodePosition?.X ?? 0,
|
||||
EndY = nearLastNode.NodePosition?.Y ?? 0,
|
||||
Degree = 1
|
||||
};
|
||||
pathEdges.Add(linearEdge);
|
||||
}
|
||||
else
|
||||
{
|
||||
// For degree 2 or 3: sample points along the curve from projection point to end
|
||||
// Resolution: 0.2m per segment
|
||||
const double resolution = 0.2;
|
||||
|
||||
// Sample points along the curve
|
||||
List<(double x, double y)> samplePoints = [];
|
||||
samplePoints.Add((projX, projY)); // Start from projection point
|
||||
|
||||
double currentTime = projTime;
|
||||
var prevPoint = new SpaceNode(projX, projY);
|
||||
|
||||
while (currentTime < 1.0)
|
||||
{
|
||||
// Find next sample point at resolution distance along the curve
|
||||
double nextTime = FindNextSampleTime(spaceEdge, currentTime, resolution);
|
||||
nextTime = Math.Min(1.0, nextTime);
|
||||
|
||||
var nextPoint = SpaceCompute.BezierPoint(nextTime, spaceEdge);
|
||||
|
||||
// Check if we've moved enough distance
|
||||
double segmentLength = Math.Sqrt(Math.Pow(nextPoint.X - prevPoint.X, 2) +
|
||||
Math.Pow(nextPoint.Y - prevPoint.Y, 2));
|
||||
|
||||
if (segmentLength >= resolution * 0.5 || nextTime >= 1.0)
|
||||
{
|
||||
samplePoints.Add((nextPoint.X, nextPoint.Y));
|
||||
prevPoint = nextPoint;
|
||||
currentTime = nextTime;
|
||||
}
|
||||
else
|
||||
{
|
||||
// If segment is too short, advance time slightly and try again
|
||||
currentTime = Math.Min(1.0, currentTime + 0.01);
|
||||
}
|
||||
|
||||
// Safety check to avoid infinite loop
|
||||
if (nextTime >= 1.0 || currentTime >= 1.0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Ensure the last point is the end node
|
||||
if (samplePoints.Count == 0 ||
|
||||
Math.Abs(samplePoints[^1].x - nearLastNode.NodePosition?.X ?? 0) > 1e-6 ||
|
||||
Math.Abs(samplePoints[^1].y - nearLastNode.NodePosition?.Y ?? 0) > 1e-6)
|
||||
{
|
||||
samplePoints.Add((nearLastNode.NodePosition?.X ?? 0, nearLastNode.NodePosition?.Y ?? 0));
|
||||
}
|
||||
|
||||
// Create linear edges between consecutive sample points
|
||||
for (int i = 0; i < samplePoints.Count - 1; i++)
|
||||
{
|
||||
var linearEdge = new NavigationPathEdge
|
||||
{
|
||||
StartX = samplePoints[i].x,
|
||||
StartY = samplePoints[i].y,
|
||||
EndX = samplePoints[i + 1].x,
|
||||
EndY = samplePoints[i + 1].y,
|
||||
Degree = 1
|
||||
};
|
||||
pathEdges.Add(linearEdge);
|
||||
}
|
||||
}
|
||||
|
||||
return [.. pathEdges];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Find next sample time at approximately resolution distance from current time along the curve
|
||||
/// </summary>
|
||||
private static double FindNextSampleTime(SpaceEdge edge, double currentTime, double resolution)
|
||||
{
|
||||
// Use binary search to find the time where distance from current point is approximately resolution
|
||||
double low = currentTime;
|
||||
double high = 1.0;
|
||||
double targetDistance = resolution;
|
||||
double tolerance = 0.01; // 1cm tolerance
|
||||
|
||||
var startPoint = SpaceCompute.BezierPoint(currentTime, edge);
|
||||
|
||||
// Binary search
|
||||
for (int iter = 0; iter < 20; iter++)
|
||||
{
|
||||
double mid = (low + high) / 2;
|
||||
var midPoint = SpaceCompute.BezierPoint(mid, edge);
|
||||
double distance = Math.Sqrt(Math.Pow(midPoint.X - startPoint.X, 2) +
|
||||
Math.Pow(midPoint.Y - startPoint.Y, 2));
|
||||
|
||||
if (Math.Abs(distance - targetDistance) < tolerance)
|
||||
{
|
||||
return mid;
|
||||
}
|
||||
|
||||
if (distance < targetDistance)
|
||||
{
|
||||
low = mid;
|
||||
}
|
||||
else
|
||||
{
|
||||
high = mid;
|
||||
}
|
||||
}
|
||||
|
||||
return (low + high) / 2;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get robot navigation path (Base and Full) from current position
|
||||
/// </summary>
|
||||
private NavigationPath GetRobotPath(string robotId, double robotX, double robotY, NodeState[] nodestates, string lastNodeId)
|
||||
{
|
||||
try
|
||||
{
|
||||
var robotRoute = _orderControlService.GetRobotRoute(robotId);
|
||||
if (robotRoute is null || robotRoute.FullRoute.Count == 0)
|
||||
{
|
||||
return new NavigationPath { NavigationState = "NoRoute" };
|
||||
}
|
||||
|
||||
if (nodestates.Length == 0 || string.IsNullOrEmpty(lastNodeId))
|
||||
{
|
||||
return new NavigationPath { NavigationState = "NoRoute" };
|
||||
}
|
||||
|
||||
var fullRoute = robotRoute.FullRoute;
|
||||
|
||||
// Find last node segment index
|
||||
var lastNodeIndex = fullRoute.FindIndex(r => r.VdaNode != null && r.VdaNode.NodeId == lastNodeId);
|
||||
if (lastNodeIndex == -1)
|
||||
{
|
||||
// Last node not found, return full route from start
|
||||
return ConvertRouteToNavigationPath(robotRoute, 0, false, robotX, robotY);
|
||||
}
|
||||
|
||||
// Check if robot is on an edge (between lastNodeIndex and next node)
|
||||
bool isOnEdge = false;
|
||||
int startIndex = lastNodeIndex;
|
||||
|
||||
// Check if there's an edge segment after last node
|
||||
if (lastNodeIndex + 1 < fullRoute.Count && fullRoute[lastNodeIndex + 1].IsEdge)
|
||||
{
|
||||
var edgeSegment = fullRoute[lastNodeIndex + 1];
|
||||
var lastNode = fullRoute[lastNodeIndex].VdaNode!;
|
||||
|
||||
// Find next node segment
|
||||
var nextNodeIndex = lastNodeIndex + 2;
|
||||
if (nextNodeIndex < fullRoute.Count && fullRoute[nextNodeIndex].IsNode)
|
||||
{
|
||||
var nextNode = fullRoute[nextNodeIndex].VdaNode!;
|
||||
|
||||
// Check if robot is between last node and next node (on the edge)
|
||||
// Simple check: if robot is closer to edge than to either node
|
||||
var distToLastNode = Math.Sqrt(Math.Pow(robotX - lastNode.NodePosition?.X ?? 0, 2) + Math.Pow(robotY - lastNode.NodePosition?.Y ?? 0, 2));
|
||||
var distToNextNode = Math.Sqrt(Math.Pow(robotX - nextNode.NodePosition?.X ?? 0, 2) + Math.Pow(robotY - nextNode.NodePosition?.Y ?? 0, 2));
|
||||
|
||||
// Get edge length
|
||||
var edgeLength = Math.Sqrt(Math.Pow(nextNode.NodePosition?.X ?? 0 - lastNode.NodePosition?.X ?? 0, 2) +
|
||||
Math.Pow(nextNode.NodePosition?.Y ?? 0 - lastNode.NodePosition?.Y ?? 0, 2));
|
||||
|
||||
// If robot is closer to edge than to nodes, and not too far from edge
|
||||
if (edgeLength > 0.1 && distToLastNode > 0.1 && distToNextNode > 0.1)
|
||||
{
|
||||
isOnEdge = true;
|
||||
startIndex = lastNodeIndex + 1; // Start from edge segment
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Convert route to navigation path starting from startIndex
|
||||
return ConvertRouteToNavigationPath(robotRoute, startIndex, isOnEdge, robotX, robotY, lastNodeIndex);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"Error getting robot path for {robotId}: {ex.Message}");
|
||||
return new NavigationPath { NavigationState = "Error" };
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Convert RobotRoute segments to NavigationPath (Base and Full)
|
||||
/// </summary>
|
||||
private static NavigationPath ConvertRouteToNavigationPath(
|
||||
RobotRoute robotRoute,
|
||||
int startIndex,
|
||||
bool isOnEdge,
|
||||
double robotX,
|
||||
double robotY,
|
||||
int? lastNodeIndex = null)
|
||||
{
|
||||
var fullPath = new List<NavigationPathEdge>();
|
||||
var basePath = new List<NavigationPathEdge>();
|
||||
var fullRoute = robotRoute.FullRoute;
|
||||
|
||||
if (startIndex >= fullRoute.Count)
|
||||
{
|
||||
return new NavigationPath { NavigationState = "Complete" };
|
||||
}
|
||||
|
||||
// If robot is on edge, split the edge
|
||||
if (isOnEdge && startIndex < fullRoute.Count && fullRoute[startIndex].IsEdge)
|
||||
{
|
||||
var edgeSegment = fullRoute[startIndex];
|
||||
if (lastNodeIndex.HasValue && lastNodeIndex.Value >= 0 && lastNodeIndex.Value < fullRoute.Count)
|
||||
{
|
||||
var lastNode = fullRoute[lastNodeIndex.Value].VdaNode;
|
||||
var nextNodeIndex = startIndex + 1;
|
||||
if (nextNodeIndex < fullRoute.Count && fullRoute[nextNodeIndex].IsNode && lastNode != null)
|
||||
{
|
||||
var nextNode = fullRoute[nextNodeIndex].VdaNode;
|
||||
if (nextNode != null && edgeSegment.VdaEdge != null)
|
||||
{
|
||||
// Split edge at robot position
|
||||
var splitEdges = SplitChecking(robotX, robotY, lastNode, nextNode, edgeSegment.VdaEdge);
|
||||
fullPath.AddRange(splitEdges);
|
||||
|
||||
// Add to base path if segment is released
|
||||
if (edgeSegment.Released)
|
||||
{
|
||||
basePath.AddRange(splitEdges);
|
||||
}
|
||||
|
||||
startIndex = nextNodeIndex + 1; // Move to segment after next node (skip the edge and next node)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (!isOnEdge && startIndex < fullRoute.Count && fullRoute[startIndex].IsNode)
|
||||
{
|
||||
// Robot is at a node, skip the node segment and start from next edge
|
||||
startIndex += 2;
|
||||
}
|
||||
|
||||
// Convert remaining segments
|
||||
for (int i = startIndex; i < fullRoute.Count; i++)
|
||||
{
|
||||
var segment = fullRoute[i];
|
||||
|
||||
if (segment.IsEdge && segment.VdaEdge != null)
|
||||
{
|
||||
// Find start and end nodes for this edge
|
||||
Node? startNode = null;
|
||||
Node? endNode = null;
|
||||
|
||||
// Start node: previous segment should be a node
|
||||
if (i > 0 && fullRoute[i - 1].IsNode)
|
||||
{
|
||||
startNode = fullRoute[i - 1].VdaNode;
|
||||
}
|
||||
|
||||
// End node: next segment should be a node
|
||||
if (i + 1 < fullRoute.Count && fullRoute[i + 1].IsNode)
|
||||
{
|
||||
endNode = fullRoute[i + 1].VdaNode;
|
||||
}
|
||||
|
||||
if (startNode != null && endNode != null)
|
||||
{
|
||||
var edge = ConvertEdgeSegmentToNavigationPathEdge(segment, startNode, endNode);
|
||||
if (edge != null)
|
||||
{
|
||||
fullPath.Add(edge);
|
||||
if (segment.Released)
|
||||
{
|
||||
basePath.Add(edge);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// Note: Node segments don't create NavigationPathEdge, only edges do
|
||||
}
|
||||
|
||||
return new NavigationPath
|
||||
{
|
||||
NavigationState = robotRoute.Base.Count > 0 ? "Active" : "Planning",
|
||||
RobotPath = [.. fullPath],
|
||||
RobotBasePath = [.. basePath]
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Convert RouteSegment (Edge) to NavigationPathEdge
|
||||
/// </summary>
|
||||
private static NavigationPathEdge? ConvertEdgeSegmentToNavigationPathEdge(RouteSegment segment, Node? startNode, Node? endNode)
|
||||
{
|
||||
if (!segment.IsEdge || segment.VdaEdge == null) return null;
|
||||
|
||||
var edge = segment.VdaEdge;
|
||||
var trajectory = edge.Trajectory;
|
||||
|
||||
// Get start and end positions from node positions (not from trajectory control points)
|
||||
if (startNode == null || endNode == null) return null;
|
||||
|
||||
var navEdge = new NavigationPathEdge
|
||||
{
|
||||
StartX = startNode.NodePosition?.X ?? 0,
|
||||
StartY = startNode.NodePosition?.Y ?? 0,
|
||||
EndX = endNode.NodePosition?.X ?? 0,
|
||||
EndY = endNode.NodePosition?.Y ?? 0,
|
||||
Degree = trajectory?.Degree ?? 1,
|
||||
};
|
||||
|
||||
// Set control points from trajectory (if available)
|
||||
if (trajectory != null && trajectory.ControlPoints.Length > 1)
|
||||
{
|
||||
// ControlPoints[1] is first control point, ControlPoints[2] is second control point (for cubic bezier)
|
||||
if (trajectory.ControlPoints.Length > 1)
|
||||
{
|
||||
navEdge.ControlPoint1X = trajectory.ControlPoints[1].X;
|
||||
navEdge.ControlPoint1Y = trajectory.ControlPoints[1].Y;
|
||||
}
|
||||
|
||||
if (trajectory.ControlPoints.Length > 2)
|
||||
{
|
||||
navEdge.ControlPoint2X = trajectory.ControlPoints[2].X;
|
||||
navEdge.ControlPoint2Y = trajectory.ControlPoints[2].Y;
|
||||
}
|
||||
}
|
||||
|
||||
return navEdge;
|
||||
}
|
||||
|
||||
public override Task StopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
_broadcastTimer?.Dispose();
|
||||
_logger.Info("Stopped robot state broadcast service");
|
||||
return base.StopAsync(cancellationToken);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user