Commit 432abceb authored by Alexander David Hellwig's avatar Alexander David Hellwig
Browse files

Model: add aIsFaster

parent 90dc2f97
......@@ -142,18 +142,16 @@ public class GenerationTest extends AbstractSymtabTest {
if(con.getSource().startsWith("stopCommQuality")){
con.getSourcePort().setMiddlewareSymbol(new RosConnectionSymbol("/v"+ indexString + "/comm/in/slowDown"+ indexString,"std_msgs/Bool","data"));
con.getTargetPort().setMiddlewareSymbol(new RosConnectionSymbol("/v"+ indexString + "/comm/in/slowDown"+ indexString,"std_msgs/Bool","data"));
}else{
con.getSourcePort().setMiddlewareSymbol(new RosConnectionSymbol("/sim/comm/slowDown" + indexString,"std_msgs/Bool","data"));
con.getTargetPort().setMiddlewareSymbol(new RosConnectionSymbol("/sim/comm/slowDown" + indexString,"std_msgs/Bool","data"));
}
}
DistributedTargetGenerator distributedTargetGenerator = new DistributedTargetGenerator();
String generationTargetPath = "./target/generated-sources-cmake/system/src/";
distributedTargetGenerator.setGenerationTargetPath(generationTargetPath);
distributedTargetGenerator.setGenDebug(true);
//distributedTargetGenerator.setGenDebug(true);
distributedTargetGenerator.add(new CPPGenImpl(),"cpp");
distributedTargetGenerator.add(new RosCppGenImpl(),"roscpp");
......
package ba.intersection;
//TODO: x = (n-1)n/2
component CollisionToStop<N1 n = 2,N1 x = 1,N1 m = 10>{
port in B collisionIn[x];
port in B active;
port out B stopOut[n];
component CollisionToStop<N1 n = 2,N1 x = 1>{
ports in B collisionIn[x],
in B aIsFasterIn[x],
in B active,
out B stopOut[n];
implementation Math{
Z^{n,1} counter;
......@@ -14,18 +15,17 @@ component CollisionToStop<N1 n = 2,N1 x = 1,N1 m = 10>{
end
Z k = 1;
//TODO: remove once generator can handle i = n-1:n
Z maxI = n - 1;
for i = 1:maxI
Z minJ = i + 1;
for j = minJ:n
counter(i,1) = counter(i,1) + 1;
counter(j,1) = counter(j,1) + 1;
if counter(i,1) <= counter(j,1)
indexLookup(k,1) = i;
else
//always stop the last one to arrive at the collision point
if aIsFasterIn(k)
indexLookup(k,1) = j;
else
indexLookup(k,1) = i;
end
k = k + 1;
end
......
......@@ -3,14 +3,14 @@ import ba.util.SingleSetCompareMatrix;
//TODO: x = (n-1)n/2
//TODO: m1 = m - 1, m2 = m1(m1+1)/2
component IntersectionController<N1 n = 2,N1 x = 1,N1 m = 10,N1 m1 = 9,N1 m2 = 45>{
ports in Q^{3,10} trajectoryIn[n],
in Q(0m : 10m) cutoffTime,
ports in Q^{3,m} trajectoryIn[n],
in Q(0s : 10s) cutoffTime,
in B isActive,
out B stop[n];
instance SingleSetCompareMatrix<3,10,2,1> singleSetCompare;
instance TrajectoryCollision<10,9,45> trajectoryCollision[x];
instance CollisionToStop<2,1,10> collisionToStop;
instance CollisionToStop<2,1> collisionToStop;
connect trajectoryIn[:] -> singleSetCompare.setIn[:];
connect singleSetCompare.outA[:] -> trajectoryCollision[:].trajectoryA;
......@@ -18,6 +18,7 @@ component IntersectionController<N1 n = 2,N1 x = 1,N1 m = 10,N1 m1 = 9,N1 m2 = 4
connect cutoffTime -> trajectoryCollision[:].cutoffTime;
connect trajectoryCollision[:].collision -> collisionToStop.collisionIn[:];
connect trajectoryCollision[:].aIsFasterOut -> collisionToStop.aIsFasterIn[:];
connect isActive -> collisionToStop.active;
connect collisionToStop.stopOut[:] -> stop[:];
......
......@@ -8,7 +8,8 @@ component TimeCutoffFilter<N1 m = 10>(adaptable Q timeCutoff){
in Q^{3,m} trajectoryInA,
in Q^{3,m} trajectoryInB,
//in Q timeCutoff,
out B collisionOut;
out B collisionOut,
out B aIsFasterOut;
implementation Math{
if ((collisionIn) && (indexInA >= 1) && (indexInB >= 1) && (indexInA <= m - 1) && (indexInB <= m - 1))
......@@ -32,6 +33,12 @@ component TimeCutoffFilter<N1 m = 10>(adaptable Q timeCutoff){
Q time1 = distp1/dist1 * (trajectoryInA(3,indexInA + 1) - trajectoryInA(3,indexInA));
Q time2 = distp2/dist2 * (trajectoryInB(3,indexInB + 1) - trajectoryInB(3,indexInB));
if time1 <= time2
aIsFasterOut = True;
else
aIsFasterOut = False;
end
collisionOut = abs(time1 - time2) < timeCutoff;
else
collisionOut = False;
......
......@@ -8,7 +8,8 @@ component TrajectoryCollision<N1 m = 10, N1 m1 = 9, N1 m2 = 45>{
ports in Q^{3,m} trajectoryA,
in Q^{3,m} trajectoryB,
in Q cutoffTime,
out B collision;
out B collision,
out B aIsFasterOut;
instance TrajectoryToLines<10,9> trajectoryToLinesA;
instance TrajectoryToLines<10,9> trajectoryToLinesB;
......@@ -31,6 +32,7 @@ component TrajectoryCollision<N1 m = 10, N1 m1 = 9, N1 m2 = 45>{
connect trajectoryB -> timeCutoffFilter.trajectoryInB;
connect cutoffTime -> timeCutoffFilter.timeCutoff;
connect timeCutoffFilter.collisionOut -> collision;
connect timeCutoffFilter.aIsFasterOut -> aIsFasterOut;
connect trajectoryToLinesA.lineOut[:] -> dualSetCompare.setInA[:];
connect trajectoryToLinesB.lineOut[:] -> dualSetCompare.setInB[:];
......
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