Initial commit
This commit is contained in:
@@ -0,0 +1,798 @@
|
||||
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; }
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user