Commit 9d51796d authored by Svetlana's avatar Svetlana
Browse files

added angle buffer model

parent 5c6b771d
<component name="libraryTable">
<library name="Maven: de.monticore.lang:Tagging:0.0.1">
<CLASSES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/Tagging/0.0.1/Tagging-0.0.1.jar!/" />
</CLASSES>
<JAVADOC>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/Tagging/0.0.1/Tagging-0.0.1-javadoc.jar!/" />
</JAVADOC>
<SOURCES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/Tagging/0.0.1/Tagging-0.0.1-sources.jar!/" />
</SOURCES>
</library>
</component>
\ No newline at end of file
<component name="libraryTable">
<library name="Maven: de.monticore.lang.monticar:enum:0.0.10-SNAPSHOT">
<CLASSES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/monticar/enum/0.0.10-SNAPSHOT/enum-0.0.10-20180104.160312-1.jar!/" />
</CLASSES>
<JAVADOC>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/monticar/enum/0.0.10-SNAPSHOT/enum-0.0.10-20180104.160312-1-javadoc.jar!/" />
</JAVADOC>
<SOURCES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/monticar/enum/0.0.10-SNAPSHOT/enum-0.0.10-20180104.160312-1-sources.jar!/" />
</SOURCES>
</library>
</component>
\ No newline at end of file
<component name="libraryTable">
<library name="Maven: de.monticore.lang.monticar:struct:0.0.10-SNAPSHOT">
<CLASSES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/monticar/struct/0.0.10-SNAPSHOT/struct-0.0.10-20180111.201020-1.jar!/" />
</CLASSES>
<JAVADOC>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/monticar/struct/0.0.10-SNAPSHOT/struct-0.0.10-20180111.201020-1-javadoc.jar!/" />
</JAVADOC>
<SOURCES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/monticar/struct/0.0.10-SNAPSHOT/struct-0.0.10-20180111.201020-1-sources.jar!/" />
</SOURCES>
</library>
</component>
\ No newline at end of file
<component name="libraryTable">
<library name="Maven: de.monticore:monticore-emf-runtime:4.5.4-SNAPSHOT">
<CLASSES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-emf-runtime/4.5.4-SNAPSHOT/monticore-emf-runtime-4.5.4-SNAPSHOT.jar!/" />
</CLASSES>
<JAVADOC>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-emf-runtime/4.5.4-SNAPSHOT/monticore-emf-runtime-4.5.4-SNAPSHOT-javadoc.jar!/" />
</JAVADOC>
<SOURCES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-emf-runtime/4.5.4-SNAPSHOT/monticore-emf-runtime-4.5.4-SNAPSHOT-sources.jar!/" />
</SOURCES>
</library>
</component>
\ No newline at end of file
<component name="libraryTable">
<library name="Maven: de.monticore:monticore-grammar:4.5.4-SNAPSHOT">
<CLASSES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-grammar/4.5.4-SNAPSHOT/monticore-grammar-4.5.4-SNAPSHOT.jar!/" />
</CLASSES>
<JAVADOC>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-grammar/4.5.4-SNAPSHOT/monticore-grammar-4.5.4-SNAPSHOT-javadoc.jar!/" />
</JAVADOC>
<SOURCES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-grammar/4.5.4-SNAPSHOT/monticore-grammar-4.5.4-SNAPSHOT-sources.jar!/" />
</SOURCES>
</library>
</component>
\ No newline at end of file
<component name="libraryTable">
<library name="Maven: de.monticore:monticore-runtime:4.5.4-SNAPSHOT">
<CLASSES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-runtime/4.5.4-SNAPSHOT/monticore-runtime-4.5.4-SNAPSHOT.jar!/" />
</CLASSES>
<JAVADOC>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-runtime/4.5.4-SNAPSHOT/monticore-runtime-4.5.4-SNAPSHOT-javadoc.jar!/" />
</JAVADOC>
<SOURCES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-runtime/4.5.4-SNAPSHOT/monticore-runtime-4.5.4-SNAPSHOT-sources.jar!/" />
</SOURCES>
</library>
</component>
\ No newline at end of file
...@@ -5,20 +5,24 @@ component Mastercomponent { ...@@ -5,20 +5,24 @@ component Mastercomponent {
ports ports
in Z(0:255)^{3, 210, 280} imageIn, in Z(0:255)^{3, 210, 280} imageIn,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn, in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
out Q(0:1)^{3,1,1} commandsOut; out Q(0:1)^{3} commandsOut;
instance Imagepreprocessing ip;
instance Dpnet dpnet; instance Dpnet dpnet;
instance Drivercontroller dc; instance Drivercontroller driverController;
instance Unnormalizer un; instance Unnormalizer unnorm;
//instance KFMastercomponent kfm; //instance KFMastercomponent kfm;
instance AngleBuffer angleBuffer;
connect imageIn -> dpnet.image;
connect dpnet.predictions -> unnorm.normalizedPredictions;
connect imageIn -> ip.imageIn;
connect ip.imageOut -> dpnet.image;
connect dpnet.predictions -> un.normalizedPredictions;
//connect un.affordance -> kfm.predictions; //connect un.affordance -> kfm.predictions;
//connect kfm.predictionsSmoothed -> dc.affordanceIn; //connect kfm.predictionsSmoothed -> driverController.affordanceIn;
connect un.affordance -> dc.affordanceIn;
connect speedIn -> dc.speedIn; connect unnorm.affordance -> angleBuffer.inputAffordance;
connect dc.commandsOut -> commandsOut; connect unnorm.affordance -> driverController.affordanceIn;
connect angleBuffer.outputBuffer -> driverController.anglesIn;
connect speedIn -> driverController.speedIn;
connect driverController.commandsOut -> commandsOut;
} }
\ No newline at end of file
...@@ -18,6 +18,4 @@ struct Affordance { ...@@ -18,6 +18,4 @@ struct Affordance {
Q(0m : 0.1m : 75m) distRR; Q(0m : 0.1m : 75m) distRR;
Q(0 : 0.1 : 1) fast; Q(0 : 0.1 : 1) fast;
} }
package dp.subcomponents;
component AngleBuffer {
ports in Affordance inputAffordance,
out Q^{4} outputBuffer;
implementation Math {
static Q^{4} buffer = [0 0 0 0];
buffer(4) = buffer(3);
buffer(3) = buffer(2);
buffer(2) = buffer(1);
buffer(1) = inputAffordance.angle;
outputBuffer(1) = buffer(1);
outputBuffer(2) = buffer(2);
outputBuffer(3) = buffer(3);
outputBuffer(4) = buffer(4);
}
}
\ No newline at end of file
package dp.subcomponents; package dp.subcomponents;
component Dpnet{ component Dpnet{
ports in Z(0:255)^{3, 210, 280} image, ports in Z(0:255)^{3, 210, 280} image,
out Q(-oo:oo)^{14,1,1} predictions; out Q(-oo:oo)^{14} predictions;
implementation CNN { implementation CNN {
...@@ -27,6 +28,5 @@ component Dpnet{ ...@@ -27,6 +28,5 @@ component Dpnet{
fc() -> fc() ->
FullyConnected(units=14, no_bias=true) -> FullyConnected(units=14, no_bias=true) ->
predictions predictions
} }
} }
...@@ -2,79 +2,75 @@ package dp.subcomponents; ...@@ -2,79 +2,75 @@ package dp.subcomponents;
component Drivercontroller { component Drivercontroller {
ports ports
in Q^{4} anglesIn,
in Affordance affordanceIn, in Affordance affordanceIn,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn, in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
out Q(0:1)^{3,1,1} commandsOut; out Q(0:1)^{3} commandsOut;
implementation Math { implementation Math {
//Q Constant = 1; Q roadWidth = 8;
//Q angle = affordanceIn.angle;
//Q roadWidth = affordanceIn.toMarkingLL + affordanceIn.toMarkingRR;
//Q laneWidth = affordanceIn(0,0,2) + affordanceIn(0,0,3);
//Q distToLaneCenter = abs(laneWidth/2 - affordanceIn(0,0,2));
Q road_width = 8;
// Acceleration // Acceleration
Q slow_down = 100; Q slowDown = 100;
if (affordanceIn.distMM < 15) if (affordanceIn.distMM < 15)
Q vMax = 20; Q vMax = 20;
Q vC = 2.772; Q vC = 2.772;
Q vD = 0.693; Q vD = 0.693;
slow_down = vMax * (exp(-(vC/vMax) * affordanceIn.distMM - vD)); slowDown = vMax * (exp(-(vC/vMax) * affordanceIn.distMM - vD));
if (slow_down < 0) if (slowDown < 0)
slow_down = 0; slowDown = 0;
end end
end end
Q center_line; Q centerLine;
Q coe_steer; Q coeSteer;
Q pre_ML; Q preML;
Q pre_MR; Q preMR;
if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5) if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
coe_steer = 1.5; coeSteer = 1.5;
center_line = (affordanceIn.toMarkingML + affordanceIn.toMarkingMR) / 2; centerLine = (affordanceIn.toMarkingML + affordanceIn.toMarkingMR) / 2;
pre_ML = affordanceIn.toMarkingML; preML = affordanceIn.toMarkingML;
pre_MR = affordanceIn.toMarkingMR; preMR = affordanceIn.toMarkingMR;
if (affordanceIn.toMarkingM < 1) if (affordanceIn.toMarkingM < 1)
coe_steer = 0.4; coeSteer = 0.4;
end end
else else
if (-pre_ML > pre_MR) if (-preML > preMR)
center_line = (affordanceIn.toMarkingL + affordanceIn.toMarkingM) / 2; centerLine = (affordanceIn.toMarkingL + affordanceIn.toMarkingM) / 2;
else else
center_line=(affordanceIn.toMarkingR + affordanceIn.toMarkingM) / 2; centerLine=(affordanceIn.toMarkingR + affordanceIn.toMarkingM) / 2;
end end
coe_steer = 0.3; coeSteer = 0.3;
end end
// steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS // steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
commandsOut(2,1,1) = (affordanceIn.angle - center_line/road_width) / 0.541052/coe_steer; commandsOut(2) = (affordanceIn.angle - centerLine/roadWidth) / 0.541052/coeSteer;
Q desired_speed = 20; Q desiredSpeed = 20;
if (desired_speed > slow_down) if (desiredSpeed > slowDown)
desired_speed = slow_down; desiredSpeed = slowDown;
end end
// acceleration and brake // acceleration and brake
if (desired_speed >= speedIn) if (desiredSpeed >= speedIn)
Q accelCmd = 0.2*(desired_speed - speedIn + 1); Q accelCmd = 0.2*(desiredSpeed - speedIn + 1);
if (accelCmd > 1) if (accelCmd > 1)
accelCmd = 1; accelCmd = 1;
else else
commandsOut(1,1,1) = accelCmd; commandsOut(1) = accelCmd;
end end
commandsOut(3,1,1) = 0; commandsOut(3) = 0;
else else
Q brakeCmd = 0.1 * (speedIn - desired_speed); Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
if (brakeCmd > 1) if (brakeCmd > 1)
commandsOut(3,1,1) = 1; commandsOut(3) = 1;
else else
commandsOut(3,1,1) = brakeCmd; commandsOut(3) = brakeCmd;
end end
commandsOut(1,1,1) = 0; commandsOut(1) = 0;
end end
} }
} }
\ No newline at end of file
package dp.subcomponents;
component Imagepreprocessing {
ports
in Z(0:255)^{3, 210, 280} imageIn,
out Z(0:255)^{3, 210, 280} imageOut;
implementation Math {
imageOut = imageIn;
}
}
\ No newline at end of file
...@@ -3,8 +3,8 @@ package dp.subcomponents; ...@@ -3,8 +3,8 @@ package dp.subcomponents;
component Safetycontroller { component Safetycontroller {
ports ports
in Z(0:255)^{3, 210, 280} imageIn, in Z(0:255)^{3, 210, 280} imageIn,
in Q(0:1)^{14,1,1} affordanceIn, in Q(0:1)^{14} affordanceIn,
out Q(0:1)^{1,1,1} safetyLevelOut; out Q(0:1) safetyLevelOut;
implementation Math { implementation Math {
safetyLevelOut=affordanceIn; safetyLevelOut=affordanceIn;
......
package dp.subcomponents; package dp.subcomponents;
component Unnormalizer { component Unnormalizer {
ports in Q^{14,1,1} normalizedPredictions, ports in Q^{14} normalizedPredictions,
out Affordance affordance; out Affordance affordance;
implementation Math { implementation Math {
...@@ -12,59 +12,59 @@ component Unnormalizer { ...@@ -12,59 +12,59 @@ component Unnormalizer {
Q newMin = -0.5; Q newMin = -0.5;
Q newMax = 0.5; Q newMax = 0.5;
Q newRange = newMax - newMin; Q newRange = newMax - newMin;
affordance.angle = (((normalizedPredictions(0,0,0) - oldMin) * newRange) / oldRange) + newMin; affordance.angle = (((normalizedPredictions(0) - oldMin) * newRange) / oldRange) + newMin;
newMin = -7; newMin = -7;
newMax = -2.5; newMax = -2.5;
newRange = newMax - newMin; newRange = newMax - newMin;
affordance.toMarkingL = (((normalizedPredictions(0,0,1) - oldMin) * newRange) / oldRange) + newMin; affordance.toMarkingL = (((normalizedPredictions(1) - oldMin) * newRange) / oldRange) + newMin;
newMin = -2; newMin = -2;
newMax = 3.5; newMax = 3.5;
newRange = newMax - newMin; newRange = newMax - newMin;
affordance.toMarkingM = (((normalizedPredictions(0,0,2) - oldMin) * newRange) / oldRange) + newMin; affordance.toMarkingM = (((normalizedPredictions(2) - oldMin) * newRange) / oldRange) + newMin;
newMin = 2.5; newMin = 2.5;
newMax = 7; newMax = 7;
newRange = newMax - newMin; newRange = newMax - newMin;
affordance.toMarkingR = (((normalizedPredictions(0,0,3) - oldMin) * newRange) / oldRange) + newMin; affordance.toMarkingR = (((normalizedPredictions(3) - oldMin) * newRange) / oldRange) + newMin;
newMin = 0; newMin = 0;
newMax = 75; newMax = 75;
newRange = newMax - newMin; newRange = newMax - newMin;
affordance.distL = (((normalizedPredictions(0,0,4) - oldMin) * newRange) / oldRange) + newMin; affordance.distL = (((normalizedPredictions(4) - oldMin) * newRange) / oldRange) + newMin;
affordance.distR = (((normalizedPredictions(0,0,5) - oldMin) * newRange) / oldRange) + newMin; affordance.distR = (((normalizedPredictions(5) - oldMin) * newRange) / oldRange) + newMin;
newMin = -9.5; newMin = -9.5;
newMax = -4; newMax = -4;
newRange = newMax - newMin; newRange = newMax - newMin;
affordance.toMarkingLL = (((normalizedPredictions(0,0,6) - oldMin) * newRange) / oldRange) + newMin; affordance.toMarkingLL = (((normalizedPredictions(6) - oldMin) * newRange) / oldRange) + newMin;
newMin = -5.5; newMin = -5.5;
newMax = -0.5; newMax = -0.5;
newRange = newMax - newMin; newRange = newMax - newMin;
affordance.toMarkingML = (((normalizedPredictions(0,0,7) - oldMin) * newRange) / oldRange) + newMin; affordance.toMarkingML = (((normalizedPredictions(7) - oldMin) * newRange) / oldRange) + newMin;
newMin = 0.5; newMin = 0.5;
newMax = 5.5; newMax = 5.5;
newRange = newMax - newMin; newRange = newMax - newMin;
affordance.toMarkingMR = (((normalizedPredictions(0,0,8) - oldMin) * newRange) / oldRange) + newMin; affordance.toMarkingMR = (((normalizedPredictions(8) - oldMin) * newRange) / oldRange) + newMin;
newMin = 4; newMin = 4;
newMax = 9.5; newMax = 9.5;
newRange = newMax - newMin; newRange = newMax - newMin;
affordance.toMarkingRR = (((normalizedPredictions(0,0,9) - oldMin) * newRange) / oldRange) + newMin; affordance.toMarkingRR = (((normalizedPredictions(9) - oldMin) * newRange) / oldRange) + newMin;
newMin = 0; newMin = 0;
newMax = 75; newMax = 75;
newRange = newMax - newMin; newRange = newMax - newMin;
affordance.distLL = (((normalizedPredictions(0,0,10) - oldMin) * newRange) / oldRange) + newMin; affordance.distLL = (((normalizedPredictions(10) - oldMin) * newRange) / oldRange) + newMin;
affordance.distMM = (((normalizedPredictions(0,0,11) - oldMin) * newRange) / oldRange) + newMin; affordance.distMM = (((normalizedPredictions(11) - oldMin) * newRange) / oldRange) + newMin;
affordance.distRR = (((normalizedPredictions(0,0,12) - oldMin) * newRange) / oldRange) + newMin; affordance.distRR = (((normalizedPredictions(12) - oldMin) * newRange) / oldRange) + newMin;
newMin = 0; newMin = 0;
newMax = 1; newMax = 1;
newRange = newMax - newMin; newRange = newMax - newMin;
affordance.fast = (((normalizedPredictions(0,0,13) - oldMin) * newRange) / oldRange) + newMin; affordance.fast = (((normalizedPredictions(13) - oldMin) * newRange) / oldRange) + newMin;
} }
} }
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