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