Commit e142e7bc authored by Alexander David Hellwig's avatar Alexander David Hellwig
Browse files

Model: Added collision detection

parent 073225e3
......@@ -136,8 +136,40 @@ public class GenerationTest extends AbstractSymtabTest {
List<File> files = distributedTargetGenerator.generate(componentInstanceSymbol, taggingResolver);
fixKnownErrors(files);
}
@Test
public void testLaneIntersection() throws IOException{
TaggingResolver taggingResolver = createSymTabAndTaggingResolver("src/test/resources/");
ExpandedComponentInstanceSymbol componentInstanceSymbol = taggingResolver.<ExpandedComponentInstanceSymbol>resolve("ba.util.lineIntersection", ExpandedComponentInstanceSymbol.KIND).orElse(null);
assertNotNull(componentInstanceSymbol);
MiddlewareGenerator middlewareGenerator = new MiddlewareGenerator();
String generationTargetPath = "./target/generated-sources-cmake/laneIntersection/src/";
middlewareGenerator.setGenerationTargetPath(generationTargetPath);
middlewareGenerator.add(new CPPGenImpl(),"cpp");
//middlewareGenerator.add(new RosCppGenImpl(),"roscpp");
List<File> files = middlewareGenerator.generate(componentInstanceSymbol, taggingResolver);
fixKnownErrors(files);
}
@Test
public void testRectIntersection() throws IOException{
TaggingResolver taggingResolver = createSymTabAndTaggingResolver("src/test/resources/");
ExpandedComponentInstanceSymbol componentInstanceSymbol = taggingResolver.<ExpandedComponentInstanceSymbol>resolve("ba.util.rectIntersection", ExpandedComponentInstanceSymbol.KIND).orElse(null);
assertNotNull(componentInstanceSymbol);
//EmbeddedMontiArcMathCoCos.createChecker().checkAll((ASTComponent)componentInstanceSymbol.getComponentType().getReferencedSymbol().getAstNode().orElse(null));
MiddlewareGenerator middlewareGenerator = new MiddlewareGenerator();
String generationTargetPath = "./target/generated-sources-cmake/rectIntersection/src/";
middlewareGenerator.setGenerationTargetPath(generationTargetPath);
middlewareGenerator.add(new CPPGenImpl(),"cpp");
//middlewareGenerator.add(new RosCppGenImpl(),"roscpp");
List<File> files = middlewareGenerator.generate(componentInstanceSymbol, taggingResolver);
fixKnownErrors(files);
}
private void fixKnownErrors(List<File> files) throws IOException {
......@@ -152,13 +184,15 @@ public class GenerationTest extends AbstractSymtabTest {
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.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("colvec tmpLine;","colvec tmpLine = colvec(4);");
// content = content.replace("sqrt","std::sqrt");
Files.write(path, content.getBytes(charset));
}
......
package ba;
import ba.util.RectIntersection;
//TODO: as generic
component CollisionDetection{
ports in Q^{2,4} hulls[2],
out B collision;
instance RectIntersection rectIntersection;
connect hulls[1] -> rectIntersection.rect1;
connect hulls[2] -> rectIntersection.rect2;
connect rectIntersection.collision -> collision;
}
\ No newline at end of file
package ba;
import ba.intersection.IntersectionController;
import ba.vehicle.Planner;
import ba.util.Position;
component System{
//Intersection
instance IntersectionController intersectionController;
instance Planner planner[2];
//Intersection
port in Q cutoffPosIn;
port in Q cutoffTimeIn;
port in B isActiveIn;
ports in Q cutoffPosIn,
in Q cutoffTimeIn,
in B isActiveIn,
in Q^{3,20} trajectory[2],
out B stopOut[2];
connect cutoffPosIn -> intersectionController.cutoffPos;
connect cutoffTimeIn -> intersectionController.cutoffTime;
connect isActiveIn -> intersectionController.isActive;
connect trajectory[:] -> intersectionController.trajectoryIn[:];
//TODO: into speed controller
connect intersectionController.stop[:] -> stopOut[:];
//Planner(s)
ports in Q^{4,10} laneletIn[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 laneletIn[:] -> planner[:].lanelet;
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;
//Collision detection
instance CollisionDetection collisionDetection;
ports in Q^{2,4} hullsIn[2],
out B collisionOut;
connect hullsIn[:] -> collisionDetection.hulls[:];
connect collisionDetection.collision -> collisionOut;
}
\ No newline at end of file
package ba.util;
component LineIntersection{
ports in Q^{4,1} line1,
in Q^{4,1} line2,
out Q^{2,1} point,
out B intersects;
implementation Math{
Q n1 = (line1(1,1)-line1(3,1))*(line2(2,1)-line2(4,1))-(line1(2,1)-line1(4,1))*(line2(1,1)-line2(3,1));
Q n2 = (line1(1,1)-line1(3,1))*(line2(2,1)-line2(4,1))-(line1(2,1)-line1(4,1))*(line2(1,1)-line2(3,1));
Q cutoff = 0.00000001;
if (abs(n1) > cutoff) && (abs(n2) > cutoff)
Q z1 = (line1(1,1)*line1(4,1)-line1(2,1)*line1(3,1))*(line2(1,1)-line2(3,1))-(line1(1,1)-line1(3,1))*(line2(1,1)*line2(4,1)-line2(2,1)*line2(3,1));
Q z2 = (line1(1,1)*line1(4,1)-line1(2,1)*line1(3,1))*(line2(2,1)-line2(4,1))-(line1(2,1)-line1(4,1))*(line2(1,1)*line2(4,1)-line2(2,1)*line2(3,1));
point(1,1) = z1/n1;
point(2,1) = z2/n2;
if abs(point(1,1)) < cutoff
point(1,1) = 0;
end
if abs(point(2,1)) < cutoff
point(2,1) = 0;
end
Q xmin1;
Q xmax1;
Q ymin1;
Q ymax1;
Q xmin2;
Q xmax2;
Q ymin2;
Q ymax2;
//bounding box 1
if line1(1,1) >= line1(3,1)
xmax1 = line1(1,1);
xmin1 = line1(3,1);
else
xmax1 = line1(3,1);
xmin1 = line1(1,1);
end
if line1(2,1) >= line1(4,1)
ymax1 = line1(2,1);
ymin1 = line1(4,1);
else
ymax1 = line1(4,1);
ymin1 = line1(2,1);
end
//bounding box 2
if line2(1,1) >= line2(3,1)
xmax2 = line2(1,1);
xmin2 = line2(3,1);
else
xmax2 = line2(3,1);
xmin2 = line2(1,1);
end
if line2(2,1) >= line2(4,1)
ymax2 = line2(2,1);
ymin2 = line2(4,1);
else
ymax2 = line2(4,1);
ymin2 = line2(2,1);
end
B onLine1 = (xmin1 <= point(1,1)) && (point(1,1) <= xmax1) && (ymin1 <= point(2,1)) && (point(2,1) <= ymax1);
B onLine2 = (xmin2 <= point(1,1)) && (point(1,1) <= xmax2) && (ymin2 <= point(2,1)) && (point(2,1) <= ymax2);
intersects = onLine1 && onLine2;
else
point(1,1) = 0;
point(2,1) = 0;
intersects = False;
end
}
}
\ No newline at end of file
package ba.util;
component MultiOr<N1 x = 2>{
ports in B boolIn[x],
out B boolOut;
implementation Math{
B flag = False;
for i = 1:x
flag = flag || boolIn(i);
end
boolOut = flag;
}
}
\ No newline at end of file
package ba.util;
component RectIntersection{
//TODO: make rect type clearer. Current: 4 points going clockwise around the rect
ports in Q^{2,4} rect1,
in Q^{2,4} rect2,
out B collision;
//check intersection of 4*4 lines w/o permutations
//4*(4+1)/2 = 10 possibilities
instance RectToLines rectToLines1;
instance RectToLines rectToLines2;
instance LineIntersection lineIntersection[10];
instance MultiOr<10> multiOr;
connect rect1 -> rectToLines1.rect;
connect rect2 -> rectToLines2.rect;
connect rectToLines1.line1 -> lineIntersection[1].line1;
connect rectToLines2.line1 -> lineIntersection[1].line2;
connect rectToLines1.line1 -> lineIntersection[2].line1;
connect rectToLines2.line2 -> lineIntersection[2].line2;
connect rectToLines1.line1 -> lineIntersection[3].line1;
connect rectToLines2.line3 -> lineIntersection[3].line2;
connect rectToLines1.line1 -> lineIntersection[4].line1;
connect rectToLines2.line4 -> lineIntersection[4].line2;
connect rectToLines1.line2 -> lineIntersection[5].line1;
connect rectToLines2.line2 -> lineIntersection[5].line2;
connect rectToLines1.line2 -> lineIntersection[6].line1;
connect rectToLines2.line3 -> lineIntersection[6].line2;
connect rectToLines1.line2 -> lineIntersection[7].line1;
connect rectToLines2.line4 -> lineIntersection[7].line2;
connect rectToLines1.line3 -> lineIntersection[8].line1;
connect rectToLines2.line3 -> lineIntersection[8].line2;
connect rectToLines1.line3 -> lineIntersection[9].line1;
connect rectToLines2.line4 -> lineIntersection[9].line2;
connect rectToLines1.line4 -> lineIntersection[10].line1;
connect rectToLines2.line4 -> lineIntersection[10].line2;
connect lineIntersection[:].intersects -> multiOr.boolIn[:];
connect multiOr.boolOut -> collision;
}
\ No newline at end of file
package ba.util;
component RectToLines{
ports in Q^{2,4} rect,
//TODO: as array once issue is resolved
out Q^{4,1} line1,
out Q^{4,1} line2,
out Q^{4,1} line3,
out Q^{4,1} line4;
implementation Math{
Q^{4,1} tmpLine;
for i = 1:4
Z j;
if i == 4
j = 1;
else
j = i + 1;
end
//x1,y1,x2,y2
tmpLine(1,1) = rect(1,i);
tmpLine(2,1) = rect(2,i);
tmpLine(3,1) = rect(1,j);
tmpLine(4,1) = rect(2,j);
//TODO: as array once issue is resolved
if i == 1
line1 = tmpLine;
elseif i == 2
line2 = tmpLine;
elseif i == 3
line3 = tmpLine;
elseif i == 4
line4 = tmpLine;
end
end
}
}
\ 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