First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

新增倒车以及项目结构优化

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-11 17:06:26 +08:00
parent a59499e638
commit 33a33af710
41 changed files with 817 additions and 2654 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Experiments/NewControllerTrackingTests.cs
+18 -37
View File
@@ -150,11 +150,6 @@ namespace MultiWheelC
return;
}
if (!MovementTestPreparation.AreWheelsForward())
{
return;
}
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
@@ -164,22 +159,20 @@ namespace MultiWheelC
return;
}
var detourStateProvider =
new DetourVehicleStateProvider();
if (!detourStateProvider.TryGetState(
var stateProvider =
ParkingVehicleStateProviderFactory.Create(
chassis);
if (!stateProvider.TryGetState(
out var initialState))
{
Console.WriteLine(
"无法读取有效Detour起点位姿：" +
detourStateProvider.LastFailureReason);
"无法读取有效停车状态起点位姿：" +
stateProvider.LastFailureReason);
_stateProvider = null;
return;
}
_stateProvider =
new WheelFeedbackVehicleStateProvider(
detourStateProvider,
chassis);
_stateProvider = stateProvider;
var trajectoryStartPose =
TrajectoryExperimentInput.OffsetPoseLaterally(
@@ -227,8 +220,6 @@ namespace MultiWheelC
{
Trajectory = trajectory,
StateProvider = _stateProvider,
StanleyUsesActualSpeed = true,
MaximumCommandSpeedMetersPerSecond = 0.50,
CycleObserver = controller =>
RecordControlCycle(
recorder,
@@ -358,13 +349,13 @@ namespace MultiWheelC
}
if (controller.LastProjection.HasValue &&
controller.LastReferenceSpeedMetersPerSecond.HasValue)
controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
{
var projection =
controller.LastProjection.Value;
recorder.UpdateControlReference(
projection.ArcLengthMeters,
controller.LastReferenceSpeedMetersPerSecond.Value,
controller.LastControlReferenceSpeedMetersPerSecond.Value,
projection.LateralErrorMeters,
projection.HeadingErrorRadians,
projection.DistanceToTrajectoryMeters,
@@ -511,11 +502,6 @@ namespace MultiWheelC
return;
}
if (!MovementTestPreparation.AreWheelsForward())
{
return;
}
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
@@ -525,22 +511,20 @@ namespace MultiWheelC
return;
}
var detourStateProvider =
new DetourVehicleStateProvider();
if (!detourStateProvider.TryGetState(
var stateProvider =
ParkingVehicleStateProviderFactory.Create(
chassis);
if (!stateProvider.TryGetState(
out var initialState))
{
Console.WriteLine(
"无法读取有效Detour起点位姿：" +
detourStateProvider.LastFailureReason);
"无法读取有效停车状态起点位姿：" +
stateProvider.LastFailureReason);
_stateProvider = null;
return;
}
_stateProvider =
new WheelFeedbackVehicleStateProvider(
detourStateProvider,
chassis);
_stateProvider = stateProvider;
var trajectoryStartPose =
TrajectoryExperimentInput.OffsetPoseLaterally(
@@ -593,9 +577,6 @@ namespace MultiWheelC
{
Trajectory = trajectory,
StateProvider = _stateProvider,
StanleyUsesActualSpeed = true,
MaximumCommandSpeedMetersPerSecond =
StraightMaximumSpeedMetersPerSecond,
CycleObserver = controller =>
RecordControlCycle(
recorder,
@@ -725,13 +706,13 @@ namespace MultiWheelC
}
if (controller.LastProjection.HasValue &&
controller.LastReferenceSpeedMetersPerSecond.HasValue)
controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
{
var projection =
controller.LastProjection.Value;
recorder.UpdateControlReference(
projection.ArcLengthMeters,
controller.LastReferenceSpeedMetersPerSecond.Value,
controller.LastControlReferenceSpeedMetersPerSecond.Value,
projection.LateralErrorMeters,
projection.HeadingErrorRadians,
projection.DistanceToTrajectoryMeters,