First_Dev_Dept/Tutorial
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update 增加停车机器人钻车等代码 实现停车机器人能力的瘦身代码

Browse Source
This commit is contained in:
shuai.li
2026-06-29 11:46:02 +08:00
parent d935e847ec
commit 8b19a15fbb
7 changed files with 1443 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/TireFollowing.cs
+467
View File
@@ -0,0 +1,467 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Numerics;
using System.Reflection;
using System.Text;
using System.Threading;
using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using ClumsyCore.Utilities;
using ClumsyDance.ClumsyWalk.Detectors;
using CommonUsage.Chassis;
using FundamentalLib;
using MDCSToolBox;
using MDCSToolBox.Clumsy.Calibration;
using MDCSToolBox.Clumsy.MotionControllers;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;
using MDCSToolBox.Clumsy.Tracks;
using MDCSToolBox.Commons.Controllers;
using static ClumsyCore.DTools.Painter;
using LineSegment = ClumsyCore.Utilities.LineSegment;
namespace MultiWheelC
{
public class TireFollowing : MovementDefinition
{
public Func<AbstractGeometricController> GetController;
/// <summary>
/// 车辆方向
/// </summary>
public float CarDirection = 0;
/// <summary>
/// 停止距离
/// </summary>
//public float FinishDistance = 1000;
/// <summary>
/// 减速距离
/// </summary>
public float SlowDistance = 1000;
/// <summary>
/// 最大速度
/// </summary>
public float MaxSpeed = 0.3f;
/// <summary>
/// 钻轮胎数量
/// </summary>
public int TireNum = 1;
/// <summary>
/// 盲走角度偏移
/// </summary>
public float WalkBlindTh = -1f;
/// <summary>
/// 是否检测到目标
/// </summary>
public bool NoTarget = false;
public float GuessRangeX;
public float GuessRangeY;
/// <summary>
/// 检测器定义
/// </summary>
public class DetectorDefinition
{
/// <summary>
/// 开始检测距离
/// </summary>
public float StartGuessingX;
/// <summary>
/// 开始检测距离
/// </summary>
public float StartGuessingY;
/// <summary>
/// 检测函数
/// </summary>
public Func<float, float, List<DetectFilter>, LineSegment> DetectFunction = null;
public Action<int> LeaveSrcFunction = null;
public int SrcId = -1;
public int DstId = -1;
/// <summary>
/// 路径偏移
/// </summary>
public Tuple<float, float, float> PathTransformation = Tuple.Create(0f, 0f, 0f);
public float PathTransformationAnchorDistance = 0f;
/// <summary>
/// 切换条件
/// </summary>
public Func<float, bool> SwitchWalkBlindCondition = null;
/// <summary>
/// 盲走停止距离
/// </summary>
public Func<float, bool> FinishWalkBlindCondition = null;
}
public Func<bool> FinishCondition;
/// <summary>
/// 多个检测器列表
/// </summary>
public List<DetectorDefinition> detectors = null;
private Painter _painter;
private List<float> _remainDistanceList = new List<float>();
private List<float> _remainAngleList = new List<float>();
private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
{
var painter = UI.GetPainter("GeneralFollowing.SetFilters", false);
painter.Clear();
painter.Clear(3000);
var box = new Vector2[]
{
new (guessCenterX - GuessRangeX, guessCenterY - GuessRangeY),
new (guessCenterX + GuessRangeX, guessCenterY - GuessRangeY),
new (guessCenterX + GuessRangeX, guessCenterY + GuessRangeY),
new (guessCenterX - GuessRangeX, guessCenterY + GuessRangeY),
};
for (var i = 0; i < box.Length; ++i)
painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
// PC filter in car coordinate frame
return new List<DetectFilter>()
{
new(CoordinateSystem.Car2D,
p => LessMath.IsPointInPolygon4(
box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
};
}
public void Stop()
{
_dt?.Stop();
}
/// <summary>
///计算车体中心的位移和角度增量
/// </summary>
/// <param name="a">a轮在车体坐标系下位置</param>
/// <param name="va">a轮在车体坐标系下位移增量</param>
/// <param name="b">b轮在车体坐标系下位置</param>
/// <param name="vb">b轮在车体坐标系下位移增量</param>
/// <returns></returns>
private static (float, float, float) CenterMoveFromPoints(Vector2 a,
Vector2 aDelta,
Vector2 b,
Vector2 bDelta)
{
float th_x = 0, th_y = 0, th = 0, x = 0, y = 0;
var eps = 0.0000001;
if (Math.Abs(a.Y - b.Y) > eps)
{
th_x = (aDelta.X - bDelta.X) / (b.Y - a.Y);
}
if (Math.Abs(a.X - b.X) > eps)
{
th_y = (aDelta.Y - bDelta.Y) / (a.X - b.X);
}
th = th_x == 0 ? th_y : th_x;
x = (aDelta.X + bDelta.X) / 2 - (a.Y - b.Y) / 2 * th;
y = (aDelta.Y + bDelta.Y) / 2 + (a.X - b.X) / 2 * th;
return (x, y, th);
}
public override IEnumerable<bool> Get()
{
_painter = UI.GetPainter("GeneralFollowing", false);
var lastDetectX = detectors[0].StartGuessingX;
var lastDetectY = detectors[0].StartGuessingY;
var detectorIndex = 0;
var controller = (MultiWheelGeometricController)GetController.Invoke();
controller.BaseSpeed = MaxSpeed;
controller.FinishDistance = float.MinValue;
controller.FirstThAccuracy = 999;
_dt = new DriveTask(controller.Track(true, CoordinateSystem.Car2D));
float WalkBlindCarPathDstX = -1f, WalkBlindCarPathDstY = -1f, WalkBlindCarPathDstTh = -1f;
bool WalkBlindStage1 = false, WalkBlindStage2 = false;
var angle2target = -1f;
float _lastLFLEncoder = -1, _lastLFREncoder = -1, _lastRFLEncoder = -1, _lastRFREncoder = -1;
float _lastLRLEncoder = -1, _lastLRREncoder = -1, _lastRRLEncoder = -1, _lastRRREncoder = -1;
(float, float, float) GetCurrentPos2Dst(float lastX, float lastY, float lastTh)
{
// Read current encoders
var curLFLEncoder = PilotDefinition.Self.LFLActualPos;
var curLFREncoder = PilotDefinition.Self.LFRActualPos;
var curRFLEncoder = PilotDefinition.Self.RFLActualPos;
var curRFREncoder = PilotDefinition.Self.RFRActualPos;
var curLRLEncoder = PilotDefinition.Self.LRLActualPos;
var curLRREncoder = PilotDefinition.Self.LRRActualPos;
var curRRLEncoder = PilotDefinition.Self.RRLActualPos;
var curRRREncoder = PilotDefinition.Self.RRRActualPos;
// Average delta per wheel pair (LF, LR, RF, RR)
var lfDelta = (curLFLEncoder - _lastLFLEncoder + curLFREncoder - _lastLFREncoder) / 2;
var lrDelta = (curLRLEncoder - _lastLRLEncoder + curLRREncoder - _lastLRREncoder) / 2;
var rfDelta = (curRFLEncoder - _lastRFLEncoder + curRFREncoder - _lastRFREncoder) / 2;
var rrDelta = (curRRLEncoder - _lastRRLEncoder + curRRREncoder - _lastRRREncoder) / 2;
var deltaList = new List<float> { lfDelta, lrDelta, rfDelta, rrDelta };
var xs = new List<float>();
var ys = new List<float>();
var ths = new List<float>();
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
var steerWheels = chassis.GetSteerWheels();
for (var i = 0; i < steerWheels.Count; ++i)
{
var sw1 = steerWheels[i];
var a = sw1.Position;
var tha = sw1.ReadAngle() / 180 * (float)Math.PI;
var deltaa = deltaList[i];
var va = new Vector2(deltaa * (float)Math.Cos(tha), deltaa * (float)Math.Sin(tha));
for (var j = i + 1; j < steerWheels.Count; ++j)
{
var sw2 = steerWheels[j];
var b = sw2.Position;
var thb = sw2.ReadAngle() / 180 * (float)Math.PI;
var deltab = deltaList[j];
var vb = new Vector2(deltab * (float)Math.Cos(thb), deltab * (float)Math.Sin(thb));
var (tempx, tempy, tempth) = CenterMoveFromPoints(a, va, b, vb);
Hedingben.ToastText($"{tempx:f2} {tempy:f2} {tempth / Math.PI * 180:f2} ", $"{i}_{j}");
xs.Add(tempx);
ys.Add(tempy);
ths.Add(tempth);
}
}
var x = xs.Average();
var y = ys.Average();
var Th = ths.Average() / (float)Math.PI * 180;
var moveTup = Tuple.Create(x, y, Th);
var moved = MathTools.SolveTransform2D(MathTools.SolveTransform2D(Tuple.Create(lastX, lastY, lastTh), moveTup), Tuple.Create(0f, 0f, 0f));
_lastLFLEncoder = curLFLEncoder;
_lastLFREncoder = curLFREncoder;
_lastRFLEncoder = curRFLEncoder;
_lastRFREncoder = curRFREncoder;
_lastLRLEncoder = curLRLEncoder;
_lastLRREncoder = curLRREncoder;
_lastRRLEncoder = curRRLEncoder;
_lastRRREncoder = curRRREncoder;
return (moved.Item1, moved.Item2, moved.Item3);
}
while (true)
{
if (detectorIndex > detectors.Count - 1)
throw new Exception("detector index out of range!");
_painter.Clear();
if (WalkBlindStage1 || WalkBlindStage2)
{
//第二次盲走时或只钻一个轮胎时
if (WalkBlindStage2 || detectors.Count == 1 || TireNum == 1)
{
controller.SlowDistance = SlowDistance;
controller.SlowingPow = 0.7f;
}
(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh) = GetCurrentPos2Dst(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
var walkBlindPathEnd = Tuple.Create(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
var walkBlindPathStart = LessMath.Transform2D(walkBlindPathEnd, Tuple.Create(CarDirection == 0 ? -3000f : 3000f, 0f, 0f));
var walkBlindPathDst = new Vector2(WalkBlindCarPathDstX, WalkBlindCarPathDstY);
var walkBlindPathSrc = new Vector2(walkBlindPathStart.Item1, walkBlindPathStart.Item2);
var walkBlindPath = new LineSegment(walkBlindPathSrc, walkBlindPathDst);
DLog.Log($"盲走目标点:{walkBlindPath.Src.X:F2} {walkBlindPath.Src.Y:F2} {walkBlindPath.Dst.X:F2} {walkBlindPath.Dst.Y:F2}", "TireFollowing");
_painter.DrawDot(Color.Purple, walkBlindPathDst, sz: 3);
_painter.DrawLine(Color.GreenYellow, walkBlindPath.Src, walkBlindPath.Dst, endArrow: true, width: 2);
var track = new LineTrack(walkBlindPath.Src, walkBlindPath.Dst);
track.CarDirectionBias = CarDirection;
controller.UpdateTracks(new List<AbstractTrack> { track });
var rd = (float)LessMath.PerpendicularPosition(0, 0, walkBlindPath.Dst.X, walkBlindPath.Dst.Y,
walkBlindPath.Src.X, walkBlindPath.Src.Y);
_remainDistanceList.Add(rd);
while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
rd = _remainDistanceList.Average();
DLog.Log($"盲走投影点剩余距离:{rd:0.0} ", "TireFollowing");
// 检查是否达到盲走结束条件
if (detectors[detectorIndex].FinishWalkBlindCondition(rd))
{
if(WalkBlindStage1)
{
DLog.Log("达到第一次盲走停止距离，停下或开始钻第二对轮胎", "TireFollowing");
//if (detectors[detectorIndex].DstId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
//{
// detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].DstId);
// DLog.Log($"释放取车点{detectors[detectorIndex].DstId}", "TireFollowing");
//}
WalkBlindStage1 = false;
_remainAngleList.Clear();
_remainDistanceList.Clear();
detectorIndex++;
if (detectors.Count == 1 || TireNum == 1)
{
_dt.Stop();
yield return false;
}
}
else if(WalkBlindStage2)
{
DLog.Log("达到第二对轮胎处，停止移动", "TireFollowing");
_dt.Stop();
yield return false;
}
}
yield return true;
continue;
}
var target = detectors[detectorIndex].DetectFunction(CarDirection, lastDetectX,
SetFilters(lastDetectX, lastDetectY));
if(target == null)
{
DLog.Log("无目标，等待下一帧", "TireFollowing");
controller.FirstRotateMaxSpeed = 0;
yield return true;
continue;
}
else controller.FirstRotateMaxSpeed = 5;
var targetAngle = CalculateAngle2YAxis(target.Src, target.Dst);
var targetPos = new Vector2((target.Src.X + target.Dst.X) / 2, (target.Src.Y + target.Dst.Y) / 2);
var dis2target = (float)Math.Sqrt(Math.Pow(targetPos.X, 2) + Math.Pow(targetPos.Y, 2));
//距离较近以后角度容易跳变
if (dis2target < 1400 && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr)
{
yield return true;
continue;
}
else _remainAngleList.Add(targetAngle);
while (_remainAngleList.Count > 10) _remainAngleList.RemoveAt(0);
angle2target = _remainAngleList.Average();
var distanceLabelPos = targetPos / 2;
_painter.DrawLine(Color.Cyan, Vector2.Zero, targetPos, width: 2);
_painter.DrawText(Color.Yellow, $"{dis2target:F3}", distanceLabelPos.X, distanceLabelPos.Y);
var path = DetectorHelper.GetApproachPath(target, CoordinateSystem.Car2D, pathLen:3000,
bias: detectors[detectorIndex].PathTransformation,
biasAnchorDistance: detectors[detectorIndex].PathTransformationAnchorDistance);
if (path == null)
{
DLog.Log("no path!", "TireFollowing");
NoTarget = true;
}
else
{
lastDetectX = ((target.Src + target.Dst) / 2).X;
lastDetectY = ((target.Src + target.Dst) / 2).Y;
_painter.DrawLine(Color.GreenYellow, path.CarPath.Src, path.CarPath.Dst, endArrow: true);
var rd = (float)LessMath.PerpendicularPosition(0, 0, path.CarPath.Dst.X, path.CarPath.Dst.Y,
path.CarPath.Src.X, path.CarPath.Src.Y);
_remainDistanceList.Add(rd);
while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
rd = _remainDistanceList.Average();
_painter.DrawText(Color.Green, $"{rd:F3}", distanceLabelPos.X, distanceLabelPos.Y - 200);
if (detectorIndex < detectors.Count - 1)
{
controller.SlowDistance = 1;
if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
{
WalkBlindStage1 = true;
if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
{
detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
}
WalkBlindCarPathDstX = path.CarPath.Dst.X;
WalkBlindCarPathDstY = path.CarPath.Dst.Y;
WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
DLog.Log($"切换至第一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
$"{WalkBlindCarPathDstY:F2} " +
$"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
_lastLFLEncoder = PilotDefinition.Self.LFLActualPos;
_lastLFREncoder = PilotDefinition.Self.LFRActualPos;
_lastRFLEncoder = PilotDefinition.Self.RFLActualPos;
_lastRFREncoder = PilotDefinition.Self.RFRActualPos;
_lastLRLEncoder = PilotDefinition.Self.LRLActualPos;
_lastLRREncoder = PilotDefinition.Self.LRRActualPos;
_lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
_lastRRREncoder = PilotDefinition.Self.RRRActualPos;
_remainDistanceList.Clear();
lastDetectX = detectors[detectorIndex+1].StartGuessingX;
lastDetectY = detectors[detectorIndex+1].StartGuessingY;
continue;
}
}
else if (detectorIndex == detectors.Count - 1)
{
if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
{
WalkBlindStage2 = true;
WalkBlindCarPathDstX = path.CarPath.Dst.X;
WalkBlindCarPathDstY = path.CarPath.Dst.Y;
WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
DLog.Log($"切换至最后一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
$"{WalkBlindCarPathDstY:F2} " +
$"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
if(detectors.Count == 1)
{
if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
{
detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
}
}
_lastLFLEncoder = PilotDefinition.Self.LFLActualPos;
_lastLFREncoder = PilotDefinition.Self.LFRActualPos;
_lastRFLEncoder = PilotDefinition.Self.RFLActualPos;
_lastRFREncoder = PilotDefinition.Self.RFRActualPos;
_lastLRLEncoder = PilotDefinition.Self.LRLActualPos;
_lastLRREncoder = PilotDefinition.Self.LRRActualPos;
_lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
_lastRRREncoder = PilotDefinition.Self.RRRActualPos;
_remainDistanceList.Clear();
continue;
}
}
DLog.Log($"投影点剩余距离:{rd:F2}", "TireFollowing");
var track = new LineTrack(path.CarPath.Src, path.CarPath.Dst);
track.CarDirectionBias = CarDirection;
controller.UpdateTracks(new List<AbstractTrack> { track });
NoTarget = false;
}
yield return true;
}
}
private static float CalculateAngle2YAxis(Vector2 point1, Vector2 point2)
{
return -(float)(Math.Atan((point1.X - point2.X) / (point1.Y - point2.Y)) * 180 / Math.PI);
}
private DriveTask _dt;
}
}