43 lines
1.4 KiB
C#
43 lines
1.4 KiB
C#
using RobotNet10.RobotApp.Client.Shared.Motion;
|
|
using RobotNet10.Shared.Geometry;
|
|
|
|
namespace RobotNet10.RobotApp.Hubs;
|
|
|
|
/// <summary>
|
|
/// Extension methods for OdometryDto conversions (server-side only)
|
|
/// </summary>
|
|
public static class OdometryDtoExtensions
|
|
{
|
|
/// <summary>
|
|
/// Convert from Pose to OdometryDto (compatibility path when only raw pose is available).
|
|
/// </summary>
|
|
public static OdometryDto ToOdometryDto(this Pose pose, double updateFrequency, string frameId = "odom")
|
|
{
|
|
return new OdometryDto
|
|
{
|
|
Timestamp = DateTime.UtcNow,
|
|
FrameId = frameId,
|
|
PositionX = pose.Position.X,
|
|
PositionY = pose.Position.Y,
|
|
PositionZ = pose.Position.Z,
|
|
OrientationX = pose.Orientation.X,
|
|
OrientationY = pose.Orientation.Y,
|
|
OrientationZ = pose.Orientation.Z,
|
|
OrientationW = pose.Orientation.W,
|
|
UpdateFrequency = updateFrequency
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert OdometryDto to Pose (for Reset operations)
|
|
/// </summary>
|
|
public static Pose ToPose(this OdometryDto dto)
|
|
{
|
|
return new Pose
|
|
{
|
|
Position = new Point(dto.PositionX, dto.PositionY, dto.PositionZ),
|
|
Orientation = new Quaternion(dto.OrientationX, dto.OrientationY, dto.OrientationZ, dto.OrientationW)
|
|
};
|
|
}
|
|
}
|