681 lines
26 KiB
C#
681 lines
26 KiB
C#
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);
|
|
}
|
|
}
|