67 lines
2.4 KiB
C#
67 lines
2.4 KiB
C#
using System;
|
|
using CommonUsage.Chassis;
|
|
using MyParking.Shared;
|
|
|
|
namespace MultiWheelC.StateEstimation
|
|
{
|
|
/// <summary>
|
|
/// 根据车载配置创建Detour位姿过滤与电机反馈纵向速度组合的停车状态源。
|
|
/// </summary>
|
|
public static class ParkingVehicleStateProviderFactory
|
|
{
|
|
/// <summary>
|
|
/// 使用当前车辆运行配置创建停车轨迹控制的默认状态源。
|
|
/// </summary>
|
|
public static WheelFeedbackVehicleStateProvider Create(
|
|
MultiWheelChassis chassis)
|
|
{
|
|
return Create(
|
|
chassis,
|
|
PilotDefinition.Conf);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 使用指定配置快照创建停车轨迹控制的默认状态源。
|
|
/// </summary>
|
|
public static WheelFeedbackVehicleStateProvider Create(
|
|
MultiWheelChassis chassis,
|
|
PilotConfig config)
|
|
{
|
|
if (chassis == null)
|
|
{
|
|
throw new ArgumentNullException(nameof(chassis));
|
|
}
|
|
|
|
if (config == null)
|
|
{
|
|
throw new ArgumentNullException(nameof(config));
|
|
}
|
|
|
|
var velocityEstimator =
|
|
new VelocityEstimator2D(
|
|
config.ParkingDetourLinearVelocityFilterSeconds,
|
|
config.ParkingDetourAngularVelocityFilterSeconds);
|
|
var detourStateProvider =
|
|
new DetourVehicleStateProvider(
|
|
velocityEstimator,
|
|
config.ParkingDetourMaximumLinearSpeed,
|
|
AngleMath.DegreesToRadians(
|
|
config
|
|
.ParkingDetourMaximumAngularSpeedDegrees),
|
|
config.ParkingDetourPositionJumpMargin,
|
|
AngleMath.DegreesToRadians(
|
|
config.ParkingDetourHeadingJumpMarginDegrees),
|
|
config.ParkingDetourVelocityPositionResidual,
|
|
AngleMath.DegreesToRadians(
|
|
config
|
|
.ParkingDetourVelocityHeadingResidualDegrees),
|
|
config.ParkingDetourStationaryConfirmationSeconds);
|
|
|
|
return new WheelFeedbackVehicleStateProvider(
|
|
detourStateProvider,
|
|
chassis,
|
|
config.ParkingWheelVelocityFilterSeconds);
|
|
}
|
|
}
|
|
}
|