Commit 08e8ee08 authored by Alexander David Hellwig's avatar Alexander David Hellwig
Browse files

Model: removed unused components

parent c4fc2f7d
package ba.tests;
stream RelToAbsTrajectory for RelToAbsTrajectoryInst{
relTrajIn1: [0,0,0,0,0;1,1,1,1,1;2,2,2,2,2];
relTrajIn2: [0,0,0,0,0;1,1,1,1,1;2,2,2,2,2];
absPosIn1: [1;1];
absPosIn2: [2;2];
absTrajOut1: [1,1,1,1,1;2,2,2,2,2;2,2,2,2,2];
absTrajOut2: [2,2,2,2,2;3,3,3,3,3;2,2,2,2,2];
}
\ No newline at end of file
package ba.tests;
import ba.util.RelToAbsTrajectory;
component RelToAbsTrajectoryInst{
ports in Q^{3,5} relTrajIn1,
in Q^{3,5} relTrajIn2,
in Q^{2,1} absPosIn1,
in Q^{2,1} absPosIn2,
out Q^{3,5} absTrajOut1,
out Q^{3,5} absTrajOut2;
instance RelToAbsTrajectory<2,5> relToAbsTrajectory;
connect relTrajIn1 -> relToAbsTrajectory.relTrajectoryIn[1];
connect relTrajIn2 -> relToAbsTrajectory.relTrajectoryIn[2];
connect absPosIn1 -> relToAbsTrajectory.absPositionIn[1];
connect absPosIn2 -> relToAbsTrajectory.absPositionIn[2];
connect relToAbsTrajectory.absTrajectoryOut[1] -> absTrajOut1;
connect relToAbsTrajectory.absTrajectoryOut[2] -> absTrajOut2;
}
\ No newline at end of file
package ba.util;
struct LaneletPair{
Q leftX;
Q leftY;
Q rightX;
Q rightY;
}
\ No newline at end of file
package ba.util;
struct Position{
Q posX;
Q posY;
}
\ No newline at end of file
package ba.util;
component RelToAbsTrajectory<N1 n = 2, N1 m = 5>{
port in Q^{3,m} relTrajectoryIn[n];
port in Q^{2,1} absPositionIn[n];
port out Q^{3,m} absTrajectoryOut[n];
implementation Math{
for i = 1:n
Q^{3,m} tmpRelTraj = relTrajectoryIn(i);
Q^{2,1} tmpAbsPos = absPositionIn(i);
Q^{3,m} tmpAbsTraj;
for j = 1:m
//x
tmpAbsTraj(1,j) = tmpRelTraj(1,j) + tmpAbsPos(1,1);
//y
tmpAbsTraj(2,j) = tmpRelTraj(2,j) + tmpAbsPos(2,1);
//dt
tmpAbsTraj(3,j) = tmpRelTraj(3,j);
end
absTrajectoryOut(i) = tmpAbsTraj;
end
}
}
\ No newline at end of file
package ba.vehicle;
import ba.util.Position;
component IndexToTrajectory<N1 k = 10, N1 m = 20>{
ports in Q planningTime,
in Q curVel,
in Position pos,
in Q^{2,k} way,
in Z indexIn,
out Q^{3,m} traj;
implementation Math{
//s = v*t
Q maxPlanDist = planningTime * curVel;
Q stepDist = maxPlanDist / k;
Q stepTime = maxPlanDist / k;
Q planDist = 0;
Q lastPosX = pos.posX;
Q lastPosY = pos.posY;
Q lastTime = 0;
Z curIndex = indexIn;
//set nextCutoff
Q dxToNext = lastPosX - way(1, curIndex);
Q dyToNext = lastPosY - way(2, curIndex);
Q nextCutoff = planDist + sqrt(dxToNext * dxToNext + dyToNext * dyToNext);
for i = 1:m
if nextCutoff < planDist
if curIndex == k
//end of way reached
if (way(1, k)== way(1, 1)) && (way(2, k) == way(2, 1))
//way is loop
curIndex = 2;
else
//Stop planning
planDist = maxPlanDist + 1;
end
else
curIndex = curIndex + 1;
end
Q dxToNext = lastPosX - way(1, curIndex);
Q dyToNext = lastPosY - way(2, curIndex);
nextCutoff = planDist + sqrt(dxToNext * dxToNext + dyToNext * dyToNext);
end
Q deltaX = lastPosX - way(1, curIndex);
Q deltaY = lastPosY - way(2, curIndex);
Q dist = sqrt(deltaX * deltaX + deltaY * deltaY);
Q angleToGoal = asin(deltaY/dist);
if planDist < maxPlanDist
lastTime = lastTime + stepTime;
lastPosX = lastPosX + cos(angleToGoal) * stepDist;
lastPosY = lastPosY + sin(angleToGoal) * stepDist;
end
planDist = planDist + stepDist;
traj(1,i) = lastPosX;
traj(2,i) = lastPosY;
traj(3,i) = lastTime;
end
}
}
\ No newline at end of file
package ba.vehicle;
import ba.util.Position;
component LaneletToWay<N1 k = 10>{
ports in Q^{4,k} laneletIn;
ports out Q^{2,k} wayOut;
implementation Math{
for i = 1:k
Q deltaX = laneletIn(1, i) - laneletIn(3, i);
Q deltaY = laneletIn(2, i) - laneletIn(4, i);
wayOut(1,i) = laneletIn(1, i) + 0.5 * deltaX;
wayOut(2,i) = laneletIn(2, i) + 0.5 * deltaY;
end
}
}
\ No newline at end of file
package ba.vehicle;
import ba.util.Position;
//component Planner<N1 k = 10,N1 m = 20>{
component Planner{
ports in Q^{4,10} lanelet,
in Position position,
in Q maxVelocity,
in Q maxAcceleration,
in Q deltaTime,
in Q planningTime,
in B slowDown,
in B resetVelocity;
ports out Q^{3,20} desiredTrajectory;
instance LaneletToWay<10,20> laneletToWay;
instance VelocityController velocityController;
instance TrajectoryComputer<10,20> trajectoryComputer;
connect lanelet -> laneletToWay.laneletIn;
connect maxVelocity -> velocityController.maxVelIn;
connect maxAcceleration -> velocityController.maxAccelIn;
connect deltaTime -> velocityController.deltaTimeIn;
connect slowDown -> velocityController.slowDownIn;
connect resetVelocity -> velocityController.resetVelIn;
connect laneletToWay.wayOut -> trajectoryComputer.wayIn;
connect velocityController.curVelOut -> trajectoryComputer.curVelIn;
connect planningTime -> trajectoryComputer.planningTimeIn;
connect position -> trajectoryComputer.posIn;
connect trajectoryComputer.trajOut -> desiredTrajectory;
}
\ No newline at end of file
package ba.vehicle;
import ba.util.Position;
component PosToIndex<N1 k = 10>{
ports in Position pos,
in Q^{2,k} way,
out Z index;
implementation Math{
Z closestInd = -1;
Z secondClosestInd = -1;
Q closestDist = 0;
Q secondClosestDist = 0;
for i = 1:k
Q deltaX = pos.posX - way(1,i);
Q deltaY = pos.posY - way(2,i);
Q curDist = sqrt(deltaX * deltaX + deltaY * deltaY);
if (closestInd == -1) || (curDist <= closestDist)
secondClosestDist = closestDist;
secondClosestInd = closestInd;
closestDist = curDist;
closestInd = i;
elseif(secondClosestInd == -1) || (curDist <= secondClosestDist)
secondClosestDist = curDist;
secondClosestInd = i;
end
end
Q betweenDX = way(1, closestInd)- way(1, secondClosestInd);
Q betweenDY = way(2, closestInd) - way(2, secondClosestInd);
Q betweenDist = sqrt(betweenDX * betweenDX + betweenDY * betweenDY);
Q angleWay = 0;
if betweenDist > 0
angleWay = asin(betweenDY/betweenDist);
end
//angle between the vector pos -> closest and the x axis
Q angle = 0;
if closestDist > 0
Q deltaY = pos.posY - way(2, closestInd);
angle = asin(deltaY/closestDist);
end
//asin= -pi/2 : pi/2 => res = angle - angleWay = -pi:pi => abs(res) < pi/4
Q deltaAngle = angle - angleWay;
if abs(deltaAngle) > (M_PI/4)
//past closest point
index = secondClosestInd;
else
//in front of closest point
index = closestInd;
end
}
}
\ No newline at end of file
package ba.vehicle;
import ba.util.Position;
component TrajectoryComputer<N1 k = 10,N1 m = 20>{
ports in Position posIn,
in Q^{2,k} wayIn,
in Q curVelIn,
in Q planningTimeIn;
ports out Q^{3,m} trajOut;
instance PosToIndex<10> posToIndex;
instance IndexToTrajectory<10,20> indexToTrajectory;
connect planningTimeIn -> indexToTrajectory.planningTime;
connect curVelIn -> indexToTrajectory.curVel;
connect posIn -> indexToTrajectory.pos;
connect wayIn -> indexToTrajectory.way;
connect posIn -> posToIndex.pos;
connect wayIn -> posToIndex.way;
connect posToIndex.index -> indexToTrajectory.indexIn;
connect indexToTrajectory.traj -> trajOut;
}
\ No newline at end of file
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment