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-14 16:19:15 +08:00
parent a13e345f83
commit 9fe8901c4c
25 changed files with 381 additions and 299 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Experiments/TestTrajectoryFactory.cs
+14 -5
View File
@@ -20,7 +20,8 @@ namespace MultiWheelC
double cruiseSpeedMetersPerSecond = 0.30,
double accelerationMetersPerSecondSquared = 0.20,
double decelerationMetersPerSecondSquared = 0.20,
double pointSpacingMeters = 0.02)
double pointSpacingMeters = 0.02,
double motionDirectionInBodyRadians = 0.0)
{
return CreateStraight(
startPoseInWorld,
@@ -28,7 +29,8 @@ namespace MultiWheelC
cruiseSpeedMetersPerSecond,
accelerationMetersPerSecondSquared,
decelerationMetersPerSecondSquared,
pointSpacingMeters);
pointSpacingMeters,
motionDirectionInBodyRadians);
}
/// <summary>
@@ -40,7 +42,8 @@ namespace MultiWheelC
double cruiseSpeedMetersPerSecond = 0.30,
double accelerationMetersPerSecondSquared = 0.20,
double decelerationMetersPerSecondSquared = 0.20,
double pointSpacingMeters = 0.02)
double pointSpacingMeters = 0.02,
double motionDirectionInBodyRadians = 0.0)
{
NumericGuard.EnsureFinite(
startPoseInWorld,
@@ -60,6 +63,9 @@ namespace MultiWheelC
NumericGuard.EnsureFinitePositive(
pointSpacingMeters,
nameof(pointSpacingMeters));
NumericGuard.EnsureFinite(
motionDirectionInBodyRadians,
nameof(motionDirectionInBodyRadians));
if (pointSpacingMeters > lengthMeters)
{
@@ -73,10 +79,13 @@ namespace MultiWheelC
pointSpacingMeters);
var points = new List<TrajectoryPoint>(
segmentCount + 1);
var worldMotionYawRadians =
startPoseInWorld.YawRadians +
motionDirectionInBodyRadians;
var directionX = travelDirection *
Math.Cos(startPoseInWorld.YawRadians);
Math.Cos(worldMotionYawRadians);
var directionY = travelDirection *
Math.Sin(startPoseInWorld.YawRadians);
Math.Sin(worldMotionYawRadians);
for (var index = 0;
index <= segmentCount;