219 lines
12 KiB
Plaintext
219 lines
12 KiB
Plaintext
@page "/motion/odometry"
|
|
@rendermode InteractiveWebAssemblyNoPrerender
|
|
@implements IAsyncDisposable
|
|
|
|
@using Microsoft.AspNetCore.SignalR.Client
|
|
@using RobotNet10.RobotApp.Client.Clients
|
|
@using RobotNet10.RobotApp.Client.Shared.Motion
|
|
@using MudBlazor
|
|
|
|
<PageTitle>Odometry (XLOC)</PageTitle>
|
|
|
|
<MudContainer MaxWidth="MaxWidth.Large" Class="mt-3 mb-4">
|
|
@* Header *@
|
|
<MudPaper Class="pa-4 mb-3 odom-page-header" Elevation="0">
|
|
<MudStack Row="true" AlignItems="AlignItems.Center" Justify="Justify.SpaceBetween" Wrap="Wrap.Wrap">
|
|
<MudStack Row="true" AlignItems="AlignItems.Center" Spacing="2">
|
|
<MudIcon Icon="@Icons.Material.Filled.Route" Color="Color.Primary" Style="font-size: 28px;" />
|
|
<div>
|
|
<MudText Typo="Typo.h6" Style="line-height: 1.2;">Odometry → XLOC</MudText>
|
|
<MudText Typo="Typo.caption" Color="Color.Secondary">OdometryService → XlocIntegrationService · ~10 Hz</MudText>
|
|
</div>
|
|
</MudStack>
|
|
<MudChip T="string"
|
|
Color="@(OdometryHubClient.IsConnected ? Color.Success : Color.Default)"
|
|
Variant="Variant.Filled"
|
|
Size="Size.Small"
|
|
Icon="@(OdometryHubClient.IsConnected ? Icons.Material.Filled.Link : Icons.Material.Filled.LinkOff)"
|
|
Style="font-weight: 500;">
|
|
@(OdometryHubClient.IsConnected ? "Connected" : "Disconnected")
|
|
</MudChip>
|
|
</MudStack>
|
|
</MudPaper>
|
|
|
|
@if (!OdometryHubClient.IsConnected)
|
|
{
|
|
<MudAlert Severity="Severity.Warning" Class="mb-4" Dense="true" Icon="@Icons.Material.Filled.Sync">
|
|
Đang kết nối tới hub odometry...
|
|
</MudAlert>
|
|
}
|
|
|
|
@if (lastOdom != null)
|
|
{
|
|
<MudGrid Spacing="3">
|
|
@* Meta: frame_id, child_frame_id, stamp *@
|
|
<MudItem xs="12">
|
|
<MudCard Elevation="0" Class="odom-card odom-card-meta">
|
|
<MudCardHeader>
|
|
<CardHeaderContent>
|
|
<MudStack Row="true" AlignItems="AlignItems.Center" Spacing="1">
|
|
<MudIcon Icon="@Icons.Material.Outlined.Tag" Size="Size.Small" Color="Color.Primary" />
|
|
<MudText Typo="Typo.subtitle2">Header</MudText>
|
|
</MudStack>
|
|
</CardHeaderContent>
|
|
</MudCardHeader>
|
|
<MudCardContent Class="pt-0">
|
|
<MudStack Row="true" Spacing="2" Wrap="Wrap.Wrap">
|
|
<MudChip T="string" Size="Size.Small" Variant="Variant.Outlined" Color="Color.Primary">@lastOdom.FrameId</MudChip>
|
|
<MudChip T="string" Size="Size.Small" Variant="Variant.Outlined" Color="Color.Secondary">@lastOdom.ChildFrameId</MudChip>
|
|
<MudChip T="string" Size="Size.Small" Variant="Variant.Filled" Color="Color.Primary">@lastOdom.Timestamp.ToString("HH:mm:ss.fff")</MudChip>
|
|
</MudStack>
|
|
</MudCardContent>
|
|
</MudCard>
|
|
</MudItem>
|
|
|
|
@* Pose: Position + Orientation *@
|
|
<MudItem xs="12" md="6">
|
|
<MudCard Elevation="0" Class="odom-card odom-card-position">
|
|
<MudCardHeader>
|
|
<CardHeaderContent>
|
|
<MudStack Row="true" AlignItems="AlignItems.Center" Spacing="1">
|
|
<MudIcon Icon="@Icons.Material.Outlined.Place" Size="Size.Small" Color="Color.Primary" />
|
|
<MudText Typo="Typo.subtitle2">Position</MudText>
|
|
<MudText Typo="Typo.caption" Color="Color.Secondary" Class="ml-1">(m)</MudText>
|
|
</MudStack>
|
|
</CardHeaderContent>
|
|
</MudCardHeader>
|
|
<MudCardContent Class="pt-0">
|
|
<MudStack Row="true" Spacing="2">
|
|
<MudTextField T="string" Label="X" Value="@Format(lastOdom.PositionX)" ReadOnly="true" Variant="Variant.Outlined" Margin="Margin.Dense" Style="max-width: 120px;" />
|
|
<MudTextField T="string" Label="Y" Value="@Format(lastOdom.PositionY)" ReadOnly="true" Variant="Variant.Outlined" Margin="Margin.Dense" Style="max-width: 120px;" />
|
|
<MudTextField T="string" Label="Z" Value="@Format(lastOdom.PositionZ)" ReadOnly="true" Variant="Variant.Outlined" Margin="Margin.Dense" Style="max-width: 120px;" />
|
|
</MudStack>
|
|
</MudCardContent>
|
|
</MudCard>
|
|
</MudItem>
|
|
|
|
<MudItem xs="12" md="6">
|
|
<MudCard Elevation="0" Class="odom-card odom-card-orientation">
|
|
<MudCardHeader>
|
|
<CardHeaderContent>
|
|
<MudStack Row="true" AlignItems="AlignItems.Center" Spacing="1">
|
|
<MudIcon Icon="@Icons.Material.Outlined.Explore" Size="Size.Small" Color="Color.Secondary" />
|
|
<MudText Typo="Typo.subtitle2">Orientation</MudText>
|
|
<MudText Typo="Typo.caption" Color="Color.Secondary" Class="ml-1">(yaw °)</MudText>
|
|
</MudStack>
|
|
</CardHeaderContent>
|
|
</MudCardHeader>
|
|
<MudCardContent Class="pt-0">
|
|
<MudStack Row="true" Spacing="2" Wrap="Wrap.Wrap" AlignItems="AlignItems.End">
|
|
<MudTextField T="string" Label="Yaw" Value="@(FormatDegrees(YawDegrees))" ReadOnly="true" Variant="Variant.Outlined" Margin="Margin.Dense" Style="max-width: 100px;" />
|
|
<MudText Typo="Typo.caption" Color="Color.Secondary" Class="pb-2">Q: (@Format(lastOdom.OrientationX), @Format(lastOdom.OrientationY), @Format(lastOdom.OrientationZ), @Format(lastOdom.OrientationW))</MudText>
|
|
</MudStack>
|
|
</MudCardContent>
|
|
</MudCard>
|
|
</MudItem>
|
|
|
|
@* Twist: Linear + Angular *@
|
|
<MudItem xs="12" md="6">
|
|
<MudCard Elevation="0" Class="odom-card odom-card-linear">
|
|
<MudCardHeader>
|
|
<CardHeaderContent>
|
|
<MudStack Row="true" AlignItems="AlignItems.Center" Spacing="1">
|
|
<MudIcon Icon="@Icons.Material.Outlined.Speed" Size="Size.Small" Color="Color.Info" />
|
|
<MudText Typo="Typo.subtitle2">Linear velocity</MudText>
|
|
<MudText Typo="Typo.caption" Color="Color.Secondary" Class="ml-1">(m/s)</MudText>
|
|
</MudStack>
|
|
</CardHeaderContent>
|
|
</MudCardHeader>
|
|
<MudCardContent Class="pt-0">
|
|
<MudStack Row="true" Spacing="2">
|
|
<MudTextField T="string" Label="Vx" Value="@Format(lastOdom.LinearVelocityX)" ReadOnly="true" Variant="Variant.Outlined" Margin="Margin.Dense" Style="max-width: 120px;" />
|
|
<MudTextField T="string" Label="Vy" Value="@Format(lastOdom.LinearVelocityY)" ReadOnly="true" Variant="Variant.Outlined" Margin="Margin.Dense" Style="max-width: 120px;" />
|
|
<MudTextField T="string" Label="Vz" Value="@Format(lastOdom.LinearVelocityZ)" ReadOnly="true" Variant="Variant.Outlined" Margin="Margin.Dense" Style="max-width: 120px;" />
|
|
</MudStack>
|
|
</MudCardContent>
|
|
</MudCard>
|
|
</MudItem>
|
|
|
|
<MudItem xs="12" md="6">
|
|
<MudCard Elevation="0" Class="odom-card odom-card-angular">
|
|
<MudCardHeader>
|
|
<CardHeaderContent>
|
|
<MudStack Row="true" AlignItems="AlignItems.Center" Spacing="1">
|
|
<MudIcon Icon="@Icons.Material.Outlined.RotateRight" Size="Size.Small" Color="Color.Warning" />
|
|
<MudText Typo="Typo.subtitle2">Angular velocity</MudText>
|
|
<MudText Typo="Typo.caption" Color="Color.Secondary" Class="ml-1">(rad/s)</MudText>
|
|
</MudStack>
|
|
</CardHeaderContent>
|
|
</MudCardHeader>
|
|
<MudCardContent Class="pt-0">
|
|
<MudStack Row="true" Spacing="2">
|
|
<MudTextField T="string" Label="ωx" Value="@Format(lastOdom.AngularVelocityX)" ReadOnly="true" Variant="Variant.Outlined" Margin="Margin.Dense" Style="max-width: 120px;" />
|
|
<MudTextField T="string" Label="ωy" Value="@Format(lastOdom.AngularVelocityY)" ReadOnly="true" Variant="Variant.Outlined" Margin="Margin.Dense" Style="max-width: 120px;" />
|
|
<MudTextField T="string" Label="ωz" Value="@Format(lastOdom.AngularVelocityZ)" ReadOnly="true" Variant="Variant.Outlined" Margin="Margin.Dense" Style="max-width: 120px;" />
|
|
</MudStack>
|
|
</MudCardContent>
|
|
</MudCard>
|
|
</MudItem>
|
|
|
|
<MudItem xs="12">
|
|
<MudPaper Class="pa-2" Elevation="0" Style="border-radius: 8px; background: var(--mud-palette-background-grey);">
|
|
<MudText Typo="Typo.caption" Color="Color.Secondary" Align="Align.Center">
|
|
pose_position · pose_orientation · twist_linear · twist_angular → ToXlocOdometry()
|
|
</MudText>
|
|
</MudPaper>
|
|
</MudItem>
|
|
</MudGrid>
|
|
}
|
|
else if (OdometryHubClient.IsConnected)
|
|
{
|
|
<MudAlert Severity="Severity.Info" Dense="true" Class="mb-4" Icon="@Icons.Material.Filled.Schedule">
|
|
Chưa nhận dữ liệu. Đang chờ broadcast từ server.
|
|
</MudAlert>
|
|
}
|
|
</MudContainer>
|
|
|
|
@code {
|
|
[Inject]
|
|
private OdometryHubClient OdometryHubClient { get; set; } = null!;
|
|
|
|
private OdometryDto? lastOdom;
|
|
|
|
protected override async Task OnInitializedAsync()
|
|
{
|
|
OdometryHubClient.OdometryReceived += OnOdometryReceived;
|
|
OdometryHubClient.ConnectionStateChanged += OnConnectionStateChanged;
|
|
await OdometryHubClient.StartAsync();
|
|
if (OdometryHubClient.IsConnected)
|
|
{
|
|
lastOdom = await OdometryHubClient.GetCurrentOdometryAsync();
|
|
}
|
|
}
|
|
|
|
private void OnOdometryReceived(OdometryDto dto)
|
|
{
|
|
lastOdom = dto;
|
|
InvokeAsync(StateHasChanged);
|
|
}
|
|
|
|
private void OnConnectionStateChanged(HubConnectionState _)
|
|
{
|
|
InvokeAsync(StateHasChanged);
|
|
}
|
|
|
|
private double YawDegrees
|
|
{
|
|
get
|
|
{
|
|
if (lastOdom == null) return 0;
|
|
var qw = lastOdom.OrientationW;
|
|
var qz = lastOdom.OrientationZ;
|
|
var qx = lastOdom.OrientationX;
|
|
var qy = lastOdom.OrientationY;
|
|
var yaw = Math.Atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));
|
|
return yaw * 180.0 / Math.PI;
|
|
}
|
|
}
|
|
|
|
private static string Format(double value) => value.ToString("F4");
|
|
private static string FormatDegrees(double value) => value.ToString("F2") + " °";
|
|
|
|
public async ValueTask DisposeAsync()
|
|
{
|
|
OdometryHubClient.OdometryReceived -= OnOdometryReceived;
|
|
OdometryHubClient.ConnectionStateChanged -= OnConnectionStateChanged;
|
|
await OdometryHubClient.DisposeAsync();
|
|
}
|
|
}
|