Initial commit
This commit is contained in:
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,53 @@
|
||||
using RobotNet10.Shared.Geometry;
|
||||
|
||||
namespace RobotNet10.RobotApp.Motion;
|
||||
|
||||
/// <summary>
|
||||
/// Cấu hình cho một bánh xe trong DifferentialDrive
|
||||
/// </summary>
|
||||
public class WheelConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// Device ID của servo điều khiển bánh xe này
|
||||
/// </summary>
|
||||
public string DeviceId { get; set; } = string.Empty;
|
||||
|
||||
/// <summary>
|
||||
/// Vị trí bánh xe so với tâm robot (Pose: x, y, orientation)
|
||||
/// Đơn vị: mét
|
||||
/// </summary>
|
||||
public Pose Position { get; set; }
|
||||
|
||||
/// <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>
|
||||
/// Polarity của encoder/motor:
|
||||
/// - false (default): Velocity dương → bánh xe roll forward theo roll direction của nó
|
||||
/// - true: Velocity dương → bánh xe roll backward (encoder/motor đấu ngược)
|
||||
/// </summary>
|
||||
public bool IsReversed { get; set; } = false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Cấu hình cho DifferentialDrive
|
||||
/// </summary>
|
||||
public class DifferentialDriveConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// Cấu hình bánh xe trái
|
||||
/// </summary>
|
||||
public WheelConfiguration LeftWheel { get; set; } = new();
|
||||
|
||||
/// <summary>
|
||||
/// Cấu hình bánh xe phải
|
||||
/// </summary>
|
||||
public WheelConfiguration RightWheel { get; set; } = new();
|
||||
}
|
||||
|
||||
@@ -0,0 +1,128 @@
|
||||
namespace RobotNet10.RobotApp.Motion;
|
||||
|
||||
/// <summary>
|
||||
/// Configuration for Extended Kalman Filter
|
||||
/// </summary>
|
||||
public class EKFConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// Device ID của IMU sensor
|
||||
/// </summary>
|
||||
public string ImuDeviceId { get; set; } = string.Empty;
|
||||
|
||||
/// <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>
|
||||
/// Tần số cập nhật EKF (Hz)
|
||||
/// </summary>
|
||||
public double UpdateRateHz { get; set; } = 20.0;
|
||||
|
||||
// Process noise covariance (diagonal values)
|
||||
|
||||
/// <summary>
|
||||
/// Process noise cho position (m^2)
|
||||
/// </summary>
|
||||
public double ProcessNoisePosition { get; set; } = 0.01;
|
||||
|
||||
/// <summary>
|
||||
/// Process noise cho orientation (rad^2)
|
||||
/// </summary>
|
||||
public double ProcessNoiseOrientation { get; set; } = 0.01;
|
||||
|
||||
/// <summary>
|
||||
/// Process noise cho velocity (m^2/s^2 hoặc rad^2/s^2)
|
||||
/// </summary>
|
||||
public double ProcessNoiseVelocity { get; set; } = 0.1;
|
||||
|
||||
// Measurement noise covariance
|
||||
|
||||
/// <summary>
|
||||
/// Measurement noise cho position từ odometry (m^2)
|
||||
/// </summary>
|
||||
public double MeasurementNoisePosition { get; set; } = 0.05;
|
||||
|
||||
/// <summary>
|
||||
/// Measurement noise cho orientation từ IMU (rad^2)
|
||||
/// </summary>
|
||||
public double MeasurementNoiseOrientation { get; set; } = 0.01;
|
||||
|
||||
// Initial state uncertainty
|
||||
|
||||
/// <summary>
|
||||
/// Initial uncertainty cho position (m^2)
|
||||
/// </summary>
|
||||
public double InitialPositionUncertainty { get; set; } = 0.1;
|
||||
|
||||
/// <summary>
|
||||
/// Initial uncertainty cho orientation (rad^2)
|
||||
/// </summary>
|
||||
public double InitialOrientationUncertainty { get; set; } = 0.1;
|
||||
|
||||
/// <summary>
|
||||
/// Initial uncertainty cho velocity (m^2/s^2 hoặc rad^2/s^2)
|
||||
/// </summary>
|
||||
public double InitialVelocityUncertainty { get; set; } = 0.5;
|
||||
|
||||
// Slip Detection Configuration
|
||||
|
||||
/// <summary>
|
||||
/// Enable wheel slip detection
|
||||
/// </summary>
|
||||
public bool EnableSlipDetection { get; set; } = true;
|
||||
|
||||
/// <summary>
|
||||
/// Enable wheels-in-air detection (uses Z-axis IMU)
|
||||
/// </summary>
|
||||
public bool EnableWheelsInAirDetection { get; set; } = true;
|
||||
|
||||
/// <summary>
|
||||
/// Enable adaptive noise scaling when slip detected
|
||||
/// If false, noise scale always stays at 1.0
|
||||
/// </summary>
|
||||
public bool EnableAdaptiveNoise { get; set; } = true;
|
||||
|
||||
/// <summary>
|
||||
/// Enable slip event logging
|
||||
/// </summary>
|
||||
public bool EnableSlipLogging { get; set; } = true;
|
||||
|
||||
/// <summary>
|
||||
/// Acceleration mismatch threshold for slip detection (m/s²)
|
||||
/// </summary>
|
||||
public double AccelerationMismatchThreshold { get; set; } = 0.5;
|
||||
|
||||
/// <summary>
|
||||
/// Innovation threshold for slip detection (sigma/standard deviations)
|
||||
/// </summary>
|
||||
public double InnovationThreshold { get; set; } = 3.0;
|
||||
|
||||
/// <summary>
|
||||
/// Maximum process noise scale factor when slip detected
|
||||
/// </summary>
|
||||
public double MaxNoiseScale { get; set; } = 10.0;
|
||||
|
||||
/// <summary>
|
||||
/// Z-axis acceleration threshold for freefall detection (m/s²)
|
||||
/// Value close to 9.81 indicates wheels in air
|
||||
/// </summary>
|
||||
public double ZAxisFreefallThreshold { get; set; } = 0.15;
|
||||
|
||||
/// <summary>
|
||||
/// Horizontal motion threshold for slip classification (m/s²)
|
||||
/// </summary>
|
||||
public double HorizontalMotionThreshold { get; set; } = 0.1;
|
||||
|
||||
/// <summary>
|
||||
/// Process noise scale factor for wheels in air scenario
|
||||
/// Very high value effectively freezes position estimation
|
||||
/// </summary>
|
||||
public double WheelsInAirNoiseScale { get; set; } = 100.0;
|
||||
}
|
||||
@@ -0,0 +1,571 @@
|
||||
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>
|
||||
/// EKFService - Extended Kalman Filter service để fusion odometry và IMU data
|
||||
/// Cải thiện độ chính xác ước lượng trạng thái robot
|
||||
/// </summary>
|
||||
public class EKFService : IHostedService, IDisposable
|
||||
{
|
||||
private readonly EKFConfiguration _config;
|
||||
private readonly IDeviceProvider _deviceProvider;
|
||||
private readonly OdometryService _odometryService;
|
||||
private readonly ILogger<EKFService> _logger;
|
||||
private readonly object _lock = new();
|
||||
|
||||
// IMU device
|
||||
private IInertialMeasurementUnit? _imu;
|
||||
|
||||
// Extended Kalman Filter
|
||||
private ExtendedKalmanFilter? _ekf;
|
||||
|
||||
// Filtered odometry output
|
||||
private Odometry _filteredOdometry = new();
|
||||
private DateTime _lastUpdateTime = DateTime.UtcNow;
|
||||
private bool _isFirstUpdate = true;
|
||||
|
||||
// Yaw offset calibration (applied at EKF level, not IMU level)
|
||||
private double _yawOffset = 0.0;
|
||||
private bool _yawOffsetCalibrated = false;
|
||||
|
||||
// Sequence number for header
|
||||
private uint _sequenceNumber = 0;
|
||||
|
||||
// EKF's own orientation (integrated from angular velocity)
|
||||
private Quaternion _ekfOrientation = new Quaternion(0, 0, 0, 1); // Identity quaternion
|
||||
private DateTime _lastOrientationUpdate = DateTime.UtcNow;
|
||||
|
||||
// Dead zone for gyroscope to prevent drift when stationary
|
||||
private const double GYRO_DEADZONE_THRESHOLD = 0.001; // rad/s (~0.057 deg/s)
|
||||
|
||||
// Timer for periodic updates
|
||||
private Timer? _updateTimer;
|
||||
private bool _disposed = false;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the filtered odometry data
|
||||
/// </summary>
|
||||
public Odometry FilteredOdometry
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
return _filteredOdometry;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public EKFService(
|
||||
IConfiguration configuration,
|
||||
IDeviceProvider deviceProvider,
|
||||
OdometryService odometryService,
|
||||
ILogger<EKFService> logger)
|
||||
{
|
||||
_deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
|
||||
_odometryService = odometryService ?? throw new ArgumentNullException(nameof(odometryService));
|
||||
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
||||
|
||||
// Load configuration
|
||||
var configSection = configuration.GetSection("Motion:EKF");
|
||||
if (!configSection.Exists())
|
||||
{
|
||||
throw new InvalidOperationException("Configuration section 'Motion:EKF' not found in appsettings.json");
|
||||
}
|
||||
|
||||
_config = new EKFConfiguration();
|
||||
configSection.Bind(_config);
|
||||
|
||||
// Validate configuration
|
||||
ValidateConfiguration();
|
||||
}
|
||||
|
||||
private void ValidateConfiguration()
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(_config.ImuDeviceId))
|
||||
throw new InvalidOperationException("ImuDeviceId is required");
|
||||
|
||||
if (_config.UpdateRateHz <= 0)
|
||||
throw new InvalidOperationException("UpdateRateHz must be greater than 0");
|
||||
|
||||
if (string.IsNullOrWhiteSpace(_config.FrameId))
|
||||
_config.FrameId = "odom";
|
||||
|
||||
if (string.IsNullOrWhiteSpace(_config.ChildFrameId))
|
||||
_config.ChildFrameId = "base_link";
|
||||
}
|
||||
|
||||
public async Task StartAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
_logger.LogInformation("Starting EKFService...");
|
||||
|
||||
try
|
||||
{
|
||||
// Wait for devices to be connected
|
||||
_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. EKFService will not be initialized.");
|
||||
return;
|
||||
}
|
||||
|
||||
// Get IMU device
|
||||
var imuDevice = _deviceProvider.GetDevice(_config.ImuDeviceId);
|
||||
if (imuDevice == null)
|
||||
{
|
||||
_logger.LogError("IMU device '{DeviceId}' not found", _config.ImuDeviceId);
|
||||
return;
|
||||
}
|
||||
|
||||
if (imuDevice is not IInertialMeasurementUnit imu)
|
||||
{
|
||||
_logger.LogError("Device '{DeviceId}' is not an IInertialMeasurementUnit", _config.ImuDeviceId);
|
||||
return;
|
||||
}
|
||||
|
||||
if (!imuDevice.IsConnected)
|
||||
{
|
||||
_logger.LogWarning("IMU device '{DeviceId}' is not connected. Status: {Status}. EKFService will wait for connection.",
|
||||
_config.ImuDeviceId, imuDevice.Status);
|
||||
// Start retry task
|
||||
_ = Task.Run(async () => await RetryInitializationWhenDevicesReadyAsync(cancellationToken));
|
||||
return;
|
||||
}
|
||||
|
||||
_imu = imu;
|
||||
|
||||
// Initialize EKF
|
||||
InitializeEKF();
|
||||
|
||||
// Start periodic update timer
|
||||
var updateInterval = TimeSpan.FromMilliseconds(1000.0 / _config.UpdateRateHz);
|
||||
var LastTime = DateTime.UtcNow;
|
||||
_updateTimer = new Timer(OnTimerCallback, null, updateInterval, updateInterval);
|
||||
|
||||
_logger.LogInformation("EKFService started successfully with update rate {UpdateRate} Hz", _config.UpdateRateHz);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error starting EKFService");
|
||||
}
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
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 imuDevice = _deviceProvider.GetDevice(_config.ImuDeviceId);
|
||||
|
||||
if (imuDevice != null && imuDevice.IsConnected && imuDevice is IInertialMeasurementUnit imu)
|
||||
{
|
||||
_logger.LogInformation("IMU device is now connected. Initializing EKFService...");
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
_imu = imu;
|
||||
}
|
||||
|
||||
InitializeEKF();
|
||||
|
||||
var updateInterval = TimeSpan.FromMilliseconds(1000.0 / _config.UpdateRateHz);
|
||||
_updateTimer = new Timer(OnTimerCallback, null, updateInterval, updateInterval);
|
||||
|
||||
_logger.LogInformation("EKFService initialized successfully after device connection");
|
||||
return;
|
||||
}
|
||||
|
||||
retryCount++;
|
||||
if (retryCount % 12 == 0) // Log every minute
|
||||
{
|
||||
_logger.LogInformation("Still waiting for IMU device to connect... (attempt {Attempt}/{MaxAttempts})",
|
||||
retryCount, maxRetries);
|
||||
}
|
||||
}
|
||||
|
||||
if (retryCount >= maxRetries)
|
||||
{
|
||||
_logger.LogWarning("Timeout waiting for IMU device to connect. EKFService will not be initialized.");
|
||||
}
|
||||
}
|
||||
|
||||
public async Task StopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
_logger.LogInformation("Stopping EKFService...");
|
||||
|
||||
try
|
||||
{
|
||||
_updateTimer?.Dispose();
|
||||
_updateTimer = null;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error stopping EKFService");
|
||||
}
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
private void OnTimerCallback(object? state)
|
||||
{
|
||||
try
|
||||
{
|
||||
UpdateEKF();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error updating EKF");
|
||||
}
|
||||
}
|
||||
|
||||
private void InitializeEKF()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
// Get initial state from odometry
|
||||
var odom = _odometryService.CurrentOdometry;
|
||||
var initialX = odom.Pose.Pose.Position.X;
|
||||
var initialY = odom.Pose.Pose.Position.Y;
|
||||
var initialTheta = QuaternionToYaw(odom.Pose.Pose.Orientation);
|
||||
|
||||
// Initial state: [x, y, theta, vx, vy, omega]
|
||||
double[] initialState = new double[6]
|
||||
{
|
||||
initialX,
|
||||
initialY,
|
||||
initialTheta,
|
||||
0.0, // vx
|
||||
0.0, // vy
|
||||
0.0 // omega
|
||||
};
|
||||
|
||||
// Initial covariance (diagonal)
|
||||
double[] initialCovDiag = new double[6]
|
||||
{
|
||||
_config.InitialPositionUncertainty, // x
|
||||
_config.InitialPositionUncertainty, // y
|
||||
_config.InitialOrientationUncertainty, // theta
|
||||
_config.InitialVelocityUncertainty, // vx
|
||||
_config.InitialVelocityUncertainty, // vy
|
||||
_config.InitialVelocityUncertainty // omega
|
||||
};
|
||||
var initialCovariance = MatrixHelper.CreateDiagonal(initialCovDiag);
|
||||
|
||||
// Process noise covariance (diagonal)
|
||||
double[] processNoiseDiag = new double[6]
|
||||
{
|
||||
_config.ProcessNoisePosition, // x
|
||||
_config.ProcessNoisePosition, // y
|
||||
_config.ProcessNoiseOrientation, // theta
|
||||
_config.ProcessNoiseVelocity, // vx
|
||||
_config.ProcessNoiseVelocity, // vy
|
||||
_config.ProcessNoiseVelocity // omega
|
||||
};
|
||||
var processNoise = MatrixHelper.CreateDiagonal(processNoiseDiag);
|
||||
|
||||
// Measurement noise covariance (diagonal) - [x, y, theta]
|
||||
double[] measurementNoiseDiag = new double[3]
|
||||
{
|
||||
_config.MeasurementNoisePosition, // x
|
||||
_config.MeasurementNoisePosition, // y
|
||||
_config.MeasurementNoiseOrientation // theta
|
||||
};
|
||||
var measurementNoise = MatrixHelper.CreateDiagonal(measurementNoiseDiag);
|
||||
|
||||
// Create EKF
|
||||
_ekf = new ExtendedKalmanFilter(
|
||||
initialState,
|
||||
initialCovariance,
|
||||
processNoise,
|
||||
measurementNoise);
|
||||
|
||||
_lastUpdateTime = DateTime.UtcNow;
|
||||
_isFirstUpdate = true;
|
||||
|
||||
// Reset yaw offset - will be calibrated on first update
|
||||
_yawOffset = 0.0;
|
||||
_yawOffsetCalibrated = false;
|
||||
|
||||
// Initialize EKF orientation (start from identity quaternion)
|
||||
_ekfOrientation = new Quaternion(0, 0, 0, 1);
|
||||
_lastOrientationUpdate = DateTime.UtcNow;
|
||||
|
||||
_logger.LogInformation("EKF initialized with state: X={X:F3}, Y={Y:F3}, Theta={Theta:F3}",
|
||||
initialX, initialY, initialTheta);
|
||||
}
|
||||
}
|
||||
|
||||
private void UpdateEKF()
|
||||
{
|
||||
if (_ekf == null || _imu == null)
|
||||
return;
|
||||
|
||||
// Check if IMU is still connected
|
||||
var imuDevice = _deviceProvider.GetDevice(_config.ImuDeviceId);
|
||||
if (imuDevice == null || !imuDevice.IsConnected)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
var currentTime = DateTime.UtcNow;
|
||||
var dt = (currentTime - _lastUpdateTime).TotalSeconds;
|
||||
|
||||
if (dt <= 0 || _isFirstUpdate)
|
||||
{
|
||||
_lastUpdateTime = currentTime;
|
||||
_isFirstUpdate = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// Get odometry data (for velocities)
|
||||
var odom = _odometryService.CurrentOdometry;
|
||||
var vx = odom.Twist.Twist.Linear.X;
|
||||
var vy = odom.Twist.Twist.Linear.Y;
|
||||
var omega = odom.Twist.Twist.Angular.Z;
|
||||
|
||||
// PREDICTION STEP: Propagate state using odometry velocities
|
||||
_ekf.Predict(vx, vy, omega, dt);
|
||||
|
||||
// Get IMU orientation (raw from sensor)
|
||||
// double rawYaw = 0.0;
|
||||
// var quaternion = _imu.CachedQuaternion;
|
||||
// if (quaternion.HasValue)
|
||||
// {
|
||||
// rawYaw = QuaternionToYaw(quaternion.Value.Quaternion);
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// // Fallback to Euler angles
|
||||
// var orientation = _imu.CachedOrientation;
|
||||
// rawYaw = orientation.Vector.Z; // Yaw is Z component
|
||||
// }
|
||||
|
||||
// Calibrate yaw offset on first reading (set current yaw as zero reference)
|
||||
// if (!_yawOffsetCalibrated)
|
||||
// {
|
||||
// _yawOffset = rawYaw;
|
||||
// _yawOffsetCalibrated = true;
|
||||
// _logger.LogInformation("IMU Yaw offset calibrated at EKF level: {Offset:F3} rad ({OffsetDeg:F1}°)",
|
||||
// _yawOffset, _yawOffset * 180.0 / Math.PI);
|
||||
// }
|
||||
|
||||
// ===== EKF TÍCH PHÂN ANGULAR VELOCITY ĐỂ TÍNH ORIENTATION =====
|
||||
// Get angular velocity from IMU (raw data, không dùng CachedOrientation)
|
||||
var angularVelocity = _imu.CachedAngularVelocity.Vector;
|
||||
double wx = angularVelocity.X;
|
||||
double wy = angularVelocity.Y;
|
||||
double wz = angularVelocity.Z;
|
||||
|
||||
// Transform from IMU frame to Odom frame (180° rotation around X-axis)
|
||||
// Rotation matrix: [1, 0, 0; 0, -1, 0; 0, 0, -1]
|
||||
// This accounts for IMU mounting orientation (upside down relative to base_link)
|
||||
wx = wx; // X unchanged
|
||||
wy = -wy; // Y negated
|
||||
wz = -wz; // Z negated
|
||||
|
||||
// Apply dead zone to filter gyro noise/drift when stationary
|
||||
// if (Math.Abs(wx) < GYRO_DEADZONE_THRESHOLD) wx = 0.0;
|
||||
// if (Math.Abs(wy) < GYRO_DEADZONE_THRESHOLD) wy = 0.0;
|
||||
// if (Math.Abs(wz) < GYRO_DEADZONE_THRESHOLD) wz = 0.0;
|
||||
|
||||
// Integrate quaternion using angular velocity
|
||||
// Formula: q_new = q_old + 0.5 * dt * [0, wx, wy, wz] * q_old
|
||||
var q = _ekfOrientation;
|
||||
|
||||
// Half of angular velocity components
|
||||
double halfWx = 0.5 * wx;
|
||||
double halfWy = 0.5 * wy;
|
||||
double halfWz = 0.5 * wz;
|
||||
|
||||
// Quaternion derivative: dq/dt = 0.5 * [0, wx, wy, wz] * q
|
||||
double dqx = halfWx * q.W + halfWy * q.Z - halfWz * q.Y;
|
||||
double dqy = halfWy * q.W - halfWx * q.Z + halfWz * q.X;
|
||||
double dqz = halfWz * q.W + halfWx * q.Y - halfWy * q.X;
|
||||
double dqw = -halfWx * q.X - halfWy * q.Y - halfWz * q.Z;
|
||||
|
||||
// Integrate: q_new = q_old + dq * dt
|
||||
double newQx = q.X + dqx * dt;
|
||||
double newQy = q.Y + dqy * dt;
|
||||
double newQz = q.Z + dqz * dt;
|
||||
double newQw = q.W + dqw * dt;
|
||||
|
||||
// Normalize quaternion to maintain unit length
|
||||
double norm = Math.Sqrt(newQx * newQx + newQy * newQy + newQz * newQz + newQw * newQw);
|
||||
if (norm > 1e-6) // Avoid division by zero
|
||||
{
|
||||
newQx /= norm;
|
||||
newQy /= norm;
|
||||
newQz /= norm;
|
||||
newQw /= norm;
|
||||
}
|
||||
|
||||
// Update EKF orientation
|
||||
_ekfOrientation = new Quaternion(newQx, newQy, newQz, newQw);
|
||||
_lastOrientationUpdate = currentTime;
|
||||
|
||||
// Convert quaternion to yaw for EKF update
|
||||
double measuredTheta = QuaternionToYaw(_ekfOrientation);
|
||||
measuredTheta = NormalizeAngle(measuredTheta);
|
||||
|
||||
// UPDATE STEP: Correct state using integrated orientation from angular velocity
|
||||
_ekf.UpdateOrientation(measuredTheta);
|
||||
// Console.WriteLine($"EKF Update: X={_ekf.X}, Y={_ekf.Y}, Theta={_ekf.Theta}, LinearVel={_ekf.Vx}, AngularVel={_ekf.Omega}");
|
||||
|
||||
// Build filtered odometry message
|
||||
var filteredPose = new Pose
|
||||
{
|
||||
Position = new Point
|
||||
{
|
||||
X = _ekf.X,
|
||||
Y = _ekf.Y,
|
||||
Z = 0.0
|
||||
},
|
||||
Orientation = new Quaternion
|
||||
{
|
||||
X = 0.0,
|
||||
Y = 0.0,
|
||||
Z = Math.Sin(_ekf.Theta / 2.0),
|
||||
W = Math.Cos(_ekf.Theta / 2.0)
|
||||
}
|
||||
};
|
||||
|
||||
var filteredTwist = new Twist
|
||||
{
|
||||
Linear = new Vector3
|
||||
{
|
||||
X = _ekf.Vx,
|
||||
Y = _ekf.Vy,
|
||||
Z = 0.0
|
||||
},
|
||||
Angular = new Vector3
|
||||
{
|
||||
X = 0.0,
|
||||
Y = 0.0,
|
||||
Z = _ekf.Omega
|
||||
}
|
||||
};
|
||||
|
||||
// Extract covariance from EKF state covariance
|
||||
var stateCov = _ekf.Covariance;
|
||||
var poseCovariance = new double[36]; // 6x6
|
||||
var twistCovariance = new double[36]; // 6x6
|
||||
|
||||
// Map state covariance to pose covariance
|
||||
// Pose: [x, y, z, roll, pitch, yaw]
|
||||
|
||||
poseCovariance[0] = stateCov[0, 0]; // x
|
||||
poseCovariance[7] = stateCov[1, 1]; // y
|
||||
poseCovariance[14] = 0.0; // z (not estimated)
|
||||
poseCovariance[21] = 0.0; // roll (not estimated)
|
||||
poseCovariance[28] = 0.0; // pitch (not estimated)
|
||||
poseCovariance[35] = stateCov[2, 2]; // yaw (theta)
|
||||
|
||||
// Map state covariance to twist covariance
|
||||
// Twist: [vx, vy, vz, wx, wy, ywz]
|
||||
|
||||
twistCovariance[0] = stateCov[3, 3]; // vx
|
||||
twistCovariance[7] = stateCov[4, 4]; // vy
|
||||
twistCovariance[14] = 0.0; // vz (not estimated)
|
||||
twistCovariance[21] = 0.0; // wx (not estimated)
|
||||
twistCovariance[28] = 0.0; // wy (not estimated)
|
||||
twistCovariance[35] = stateCov[5, 5]; // wz (omega)
|
||||
|
||||
_filteredOdometry = new Odometry
|
||||
{
|
||||
Header = new Header
|
||||
{
|
||||
Seq = _sequenceNumber++,
|
||||
Stamp = currentTime,
|
||||
FrameId = _config.FrameId
|
||||
},
|
||||
ChildFrameId = _config.ChildFrameId,
|
||||
Pose = new PoseWithCovariance
|
||||
{
|
||||
Pose = filteredPose,
|
||||
Covariance = poseCovariance
|
||||
},
|
||||
Twist = new TwistWithCovariance
|
||||
{
|
||||
Twist = filteredTwist,
|
||||
Covariance = twistCovariance
|
||||
}
|
||||
};
|
||||
|
||||
_lastUpdateTime = currentTime;
|
||||
|
||||
// Log periodically (every 2 seconds)
|
||||
if (_sequenceNumber % 40 == 0) // At 20Hz, 40 updates = 2 seconds
|
||||
{
|
||||
_logger.LogDebug("EKF State: X={X:F3}, Y={Y:F3}, Theta={Theta:F3}, Vx={Vx:F3}, Omega={Omega:F3}",
|
||||
_ekf.X, _ekf.Y, _ekf.Theta, _ekf.Vx, _ekf.Omega);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Normalize angle to [-π, π] range
|
||||
/// </summary>
|
||||
private static double NormalizeAngle(double angle)
|
||||
{
|
||||
while (angle > Math.PI) angle -= 2.0 * Math.PI;
|
||||
while (angle < -Math.PI) angle += 2.0 * Math.PI;
|
||||
return angle;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
// /// Manually recalibrate yaw offset - reset current yaw as new zero reference
|
||||
// /// </summary>
|
||||
// public void RecalibrateYawOffset()
|
||||
// {
|
||||
// lock (_lock)
|
||||
// {
|
||||
// _yawOffsetCalibrated = false;
|
||||
// _logger.LogInformation("Yaw offset recalibration requested. Will recalibrate on next update.");
|
||||
// }
|
||||
// }
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
return;
|
||||
|
||||
_disposed = true;
|
||||
|
||||
try
|
||||
{
|
||||
_updateTimer?.Dispose();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error disposing EKFService");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,593 @@
|
||||
namespace RobotNet10.RobotApp.Motion;
|
||||
|
||||
/// <summary>
|
||||
/// Extended Kalman Filter for 2D differential drive robot localization
|
||||
/// Uses CONTROL INPUT-BASED model (encoder deltas) instead of constant velocity
|
||||
///
|
||||
/// State vector: [x, y, θ, vx, vy, ω] (6-DOF)
|
||||
/// - x, y: position in meters
|
||||
/// - θ: orientation (yaw) in radians
|
||||
/// - vx, vy: linear velocity in m/s (body frame)
|
||||
/// - ω: angular velocity in rad/s
|
||||
///
|
||||
/// Control input: [deltaLeft, deltaRight] in meters (from wheel encoders)
|
||||
///
|
||||
/// This approach is more accurate than velocity-based models because:
|
||||
/// 1. Encoder deltas are absolute measurements (no timing dependency)
|
||||
/// 2. No constant velocity assumption (robot can accelerate/decelerate)
|
||||
/// 3. Matches the proven accuracy of pure odometry calculation
|
||||
/// </summary>
|
||||
public class ExtendedKalmanFilter
|
||||
{
|
||||
private const int STATE_SIZE = 6;
|
||||
private const int MEASUREMENT_IMU_SIZE = 1; // Only ω from IMU gyro
|
||||
|
||||
// Covariance clamping to prevent unbounded growth
|
||||
// When robot stands still, IMU gyro only observes omega (not theta directly),
|
||||
// so theta variance grows without bound. Clamping prevents this.
|
||||
private const double MAX_POSITION_COVARIANCE = 1.0; // m²
|
||||
private const double MAX_ORIENTATION_COVARIANCE = 0.1; // rad² (~18°)
|
||||
private const double MAX_VELOCITY_COVARIANCE = 1.0; // (m/s)²
|
||||
|
||||
// State vector [x, y, θ, vx, vy, ω]
|
||||
private double[] _state;
|
||||
|
||||
// State covariance matrix P (6x6)
|
||||
private double[,] _covariance;
|
||||
|
||||
// Process noise covariance Q (6x6) - continuous-time noise power spectral density
|
||||
// Scaled by dt in Predict step: P += Q * dt (not Q per step)
|
||||
private double[,] _processNoise;
|
||||
|
||||
// Measurement noise covariance - IMU gyroscope (1x1)
|
||||
private double _measurementNoiseImuGyro;
|
||||
|
||||
// FIX #3: Store dt for Jacobian calculation (cross-correlation between theta and omega)
|
||||
private double _lastDt = 0.005;
|
||||
|
||||
// FIX #3 & #4: Tuning constants for motion model
|
||||
// ENCODER_WEIGHT: How much to trust encoder vs omega state for theta prediction
|
||||
// Higher = more trust in encoder (more accurate short-term), lower = more IMU influence
|
||||
private const double ENCODER_WEIGHT = 0.8;
|
||||
|
||||
// VELOCITY_ALPHA: How much to trust new encoder measurement vs previous velocity state
|
||||
// Higher = more responsive but noisier, lower = smoother but more lag
|
||||
private const double VELOCITY_ALPHA = 0.7;
|
||||
|
||||
/// <summary>
|
||||
/// Current state estimate [x, y, θ, vx, vy, ω]
|
||||
/// </summary>
|
||||
public double[] State => (double[])_state.Clone();
|
||||
|
||||
/// <summary>
|
||||
/// Position X (m)
|
||||
/// </summary>
|
||||
public double X => _state[0];
|
||||
|
||||
/// <summary>
|
||||
/// Position Y (m)
|
||||
/// </summary>
|
||||
public double Y => _state[1];
|
||||
|
||||
/// <summary>
|
||||
/// Orientation θ (rad)
|
||||
/// </summary>
|
||||
public double Theta => _state[2];
|
||||
|
||||
/// <summary>
|
||||
/// Velocity X (m/s) in body frame
|
||||
/// </summary>
|
||||
public double VelocityX => _state[3];
|
||||
|
||||
/// <summary>
|
||||
/// Velocity Y (m/s) in body frame
|
||||
/// </summary>
|
||||
public double VelocityY => _state[4];
|
||||
|
||||
/// <summary>
|
||||
/// Angular velocity ω (rad/s)
|
||||
/// </summary>
|
||||
public double AngularVelocity => _state[5];
|
||||
|
||||
// Backward-compatible aliases used by legacy EKFService.
|
||||
public double Vx => VelocityX;
|
||||
public double Vy => VelocityY;
|
||||
public double Omega => AngularVelocity;
|
||||
public double[,] Covariance => (double[,])_covariance.Clone();
|
||||
|
||||
/// <summary>
|
||||
/// Position variance (m²) - average of x and y variances
|
||||
/// </summary>
|
||||
public double PositionVariance => (_covariance[0, 0] + _covariance[1, 1]) / 2.0;
|
||||
|
||||
/// <summary>
|
||||
/// Orientation variance (rad²)
|
||||
/// </summary>
|
||||
public double OrientationVariance => _covariance[2, 2];
|
||||
|
||||
/// <summary>
|
||||
/// Cross-correlation between theta and omega (rad·rad/s)
|
||||
/// Used for debugging EKF behavior
|
||||
/// </summary>
|
||||
public double ThetaOmegaCovariance => _covariance[2, 5];
|
||||
|
||||
/// <summary>
|
||||
/// Initialize EKF with initial state and covariances
|
||||
/// </summary>
|
||||
public ExtendedKalmanFilter(
|
||||
double[] initialState,
|
||||
double initialPositionVariance = 0.001, // m² (std dev = ~3cm)
|
||||
double initialOrientationVariance = 0.001, // rad² (std dev = ~1.8°)
|
||||
double initialVelocityVariance = 0.01, // (m/s)² (std dev = 0.1 m/s)
|
||||
double processNoisePosition = 0.0001, // Position uncertainty growth
|
||||
double processNoiseOrientation = 0.00001, // Orientation uncertainty growth
|
||||
double processNoiseVelocity = 0.01, // Velocity uncertainty growth
|
||||
double measurementNoiseImuGyro = 0.01) // IMU gyro noise (rad/s)
|
||||
{
|
||||
if (initialState == null || initialState.Length != STATE_SIZE)
|
||||
{
|
||||
throw new ArgumentException($"Initial state must have {STATE_SIZE} elements [x, y, θ, vx, vy, ω]");
|
||||
}
|
||||
|
||||
_state = (double[])initialState.Clone();
|
||||
|
||||
// Initialize covariance matrix P
|
||||
_covariance = new double[STATE_SIZE, STATE_SIZE];
|
||||
_covariance[0, 0] = initialPositionVariance; // x
|
||||
_covariance[1, 1] = initialPositionVariance; // y
|
||||
_covariance[2, 2] = initialOrientationVariance; // θ
|
||||
_covariance[3, 3] = initialVelocityVariance; // vx
|
||||
_covariance[4, 4] = initialVelocityVariance; // vy
|
||||
_covariance[5, 5] = initialVelocityVariance; // ω
|
||||
|
||||
// Initialize process noise Q
|
||||
_processNoise = new double[STATE_SIZE, STATE_SIZE];
|
||||
_processNoise[0, 0] = processNoisePosition;
|
||||
_processNoise[1, 1] = processNoisePosition;
|
||||
_processNoise[2, 2] = processNoiseOrientation;
|
||||
_processNoise[3, 3] = processNoiseVelocity;
|
||||
_processNoise[4, 4] = processNoiseVelocity;
|
||||
_processNoise[5, 5] = processNoiseVelocity;
|
||||
|
||||
// Initialize measurement noise - IMU gyroscope only
|
||||
_measurementNoiseImuGyro = measurementNoiseImuGyro * measurementNoiseImuGyro;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Backward-compatible constructor used by legacy EKFService.
|
||||
/// </summary>
|
||||
public ExtendedKalmanFilter(
|
||||
double[] initialState,
|
||||
double[,] initialCovariance,
|
||||
double[,] processNoise,
|
||||
double[,] measurementNoise)
|
||||
{
|
||||
if (initialState == null || initialState.Length != STATE_SIZE)
|
||||
throw new ArgumentException($"Initial state must have {STATE_SIZE} elements [x, y, θ, vx, vy, ω]");
|
||||
|
||||
if (initialCovariance == null || initialCovariance.GetLength(0) != STATE_SIZE || initialCovariance.GetLength(1) != STATE_SIZE)
|
||||
throw new ArgumentException("Initial covariance must be 6x6");
|
||||
|
||||
if (processNoise == null || processNoise.GetLength(0) != STATE_SIZE || processNoise.GetLength(1) != STATE_SIZE)
|
||||
throw new ArgumentException("Process noise must be 6x6");
|
||||
|
||||
_state = (double[])initialState.Clone();
|
||||
_covariance = (double[,])initialCovariance.Clone();
|
||||
_processNoise = (double[,])processNoise.Clone();
|
||||
|
||||
// Legacy EKFService provides measurement noise as [x, y, theta].
|
||||
_measurementNoiseImuGyro = measurementNoise.GetLength(0) >= 3 && measurementNoise.GetLength(1) >= 3
|
||||
? measurementNoise[2, 2]
|
||||
: 0.01;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Prediction step using control input (encoder deltas)
|
||||
///
|
||||
/// Motion model for differential drive:
|
||||
/// distance = (deltaLeft + deltaRight) / 2
|
||||
/// deltaTheta = (deltaRight - deltaLeft) / wheelbase
|
||||
/// midTheta = θ + deltaTheta / 2
|
||||
/// x_new = x + distance * cos(midTheta)
|
||||
/// y_new = y + distance * sin(midTheta)
|
||||
///
|
||||
/// FIX #3: θ_new now blends encoder and omega state for IMU correction:
|
||||
/// θ_new = ENCODER_WEIGHT * (θ + deltaTheta) + (1-ENCODER_WEIGHT) * (θ + ω*dt)
|
||||
/// This creates mathematical dependency ∂θ/∂ω enabling IMU to correct theta
|
||||
///
|
||||
/// FIX #4: Velocities use complementary filter instead of pure overwrite:
|
||||
/// vx_new = VELOCITY_ALPHA * vx_measured + (1-VELOCITY_ALPHA) * vx_prev
|
||||
/// This provides smoothing while tracking encoder measurements
|
||||
/// </summary>
|
||||
public void Predict(double deltaLeftMeters, double deltaRightMeters, double wheelbase, double dt)
|
||||
{
|
||||
if (dt <= 0 || wheelbase <= 0) return;
|
||||
|
||||
_lastDt = dt; // Store for Jacobian calculation
|
||||
|
||||
double x = _state[0];
|
||||
double y = _state[1];
|
||||
double theta = _state[2];
|
||||
double vx_prev = _state[3];
|
||||
double vy_prev = _state[4];
|
||||
double omega_prev = _state[5];
|
||||
|
||||
// Calculate motion from encoder deltas (control input)
|
||||
double distance = (deltaLeftMeters + deltaRightMeters) / 2.0;
|
||||
double deltaTheta = (deltaRightMeters - deltaLeftMeters) / wheelbase;
|
||||
|
||||
// Use midpoint orientation for arc motion (same as pure odometry)
|
||||
double midTheta = theta + deltaTheta / 2.0;
|
||||
|
||||
// Predict new position
|
||||
double x_new = x + distance * Math.Cos(midTheta);
|
||||
double y_new = y + distance * Math.Sin(midTheta);
|
||||
|
||||
// FIX #3: Theta prediction blends encoder and omega state
|
||||
// This creates the mathematical dependency: θ_new depends on ω
|
||||
// Enabling IMU gyro updates to affect theta through Kalman gain
|
||||
double theta_from_encoder = theta + deltaTheta;
|
||||
double theta_from_omega = theta + omega_prev * dt;
|
||||
|
||||
// Use encoder-dominant blend (encoder is more accurate short-term)
|
||||
// But keep some omega influence for IMU correction to work
|
||||
double theta_new = NormalizeAngle(
|
||||
ENCODER_WEIGHT * theta_from_encoder +
|
||||
(1.0 - ENCODER_WEIGHT) * theta_from_omega);
|
||||
|
||||
// FIX #4: Velocity estimation with complementary filter (not pure overwrite)
|
||||
// This provides smoothing while still tracking encoder measurements
|
||||
double vx_measured = distance / dt;
|
||||
double omega_measured = deltaTheta / dt;
|
||||
|
||||
double vx_new = VELOCITY_ALPHA * vx_measured + (1.0 - VELOCITY_ALPHA) * vx_prev;
|
||||
double vy_new = 0; // No lateral motion for differential drive
|
||||
double omega_new = VELOCITY_ALPHA * omega_measured + (1.0 - VELOCITY_ALPHA) * omega_prev;
|
||||
|
||||
// Update state
|
||||
_state[0] = x_new;
|
||||
_state[1] = y_new;
|
||||
_state[2] = theta_new;
|
||||
_state[3] = vx_new;
|
||||
_state[4] = vy_new;
|
||||
_state[5] = omega_new;
|
||||
|
||||
// Compute Jacobian F = ∂f/∂x for covariance propagation
|
||||
double[,] F = ComputeProcessJacobian(theta, distance, deltaTheta, dt);
|
||||
|
||||
// Predict covariance: P = F*P*F^T + Q*dt
|
||||
// Q is continuous-time noise PSD, must scale by dt so uncertainty growth rate
|
||||
// is independent of update frequency (200Hz vs 100Hz gives same growth per second)
|
||||
_covariance = AddMatrices(
|
||||
MultiplyMatrices(MultiplyMatrices(F, _covariance), Transpose(F)),
|
||||
ScaleMatrix(_processNoise, dt)
|
||||
);
|
||||
|
||||
// Clamp covariance to prevent unbounded growth
|
||||
ClampCovariance();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Update step with IMU gyroscope measurement (orientation correction only)
|
||||
///
|
||||
/// Measurement model: z = ω_gyro (angular velocity from IMU)
|
||||
/// Predicted measurement: h(x) = ω (state angular velocity)
|
||||
///
|
||||
/// We ONLY use IMU gyroscope for orientation correction because:
|
||||
/// - IMU gyros are very accurate for angular velocity (drift-free short-term)
|
||||
/// - Encoder-based orientation accumulates drift over time
|
||||
/// - IMU accelerometers are noisy and don't help for position (no double integration)
|
||||
///
|
||||
/// This simple measurement model avoids the bugs in the original EKF:
|
||||
/// - No constant acceleration assumption (was wrong)
|
||||
/// - No trying to fuse accelerometer (adds noise, not value)
|
||||
/// - Clean, simple, effective
|
||||
/// </summary>
|
||||
public void UpdateWithImuGyro(double omegaGyroMeasured)
|
||||
{
|
||||
// Measurement: z = ω_gyro
|
||||
double z = omegaGyroMeasured;
|
||||
|
||||
// Predicted measurement: h(x) = ω (from state)
|
||||
double omega = _state[5];
|
||||
double h = omega;
|
||||
|
||||
// Innovation (measurement residual): y = z - h(x)
|
||||
double innovation = z - h;
|
||||
|
||||
// Measurement Jacobian H = ∂h/∂x
|
||||
// h = ω → only depends on state[5]
|
||||
// H = [0, 0, 0, 0, 0, 1]
|
||||
double[,] H = new double[1, STATE_SIZE];
|
||||
H[0, 5] = 1.0;
|
||||
|
||||
// Innovation covariance: S = H*P*H^T + R
|
||||
// Since H has only one non-zero element, this simplifies to:
|
||||
// S = P[5,5] + R_gyro
|
||||
double S = _covariance[5, 5] + _measurementNoiseImuGyro;
|
||||
|
||||
if (Math.Abs(S) < 1e-10)
|
||||
{
|
||||
// Avoid division by zero
|
||||
return;
|
||||
}
|
||||
|
||||
// Kalman gain: K = P*H^T*S^(-1)
|
||||
// Simplified: K = P[:,5] / S (column 5 of P divided by scalar S)
|
||||
double[] K = new double[STATE_SIZE];
|
||||
for (int i = 0; i < STATE_SIZE; i++)
|
||||
{
|
||||
K[i] = _covariance[i, 5] / S;
|
||||
}
|
||||
|
||||
// Update state: x = x + K*y
|
||||
for (int i = 0; i < STATE_SIZE; i++)
|
||||
{
|
||||
_state[i] += K[i] * innovation;
|
||||
}
|
||||
|
||||
// Normalize theta after update
|
||||
_state[2] = NormalizeAngle(_state[2]);
|
||||
|
||||
// Update covariance: P = (I - K*H)*P
|
||||
// Simplified: P = P - K * P[5,:] (subtract K times row 5 of P)
|
||||
// Must save row 5 first since it's modified during the loop
|
||||
double[] row5 = new double[STATE_SIZE];
|
||||
for (int j = 0; j < STATE_SIZE; j++)
|
||||
row5[j] = _covariance[5, j];
|
||||
|
||||
for (int i = 0; i < STATE_SIZE; i++)
|
||||
{
|
||||
for (int j = 0; j < STATE_SIZE; j++)
|
||||
{
|
||||
_covariance[i, j] -= K[i] * row5[j];
|
||||
}
|
||||
}
|
||||
|
||||
// Clamp covariance after measurement update
|
||||
ClampCovariance();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Backward-compatible orientation update used by legacy EKFService.
|
||||
/// </summary>
|
||||
public void UpdateOrientation(double measuredTheta)
|
||||
{
|
||||
var innovation = NormalizeAngle(measuredTheta - _state[2]);
|
||||
var orientationVariance = _covariance[2, 2];
|
||||
var measurementVariance = 0.01;
|
||||
var kalmanGain = orientationVariance / (orientationVariance + measurementVariance);
|
||||
|
||||
_state[2] = NormalizeAngle(_state[2] + kalmanGain * innovation);
|
||||
_covariance[2, 2] = Math.Max(1e-9, (1.0 - kalmanGain) * orientationVariance);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Reset EKF state to a new pose
|
||||
/// </summary>
|
||||
public void Reset(double x, double y, double theta, double vx = 0, double vy = 0, double omega = 0)
|
||||
{
|
||||
_state[0] = x;
|
||||
_state[1] = y;
|
||||
_state[2] = NormalizeAngle(theta);
|
||||
_state[3] = vx;
|
||||
_state[4] = vy;
|
||||
_state[5] = omega;
|
||||
|
||||
// Reset covariance to initial values
|
||||
for (int i = 0; i < STATE_SIZE; i++)
|
||||
{
|
||||
for (int j = 0; j < STATE_SIZE; j++)
|
||||
{
|
||||
if (i == j)
|
||||
{
|
||||
if (i < 2) _covariance[i, j] = 0.001; // position
|
||||
else if (i == 2) _covariance[i, j] = 0.001; // orientation
|
||||
else _covariance[i, j] = 0.01; // velocity
|
||||
}
|
||||
else
|
||||
{
|
||||
_covariance[i, j] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// FIX #1: Apply external theta correction directly to EKF state
|
||||
/// This ensures the correction persists in subsequent predictions
|
||||
/// Called after hybrid theta correction is calculated from IMU innovation
|
||||
/// </summary>
|
||||
/// <param name="thetaCorrection">Theta correction in radians to add to current theta</param>
|
||||
public void ApplyThetaCorrection(double thetaCorrection)
|
||||
{
|
||||
if (Math.Abs(thetaCorrection) < 1e-9) return;
|
||||
|
||||
_state[2] = NormalizeAngle(_state[2] + thetaCorrection);
|
||||
}
|
||||
|
||||
#region Jacobian Computations
|
||||
|
||||
/// <summary>
|
||||
/// Compute process model Jacobian F = ∂f/∂x
|
||||
///
|
||||
/// FIX #3: Added ∂θ/∂ω term to create cross-correlation
|
||||
/// This enables IMU gyro updates to affect theta estimate through Kalman gain
|
||||
///
|
||||
/// Motion model (with blending):
|
||||
/// x_new = x + distance * cos(midTheta)
|
||||
/// y_new = y + distance * sin(midTheta)
|
||||
/// θ_new = ENCODER_WEIGHT * (θ + deltaTheta) + (1-ENCODER_WEIGHT) * (θ + ω*dt)
|
||||
/// vx_new = VELOCITY_ALPHA * vx_measured + (1-VELOCITY_ALPHA) * vx_prev
|
||||
/// ω_new = VELOCITY_ALPHA * ω_measured + (1-VELOCITY_ALPHA) * ω_prev
|
||||
///
|
||||
/// Key Jacobian terms:
|
||||
/// ∂θ_new/∂θ = 1 (both terms have θ)
|
||||
/// ∂θ_new/∂ω = (1-ENCODER_WEIGHT) * dt ← THIS IS THE FIX!
|
||||
/// ∂vx_new/∂vx = (1-VELOCITY_ALPHA)
|
||||
/// ∂ω_new/∂ω = (1-VELOCITY_ALPHA)
|
||||
/// </summary>
|
||||
private double[,] ComputeProcessJacobian(double theta, double distance, double deltaTheta, double dt)
|
||||
{
|
||||
double midTheta = theta + deltaTheta / 2.0;
|
||||
double cosM = Math.Cos(midTheta);
|
||||
double sinM = Math.Sin(midTheta);
|
||||
|
||||
// Start with identity matrix
|
||||
double[,] F = Identity(STATE_SIZE);
|
||||
|
||||
// ∂x/∂θ = -distance * sin(midTheta)
|
||||
F[0, 2] = -distance * sinM;
|
||||
|
||||
// ∂y/∂θ = distance * cos(midTheta)
|
||||
F[1, 2] = distance * cosM;
|
||||
|
||||
// FIX #3: ∂θ/∂ω = (1-ENCODER_WEIGHT) * dt
|
||||
// This creates cross-correlation between theta and omega in covariance matrix
|
||||
// Enabling IMU gyro updates to affect theta through Kalman gain K[2]
|
||||
F[2, 5] = (1.0 - ENCODER_WEIGHT) * dt;
|
||||
|
||||
// FIX #4: Velocity state dependencies (complementary filter)
|
||||
// ∂vx/∂vx_prev = (1-VELOCITY_ALPHA)
|
||||
F[3, 3] = 1.0 - VELOCITY_ALPHA;
|
||||
|
||||
// ∂ω/∂ω_prev = (1-VELOCITY_ALPHA)
|
||||
F[5, 5] = 1.0 - VELOCITY_ALPHA;
|
||||
|
||||
return F;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Matrix Operations
|
||||
|
||||
private static double[,] Identity(int size)
|
||||
{
|
||||
double[,] I = new double[size, size];
|
||||
for (int i = 0; i < size; i++)
|
||||
I[i, i] = 1.0;
|
||||
return I;
|
||||
}
|
||||
|
||||
private static double[,] Transpose(double[,] matrix)
|
||||
{
|
||||
int rows = matrix.GetLength(0);
|
||||
int cols = matrix.GetLength(1);
|
||||
double[,] result = new double[cols, rows];
|
||||
|
||||
for (int i = 0; i < rows; i++)
|
||||
for (int j = 0; j < cols; j++)
|
||||
result[j, i] = matrix[i, j];
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
private static double[,] MultiplyMatrices(double[,] A, double[,] B)
|
||||
{
|
||||
int rowsA = A.GetLength(0);
|
||||
int colsA = A.GetLength(1);
|
||||
int rowsB = B.GetLength(0);
|
||||
int colsB = B.GetLength(1);
|
||||
|
||||
if (colsA != rowsB)
|
||||
throw new ArgumentException("Matrix dimensions do not match for multiplication");
|
||||
|
||||
double[,] result = new double[rowsA, colsB];
|
||||
|
||||
for (int i = 0; i < rowsA; i++)
|
||||
{
|
||||
for (int j = 0; j < colsB; j++)
|
||||
{
|
||||
double sum = 0;
|
||||
for (int k = 0; k < colsA; k++)
|
||||
{
|
||||
sum += A[i, k] * B[k, j];
|
||||
}
|
||||
result[i, j] = sum;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
private static double[,] ScaleMatrix(double[,] matrix, double scalar)
|
||||
{
|
||||
int rows = matrix.GetLength(0);
|
||||
int cols = matrix.GetLength(1);
|
||||
double[,] result = new double[rows, cols];
|
||||
|
||||
for (int i = 0; i < rows; i++)
|
||||
for (int j = 0; j < cols; j++)
|
||||
result[i, j] = matrix[i, j] * scalar;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
private static double[,] AddMatrices(double[,] A, double[,] B)
|
||||
{
|
||||
int rows = A.GetLength(0);
|
||||
int cols = A.GetLength(1);
|
||||
|
||||
if (rows != B.GetLength(0) || cols != B.GetLength(1))
|
||||
throw new ArgumentException("Matrix dimensions must match for addition");
|
||||
|
||||
double[,] result = new double[rows, cols];
|
||||
|
||||
for (int i = 0; i < rows; i++)
|
||||
for (int j = 0; j < cols; j++)
|
||||
result[i, j] = A[i, j] + B[i, j];
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
/// <summary>
|
||||
/// Clamp covariance diagonal elements to prevent unbounded growth.
|
||||
/// When clamping a diagonal element, proportionally scale its off-diagonal elements
|
||||
/// to maintain the correlation structure (Cauchy-Schwarz consistency).
|
||||
/// </summary>
|
||||
private void ClampCovariance()
|
||||
{
|
||||
double[] maxDiag = [
|
||||
MAX_POSITION_COVARIANCE, // x
|
||||
MAX_POSITION_COVARIANCE, // y
|
||||
MAX_ORIENTATION_COVARIANCE, // θ
|
||||
MAX_VELOCITY_COVARIANCE, // vx
|
||||
MAX_VELOCITY_COVARIANCE, // vy
|
||||
MAX_VELOCITY_COVARIANCE // ω
|
||||
];
|
||||
|
||||
for (int i = 0; i < STATE_SIZE; i++)
|
||||
{
|
||||
if (_covariance[i, i] > maxDiag[i])
|
||||
{
|
||||
double scale = Math.Sqrt(maxDiag[i] / _covariance[i, i]);
|
||||
_covariance[i, i] = maxDiag[i];
|
||||
// Scale off-diagonal elements proportionally to maintain correlation structure
|
||||
for (int j = 0; j < STATE_SIZE; j++)
|
||||
{
|
||||
if (j != i)
|
||||
{
|
||||
_covariance[i, j] *= scale;
|
||||
_covariance[j, i] *= scale;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#region Utility Methods
|
||||
|
||||
/// <summary>
|
||||
/// Normalize angle to [-π, π]
|
||||
/// </summary>
|
||||
private static double NormalizeAngle(double angle)
|
||||
{
|
||||
while (angle > Math.PI) angle -= 2 * Math.PI;
|
||||
while (angle < -Math.PI) angle += 2 * Math.PI;
|
||||
return angle;
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
@@ -0,0 +1,97 @@
|
||||
using RobotNet10.CANOpen.CiA402.Enums;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace RobotNet10.RobotApp.Motion;
|
||||
|
||||
/// <summary>
|
||||
/// Interface cho Inverse Kinematics - chuyển đổi vận tốc robot (Twist) thành vận tốc các bánh xe và điều khiển robot
|
||||
/// Hỗ trợ state machine và operation modes tương tự CiA402
|
||||
/// </summary>
|
||||
public interface IInverseKinematics
|
||||
{
|
||||
/// <summary>
|
||||
/// Gets the current state of the drive system (tương tự CiA402 state machine)
|
||||
/// </summary>
|
||||
bool IsOperationEnabled { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Tính toán vận tốc các bánh xe từ vận tốc robot
|
||||
/// </summary>
|
||||
/// <param name="twist">Vận tốc robot (linear X, angular Z)</param>
|
||||
/// <returns>Tuple (leftWheelVelocity, rightWheelVelocity) - vận tốc bánh trái và bánh phải (encoder counts/s)</returns>
|
||||
(int leftWheelVelocity, int rightWheelVelocity) CalculateWheelVelocities(Twist twist);
|
||||
|
||||
/// <summary>
|
||||
/// Đặt vận tốc robot (Twist) - sẽ tự động tính toán và điều khiển các bánh xe
|
||||
/// </summary>
|
||||
/// <param name="twist">Vận tốc robot (linear X, angular Z)</param>
|
||||
/// <param name="ct">Cancellation token</param>
|
||||
Task SetVelocityAsync(Twist twist, CancellationToken ct = default);
|
||||
|
||||
/// <summary>
|
||||
/// Enable drive system - convenience method để tự động transition qua các states
|
||||
/// đến OperationEnabled. Gọi method này sẽ tự động thực hiện tất cả các bước cần thiết.
|
||||
/// </summary>
|
||||
void Enable();
|
||||
|
||||
/// <summary>
|
||||
/// Enable 2 động cơ giống enable bằng tay trên device: gửi lệnh trực tiếp xuống từng servo,
|
||||
/// await từng bước (SetOperationMode, SetProfileVelocity 0, SwitchOn, EnableOperation) rồi mới xong.
|
||||
/// </summary>
|
||||
Task EnableAsync(CancellationToken cancellationToken = default);
|
||||
|
||||
/// <summary>
|
||||
/// Disable drive system - convenience method để tắt drive về trạng thái an toàn
|
||||
/// </summary>
|
||||
void Disable();
|
||||
|
||||
/// <summary>
|
||||
/// Enable operation - chuyển từ SwitchedOn sang OperationEnabled (single step)
|
||||
/// </summary>
|
||||
void EnableOperation();
|
||||
|
||||
/// <summary>
|
||||
/// Disable operation - chuyển từ OperationEnabled về SwitchedOn (single step)
|
||||
/// </summary>
|
||||
void DisableOperation();
|
||||
|
||||
/// <summary>
|
||||
/// Quick stop - dừng khẩn cấp drive system
|
||||
/// </summary>
|
||||
void QuickStop();
|
||||
|
||||
/// <summary>
|
||||
/// Fault reset - reset lỗi và đưa drive về trạng thái ban đầu
|
||||
/// </summary>
|
||||
void FaultReset();
|
||||
|
||||
/// <summary>
|
||||
/// Set operation mode cho drive system (ProfileVelocity, ProfilePosition, etc.)
|
||||
/// </summary>
|
||||
/// <param name="mode">Operation mode to set</param>
|
||||
/// <param name="ct">Cancellation token</param>
|
||||
Task SetOperationModeAsync(OperationMode mode, CancellationToken ct = default);
|
||||
|
||||
/// <summary>
|
||||
/// Get current operation mode của drive system
|
||||
/// </summary>
|
||||
/// <param name="ct">Cancellation token</param>
|
||||
Task<OperationMode> GetOperationModeAsync(CancellationToken ct = default);
|
||||
|
||||
/// <summary>
|
||||
/// Set profile acceleration cho drive system
|
||||
/// </summary>
|
||||
/// <param name="acceleration">Gia tốc tăng tốc (m/s²)</param>
|
||||
/// <param name="ct">Cancellation token</param>
|
||||
Task SetAccelerationAsync(double acceleration, CancellationToken ct = default);
|
||||
|
||||
/// <summary>
|
||||
/// Set profile deceleration cho drive system
|
||||
/// </summary>
|
||||
/// <param name="deceleration">Gia tốc giảm tốc (m/s²)</param>
|
||||
/// <param name="ct">Cancellation token</param>
|
||||
Task SetDecelerationAsync(double deceleration, CancellationToken ct = default);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,49 @@
|
||||
using RobotNet10.Shared.Geometry;
|
||||
|
||||
namespace RobotNet10.RobotApp.Motion;
|
||||
|
||||
/// <summary>
|
||||
/// Interface cho Odometry Estimator - tính toán tọa độ odometry từ vị trí các bánh xe
|
||||
/// </summary>
|
||||
public interface IOdometryEstimator
|
||||
{
|
||||
/// <summary>
|
||||
/// Lấy pose hiện tại của robot (odometry)
|
||||
/// </summary>
|
||||
Pose CurrentPose { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Tần suất cập nhật odometry (Hz), dùng cho UI/telemetry tương thích ngược.
|
||||
/// </summary>
|
||||
double UpdateFrequency => 0;
|
||||
|
||||
/// <summary>
|
||||
/// Reset odometry về vị trí ban đầu
|
||||
/// </summary>
|
||||
void Reset();
|
||||
|
||||
/// <summary>
|
||||
/// Reset odometry về một pose cụ thể
|
||||
/// </summary>
|
||||
/// <param name="pose">Pose để reset về</param>
|
||||
void Reset(Pose pose);
|
||||
|
||||
/// <summary>
|
||||
/// Alias tương thích ngược cho các call site cũ.
|
||||
/// </summary>
|
||||
void ResetOdometry() => Reset();
|
||||
|
||||
/// <summary>
|
||||
/// Alias tương thích ngược cho các call site cũ.
|
||||
/// </summary>
|
||||
/// <param name="pose">Pose để reset về</param>
|
||||
void ResetOdometry(Pose pose) => Reset(pose);
|
||||
|
||||
/// <summary>
|
||||
/// Cập nhật odometry từ vị trí encoder của các bánh xe
|
||||
/// </summary>
|
||||
/// <param name="leftWheelPosition">Vị trí encoder bánh trái (encoder counts)</param>
|
||||
/// <param name="rightWheelPosition">Vị trí encoder bánh phải (encoder counts)</param>
|
||||
void Update(int leftWheelPosition, int rightWheelPosition);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,52 @@
|
||||
namespace RobotNet10.RobotApp.Motion;
|
||||
|
||||
/// <summary>
|
||||
/// Cấu hình cho ManualControlService
|
||||
/// </summary>
|
||||
public class ManualControlConfiguration
|
||||
{
|
||||
public bool Enable { get; set; }
|
||||
|
||||
public bool UsingKeyboard { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Device ID của IRfHandle để điều khiển robot
|
||||
/// </summary>
|
||||
public string RfHandleDeviceId { get; set; } = string.Empty;
|
||||
|
||||
/// <summary>
|
||||
/// Vận tốc dài tối thiểu (m/s)
|
||||
/// </summary>
|
||||
public double MinLinearVelocity { get; set; } = 0.0;
|
||||
|
||||
/// <summary>
|
||||
/// Vận tốc dài tối đa (m/s)
|
||||
/// </summary>
|
||||
public double MaxLinearVelocity { get; set; } = 1.0;
|
||||
|
||||
/// <summary>
|
||||
/// Vận tốc xoay tối thiểu (rad/s)
|
||||
/// </summary>
|
||||
public double MinAngularVelocity { get; set; } = 0.0;
|
||||
|
||||
/// <summary>
|
||||
/// Vận tốc xoay tối đa (rad/s)
|
||||
/// </summary>
|
||||
public double MaxAngularVelocity { get; set; } = 1.0;
|
||||
|
||||
/// <summary>
|
||||
/// Tần suất cập nhật vận tốc (Hz)
|
||||
/// </summary>
|
||||
public double UpdateRate { get; set; } = 20.0;
|
||||
|
||||
/// <summary>
|
||||
/// Gia tốc tăng tốc (m/s²) - sẽ được set cho drive system khi enable
|
||||
/// </summary>
|
||||
public double Acceleration { get; set; } = 0.5;
|
||||
|
||||
/// <summary>
|
||||
/// Gia tốc giảm tốc (m/s²) - sẽ được set cho drive system khi enable
|
||||
/// </summary>
|
||||
public double Deceleration { get; set; } = 0.5;
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,236 @@
|
||||
namespace RobotNet10.RobotApp.Motion;
|
||||
|
||||
/// <summary>
|
||||
/// Helper class for matrix operations used in Extended Kalman Filter
|
||||
/// Implements basic operations for small matrices (up to 6x6)
|
||||
/// </summary>
|
||||
public static class MatrixHelper
|
||||
{
|
||||
/// <summary>
|
||||
/// Create identity matrix of size n x n
|
||||
/// </summary>
|
||||
public static double[,] CreateIdentity(int n)
|
||||
{
|
||||
var result = new double[n, n];
|
||||
for (int i = 0; i < n; i++)
|
||||
result[i, i] = 1.0;
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Create zero matrix of size rows x cols
|
||||
/// </summary>
|
||||
public static double[,] CreateZeros(int rows, int cols)
|
||||
{
|
||||
return new double[rows, cols];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Create diagonal matrix from array of diagonal values
|
||||
/// </summary>
|
||||
public static double[,] CreateDiagonal(double[] diagonal)
|
||||
{
|
||||
int n = diagonal.Length;
|
||||
var result = new double[n, n];
|
||||
for (int i = 0; i < n; i++)
|
||||
result[i, i] = diagonal[i];
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Matrix multiplication: C = A * B
|
||||
/// </summary>
|
||||
public static double[,] Multiply(double[,] a, double[,] b)
|
||||
{
|
||||
int aRows = a.GetLength(0);
|
||||
int aCols = a.GetLength(1);
|
||||
int bRows = b.GetLength(0);
|
||||
int bCols = b.GetLength(1);
|
||||
|
||||
if (aCols != bRows)
|
||||
throw new ArgumentException($"Matrix dimensions incompatible for multiplication: ({aRows}x{aCols}) * ({bRows}x{bCols})");
|
||||
|
||||
var result = new double[aRows, bCols];
|
||||
for (int i = 0; i < aRows; i++)
|
||||
{
|
||||
for (int j = 0; j < bCols; j++)
|
||||
{
|
||||
double sum = 0.0;
|
||||
for (int k = 0; k < aCols; k++)
|
||||
sum += a[i, k] * b[k, j];
|
||||
result[i, j] = sum;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Matrix addition: C = A + B
|
||||
/// </summary>
|
||||
public static double[,] Add(double[,] a, double[,] b)
|
||||
{
|
||||
int rows = a.GetLength(0);
|
||||
int cols = a.GetLength(1);
|
||||
|
||||
if (rows != b.GetLength(0) || cols != b.GetLength(1))
|
||||
throw new ArgumentException("Matrix dimensions must match for addition");
|
||||
|
||||
var result = new double[rows, cols];
|
||||
for (int i = 0; i < rows; i++)
|
||||
for (int j = 0; j < cols; j++)
|
||||
result[i, j] = a[i, j] + b[i, j];
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Matrix subtraction: C = A - B
|
||||
/// </summary>
|
||||
public static double[,] Subtract(double[,] a, double[,] b)
|
||||
{
|
||||
int rows = a.GetLength(0);
|
||||
int cols = a.GetLength(1);
|
||||
|
||||
if (rows != b.GetLength(0) || cols != b.GetLength(1))
|
||||
throw new ArgumentException("Matrix dimensions must match for subtraction");
|
||||
|
||||
var result = new double[rows, cols];
|
||||
for (int i = 0; i < rows; i++)
|
||||
for (int j = 0; j < cols; j++)
|
||||
result[i, j] = a[i, j] - b[i, j];
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Matrix transpose: B = A^T
|
||||
/// </summary>
|
||||
public static double[,] Transpose(double[,] a)
|
||||
{
|
||||
int rows = a.GetLength(0);
|
||||
int cols = a.GetLength(1);
|
||||
var result = new double[cols, rows];
|
||||
|
||||
for (int i = 0; i < rows; i++)
|
||||
for (int j = 0; j < cols; j++)
|
||||
result[j, i] = a[i, j];
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Matrix scalar multiplication: B = scalar * A
|
||||
/// </summary>
|
||||
public static double[,] ScalarMultiply(double scalar, double[,] a)
|
||||
{
|
||||
int rows = a.GetLength(0);
|
||||
int cols = a.GetLength(1);
|
||||
var result = new double[rows, cols];
|
||||
|
||||
for (int i = 0; i < rows; i++)
|
||||
for (int j = 0; j < cols; j++)
|
||||
result[i, j] = scalar * a[i, j];
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Matrix inverse using Gauss-Jordan elimination (for small matrices)
|
||||
/// </summary>
|
||||
public static double[,] Inverse(double[,] a)
|
||||
{
|
||||
int n = a.GetLength(0);
|
||||
if (n != a.GetLength(1))
|
||||
throw new ArgumentException("Matrix must be square for inversion");
|
||||
|
||||
// Create augmented matrix [A | I]
|
||||
var augmented = new double[n, 2 * n];
|
||||
for (int i = 0; i < n; i++)
|
||||
{
|
||||
for (int j = 0; j < n; j++)
|
||||
augmented[i, j] = a[i, j];
|
||||
augmented[i, n + i] = 1.0;
|
||||
}
|
||||
|
||||
// Gauss-Jordan elimination
|
||||
for (int i = 0; i < n; i++)
|
||||
{
|
||||
// Find pivot
|
||||
int maxRow = i;
|
||||
for (int k = i + 1; k < n; k++)
|
||||
{
|
||||
if (Math.Abs(augmented[k, i]) > Math.Abs(augmented[maxRow, i]))
|
||||
maxRow = k;
|
||||
}
|
||||
|
||||
// Swap rows
|
||||
if (maxRow != i)
|
||||
{
|
||||
for (int k = 0; k < 2 * n; k++)
|
||||
(augmented[i, k], augmented[maxRow, k]) = (augmented[maxRow, k], augmented[i, k]);
|
||||
}
|
||||
|
||||
// Check for singular matrix
|
||||
if (Math.Abs(augmented[i, i]) < 1e-10)
|
||||
throw new InvalidOperationException("Matrix is singular and cannot be inverted");
|
||||
|
||||
// Scale pivot row
|
||||
double pivot = augmented[i, i];
|
||||
for (int j = 0; j < 2 * n; j++)
|
||||
augmented[i, j] /= pivot;
|
||||
|
||||
// Eliminate column
|
||||
for (int k = 0; k < n; k++)
|
||||
{
|
||||
if (k != i)
|
||||
{
|
||||
double factor = augmented[k, i];
|
||||
for (int j = 0; j < 2 * n; j++)
|
||||
augmented[k, j] -= factor * augmented[i, j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Extract inverse from augmented matrix
|
||||
var result = new double[n, n];
|
||||
for (int i = 0; i < n; i++)
|
||||
for (int j = 0; j < n; j++)
|
||||
result[i, j] = augmented[i, n + j];
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Copy matrix
|
||||
/// </summary>
|
||||
public static double[,] Copy(double[,] a)
|
||||
{
|
||||
int rows = a.GetLength(0);
|
||||
int cols = a.GetLength(1);
|
||||
var result = new double[rows, cols];
|
||||
|
||||
for (int i = 0; i < rows; i++)
|
||||
for (int j = 0; j < cols; j++)
|
||||
result[i, j] = a[i, j];
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Print matrix (for debugging)
|
||||
/// </summary>
|
||||
public static string ToString(double[,] a, string format = "F4")
|
||||
{
|
||||
int rows = a.GetLength(0);
|
||||
int cols = a.GetLength(1);
|
||||
var sb = new System.Text.StringBuilder();
|
||||
|
||||
for (int i = 0; i < rows; i++)
|
||||
{
|
||||
for (int j = 0; j < cols; j++)
|
||||
{
|
||||
sb.Append(a[i, j].ToString(format));
|
||||
if (j < cols - 1)
|
||||
sb.Append(" ");
|
||||
}
|
||||
if (i < rows - 1)
|
||||
sb.AppendLine();
|
||||
}
|
||||
return sb.ToString();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
namespace RobotNet10.RobotApp.Motion;
|
||||
|
||||
public static class MotionApiEndpoints
|
||||
{
|
||||
public static IEndpointRouteBuilder MapMotionApiEndpoints(this IEndpointRouteBuilder app)
|
||||
{
|
||||
app.MapGet("/api/motion/ps5/status", (PS5ControllerService svc) =>
|
||||
{
|
||||
return Results.Json(new { state = svc.State.ToString() });
|
||||
});
|
||||
|
||||
app.MapPost("/api/motion/ps5/enable", async (PS5ControllerService svc) =>
|
||||
{
|
||||
svc.Enable();
|
||||
return Results.Ok("PS5 controller enabled");
|
||||
});
|
||||
|
||||
app.MapPost("/api/motion/ps5/disable", (PS5ControllerService svc) =>
|
||||
{
|
||||
svc.Disable();
|
||||
return Results.Ok("PS5 controller disabled");
|
||||
});
|
||||
|
||||
return app;
|
||||
}
|
||||
}
|
||||
@@ -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; }
|
||||
}
|
||||
|
||||
@@ -0,0 +1,43 @@
|
||||
namespace RobotNet10.RobotApp.Motion;
|
||||
|
||||
/// <summary>
|
||||
/// Cấu hình cho PS5ControllerService
|
||||
/// </summary>
|
||||
public class PS5ControllerConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// Vận tốc khi nhấn R1 (m/s)
|
||||
/// </summary>
|
||||
public double R1Velocity { get; set; } = 0.3;
|
||||
|
||||
/// <summary>
|
||||
/// Vận tốc khi nhấn R2 (m/s)
|
||||
/// </summary>
|
||||
public double R2Velocity { get; set; } = 0.6;
|
||||
|
||||
/// <summary>
|
||||
/// Vận tốc khi nhấn L1 (m/s)
|
||||
/// </summary>
|
||||
public double L1Velocity { get; set; } = 0.9;
|
||||
|
||||
/// <summary>
|
||||
/// Vận tốc khi nhấn L2 (m/s)
|
||||
/// </summary>
|
||||
public double L2Velocity { get; set; } = 1.2;
|
||||
|
||||
/// <summary>
|
||||
/// Vận tốc góc tối đa khi quay (rad/s)
|
||||
/// </summary>
|
||||
public double MaxAngularVelocity { get; set; } = 1.0;
|
||||
|
||||
/// <summary>
|
||||
/// Tần suất cập nhật vận tốc (Hz)
|
||||
/// </summary>
|
||||
public double UpdateRate { get; set; } = 20.0;
|
||||
|
||||
/// <summary>
|
||||
/// ID của gamepad để sử dụng (0 = gamepad đầu tiên). Nếu null, sẽ sử dụng gamepad đầu tiên tìm thấy
|
||||
/// </summary>
|
||||
public int? GamepadIndex { get; set; } = null;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,698 @@
|
||||
using Appccelerate.StateMachine;
|
||||
using Appccelerate.StateMachine.Machine;
|
||||
using Microsoft.Extensions.Configuration;
|
||||
using Microsoft.Extensions.Hosting;
|
||||
using Microsoft.Extensions.Logging;
|
||||
using RobotNet10.RobotApp.Motion;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
// using RobotNet10.Shared.Numbers;
|
||||
using SDL2;
|
||||
using System;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace RobotNet10.RobotApp.Motion;
|
||||
|
||||
/// <summary>
|
||||
/// States cho PS5ControllerService state machine
|
||||
/// </summary>
|
||||
public enum PS5ControllerState
|
||||
{
|
||||
Disabled,
|
||||
Active
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Triggers cho PS5ControllerService state machine
|
||||
/// </summary>
|
||||
public enum PS5ControllerTrigger
|
||||
{
|
||||
Enable,
|
||||
Disable,
|
||||
SafetyStop
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Service điều khiển robot từ tay cầm PS5
|
||||
/// - R1, R2, L1, L2: Chọn mức vận tốc (0.3, 0.6, 0.9, 1.2 m/s)
|
||||
/// - Left stick (núm trái): Điều khiển lên/xuống (linear velocity)
|
||||
/// - Right stick (núm phải): Điều khiển trái/phải (angular velocity)
|
||||
/// </summary>
|
||||
public class PS5ControllerService : IHostedService, IDisposable
|
||||
{
|
||||
private readonly PassiveStateMachine<PS5ControllerState, PS5ControllerTrigger> _stateMachine;
|
||||
private readonly PS5ControllerConfiguration _config;
|
||||
private readonly IInverseKinematics? _inverseKinematics;
|
||||
private readonly ILogger<PS5ControllerService> _logger;
|
||||
private readonly object _lock = new();
|
||||
|
||||
private IntPtr _gamepad = IntPtr.Zero;
|
||||
private int _gamepadIndex = -1;
|
||||
private CancellationTokenSource? _updateCts;
|
||||
private Task? _updateTask;
|
||||
private PS5ControllerState _currentState = PS5ControllerState.Disabled;
|
||||
private bool _disposed = false;
|
||||
private bool _sdlInitialized = false;
|
||||
private bool _wasGamepadConnected = false;
|
||||
// Current velocity being sent to robot
|
||||
private Twist _currentTwist = new();
|
||||
|
||||
// Current selected velocity level (determined by trigger buttons)
|
||||
private double _currentMaxVelocity = 0.0;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the current state of the PS5ControllerService
|
||||
/// </summary>
|
||||
public PS5ControllerState State => _currentState;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the current twist being sent to robot
|
||||
/// </summary>
|
||||
public Twist CurrentTwist
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
return _currentTwist;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the current selected velocity level (m/s)
|
||||
/// </summary>
|
||||
public double CurrentMaxVelocity
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
return _currentMaxVelocity;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public PS5ControllerService(
|
||||
IConfiguration configuration,
|
||||
IInverseKinematics inverseKinematics,
|
||||
ILogger<PS5ControllerService> logger)
|
||||
{
|
||||
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
||||
|
||||
// Try to get IInverseKinematics (optional - may not be available)
|
||||
_inverseKinematics = inverseKinematics;
|
||||
|
||||
// Load configuration
|
||||
var configSection = configuration.GetSection("Motion:PS5Controller");
|
||||
if (!configSection.Exists())
|
||||
{
|
||||
throw new InvalidOperationException("Configuration section 'Motion:PS5Controller' not found in appsettings.json");
|
||||
}
|
||||
|
||||
_config = new PS5ControllerConfiguration();
|
||||
configSection.Bind(_config);
|
||||
|
||||
// Validate configuration
|
||||
ValidateConfiguration();
|
||||
|
||||
// Build state machine
|
||||
_stateMachine = BuildStateMachine();
|
||||
_stateMachine.Start();
|
||||
}
|
||||
|
||||
private void ValidateConfiguration()
|
||||
{
|
||||
if (_config.R1Velocity <= 0)
|
||||
throw new InvalidOperationException("R1Velocity must be > 0");
|
||||
|
||||
if (_config.R2Velocity <= 0)
|
||||
throw new InvalidOperationException("R2Velocity must be > 0");
|
||||
|
||||
if (_config.L1Velocity <= 0)
|
||||
throw new InvalidOperationException("L1Velocity must be > 0");
|
||||
|
||||
if (_config.L2Velocity <= 0)
|
||||
throw new InvalidOperationException("L2Velocity must be > 0");
|
||||
|
||||
if (_config.MaxAngularVelocity <= 0)
|
||||
throw new InvalidOperationException("MaxAngularVelocity must be > 0");
|
||||
|
||||
if (_config.UpdateRate <= 0)
|
||||
throw new InvalidOperationException("UpdateRate must be > 0");
|
||||
}
|
||||
|
||||
private PassiveStateMachine<PS5ControllerState, PS5ControllerTrigger> BuildStateMachine()
|
||||
{
|
||||
var builder = new StateMachineDefinitionBuilder<PS5ControllerState, PS5ControllerTrigger>();
|
||||
|
||||
// Disabled state
|
||||
builder.In(PS5ControllerState.Disabled)
|
||||
.ExecuteOnEntry(() =>
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_currentState = PS5ControllerState.Disabled;
|
||||
}
|
||||
StopUpdateLoop();
|
||||
StopRobot();
|
||||
_logger.LogInformation("PS5ControllerService state: Disabled");
|
||||
})
|
||||
.On(PS5ControllerTrigger.Enable)
|
||||
.Goto(PS5ControllerState.Active);
|
||||
|
||||
// Active state
|
||||
builder.In(PS5ControllerState.Active)
|
||||
.ExecuteOnEntry(() =>
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_currentState = PS5ControllerState.Active;
|
||||
}
|
||||
_logger.LogInformation("PS5ControllerService state: Active");
|
||||
StartUpdateLoop();
|
||||
})
|
||||
.On(PS5ControllerTrigger.Disable)
|
||||
.Goto(PS5ControllerState.Disabled)
|
||||
.On(PS5ControllerTrigger.SafetyStop)
|
||||
.Goto(PS5ControllerState.Disabled)
|
||||
.Execute(() =>
|
||||
{
|
||||
_logger.LogWarning("PS5ControllerService: Safety stop triggered");
|
||||
});
|
||||
|
||||
return builder
|
||||
.WithInitialState(PS5ControllerState.Disabled)
|
||||
.Build()
|
||||
.CreatePassiveStateMachine();
|
||||
}
|
||||
|
||||
private void StartUpdateLoop()
|
||||
{
|
||||
StopUpdateLoop(); // Stop existing loop if any
|
||||
|
||||
if (_gamepad == IntPtr.Zero || !IsTrue(SDL.SDL_GameControllerGetAttached(_gamepad)))
|
||||
{
|
||||
if (!TryReconnectGamepad(false))
|
||||
{
|
||||
_logger.LogWarning("PS5ControllerService: No gamepad connected, cannot start update loop");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
_updateCts = new CancellationTokenSource();
|
||||
var token = _updateCts.Token;
|
||||
_updateTask = Task.Run(async () => await UpdateLoopAsync(token), token);
|
||||
}
|
||||
}
|
||||
|
||||
private void StopUpdateLoop()
|
||||
{
|
||||
Task? taskToWait = null;
|
||||
CancellationTokenSource? ctsToCancel = null;
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
ctsToCancel = _updateCts;
|
||||
taskToWait = _updateTask;
|
||||
_updateCts = null;
|
||||
_updateTask = null;
|
||||
}
|
||||
|
||||
if (ctsToCancel != null)
|
||||
{
|
||||
try
|
||||
{
|
||||
ctsToCancel.Cancel();
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
// CTS already disposed, ignore
|
||||
}
|
||||
}
|
||||
|
||||
if (taskToWait != null)
|
||||
{
|
||||
try
|
||||
{
|
||||
taskToWait.Wait(TimeSpan.FromSeconds(1));
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error stopping update loop");
|
||||
}
|
||||
}
|
||||
|
||||
ctsToCancel?.Dispose();
|
||||
}
|
||||
|
||||
private async Task UpdateLoopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
var updateInterval = TimeSpan.FromMilliseconds(1000.0 / _config.UpdateRate);
|
||||
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
try
|
||||
{
|
||||
// Check if gamepad is still connected
|
||||
if (_gamepad == IntPtr.Zero || !IsTrue(SDL.SDL_GameControllerGetAttached(_gamepad)))
|
||||
{
|
||||
StopRobot();
|
||||
|
||||
if (!TryReconnectGamepad(false))
|
||||
{
|
||||
await Task.Delay(updateInterval, cancellationToken);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
// Update velocity from gamepad
|
||||
await UpdateVelocityFromGamepadAsync(cancellationToken);
|
||||
|
||||
await Task.Delay(updateInterval, cancellationToken);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
break;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error in update loop");
|
||||
await Task.Delay(updateInterval, cancellationToken);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private async Task UpdateVelocityFromGamepadAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
if (_gamepad == IntPtr.Zero)
|
||||
return;
|
||||
|
||||
// Update SDL events (required for gamepad state)
|
||||
SDL.SDL_PumpEvents();
|
||||
|
||||
// Get button states
|
||||
byte r1Pressed = SDL.SDL_GameControllerGetButton(_gamepad, SDL.SDL_GameControllerButton.SDL_CONTROLLER_BUTTON_RIGHTSHOULDER);
|
||||
byte r2Pressed = SDL.SDL_GameControllerGetAxis(_gamepad, SDL.SDL_GameControllerAxis.SDL_CONTROLLER_AXIS_TRIGGERRIGHT) > 0 ? (byte)1 : (byte)0;
|
||||
byte l1Pressed = SDL.SDL_GameControllerGetButton(_gamepad, SDL.SDL_GameControllerButton.SDL_CONTROLLER_BUTTON_LEFTSHOULDER);
|
||||
byte l2Pressed = SDL.SDL_GameControllerGetAxis(_gamepad, SDL.SDL_GameControllerAxis.SDL_CONTROLLER_AXIS_TRIGGERLEFT) > 0 ? (byte)1 : (byte)0;
|
||||
|
||||
// Determine velocity level based on trigger buttons (priority: L2 > L1 > R2 > R1)
|
||||
double maxVelocity = 0.0;
|
||||
//Velocity
|
||||
double linearVelocity = 0.0;
|
||||
double angularVelocity = 0.0;
|
||||
if (l2Pressed != 0)
|
||||
maxVelocity = _config.L2Velocity;
|
||||
else if (l1Pressed != 0)
|
||||
maxVelocity = _config.L1Velocity;
|
||||
else if (r2Pressed != 0)
|
||||
maxVelocity = _config.R2Velocity;
|
||||
else if (r1Pressed != 0)
|
||||
maxVelocity = _config.R1Velocity;
|
||||
else if( l2Pressed == 0 && l1Pressed == 0 && r2Pressed == 0 && r1Pressed == 0)
|
||||
StopRobot();
|
||||
// Console.WriteLine($"l2: {l2Pressed}, l1: {l1Pressed}, r2: {r2Pressed}, r1: {r1Pressed}, maxVelocity: {maxVelocity}, linearVelocity: {linearVelocity}");
|
||||
lock (_lock)
|
||||
{
|
||||
_currentMaxVelocity = maxVelocity;
|
||||
}
|
||||
|
||||
// Get stick values (left stick for linear, right stick for angular)
|
||||
short leftStickY = SDL.SDL_GameControllerGetAxis(_gamepad, SDL.SDL_GameControllerAxis.SDL_CONTROLLER_AXIS_LEFTY);
|
||||
short rightStickX = SDL.SDL_GameControllerGetAxis(_gamepad, SDL.SDL_GameControllerAxis.SDL_CONTROLLER_AXIS_RIGHTX);
|
||||
|
||||
// Normalize stick values from [-32768, 32767] to [-1.0, 1.0]
|
||||
// Left stick Y: up is negative, down is positive (invert for forward/backward)
|
||||
// Right stick X: left is negative, right is positive
|
||||
double leftStickYNormalized = -leftStickY / 32768.0; // Invert so up = positive (forward)
|
||||
double rightStickXNormalized = -rightStickX / 32768.0;
|
||||
|
||||
// Apply deadzone (5% deadzone)
|
||||
const double deadzone = 0.1;
|
||||
if (Math.Abs(leftStickYNormalized) < deadzone)
|
||||
leftStickYNormalized = 0.0;
|
||||
else
|
||||
{
|
||||
// Rescale after deadzone
|
||||
var sign = Math.Sign(leftStickYNormalized);
|
||||
leftStickYNormalized = sign * ((Math.Abs(leftStickYNormalized) - deadzone) / (1.0 - deadzone));
|
||||
}
|
||||
|
||||
if (Math.Abs(rightStickXNormalized) < deadzone)
|
||||
rightStickXNormalized = 0.0;
|
||||
else
|
||||
{
|
||||
// Rescale after deadzone
|
||||
var sign = Math.Sign(rightStickXNormalized);
|
||||
rightStickXNormalized = sign * ((Math.Abs(rightStickXNormalized) - deadzone) / (1.0 - deadzone));
|
||||
}
|
||||
|
||||
// Calculate velocities
|
||||
linearVelocity = leftStickYNormalized * maxVelocity;
|
||||
angularVelocity = rightStickXNormalized * _config.MaxAngularVelocity;
|
||||
// Console.WriteLine($"linearVelocity: {linearVelocity}, angularVelocity: {angularVelocity}");
|
||||
// Console.WriteLine($"leftStickYNormalized: {leftStickYNormalized}, maxVelocity: {maxVelocity}, linearVelocity: {linearVelocity}");
|
||||
// if(leftStickYNormalized == 0 || rightStickXNormalized == 0)
|
||||
// {
|
||||
// linearVelocity = 0;
|
||||
// angularVelocity = 0;
|
||||
// }
|
||||
// Build twist
|
||||
var twist = new Twist
|
||||
{
|
||||
Linear = new Vector3(linearVelocity, 0, 0),
|
||||
Angular = new Vector3(0, 0, angularVelocity)
|
||||
};
|
||||
|
||||
// Update current twist
|
||||
lock (_lock)
|
||||
{
|
||||
_currentTwist = twist;
|
||||
}
|
||||
|
||||
// Send to IInverseKinematics if available
|
||||
if (_inverseKinematics != null && maxVelocity > 0.0)
|
||||
{
|
||||
try
|
||||
{
|
||||
await _inverseKinematics.SetVelocityAsync(twist, cancellationToken);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error sending velocity to IInverseKinematics");
|
||||
}
|
||||
}
|
||||
|
||||
// Log occasionally for debugging
|
||||
if (DateTime.Now.Millisecond % 500 < 50) // Log roughly every 500ms
|
||||
{
|
||||
_logger.LogDebug("PS5Controller: MaxVel={MaxVel:F2}, Linear={Linear:F2}, Angular={Angular:F2}",
|
||||
maxVelocity, linearVelocity, angularVelocity);
|
||||
}
|
||||
}
|
||||
|
||||
private void StopRobot()
|
||||
{
|
||||
var zeroTwist = new Twist();
|
||||
lock (_lock)
|
||||
{
|
||||
_currentTwist = zeroTwist;
|
||||
_currentMaxVelocity = 0.0;
|
||||
}
|
||||
|
||||
// Send zero velocity to IInverseKinematics if available
|
||||
if (_inverseKinematics != null)
|
||||
{
|
||||
try
|
||||
{
|
||||
_ = Task.Run(async () => await _inverseKinematics.SetVelocityAsync(zeroTwist));
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error stopping robot");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private bool InitializeSDL()
|
||||
{
|
||||
if (_sdlInitialized)
|
||||
return true;
|
||||
|
||||
try
|
||||
{
|
||||
if (SDL.SDL_Init(SDL.SDL_INIT_GAMECONTROLLER) < 0)
|
||||
{
|
||||
_logger.LogError("Failed to initialize SDL: {Error}", SDL.SDL_GetError());
|
||||
return false;
|
||||
}
|
||||
|
||||
SDL.SDL_GameControllerEventState(SDL.SDL_ENABLE);
|
||||
_sdlInitialized = true;
|
||||
_logger.LogInformation("SDL initialized successfully for gamepad support");
|
||||
return true;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error initializing SDL");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private bool OpenGamepad()
|
||||
{
|
||||
if (_gamepad != IntPtr.Zero)
|
||||
{
|
||||
if (IsTrue(SDL.SDL_GameControllerGetAttached(_gamepad)))
|
||||
return true;
|
||||
|
||||
SDL.SDL_GameControllerClose(_gamepad);
|
||||
_gamepad = IntPtr.Zero;
|
||||
_gamepadIndex = -1;
|
||||
}
|
||||
|
||||
SDL.SDL_PumpEvents();
|
||||
|
||||
int numJoysticks = SDL.SDL_NumJoysticks();
|
||||
_logger.LogDebug("Found {Count} joystick(s)", numJoysticks);
|
||||
|
||||
if (numJoysticks <= 0)
|
||||
return false;
|
||||
|
||||
// If GamepadIndex is configured, always try that exact index.
|
||||
if (_config.GamepadIndex.HasValue)
|
||||
{
|
||||
return TryOpenGamepadAtIndex(_config.GamepadIndex.Value);
|
||||
}
|
||||
|
||||
// Otherwise auto-detect first available game controller.
|
||||
for (int i = 0; i < numJoysticks; i++)
|
||||
{
|
||||
if (TryOpenGamepadAtIndex(i))
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
private bool TryOpenGamepadAtIndex(int index)
|
||||
{
|
||||
if (!IsTrue(SDL.SDL_IsGameController(index)))
|
||||
return false;
|
||||
|
||||
_gamepad = SDL.SDL_GameControllerOpen(index);
|
||||
if (_gamepad == IntPtr.Zero)
|
||||
{
|
||||
_logger.LogWarning("Failed to open game controller {Index}: {Error}", index, SDL.SDL_GetError());
|
||||
return false;
|
||||
}
|
||||
|
||||
_gamepadIndex = index;
|
||||
var name = SDL.SDL_GameControllerName(_gamepad);
|
||||
_logger.LogInformation("Opened game controller: {Name} (index {Index})", name, index);
|
||||
return true;
|
||||
}
|
||||
|
||||
private bool TryReconnectGamepad(bool logWhenUnavailable)
|
||||
{
|
||||
var reconnected = OpenGamepad();
|
||||
|
||||
if (reconnected)
|
||||
{
|
||||
if (!_wasGamepadConnected)
|
||||
_logger.LogInformation("PS5ControllerService: Gamepad connected/reconnected");
|
||||
|
||||
_wasGamepadConnected = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (_wasGamepadConnected)
|
||||
{
|
||||
_logger.LogWarning("PS5ControllerService: Gamepad disconnected, waiting for reconnect");
|
||||
}
|
||||
else if (logWhenUnavailable)
|
||||
{
|
||||
_logger.LogWarning("PS5ControllerService: No gamepad connected");
|
||||
}
|
||||
|
||||
_wasGamepadConnected = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
private static bool IsTrue(SDL.SDL_bool value) => value == SDL.SDL_bool.SDL_TRUE;
|
||||
|
||||
#region IHostedService
|
||||
|
||||
public async Task StartAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
_logger.LogInformation("Starting PS5ControllerService...");
|
||||
|
||||
try
|
||||
{
|
||||
if (!InitializeSDL())
|
||||
{
|
||||
_logger.LogWarning("Failed to initialize SDL. PS5ControllerService will not be available.");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!TryReconnectGamepad(true))
|
||||
{
|
||||
_logger.LogWarning("No gamepad found at startup. PS5ControllerService will auto-reconnect when gamepad is plugged in.");
|
||||
}
|
||||
|
||||
_logger.LogInformation("PS5ControllerService initialized successfully");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error starting PS5ControllerService");
|
||||
}
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
public async Task StopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
_logger.LogInformation("Stopping PS5ControllerService...");
|
||||
|
||||
try
|
||||
{
|
||||
Disable();
|
||||
StopUpdateLoop();
|
||||
|
||||
if (_gamepad != IntPtr.Zero)
|
||||
{
|
||||
SDL.SDL_GameControllerClose(_gamepad);
|
||||
_gamepad = IntPtr.Zero;
|
||||
_gamepadIndex = -1;
|
||||
}
|
||||
|
||||
if (_sdlInitialized)
|
||||
{
|
||||
SDL.SDL_QuitSubSystem(SDL.SDL_INIT_GAMECONTROLLER);
|
||||
_sdlInitialized = false;
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error stopping PS5ControllerService");
|
||||
}
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Control Methods
|
||||
|
||||
/// <summary>
|
||||
/// Enable PS5 controller control
|
||||
/// </summary>
|
||||
public void Enable()
|
||||
{
|
||||
// Try to open gamepad if not already open
|
||||
if (_gamepad == IntPtr.Zero)
|
||||
{
|
||||
if (!TryReconnectGamepad(true))
|
||||
{
|
||||
_logger.LogWarning("Cannot enable PS5 controller: No gamepad connected");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Check if gamepad is still connected
|
||||
if (!IsTrue(SDL.SDL_GameControllerGetAttached(_gamepad)))
|
||||
{
|
||||
_logger.LogWarning("Cannot enable PS5 controller: Gamepad not connected");
|
||||
return;
|
||||
}
|
||||
|
||||
// Check if IInverseKinematics (DifferentialDrive) is ready
|
||||
if (_inverseKinematics is DifferentialDrive differentialDrive)
|
||||
{
|
||||
if (differentialDrive.State != DifferentialDriveState.OperationEnabled)
|
||||
{
|
||||
_logger.LogInformation("DifferentialDrive is not in OperationEnabled state (current: {State}). Enabling it...", differentialDrive.State);
|
||||
|
||||
int maxAttempts = 10;
|
||||
int attemptDelay = 300; // ms
|
||||
|
||||
for (int i = 0; i < maxAttempts && differentialDrive.State != DifferentialDriveState.OperationEnabled; i++)
|
||||
{
|
||||
var currentState = differentialDrive.State;
|
||||
differentialDrive.Enable();
|
||||
|
||||
System.Threading.Thread.Sleep(attemptDelay);
|
||||
|
||||
if (differentialDrive.State == currentState && i > 0)
|
||||
{
|
||||
_logger.LogWarning("DifferentialDrive state did not change after Enable() call. State: {State}", currentState);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (differentialDrive.State != DifferentialDriveState.OperationEnabled)
|
||||
{
|
||||
_logger.LogWarning("Cannot enable PS5 controller: DifferentialDrive is not ready. Current state: {State}", differentialDrive.State);
|
||||
return;
|
||||
}
|
||||
|
||||
_logger.LogInformation("DifferentialDrive is now in OperationEnabled state");
|
||||
}
|
||||
}
|
||||
else if (_inverseKinematics == null)
|
||||
{
|
||||
_logger.LogWarning("Cannot enable PS5 controller: IInverseKinematics is not available");
|
||||
return;
|
||||
}
|
||||
|
||||
_stateMachine.Fire(PS5ControllerTrigger.Enable);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Disable PS5 controller control
|
||||
/// </summary>
|
||||
public void Disable()
|
||||
{
|
||||
_stateMachine.Fire(PS5ControllerTrigger.Disable);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region IDisposable
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
return;
|
||||
|
||||
_disposed = true;
|
||||
|
||||
try
|
||||
{
|
||||
Disable();
|
||||
StopUpdateLoop();
|
||||
_stateMachine.Stop();
|
||||
|
||||
if (_gamepad != IntPtr.Zero)
|
||||
{
|
||||
SDL.SDL_GameControllerClose(_gamepad);
|
||||
_gamepad = IntPtr.Zero;
|
||||
}
|
||||
|
||||
if (_sdlInitialized)
|
||||
{
|
||||
SDL.SDL_QuitSubSystem(SDL.SDL_INIT_GAMECONTROLLER);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error disposing PS5ControllerService");
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 371 KiB |
Reference in New Issue
Block a user