54 lines
1.9 KiB
C#
54 lines
1.9 KiB
C#
using Microsoft.AspNetCore.SignalR;
|
|
using RobotNet10.RobotApp.Client.Shared.Motion;
|
|
using RobotNet10.RobotApp.Motion;
|
|
using RobotNet10.Shared.Sensor;
|
|
|
|
namespace RobotNet10.RobotApp.Hubs;
|
|
|
|
/// <summary>
|
|
/// SignalR Hub cho Odometry - hiển thị pose và velocity từ OdometryService (wheel encoder + IMU).
|
|
/// Cùng nguồn OdometryService.CurrentOdometry được XlocIntegrationService dùng để DispatchOdometry sang XLOC
|
|
/// (khi EnableRawOdometry bật) và NavigationIntegrationService dùng để dispatch sang navigation.
|
|
/// </summary>
|
|
public class OdometryHub(OdometryService odometryService) : Hub
|
|
{
|
|
/// <summary>
|
|
/// Lấy odometry hiện tại (pose + velocity) để hiển thị trên UI
|
|
/// </summary>
|
|
public Task<OdometryDto?> GetCurrentOdometry()
|
|
{
|
|
var odom = odometryService.CurrentOdometry;
|
|
var dto = MapToDto(odom);
|
|
return Task.FromResult<OdometryDto?>(dto);
|
|
}
|
|
|
|
private static OdometryDto MapToDto(Odometry odom)
|
|
{
|
|
var p = odom.Pose.Pose.Position;
|
|
var q = odom.Pose.Pose.Orientation;
|
|
var linear = odom.Twist.Twist.Linear;
|
|
var angular = odom.Twist.Twist.Angular;
|
|
|
|
return new OdometryDto
|
|
{
|
|
Timestamp = odom.Header.Stamp,
|
|
FrameId = odom.Header.FrameId ?? "odom",
|
|
ChildFrameId = odom.ChildFrameId ?? "base_link",
|
|
PositionX = p.X,
|
|
PositionY = p.Y,
|
|
PositionZ = p.Z,
|
|
OrientationX = q.X,
|
|
OrientationY = q.Y,
|
|
OrientationZ = q.Z,
|
|
OrientationW = q.W,
|
|
LinearVelocityX = linear.X,
|
|
LinearVelocityY = linear.Y,
|
|
LinearVelocityZ = linear.Z,
|
|
AngularVelocityX = angular.X,
|
|
AngularVelocityY = angular.Y,
|
|
AngularVelocityZ = angular.Z,
|
|
UpdateFrequency = 0 // Caller can compute if needed
|
|
};
|
|
}
|
|
}
|