Commit 5fc8ae41 authored by Alexander David Hellwig's avatar Alexander David Hellwig
Browse files

ba modeling

parent 3ffe489a
package de.monticore.lang.monticar.generator.middleware;
import de.monticore.lang.embeddedmontiarc.embeddedmontiarc._ast.ASTComponent;
import de.monticore.lang.embeddedmontiarc.embeddedmontiarc._symboltable.ComponentSymbol;
import de.monticore.lang.embeddedmontiarc.embeddedmontiarc._symboltable.ExpandedComponentInstanceSymbol;
import de.monticore.lang.embeddedmontiarc.embeddedmontiarc._symboltable.PortSymbol;
import de.monticore.lang.embeddedmontiarc.embeddedmontiarcmath.cocos.EmbeddedMontiArcMathCoCos;
import de.monticore.lang.embeddedmontiarc.tagging.RosConnectionSymbol;
import de.monticore.lang.embeddedmontiarc.tagging.RosToEmamTagSchema;
import de.monticore.lang.monticar.generator.cpp.GeneratorCPP;
......@@ -81,6 +84,87 @@ public class GenerationTest extends AbstractSymtabTest {
}
@Test
public void plannerTest() throws IOException{
TaggingResolver taggingResolver = createSymTabAndTaggingResolver("src/test/resources/");
ExpandedComponentInstanceSymbol componentInstanceSymbol = taggingResolver.<ExpandedComponentInstanceSymbol>resolve("ba.vehicle.planner", ExpandedComponentInstanceSymbol.KIND).orElse(null);
assertNotNull(componentInstanceSymbol);
ComponentSymbol componentSymbol = taggingResolver.<ComponentSymbol>resolve("ba.vehicle.Planner",ComponentSymbol.KIND).orElse(null);
EmbeddedMontiArcMathCoCos.createChecker().checkAll((ASTComponent)componentSymbol.getAstNode().orElse(null));
MiddlewareGenerator middlewareGenerator = new MiddlewareGenerator();
String generationTargetPath = "./target/generated-sources-cmake/Planner/src/";
middlewareGenerator.setGenerationTargetPath(generationTargetPath);
middlewareGenerator.add(new CPPGenImpl(),"cpp");
List<File> files = middlewareGenerator.generate(componentInstanceSymbol, taggingResolver);
//known errors:
//wayOut(1-1, i-1)
//wayout[1-1,i-1]
//Helper::getDoubleFromOctaveListFirstResult(Fasin(Helper::convertToOctaveValueList(deltaY/dist),1)
//=> Fasin(deltaY/dist) => std::sin(...)
//...
fixKnownErrors(files);
testFilesAreEqual(files,"Planner/src/",generationTargetPath);
}
@Test
public void testBaSystem() throws IOException {
TaggingResolver taggingResolver = createSymTabAndTaggingResolver("src/test/resources/");
ExpandedComponentInstanceSymbol componentInstanceSymbol = taggingResolver.<ExpandedComponentInstanceSymbol>resolve("ba.system", ExpandedComponentInstanceSymbol.KIND).orElse(null);
assertNotNull(componentInstanceSymbol);
ComponentSymbol componentSymbol = taggingResolver.<ComponentSymbol>resolve("ba.System",ComponentSymbol.KIND).orElse(null);
EmbeddedMontiArcMathCoCos.createChecker().checkAll((ASTComponent)componentSymbol.getAstNode().orElse(null));
componentInstanceSymbol.getPorts().forEach(p -> p.setMiddlewareSymbol(new RosConnectionSymbol()));
componentInstanceSymbol.getSubComponents().stream()
.flatMap(sc -> sc.getPorts().stream())
.forEach(p -> p.setMiddlewareSymbol(new RosConnectionSymbol()));
DistributedTargetGenerator distributedTargetGenerator = new DistributedTargetGenerator();
String generationTargetPath = "./target/generated-sources-cmake/system/src/";
distributedTargetGenerator.setGenerationTargetPath(generationTargetPath);
distributedTargetGenerator.add(new CPPGenImpl(),"cpp");
distributedTargetGenerator.add(new RosCppGenImpl(),"roscpp");
List<File> files = distributedTargetGenerator.generate(componentInstanceSymbol, taggingResolver);
fixKnownErrors(files);
}
private void fixKnownErrors(List<File> files) throws IOException {
//known errors:
//wayOut(1-1, i-1)
//=> wayOut[1-1,i-1]
//Helper::getDoubleFromOctaveListFirstResult(Fasin(Helper::convertToOctaveValueList(deltaY/dist),1)
//=> Fasin(deltaY/dist) => std::sin(...)
//...
for(File f : files) {
Path path = Paths.get(f.getAbsolutePath());
Charset charset = StandardCharsets.UTF_8;
String content = new String(Files.readAllBytes(path), charset);
content = content.replace("#include \"octave/builtin-defun-decls.h\"","#include <cmath>");
content = content.replaceAll("\\(Helper::getDoubleFromOctaveListFirstResult\\(([\\w|/]*)\\(Helper::convertToOctaveValueList\\(([\\w|/]*)\\),1\\)\\)\\)","$1($2)");
content = content.replaceAll("(\\w+)\\(([\\w|\\-|,|\" \"]+\\-1)\\)","$1[$2]");
content = content.replace("LaneletPaircurLaneletPair", "ba_util_LaneletPair curLaneletPair");
content = content.replace("Fasin","std::asin");
content = content.replace("Fsin","std::sin");
content = content.replace("Fcos","std::cos");
content = content.replace("Fabs","std::abs");
// content = content.replace("sqrt","std::sqrt");
Files.write(path, content.getBytes(charset));
}
}
@Test
public void testMiddlewareGenerator() throws IOException {
TaggingResolver taggingResolver = createSymTabAndTaggingResolver("src/test/resources/");
......@@ -155,12 +239,14 @@ public class GenerationTest extends AbstractSymtabTest {
.filter(c -> c.getSourcePort().equals(c.getTargetPort()))
.forEach(c -> System.out.println("Source = Target:"+c.getSource() + " -> " + c.getTargetPort()));
componentInstanceSymbol.getPorts().forEach(p -> p.setMiddlewareSymbol(new RosConnectionSymbol()));
componentInstanceSymbol.getSubComponents().stream()
.flatMap(subc -> subc.getConnectors().stream())
.filter(c -> c.getSourcePort().equals(c.getTargetPort()))
.forEach(c -> System.out.println("Source = Target in comp "+c.getComponentInstance().get().getName()+":"+c.getSource() + " -> " + c.getTargetPort()));
componentInstanceSymbol.getPorts().forEach(p -> p.setMiddlewareSymbol(new RosConnectionSymbol()));
componentInstanceSymbol.getSubComponents().stream()
.flatMap(subc -> subc.getPorts().stream())
.forEach(p -> p.setMiddlewareSymbol(new RosConnectionSymbol()));
......
package ba;
import ba.intersection.IntersectionController;
import ba.vehicle.Planner;
import ba.util.Position;
component System{
instance IntersectionController intersectionController;
instance Planner planner[2];
//Intersection
port in Q cutoffPosIn;
port in Q cutoffTimeIn;
port in B isActiveIn;
connect cutoffPosIn -> intersectionController.cutoffPos;
connect cutoffTimeIn -> intersectionController.cutoffTime;
connect isActiveIn -> intersectionController.isActive;
//Planner(s)
ports in Q^{4,10} lanelatIn[2],
in Position positionIn[2],
in Q maxVelocityIn[2],
in Q maxAccelerationIn[2],
in Q deltaTimeIn[2],
in Q planningTimeIn[2],
in B resetVelocityIn[2],
out Q^{3,20} desiredTrajectoryOut[2];
connect lanelatIn[:] -> planner[:].lanelat;
connect positionIn[:] -> planner[:].position;
connect maxVelocityIn[:] -> planner[:].maxVelocity;
connect maxAccelerationIn[:] -> planner[:].maxAcceleration;
connect deltaTimeIn[:] -> planner[:].deltaTime;
connect planningTimeIn[:] -> planner[:].planningTime;
connect resetVelocityIn[:] -> planner[:].resetVelocity;
connect planner[:] -> desiredTrajectoryOut[:];
//Both
connect planner[:].desiredTrajectory -> intersectionController.trajectoryIn[:];
connect intersectionController.stop[:] -> planner[:].slowDown;
}
\ No newline at end of file
......@@ -4,6 +4,7 @@ package ba.intersection;
component ConflictToStopConverter<N1 n = 2,N1 x = 1,N1 m = 5>{
port in Z(1:n) indexLookupIn[x];
port in B conflictIn[x];
port in B active;
port out B stopOut[n];
implementation Math{
......@@ -11,10 +12,12 @@ component ConflictToStopConverter<N1 n = 2,N1 x = 1,N1 m = 5>{
stopOut(i) = False;
end
for i = 1:x
if conflictIn(i)
Z curIndex = indexLookupIn(i);
stopOut(curIndex) = True;
if active
for i = 1:x
if conflictIn(i)
Z curIndex = indexLookupIn(i);
stopOut(curIndex) = True;
end
end
end
......
package ba.intersection;
import ba.util.RelToAbsTrajectory;
//component IntersectionController<N1 n = 2,N1 x = 1,N1 m = 20>{
component IntersectionController{
port in Q^{3,5} relTrajIn[2];
port in Q^{2,1} absPosIn[2];
port in Q cutoffPosIn;
port in Q cutoffTimeIn;
port out B stopOut[2];
instance RelToAbsTrajectory<2,5> relToAbsTrajectory;
instance TrajectoryToStop<2,1,5> trajectoryToStop;
connect cutoffPosIn -> trajectoryToStop.cutoffPos;
connect cutoffTimeIn -> trajectoryToStop.cutoffTime;
connect relTrajIn[:] -> relToAbsTrajectory.relTrajectoryIn[:];
connect absPosIn[:] -> relToAbsTrajectory.absPositionIn[:];
connect relToAbsTrajectory.absTrajectoryOut[:] -> trajectoryToStop.trajectoryIn[:];
connect trajectoryToStop.stop[:] -> stopOut[:];
port in Q^{3,20} trajectoryIn[2];
port in Q cutoffPos;
port in Q cutoffTime;
port in B isActive;
port out B stop[2];
instance ConflictToStopLookup<2,1> conflictToStopLookup;
instance ConflictComputer<2,1,20> conflictComputer;
instance ConflictToStopConverter<2,1,20> conflictToStopConverter;
connect trajectoryIn[:] -> conflictComputer.trajectory[:];
connect cutoffPos -> conflictComputer.cutoffPosIn;
connect cutoffTime -> conflictComputer.cutoffTimeIn;
connect conflictComputer.conflictOut[:] -> conflictToStopConverter.conflictIn[:];
connect conflictToStopLookup.indexLookup[:] -> conflictToStopConverter.indexLookupIn[:];
connect conflictToStopConverter.stopOut[:] -> stop[:];
connect isActive -> conflictToStopConverter.active;
}
\ No newline at end of file
package ba.intersection;
//TODO: x = (n-1)n/2
component TrajectoryToStop<N1 n = 2,N1 x = 1,N1 m = 5>{
port in Q^{3,m} trajectoryIn[n];
port in Q cutoffPos;
port in Q cutoffTime;
port out B stop[n];
instance ConflictToStopLookup<n,x> conflictToStopLookup;
instance ConflictComputer<n,x,m> conflictComputer;
instance ConflictToStopConverter<n,x,m> conflictToStopConverter;
connect trajectoryIn[:] -> conflictComputer.trajectory[:];
connect cutoffPos -> conflictComputer.cutoffPosIn;
connect cutoffTime -> conflictComputer.cutoffTimeIn;
connect conflictComputer.conflictOut[:] -> conflictToStopConverter.conflictIn[:];
connect conflictToStopLookup.indexLookup[:] -> conflictToStopConverter.indexLookupIn[:];
connect conflictToStopConverter.stopOut[:] -> stop[:];
}
\ 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.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} lanelat,
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> lanelatToWay;
instance VelocityController velocityController;
instance TrajectoryComputer<10,20> trajectoryComputer;
connect lanelat -> lanelatToWay.laneletIn;
connect maxVelocity -> velocityController.maxVelIn;
connect maxAcceleration -> velocityController.maxAccelIn;
connect deltaTime -> velocityController.deltaTimeIn;
connect slowDown -> velocityController.slowDownIn;
connect resetVelocity -> velocityController.resetVelIn;
connect lanelatToWay.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;
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
package ba.vehicle;
component VelocityController{
ports in Q maxVelIn,
in Q maxAccelIn,
in Q deltaTimeIn,
in B slowDownIn,
in B resetVelIn;
ports out Q curVelOut;
implementation Math{
static Q lastVel = 0;
Q resVel;
if resetVelIn
lastVel = maxVelIn;
else
Q deltaVel = 0.5 * maxAccelIn * deltaTimeIn * deltaTimeIn;
if slowDownIn
resVel = lastVel - deltaVel;
else
resVel = lastVel + deltaVel;
end
end
if resVel < 0
curVelOut = 0;
elseif resVel > maxVelIn
curVelOut = maxVelIn;
end
lastVel = curVelOut;
}
}
\ 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