private SLAMTransfer GetTransferBySection(MapSection section)
{
SLAMTransfer temp = new SLAMTransfer();
temp.Step1Offset = slamPosition[section.FromAddress.Id].Position;
temp.Step4Offset = section.FromAddress.AGVPosition.Position;
temp.Step3Mag = section.Distance / computeFunction.GetTwoPositionDistance(slamPosition[section.FromAddress.Id].Position, slamPosition[section.ToAddress.Id].Position);
double mapAngle = computeFunction.ComputeAngle(section.FromAddress.AGVPosition.Position, section.ToAddress.AGVPosition.Position);
MapPosition start = new MapPosition(slamPosition[section.FromAddress.Id].Position.X, -slamPosition[section.FromAddress.Id].Position.Y);
MapPosition end = new MapPosition(slamPosition[section.ToAddress.Id].Position.X, -slamPosition[section.ToAddress.Id].Position.Y);
double slamAngle = computeFunction.ComputeAngle(start, end);
temp.ThetaOffset = mapAngle - slamAngle;
// theta 要帶-/+?
temp.Step2Sin = Math.Sin(-temp.ThetaOffset * Math.PI / 180);
temp.Step2Cos = Math.Cos(-temp.ThetaOffset * Math.PI / 180);
temp.ThetaOffset = (slamPosition[section.FromAddress.Id].Angle + slamPosition[section.ToAddress.Id].Angle) / 2;
temp.ThetaOffsetStart = section.FromVehicleAngle + slamPosition[section.FromAddress.Id].Angle + (section.FromAddress.AGVPosition.Angle - section.FromVehicleAngle);
temp.ThetaOffsetEnd = section.ToVehicleAngle + slamPosition[section.ToAddress.Id].Angle + (section.ToAddress.AGVPosition.Angle - section.ToVehicleAngle);
double angle = computeFunction.ComputeAngle(start, end);
temp.SinTheta = Math.Sin(-angle * Math.PI / 180);
temp.CosTheta = Math.Cos(-angle * Math.PI / 180);
temp.Distance = computeFunction.GetTwoPositionDistance(start, end);
return(temp);
}
private void TransferToMapPosition_BySLAMTransferData(SLAMTransfer transfer)
{
if (transfer == null)
{
nowAGVPosition = null;
}
else
{
LocateAGVPosition temp = new LocateAGVPosition(offsetAGVPosition);
double x = offsetAGVPosition.AGVPosition.Position.X - transfer.Step1Offset.X;
double y = offsetAGVPosition.AGVPosition.Position.Y - transfer.Step1Offset.Y;
double newX = x * transfer.CosTheta + y * transfer.SinTheta;
if (newX < 0)
{
newX = 0;
}
else if (newX > transfer.Distance)
{
newX = transfer.Distance;
}
// Theta顛倒.
temp.AGVPosition.Angle = -temp.AGVPosition.Angle;
temp.AGVPosition.Angle = computeFunction.GetCurrectAngle(temp.AGVPosition.Angle +
(transfer.ThetaOffsetStart + computeFunction.GetCurrectAngle(transfer.ThetaOffsetEnd - transfer.ThetaOffsetStart) * newX / transfer.Distance));
// 上下顛倒.
y = -y;
// 旋轉.
temp.AGVPosition.Position.X = x * transfer.Step2Cos + y * transfer.Step2Sin;
temp.AGVPosition.Position.Y = -x * transfer.Step2Sin + y * transfer.Step2Cos;
temp.AGVPosition.Position.X *= transfer.Step3Mag;
temp.AGVPosition.Position.Y *= transfer.Step3Mag;
temp.AGVPosition.Position.X += transfer.Step4Offset.X;
temp.AGVPosition.Position.Y += transfer.Step4Offset.Y;
nowAGVPosition = temp;
}
}
private SLAMTransfer GetTransferByFourPosition(MapPosition startMap, MapPosition endMap, MapPosition startSLAM, MapPosition endSLAM, double startAngle, double endAngle)
{
SLAMTransfer temp = new SLAMTransfer();
temp.Step1Offset = startSLAM;
temp.Step4Offset = startMap;
temp.Step3Mag = computeFunction.GetTwoPositionDistance(startMap, endMap) / computeFunction.GetTwoPositionDistance(startSLAM, endSLAM);
double mapAngle = computeFunction.ComputeAngle(startMap, endMap);
MapPosition start = new MapPosition(startSLAM.X, -startSLAM.Y);
MapPosition end = new MapPosition(endSLAM.X, -endSLAM.Y);
double slamAngle = computeFunction.ComputeAngle(start, end);
temp.ThetaOffset = mapAngle - slamAngle;
// theta 要帶-/+?
temp.Step2Sin = Math.Sin(-temp.ThetaOffset * Math.PI / 180);
temp.Step2Cos = Math.Cos(-temp.ThetaOffset * Math.PI / 180);
temp.Distance = computeFunction.GetTwoPositionDistance(start, end);
return(temp);
}
protected void SetFindSectionData()
{
SectionLine slamSection;
SLAMTransfer slamTransfer;
MapAddress from;
MapAddress to;
double sectionAngle;
double sectionDistance;
foreach (MapSection section in localData.TheMapInfo.AllSection.Values)
{
if (slamPosition.ContainsKey(section.FromAddress.Id) && slamPosition[section.FromAddress.Id] != null &&
slamPosition.ContainsKey(section.ToAddress.Id) && slamPosition[section.ToAddress.Id] != null)
{
from = new MapAddress();
from.Id = section.FromAddress.Id;
from.AGVPosition = slamPosition[section.FromAddress.Id];
to = new MapAddress();
to.Id = section.ToAddress.Id;
to.AGVPosition = slamPosition[section.ToAddress.Id];
sectionAngle = computeFunction.ComputeAngle(from.AGVPosition, to.AGVPosition);
sectionDistance = computeFunction.GetDistanceFormTwoAGVPosition(from.AGVPosition, to.AGVPosition);
slamSection = new SectionLine(section, from, to, sectionAngle, sectionDistance, 0, true, 0);
findSectionList.Add(section.Id, slamSection);
slamTransfer = new SLAMTransfer();
slamTransfer.Step1Offset = slamPosition[section.FromAddress.Id].Position;
sectionSLAMTransferLit.Add(section.Id, GetTransferBySection(section));
//sectionSLAMTransferLit.Add(section.Id, GetTransferByFourPosition(section.FromAddress.AGVPosition.Position, section.ToAddress.AGVPosition.Position,
// slamPosition[section.FromAddress.Id].Position, slamPosition[section.ToAddress.Id].Position, section.FromVehicleAngle, section.ToVehicleAngle));
SLAMTransfer temp = sectionSLAMTransferLit[section.Id];
WriteLog(7, "", String.Concat("Section : ", section.Id,
", Step1Offset (", computeFunction.GetMapPositionString(temp.Step1Offset, "0"),
"), Step2Cos : ", temp.Step2Cos.ToString("0.0"),
", Step2Sin : ", temp.Step2Sin.ToString("0.0"),
", Step3Mag : ", temp.Step3Mag.ToString("0.0"),
", Step4Offset (", computeFunction.GetMapPositionString(temp.Step4Offset, "0"),
"), ThetaOffset : ", temp.ThetaOffset.ToString("0.0"),
", ThetaOffsetStart : ", temp.ThetaOffsetStart.ToString("0.0"),
", ThetaOffsetEnd : ", temp.ThetaOffsetEnd.ToString("0.0"),
", Distance : ", temp.Distance.ToString("0.0")));
}
}
List <string> tempList;
foreach (MapSection section in localData.TheMapInfo.AllSection.Values)
{
if (findSectionList.ContainsKey(section.Id))
{
tempList = new List <string>();
for (int i = 0; i < section.NearbySection.Count; i++)
{
if (findSectionList.ContainsKey(section.NearbySection[i].Id))
{
tempList.Add(section.NearbySection[i].Id);
}
}
sectionConnectSectionList.Add(section.Id, tempList);
}
}
}
