799 lines
30 KiB
C#
799 lines
30 KiB
C#
using Microsoft.Extensions.Configuration;
|
|
using Microsoft.Extensions.Hosting;
|
|
using Microsoft.Extensions.Logging;
|
|
using RobotNet10.RobotApp.Devices;
|
|
using RobotNet10.Shared;
|
|
using RobotNet10.Shared.Geometry;
|
|
using RobotNet10.Shared.Sensor;
|
|
using System;
|
|
using System.Threading;
|
|
using System.Threading.Tasks;
|
|
|
|
namespace RobotNet10.RobotApp.Motion;
|
|
|
|
/// <summary>
|
|
/// OdometryService - tính toán odometry từ encoder data của hai động cơ CiA402 Servo
|
|
/// Sử dụng động học thuận (forward kinematics) để tính toán vị trí, hướng và vận tốc của robot
|
|
/// Cấu trúc dữ liệu tương tự nav_msgs/Odometry.msg trong ROS
|
|
/// </summary>
|
|
public class OdometryService : IHostedService, IDisposable
|
|
{
|
|
private readonly OdometryConfiguration _config;
|
|
private readonly IDeviceProvider _deviceProvider;
|
|
// private readonly IOdometryEstimator _odometryEstimator; // Unused
|
|
private readonly ILogger<OdometryService> _logger;
|
|
private readonly object _lock = new();
|
|
|
|
// Servo devices
|
|
private ICiA402Servo? _leftWheelServo;
|
|
private ICiA402Servo? _rightWheelServo;
|
|
|
|
// IMU device for orientation (theta) calculation
|
|
private IInertialMeasurementUnit? _imu;
|
|
private double DeltaTheta = 0.0;
|
|
private double _imuYawZeroOffset = 0.0;
|
|
private bool _imuYawZeroOffsetCaptured = false;
|
|
|
|
private double PoseX = 0.0;
|
|
private double PoseY = 0.0;
|
|
private double PoseTheta = 0.0;
|
|
|
|
// Odometry state
|
|
private Odometry _currentOdometry = new();
|
|
private int _lastLeftWheelPosition;
|
|
private int _lastRightWheelPosition;
|
|
private int _lastLeftWheelVelocity;
|
|
private int _lastRightWheelVelocity;
|
|
private DateTime _lastUpdateTime = DateTime.UtcNow;
|
|
private bool _isFirstUpdate = true;
|
|
|
|
// Sequence number for header
|
|
private uint _sequenceNumber = 0;
|
|
private int count = 0;
|
|
|
|
// Timer for periodic updates
|
|
private Timer? _updateTimer;
|
|
private bool _disposed = false;
|
|
private long _sourceUpdateCount = 0;
|
|
private long _lastSourceUpdateCount = 0;
|
|
private DateTime _lastSourceRateLogTime = DateTime.UtcNow;
|
|
|
|
// Conversion factors (calculated from configuration)
|
|
private double _leftWheelMetersPerCount;
|
|
private double _rightWheelMetersPerCount;
|
|
|
|
/// <summary>
|
|
/// Fired when a new odometry sample is produced.
|
|
/// </summary>
|
|
public event EventHandler<OdometryUpdatedEventArgs>? OdometryUpdated;
|
|
|
|
/// <summary>
|
|
/// Gets the current odometry data
|
|
/// </summary>
|
|
public Odometry CurrentOdometry
|
|
{
|
|
get
|
|
{
|
|
lock (_lock)
|
|
{
|
|
return _currentOdometry;
|
|
}
|
|
}
|
|
}
|
|
|
|
public OdometryService(
|
|
IConfiguration configuration,
|
|
IDeviceProvider deviceProvider,
|
|
ILogger<OdometryService> logger)
|
|
{
|
|
_deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
|
|
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
|
|
|
// Load configuration
|
|
var configSection = configuration.GetSection("Motion:Odometry");
|
|
if (!configSection.Exists())
|
|
{
|
|
throw new InvalidOperationException("Configuration section 'Motion:Odometry' not found in appsettings.json");
|
|
}
|
|
|
|
_config = new OdometryConfiguration();
|
|
configSection.Bind(_config);
|
|
|
|
// Validate configuration
|
|
ValidateConfiguration();
|
|
|
|
// Calculate conversion factors
|
|
CalculateConversionFactors();
|
|
}
|
|
|
|
private void ValidateConfiguration()
|
|
{
|
|
if (string.IsNullOrWhiteSpace(_config.LeftWheel.DeviceId))
|
|
throw new InvalidOperationException("LeftWheel.DeviceId is required");
|
|
|
|
if (string.IsNullOrWhiteSpace(_config.RightWheel.DeviceId))
|
|
throw new InvalidOperationException("RightWheel.DeviceId is required");
|
|
|
|
if (string.IsNullOrWhiteSpace(_config.ImuDeviceId))
|
|
throw new InvalidOperationException("ImuDeviceId is required");
|
|
|
|
if (_config.LeftWheel.WheelDiameter <= 0)
|
|
throw new InvalidOperationException("LeftWheel.WheelDiameter must be greater than 0");
|
|
|
|
if (_config.RightWheel.WheelDiameter <= 0)
|
|
throw new InvalidOperationException("RightWheel.WheelDiameter must be greater than 0");
|
|
|
|
if (_config.LeftWheel.PulsesPerRevolution <= 0)
|
|
throw new InvalidOperationException("LeftWheel.PulsesPerRevolution must be greater than 0");
|
|
|
|
if (_config.RightWheel.PulsesPerRevolution <= 0)
|
|
throw new InvalidOperationException("RightWheel.PulsesPerRevolution must be greater than 0");
|
|
|
|
if (string.IsNullOrWhiteSpace(_config.FrameId))
|
|
_config.FrameId = "odom";
|
|
|
|
if (string.IsNullOrWhiteSpace(_config.ChildFrameId))
|
|
_config.ChildFrameId = "base_link";
|
|
}
|
|
|
|
private void CalculateConversionFactors()
|
|
{
|
|
// Calculate meters per encoder count
|
|
var leftWheelCircumference = Math.PI * _config.LeftWheel.WheelDiameter;
|
|
var rightWheelCircumference = Math.PI * _config.RightWheel.WheelDiameter;
|
|
_leftWheelMetersPerCount = leftWheelCircumference / _config.LeftWheel.PulsesPerRevolution;
|
|
_rightWheelMetersPerCount = rightWheelCircumference / _config.RightWheel.PulsesPerRevolution;
|
|
|
|
_logger.LogInformation(
|
|
"OdometryService conversion factors calculated: LeftMetersPerCount={LeftMetersPerCount}, RightMetersPerCount={RightMetersPerCount}",
|
|
_leftWheelMetersPerCount, _rightWheelMetersPerCount);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Start OdometryService — init chạy nền để không chặn Kestrel/web UI lúc startup.
|
|
/// </summary>
|
|
public Task StartAsync(CancellationToken cancellationToken)
|
|
{
|
|
_logger.LogInformation("Starting OdometryService (non-blocking)...");
|
|
_ = Task.Run(() => StartInBackgroundAsync(cancellationToken), cancellationToken);
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
private async Task StartInBackgroundAsync(CancellationToken cancellationToken)
|
|
{
|
|
try
|
|
{
|
|
_logger.LogInformation("Waiting for all devices to be connected...");
|
|
var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);
|
|
|
|
if (!connected)
|
|
{
|
|
_logger.LogWarning("Timeout waiting for devices to connect. OdometryService will not be initialized.");
|
|
return;
|
|
}
|
|
|
|
_logger.LogInformation("All devices connected. Initializing OdometryService...");
|
|
|
|
// Get servo devices
|
|
var leftDevice = _deviceProvider.GetDevice(_config.LeftWheel.DeviceId);
|
|
var rightDevice = _deviceProvider.GetDevice(_config.RightWheel.DeviceId);
|
|
var imuDevice = _deviceProvider.GetDevice(_config.ImuDeviceId);
|
|
|
|
if (leftDevice == null)
|
|
{
|
|
_logger.LogError("Left wheel device '{DeviceId}' not found", _config.LeftWheel.DeviceId);
|
|
return;
|
|
}
|
|
|
|
if (rightDevice == null)
|
|
{
|
|
_logger.LogError("Right wheel device '{DeviceId}' not found", _config.RightWheel.DeviceId);
|
|
return;
|
|
}
|
|
|
|
if (imuDevice == null)
|
|
{
|
|
_logger.LogError("IMU device '{DeviceId}' not found", _config.ImuDeviceId);
|
|
return;
|
|
}
|
|
|
|
if (leftDevice is not ICiA402Servo leftServo)
|
|
{
|
|
_logger.LogError("Left wheel device '{DeviceId}' is not an ICiA402Servo", _config.LeftWheel.DeviceId);
|
|
return;
|
|
}
|
|
|
|
if (rightDevice is not ICiA402Servo rightServo)
|
|
{
|
|
_logger.LogError("Right wheel device '{DeviceId}' is not an ICiA402Servo", _config.RightWheel.DeviceId);
|
|
return;
|
|
}
|
|
|
|
if (imuDevice is not IInertialMeasurementUnit imu)
|
|
{
|
|
_logger.LogError("IMU device '{DeviceId}' is not an IInertialMeasurementUnit", _config.ImuDeviceId);
|
|
return;
|
|
}
|
|
|
|
// Check if devices are connected
|
|
if (!leftDevice.IsConnected)
|
|
{
|
|
_logger.LogWarning("Left wheel device '{DeviceId}' is not connected. Status: {Status}. OdometryService will wait for connection.",
|
|
_config.LeftWheel.DeviceId, leftDevice.Status);
|
|
// Don't return - we'll retry later
|
|
}
|
|
|
|
if (!rightDevice.IsConnected)
|
|
{
|
|
_logger.LogWarning("Right wheel device '{DeviceId}' is not connected. Status: {Status}. OdometryService will wait for connection.",
|
|
_config.RightWheel.DeviceId, rightDevice.Status);
|
|
// Don't return - we'll retry later
|
|
}
|
|
|
|
if (!imuDevice.IsConnected)
|
|
{
|
|
_logger.LogWarning("IMU device '{DeviceId}' is not connected. Status: {Status}. OdometryService will wait for connection.",
|
|
_config.ImuDeviceId, imuDevice.Status);
|
|
}
|
|
|
|
// Only initialize if all devices are connected
|
|
if (!leftDevice.IsConnected || !rightDevice.IsConnected || !imuDevice.IsConnected)
|
|
{
|
|
_logger.LogWarning("One or more devices are not connected. OdometryService will not start until all devices are connected.");
|
|
// Start a background task to retry initialization when devices connect
|
|
_ = Task.Run(async () => await RetryInitializationWhenDevicesReadyAsync(cancellationToken));
|
|
return;
|
|
}
|
|
|
|
_leftWheelServo = leftServo;
|
|
_rightWheelServo = rightServo;
|
|
_imu = imu;
|
|
|
|
// Get initial positions
|
|
_lastLeftWheelPosition = _leftWheelServo.CachedPosition;
|
|
_lastRightWheelPosition = _rightWheelServo.CachedPosition;
|
|
_lastLeftWheelVelocity = _leftWheelServo.CachedVelocity;
|
|
_lastRightWheelVelocity = _rightWheelServo.CachedVelocity;
|
|
_lastUpdateTime = DateTime.UtcNow;
|
|
|
|
// Initialize theta integration
|
|
DeltaTheta = 0.0;
|
|
_imuYawZeroOffset = 0.0;
|
|
_imuYawZeroOffsetCaptured = false;
|
|
|
|
// Initialize odometry
|
|
InitializeOdometry();
|
|
|
|
// Start periodic update timer (update every 50ms = 20Hz)
|
|
// var updateInterval = TimeSpan.FromMilliseconds(50);
|
|
// _updateTimer = new Timer(OnTimerCallback, null, updateInterval, updateInterval);
|
|
_ = Task.Run(async () => await OdometryLoopAsync(cancellationToken), cancellationToken);
|
|
|
|
|
|
_logger.LogInformation("OdometryService started successfully with IMU integration for theta");
|
|
}
|
|
catch (OperationCanceledException) when (cancellationToken.IsCancellationRequested)
|
|
{
|
|
_logger.LogInformation("OdometryService initialization cancelled");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error starting OdometryService");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Retry initialization when devices become ready
|
|
/// </summary>
|
|
private async Task RetryInitializationWhenDevicesReadyAsync(CancellationToken cancellationToken)
|
|
{
|
|
const int maxRetries = 60; // 5 minutes with 5 second intervals
|
|
int retryCount = 0;
|
|
|
|
while (retryCount < maxRetries && !cancellationToken.IsCancellationRequested)
|
|
{
|
|
await Task.Delay(TimeSpan.FromSeconds(5), cancellationToken);
|
|
|
|
var leftDevice = _deviceProvider.GetDevice(_config.LeftWheel.DeviceId);
|
|
var rightDevice = _deviceProvider.GetDevice(_config.RightWheel.DeviceId);
|
|
var imuDevice = _deviceProvider.GetDevice(_config.ImuDeviceId);
|
|
|
|
if (leftDevice != null && rightDevice != null && imuDevice != null &&
|
|
leftDevice.IsConnected && rightDevice.IsConnected && imuDevice.IsConnected &&
|
|
leftDevice is ICiA402Servo leftServo &&
|
|
rightDevice is ICiA402Servo rightServo &&
|
|
imuDevice is IInertialMeasurementUnit imu)
|
|
{
|
|
_logger.LogInformation("All devices are now connected. Initializing OdometryService...");
|
|
|
|
lock (_lock)
|
|
{
|
|
_leftWheelServo = leftServo;
|
|
_rightWheelServo = rightServo;
|
|
_imu = imu;
|
|
|
|
// Get initial positions
|
|
_lastLeftWheelPosition = _leftWheelServo.CachedPosition;
|
|
_lastRightWheelPosition = _rightWheelServo.CachedPosition;
|
|
_lastLeftWheelVelocity = _leftWheelServo.CachedVelocity;
|
|
_lastRightWheelVelocity = _rightWheelServo.CachedVelocity;
|
|
_lastUpdateTime = DateTime.UtcNow;
|
|
|
|
// Initialize theta
|
|
DeltaTheta = 0.0;
|
|
_imuYawZeroOffset = 0.0;
|
|
_imuYawZeroOffsetCaptured = false;
|
|
}
|
|
|
|
// Initialize odometry
|
|
InitializeOdometry();
|
|
|
|
// Start periodic update timer (update every 50ms = 20Hz)
|
|
// var updateInterval = TimeSpan.FromMilliseconds(50);
|
|
// _updateTimer = new Timer(OnTimerCallback, null, updateInterval, updateInterval);
|
|
_ = Task.Run(async () => await OdometryLoopAsync(cancellationToken), cancellationToken);
|
|
|
|
_logger.LogInformation("OdometryService initialized successfully after device connection");
|
|
return;
|
|
}
|
|
|
|
retryCount++;
|
|
if (retryCount % 12 == 0) // Log every minute
|
|
{
|
|
_logger.LogInformation("Still waiting for devices to connect... (attempt {Attempt}/{MaxAttempts})",
|
|
retryCount, maxRetries);
|
|
}
|
|
}
|
|
|
|
if (retryCount >= maxRetries)
|
|
{
|
|
_logger.LogWarning("Timeout waiting for devices to connect. OdometryService will not be initialized.");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Stop OdometryService
|
|
/// </summary>
|
|
public async Task StopAsync(CancellationToken cancellationToken)
|
|
{
|
|
_logger.LogInformation("Stopping OdometryService...");
|
|
|
|
try
|
|
{
|
|
_updateTimer?.Dispose();
|
|
_updateTimer = null;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error stopping OdometryService");
|
|
}
|
|
|
|
await Task.CompletedTask;
|
|
}
|
|
|
|
// private void OnTimerCallback(object? state)
|
|
// {
|
|
// try
|
|
// {
|
|
// UpdateOdometry();
|
|
// }
|
|
// catch (Exception ex)
|
|
// {
|
|
// _logger.LogError(ex, "Error updating odometry");
|
|
// }
|
|
// }
|
|
private async Task OdometryLoopAsync(CancellationToken cancellationToken)
|
|
{
|
|
while (!cancellationToken.IsCancellationRequested)
|
|
{
|
|
try
|
|
{
|
|
UpdateOdometry();
|
|
await Task.Delay(1, cancellationToken); // 100Hz update rate
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error updating odometry");
|
|
}
|
|
}
|
|
}
|
|
|
|
private void InitializeOdometry()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
// var currentPose = _odometryEstimator.CurrentPose;
|
|
var currentTime = DateTime.UtcNow;
|
|
|
|
_currentOdometry = new Odometry
|
|
{
|
|
Header = new Header
|
|
{
|
|
Seq = _sequenceNumber++,
|
|
Stamp = DateTime.UtcNow,
|
|
FrameId = _config.FrameId
|
|
},
|
|
ChildFrameId = _config.ChildFrameId,
|
|
Pose = new PoseWithCovariance
|
|
{
|
|
// Pose = currentPose,
|
|
Pose = new Pose
|
|
{
|
|
Position = new Point(),
|
|
|
|
Orientation = new Quaternion(0, 0, 0, 1) // Identity quaternion (valid)
|
|
|
|
},
|
|
Covariance = new double[PoseWithCovariance.CovarianceSize]
|
|
},
|
|
Twist = new TwistWithCovariance
|
|
{
|
|
Twist = new Twist
|
|
{
|
|
Linear = new Vector3(),
|
|
Angular = new Vector3()
|
|
},
|
|
Covariance = new double[TwistWithCovariance.CovarianceSize]
|
|
}
|
|
};
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Cập nhật odometry từ encoder data và IMU
|
|
/// Sử dụng IMU orientation (yaw tuyệt đối) để tính theta
|
|
/// Sử dụng encoder để tính linear displacement
|
|
/// </summary>
|
|
private void UpdateOdometry()
|
|
{
|
|
if (_leftWheelServo == null || _rightWheelServo == null || _imu == null)
|
|
return;
|
|
|
|
Odometry? updatedOdometry = null;
|
|
|
|
// Check if devices are still connected
|
|
var leftDevice = _deviceProvider.GetDevice(_config.LeftWheel.DeviceId);
|
|
var rightDevice = _deviceProvider.GetDevice(_config.RightWheel.DeviceId);
|
|
var imuDevice = _deviceProvider.GetDevice(_config.ImuDeviceId);
|
|
|
|
if (leftDevice == null || rightDevice == null || imuDevice == null ||
|
|
!leftDevice.IsConnected || !rightDevice.IsConnected || !imuDevice.IsConnected)
|
|
{
|
|
// Devices disconnected - stop updating but don't log every time
|
|
return;
|
|
}
|
|
|
|
lock (_lock)
|
|
{
|
|
// Capture timestamp IMMEDIATELY when reading encoder (more accurate than later)
|
|
var currentTime = DateTime.UtcNow;
|
|
|
|
// Get current encoder positions and velocities
|
|
var currentLeftPosition = -_leftWheelServo.CachedPosition;
|
|
var currentRightPosition = _rightWheelServo.CachedPosition;
|
|
var currentLeftVelocity = -_leftWheelServo.CachedVelocity;
|
|
var currentRightVelocity = _rightWheelServo.CachedVelocity;
|
|
// var currentTime = encoderTimestamp; // Use captured timestamp
|
|
|
|
// Calculate time delta
|
|
var timeDelta = (currentTime - _lastUpdateTime).TotalSeconds;
|
|
if (timeDelta <= 0 || _isFirstUpdate)
|
|
{
|
|
_lastLeftWheelPosition = currentLeftPosition;
|
|
_lastRightWheelPosition = currentRightPosition;
|
|
_lastLeftWheelVelocity = currentLeftVelocity;
|
|
_lastRightWheelVelocity = currentRightVelocity;
|
|
_lastUpdateTime = currentTime;
|
|
// _lastThetaUpdateTime = currentTime;
|
|
_isFirstUpdate = false;
|
|
return;
|
|
}
|
|
|
|
// ===== LẤY GÓC YAW TUYỆT ĐỐI TỪ IMU ĐỂ TÍNH THETA =====
|
|
var imuYawRaw = _imu.CachedOrientation.Vector.Z;
|
|
|
|
// Transform from IMU frame to Odom frame (180° rotation around X-axis)
|
|
// Yaw in odom frame is negated relative to raw IMU yaw.
|
|
var imuYawInOdomFrame = NormalizeAngle(-imuYawRaw);
|
|
|
|
// Capture startup yaw as zero reference so odom starts at 0 rad.
|
|
if (!_imuYawZeroOffsetCaptured)
|
|
{
|
|
_imuYawZeroOffset = imuYawInOdomFrame;
|
|
_imuYawZeroOffsetCaptured = true;
|
|
_logger.LogInformation("Odometry IMU yaw zero-offset captured at {Offset:F6} rad", _imuYawZeroOffset);
|
|
}
|
|
|
|
// DeltaTheta = NormalizeAngle(imuYawInOdomFrame - _imuYawZeroOffset);
|
|
DeltaTheta = imuYawInOdomFrame - _imuYawZeroOffset;
|
|
while (DeltaTheta > Math.PI) DeltaTheta -= 2.0 * Math.PI;
|
|
while (DeltaTheta < -Math.PI) DeltaTheta += 2.0 * Math.PI;
|
|
|
|
|
|
// Calculate wheel displacements (in encoder counts)
|
|
var deltaLeft = currentLeftPosition - _lastLeftWheelPosition;
|
|
var deltaRight = currentRightPosition - _lastRightWheelPosition;
|
|
|
|
// Convert to meters
|
|
var deltaLeftMeters = deltaLeft * _leftWheelMetersPerCount;
|
|
var deltaRightMeters = deltaRight * _rightWheelMetersPerCount;
|
|
// Console.WriteLine($"Delta Left: {deltaLeftMeters}, Delta Right: {deltaRightMeters}, deltaLeft: {deltaLeft}, deltaRight: {deltaRight}");
|
|
|
|
// Calculate velocities (m/s)
|
|
var leftVelocityMetersPerSec = currentLeftVelocity * _leftWheelMetersPerCount;
|
|
var rightVelocityMetersPerSec = currentRightVelocity * _rightWheelMetersPerCount;
|
|
|
|
// Calculate average velocity (không dùng angular velocity từ encoder)
|
|
var linearVelocity = (leftVelocityMetersPerSec + rightVelocityMetersPerSec) / 2.0;
|
|
var angularVelocityFromEncoders = (rightVelocityMetersPerSec - leftVelocityMetersPerSec) / _config.Wheelbase;
|
|
|
|
// Tính độ dài
|
|
var averageVelocity = (deltaLeftMeters + deltaRightMeters) / 2.0;
|
|
var averageAngularVelocity = (-deltaRightMeters + deltaLeftMeters) / _config.Wheelbase;
|
|
|
|
var previousTheta = PoseTheta;
|
|
var currentTheta = DeltaTheta;
|
|
var headingForIntegration = NormalizeAngle((previousTheta + currentTheta) * 0.5);
|
|
|
|
// Use encoder position-delta displacement (averageVelocity) directly — no timeDelta needed
|
|
PoseX = PoseX + linearVelocity * Math.Cos(DeltaTheta) * timeDelta;
|
|
|
|
PoseY = PoseY + linearVelocity * Math.Sin(DeltaTheta) * timeDelta;
|
|
|
|
// Integrate and normalize theta
|
|
PoseTheta = DeltaTheta;
|
|
|
|
if(count >= 200)
|
|
{
|
|
// logging
|
|
Console.WriteLine($"linearVelocity: {linearVelocity}, headingForIntegration (rad): {headingForIntegration}, headingForIntegration (deg): {headingForIntegration * 180.0 / Math.PI}, timeDelta: {timeDelta}");
|
|
Console.WriteLine($"PoseX: {PoseX}, PoseY: {PoseY}");
|
|
Console.WriteLine($"PoseTheta (rad): {PoseTheta}, PoseTheta (deg): {PoseTheta * 180.0 / Math.PI}");
|
|
count = 0;
|
|
}
|
|
else
|
|
{
|
|
count++;
|
|
}
|
|
// Build pose using integrated theta from IMU
|
|
var pose = new Pose
|
|
{
|
|
Position = new Point
|
|
{
|
|
X = PoseX,
|
|
Y = PoseY,
|
|
Z = 0.0
|
|
},
|
|
Orientation = EulerToQuaternion(0, 0, PoseTheta)
|
|
};
|
|
|
|
var twist = new Twist
|
|
{
|
|
Linear = new Vector3
|
|
{
|
|
X = linearVelocity,
|
|
Y = 0.0,
|
|
Z = 0.0
|
|
},
|
|
Angular = new Vector3
|
|
{
|
|
X = 0.0,
|
|
Y = 0.0,
|
|
// Z = wz // Use IMU angular velocity
|
|
Z = angularVelocityFromEncoders // Use encoder-based angular velocity for twist (for control)
|
|
}
|
|
};
|
|
// Console.WriteLine($"Quaternion: {pose.Orientation.X}, {pose.Orientation.Y}, {pose.Orientation.Z}, {pose.Orientation.W}");
|
|
// Console.WriteLine($"PoseTheta: {PoseTheta}, IMU wz: {wz}, Encoder Angular Vel: {angularVelocityFromEncoders}");
|
|
// Build covariance matrices (simplified - can be improved with actual measurement uncertainty)
|
|
var poseCovariance = new double[PoseWithCovariance.CovarianceSize];
|
|
// Set diagonal values (position uncertainty: 0.01 m^2, orientation uncertainty: 0.01 rad^2)
|
|
poseCovariance[0] = 0.0; // x
|
|
poseCovariance[7] = 0.0; // y
|
|
poseCovariance[14] = 0.0; // z
|
|
poseCovariance[21] = 0.0; // roll
|
|
poseCovariance[28] = 0.0; // pitch
|
|
poseCovariance[35] = 0.0; // yaw
|
|
|
|
var twistCovariance = new double[TwistWithCovariance.CovarianceSize];
|
|
// Set diagonal values (linear velocity uncertainty: 0.1 m^2/s^2, angular velocity uncertainty: 0.1 rad^2/s^2)
|
|
twistCovariance[0] = 0.0; // vx
|
|
twistCovariance[7] = 0.0; // vy
|
|
twistCovariance[14] = 0.0; // vz
|
|
twistCovariance[21] = 0.0; // wx
|
|
twistCovariance[28] = 0.0; // wy
|
|
twistCovariance[35] = 0.0; // wz
|
|
|
|
// Update odometry message
|
|
_currentOdometry = new Odometry
|
|
{
|
|
Header = new Header
|
|
{
|
|
Seq = _sequenceNumber++,
|
|
Stamp = DateTime.UtcNow,
|
|
FrameId = _config.FrameId
|
|
},
|
|
ChildFrameId = _config.ChildFrameId,
|
|
Pose = new PoseWithCovariance
|
|
{
|
|
Pose = pose,
|
|
Covariance = poseCovariance
|
|
},
|
|
Twist = new TwistWithCovariance
|
|
{
|
|
Twist = twist,
|
|
Covariance = twistCovariance
|
|
}
|
|
};
|
|
|
|
updatedOdometry = _currentOdometry;
|
|
|
|
_sourceUpdateCount++;
|
|
var elapsedSec = (currentTime - _lastSourceRateLogTime).TotalSeconds;
|
|
if (elapsedSec >= 2.0)
|
|
{
|
|
var deltaCount = _sourceUpdateCount - _lastSourceUpdateCount;
|
|
var sourceHz = deltaCount / elapsedSec;
|
|
_logger.LogInformation(
|
|
"[ODOM-SOURCE-FREQ] rate={Rate:F1}Hz samples={Samples} window={Window:F2}s",
|
|
sourceHz,
|
|
deltaCount,
|
|
elapsedSec);
|
|
_lastSourceRateLogTime = currentTime;
|
|
_lastSourceUpdateCount = _sourceUpdateCount;
|
|
}
|
|
|
|
// Update last values
|
|
_lastLeftWheelPosition = currentLeftPosition;
|
|
_lastRightWheelPosition = currentRightPosition;
|
|
_lastLeftWheelVelocity = currentLeftVelocity;
|
|
_lastRightWheelVelocity = currentRightVelocity;
|
|
_lastUpdateTime = currentTime;
|
|
|
|
// DEBUG: Log raw odometry every 40 updates (~2 seconds at 20Hz)
|
|
// if (_sequenceNumber % 40 == 0)
|
|
// {
|
|
// var yaw = PoseTheta * 180.0 / Math.PI;
|
|
// _logger.LogInformation(
|
|
// "[RAW_ODOM] Pos: X={X:F3}m Y={Y:F3}m | Yaw={Yaw:F1}° | Vel: Linear={Vx:F3} Angular={Omega:F3} |Quaternion: {x},{y},{z},{w}" ,
|
|
// PoseX, PoseY, yaw, linearVelocity, wz, _currentOdometry.Pose.Pose.Orientation.X, _currentOdometry.Pose.Pose.Orientation.Y, _currentOdometry.Pose.Pose.Orientation.Z, _currentOdometry.Pose.Pose.Orientation.W
|
|
// );
|
|
// }
|
|
}
|
|
|
|
if (updatedOdometry != null)
|
|
{
|
|
OdometryUpdated?.Invoke(this, new OdometryUpdatedEventArgs(updatedOdometry.Value));
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Chuyển đổi quaternion sang yaw angle (radians)
|
|
/// </summary>
|
|
private static double QuaternionToYaw(Quaternion q)
|
|
{
|
|
// Yaw = atan2(2*(w*z + x*y), 1 - 2*(y^2 + z^2))
|
|
var sinYaw = 2.0 * (q.W * q.Z + q.X * q.Y);
|
|
var cosYaw = 1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z);
|
|
return Math.Atan2(sinYaw, cosYaw);
|
|
}
|
|
public static Quaternion EulerToQuaternion(double roll, double pitch, double yaw)
|
|
{
|
|
// Half angles
|
|
double halfRoll = roll * 0.5;
|
|
double halfPitch = pitch * 0.5;
|
|
double halfYaw = yaw * 0.5;
|
|
|
|
// Trigonometry
|
|
double cr = Math.Cos(halfRoll);
|
|
double sr = Math.Sin(halfRoll);
|
|
double cp = Math.Cos(halfPitch);
|
|
double sp = Math.Sin(halfPitch);
|
|
double cy = Math.Cos(halfYaw);
|
|
double sy = Math.Sin(halfYaw);
|
|
|
|
// Quaternion (x, y, z, w)
|
|
double w = cr * cp * cy + sr * sp * sy;
|
|
double x = sr * cp * cy - cr * sp * sy;
|
|
double y = cr * sp * cy + sr * cp * sy;
|
|
double z = cr * cp * sy - sr * sp * cy;
|
|
|
|
return new Quaternion(x, y, z, w);
|
|
}
|
|
private double NormalizeAngle(double angle)
|
|
{
|
|
while (angle > Math.PI) angle -= 2.0 * Math.PI;
|
|
while (angle < -Math.PI) angle += 2.0 * Math.PI;
|
|
return angle;
|
|
}
|
|
public void Dispose()
|
|
{
|
|
if (_disposed)
|
|
return;
|
|
|
|
_disposed = true;
|
|
|
|
try
|
|
{
|
|
_updateTimer?.Dispose();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error disposing OdometryService");
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Cấu hình cho một bánh xe trong OdometryService
|
|
/// </summary>
|
|
public class OdometryWheelConfiguration
|
|
{
|
|
/// <summary>
|
|
/// Device ID của servo điều khiển bánh xe này
|
|
/// </summary>
|
|
public string DeviceId { get; set; } = string.Empty;
|
|
|
|
/// <summary>
|
|
/// Đường kính bánh xe (mét)
|
|
/// </summary>
|
|
public double WheelDiameter { get; set; }
|
|
|
|
/// <summary>
|
|
/// Số xung encoder trên 1 vòng quay của bánh xe
|
|
/// </summary>
|
|
public int PulsesPerRevolution { get; set; }
|
|
|
|
/// <summary>
|
|
/// Đảo chiều encoder/motor nếu hướng tăng xung ngược với chiều tiến của robot
|
|
/// </summary>
|
|
public bool IsReversed { get; set; } = false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Cấu hình cho OdometryService
|
|
/// </summary>
|
|
public class OdometryConfiguration
|
|
{
|
|
/// <summary>
|
|
/// Cấu hình bánh xe trái
|
|
/// </summary>
|
|
public OdometryWheelConfiguration LeftWheel { get; set; } = new();
|
|
|
|
/// <summary>
|
|
/// Cấu hình bánh xe phải
|
|
/// </summary>
|
|
public OdometryWheelConfiguration RightWheel { get; set; } = new();
|
|
|
|
/// <summary>
|
|
/// Khoảng cách giữa hai bánh xe (wheelbase) - mét
|
|
/// </summary>
|
|
public double Wheelbase { get; set; }
|
|
|
|
/// <summary>
|
|
/// Frame ID của odometry frame (thường là "odom")
|
|
/// </summary>
|
|
public string FrameId { get; set; } = "odom";
|
|
|
|
/// <summary>
|
|
/// Child frame ID (thường là "base_link")
|
|
/// </summary>
|
|
public string ChildFrameId { get; set; } = "base_link";
|
|
|
|
/// <summary>
|
|
/// IMU Device ID để lấy angular velocity
|
|
/// </summary>
|
|
public string ImuDeviceId { get; set; } = string.Empty;
|
|
}
|
|
|
|
public sealed class OdometryUpdatedEventArgs : EventArgs
|
|
{
|
|
public OdometryUpdatedEventArgs(Odometry odometry)
|
|
{
|
|
Odometry = odometry;
|
|
}
|
|
|
|
public Odometry Odometry { get; }
|
|
}
|
|
|