Added groundTruthLabels variant. Support for dpnet variant is kept.

parent 6b62b291
......@@ -4,7 +4,7 @@ echo "Cleaning target directory..."
rm -rf target/*
echo "Generating code..."
java -cp embedded-montiarc-math-middleware-generator-0.0.11-SNAPSHOT-jar-with-dependencies.jar de.monticore.lang.monticar.generator.middleware.DistributedTargetGeneratorCli --models-dir=src/main/ --root-model=dp.mastercomponent --output-dir=target/Mastercomponent --generators=emadlcpp,roscpp --backend=MXNET
java -cp embedded-montiarc-math-middleware-generator-0.0.12-SNAPSHOT-jar-with-dependencies.jar de.monticore.lang.monticar.generator.middleware.DistributedTargetGeneratorCli --models-dir=src/dpnet_variant/ --root-model=dp.mastercomponent --output-dir=target/Mastercomponent --generators=emadlcpp,roscpp --backend=MXNET
echo "Copying build and run scripts..."
cp -rf resources/scripts/* target/
......
#!/bin/bash
echo "Cleaning target directory..."
rm -rf target/*
echo "Generating code..."
java -cp embedded-montiarc-math-middleware-generator-0.0.12-SNAPSHOT-jar-with-dependencies.jar de.monticore.lang.monticar.generator.middleware.DistributedTargetGeneratorCli --models-dir=src/groundTruthLabels_variant/ --root-model=dp.mastercomponent --output-dir=target/Mastercomponent --generators=emadlcpp,roscpp --backend=MXNET
echo "Copying build and run scripts..."
cp -rf resources/scripts/* target/
chmod u+x target/*.sh
echo "Copying CNN weights..."
cp -rf resources/TORCSComponent target/TORCSComponent
chmod u+x resources/TORCSComponent/*.sh
cp -rf resources/dpnet target/model
echo "Building generated code..."
cd target
./build_all.sh
echo "Please start TORCS, configure race and run ./run_all.sh"
package dp;
import dp.subcomponents.*;
component Mastercomponent {
ports
in Q^{16} groundTruthAffordance,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
out Q(-1:1)^{3} commandsOut;
instance VectorToAffordance vectorToAff;
instance DriverController driverController;
instance SteeringBuffer steeringBuffer;
instance Localization locController;
connect groundTruthAffordance -> vectorToAff.affordanceIndicators;
connect vectorToAff.affordance -> locController.affordanceIn, driverController.affordanceIn;
connect locController.numLanes -> driverController.lanesCountIn;
connect steeringBuffer.outputBuffer -> driverController.steeringRecordIn;
connect speedIn -> driverController.speedIn;
connect steeringBuffer.timerLeftOut -> driverController.timerLeftIn;
connect steeringBuffer.timerRightOut -> driverController.timerRightIn;
connect steeringBuffer.laneChangeOut -> driverController.laneChangeIn;
connect driverController.commandsOut -> commandsOut;
connect driverController.steerCmd -> steeringBuffer.inSteerCmd;
connect driverController.timerLeftOut -> steeringBuffer.timerLeftIn;
connect driverController.timerRightOut -> steeringBuffer.timerRightIn;
connect driverController.laneChangeOut -> steeringBuffer.laneChangeIn;
}
package dp;
conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema;
tags Mastercomponent {
tag mastercomponent.groundTruthAffordance with RosConnection = {topic=(/affordance,std_msgs/Float32MultiArray)};
tag mastercomponent.speedIn with RosConnection = {topic=(/speed,std_msgs/Float32)};
tag mastercomponent.commandsOut with RosConnection = {topic=(/commands,std_msgs/Float32MultiArray)};
}
package dp.subcomponents;
struct Affordance {
Q(-0.5rad:0.001rad:0.5rad) angle;
Q(-7m : 0.01m : -2.5m) toMarkingL;
Q(-2m : 0.01m : 3.5m) toMarkingM;
Q(2.5m : 0.01m : 7m) toMarkingR;
Q(0m : 0.1m : 75m) distL;
Q(0m : 0.1m : 75m) distR;
Q(-9.5m : 0.01m : -4m) toMarkingLL;
Q(-5.5m : 0.01m : -0.5m) toMarkingML;
Q(0.5m : 0.01m : 5.5m) toMarkingMR;
Q(4m : 0.01m : 9.5m) toMarkingRR;
Q(0m : 0.1m : 75m) distLL;
Q(0m : 0.1m : 75m) distMM;
Q(0m : 0.1m : 75m) distRR;
Q(0 : 0.1 : 1) fast;
}
package dp.subcomponents;
import ChangeRange;
component AffordanceToVector {
ports in Affordance affordanceIn,
out Q^{16} affordanceOut;
implementation Math {
affordanceOut(1) = affordanceIn.fast;
affordanceOut(2) = affordanceIn.distL;
affordanceOut(3) = affordanceIn.distR;
affordanceOut(4) = affordanceIn.toMarkingL;
affordanceOut(5) = affordanceIn.toMarkingM;
affordanceOut(6) = affordanceIn.toMarkingR;
affordanceOut(7) = affordanceIn.distLL;
affordanceOut(8) = affordanceIn.distMM;
affordanceOut(9) = affordanceIn.distRR;
affordanceOut(10) = affordanceIn.toMarkingLL;
affordanceOut(11) = affordanceIn.toMarkingML;
affordanceOut(12) = affordanceIn.toMarkingMR;
affordanceOut(13) = affordanceIn.toMarkingRR;
affordanceOut(15) = affordanceIn.angle;
}
}
package dp.subcomponents;
component ConstantMatrix<Z n=1, Z m=1>(Q^{n,m} value) {
port
out Q^{n,m} out1;
implementation Math{
out1=value;
}
}
package dp.subcomponents;
import ChangeRange;
component Denormalizer {
ports in Q^{14} normalizedPredictions,
out Affordance affordance;
implementation Math {
Q oldMin = 0.1;
Q oldMax = 0.9;
Q oldRange = oldMax - oldMin;
for i=1:size(normalizedPredictions, 1)
if normalizedPredictions(i) < oldMin
normalizedPredictions(i) = oldMin;
elseif normalizedPredictions(i) > oldMax
normalizedPredictions(i) = oldMax;
end
end
Q newMinAngle = -0.5;
Q newRangeAngle = 0.5 - newMinAngle;
affordance.angle = (((normalizedPredictions(1) - oldMin) * newRangeAngle) / oldRange) + newMinAngle;
Q newMinMarkingL = -7;
Q newRangeMarkingL = -2.5 - newMinMarkingL;
affordance.toMarkingL = (((normalizedPredictions(2) - oldMin) * newRangeMarkingL) / oldRange) + newMinMarkingL;
Q newMinMarkingM = -2;
Q newRangeMarkingM = 3.5 - newMinMarkingM;
affordance.toMarkingM = (((normalizedPredictions(3) - oldMin) * newRangeMarkingM) / oldRange) + newMinMarkingM;
Q newMinMarkingR = 2.5;
Q newRangeMarkingR = 7 - newMinMarkingR;
affordance.toMarkingR = (((normalizedPredictions(4) - oldMin) * newRangeMarkingR) / oldRange) + newMinMarkingR;
Q newMinDist = 0;
Q newRangeDist = 75 - newMinDist;
affordance.distL = (((normalizedPredictions(5) - oldMin) * newRangeDist) / oldRange) + newMinDist;
affordance.distR = (((normalizedPredictions(6) - oldMin) * newRangeDist) / oldRange) + newMinDist;
Q newMinMarkingLL = -9.5;
Q newRangeMarkingLL = -4 - newMinMarkingLL;
affordance.toMarkingLL = (((normalizedPredictions(7) - oldMin) * newRangeMarkingLL) / oldRange) + newMinMarkingLL;
Q newMinMarkingML = -5.5;
Q newRangeMarkingML = -0.5 - newMinMarkingML;
affordance.toMarkingML = (((normalizedPredictions(8) - oldMin) * newRangeMarkingML) / oldRange) + newMinMarkingML;
Q newMinMarkingMR = 0.5;
Q newRangeMarkingMR = 5.5 - newMinMarkingMR;
affordance.toMarkingMR = (((normalizedPredictions(9) - oldMin) * newRangeMarkingMR) / oldRange) + newMinMarkingMR;
Q newMinMarkingRR = 4;
Q newRangeMarkingRR = 9.5 - newMinMarkingRR;
affordance.toMarkingRR = (((normalizedPredictions(10) - oldMin) * newRangeMarkingRR) / oldRange) + newMinMarkingRR;
Q newMinDistLMR = 0;
Q newRangeDistLMR = 75 - newMinDistLMR;
affordance.distLL = (((normalizedPredictions(11) - oldMin) * newRangeDistLMR) / oldRange) + newMinDistLMR;
affordance.distMM = (((normalizedPredictions(12) - oldMin) * newRangeDistLMR) / oldRange) + newMinDistLMR;
affordance.distRR = (((normalizedPredictions(13) - oldMin) * newRangeDistLMR) / oldRange) + newMinDistLMR;
Q newMinFast = 0;
Q newRangeFast = 1 - newMinFast;
affordance.fast = (((normalizedPredictions(14) - oldMin) * newRangeFast) / oldRange) + newMinFast;
}
}
configuration Dpnet{
num_epoch : 100
batch_size : 64
eval_metric : mse
context : gpu
load_checkpoint : true
normalize : true
optimizer : sgd{
learning_rate : 0.01
learning_rate_decay : 0.9
step_size : 8000
}
}
package dp.subcomponents;
component Dpnet{
ports in Z(0:255)^{3, 210, 280} data,
out Q(-oo:oo)^{14} predictions;
implementation CNN {
def conv(kernel, channels, hasPool=true, convStride=(1,1)){
Convolution(kernel=kernel, channels=channels, stride=convStride) ->
Relu() ->
Pooling(pool_type="max", kernel=(3,3), stride=(2,2), ?=hasPool)
}
def fc(){
FullyConnected(units=4096) ->
Relu() ->
Dropout()
}
data ->
conv(kernel=(11,11), channels=96, convStride=(4,4)) ->
conv(kernel=(5,5), channels=256, convStride=(4,4)) ->
conv(kernel=(3,3), channels=384, hasPool=false) ->
conv(kernel=(3,3), channels=384, hasPool=false) ->
conv(kernel=(3,3), channels=256) ->
fc() ->
fc() ->
FullyConnected(units=256) ->
Relu() ->
Dropout() ->
FullyConnected(units=14, no_bias=true) ->
predictions
}
}
package dp.subcomponents;
component KFMastercomponent {
ports in Affordance affordanceIn,
out Affordance affordanceSmoothed;
component AffordanceUpdater{
ports in Affordance affordanceIn,
in Q distLL,
in Q distMM,
in Q distRR,
in Q toMarkingML,
in Q toMarkingMR,
out Affordance affordanceOut;
implementation Math {
affordanceOut = affordanceIn;
affordanceOut.distLL = distLL;
affordanceOut.distRR = distRR;
affordanceOut.toMarkingML = toMarkingML;
affordanceOut.toMarkingMR = toMarkingMR;
}
}
// new measurement
instance ConstantMatrix<1, 1>([affordanceIn.distLL]) y_distLL;
instance ConstantMatrix<1, 1>([affordanceIn.distMM]) y_distMM;
instance ConstantMatrix<1, 1>([affordanceIn.distRR]) y_distRR;
instance ConstantMatrix<1, 1>([affordanceIn.toMarkingML]) y_toMarkingML;
instance ConstantMatrix<1, 1>([affordanceIn.toMarkingMR]) y_toMarkingMR;
// 0.1 is a time Step between filter steps
instance ConstantMatrix<2, 2>([1, 0.1; 0, 1]) stateTrans; // state transition matrix
instance ConstantMatrix<1, 2>([1, 0]) measM; // C: measurement matrix
instance ConstantMatrix<2, 2>([0.1*0.1*0.1/3, 0.1*0.1/2; 0.1*0.1/2, 0.1]) procNoiseCov; // Q: covariance of process noise
instance ConstantMatrix<1, 1>([5]) measNoiseCov; // R: covariance of measurement noise
instance KalmanFilter kfLL;
instance KalmanFilter kfMM;
instance KalmanFilter kfRR;
instance KalmanFilter kfML;
instance KalmanFilter kfMR;
// instances to store updated X and error covariance
instance KalmanStatsBuffer(affordanceIn.distLL) statsLL;
instance KalmanStatsBuffer(affordanceIn.distMM) statsMM;
instance KalmanStatsBuffer(affordanceIn.distRR) statsRR;
instance KalmanStatsBuffer(affordanceIn.toMarkingML) statsML;
instance KalmanStatsBuffer(affordanceIn.toMarkingMR) statsMR;
instance AffordanceUpdater affordanceUpdater;
connect affordanceIn -> affordanceUpdater.affordanceIn;
connect stateTrans.out1 -> kfLL.stateTrans, kfMM.stateTrans, kfRR.stateTrans, kfML.stateTrans, kfMR.stateTrans;
connect measM.out1 -> kfLL.measM, kfMM.measM, kfRR.measM, kfML.measM, kfMR.measM;
connect procNoiseCov.out1 -> kfLL.procNoiseCov, kfMM.procNoiseCov, kfRR.procNoiseCov, kfML.procNoiseCov, kfMR.procNoiseCov;
connect measNoiseCov.out1 -> kfLL.measNoiseCov, kfMM.measNoiseCov, kfRR.measNoiseCov, kfML.measNoiseCov, kfMR.measNoiseCov;
connect statsLL.outX -> kfLL.X;
connect statsLL.outErrCov -> kfLL.errCov;
connect y_distLL.out1 -> kfLL.Y;
connect kfLL.correctedX -> statsLL.X;
connect kfLL.correctedErrCov -> statsLL.errCov;
connect kfLL.predictedMeasurement -> affordanceUpdater.distLL;
connect statsMM.outX -> kfMM.X;
connect statsMM.outErrCov -> kfMM.errCov;
connect y_distLL.out1 -> kfMM.Y;
connect kfMM.correctedX -> statsMM.X;
connect kfMM.correctedErrCov -> statsMM.errCov;
connect kfMM.predictedMeasurement -> affordanceUpdater.distMM;
connect statsRR.outX -> kfRR.X;
connect statsRR.outErrCov -> kfRR.errCov;
connect y_distRR.out1 -> kfRR.Y;
connect kfRR.correctedX -> statsRR.X;
connect kfRR.correctedErrCov -> statsRR.errCov;
connect kfRR.predictedMeasurement -> affordanceUpdater.distRR;
connect statsML.outX -> kfML.X;
connect statsML.outErrCov -> kfML.errCov;
connect y_toMarkingML.out1 -> kfML.Y;
connect kfML.correctedX -> statsML.X;
connect kfML.correctedErrCov -> statsML.errCov;
connect kfML.predictedMeasurement -> affordanceUpdater.toMarkingML;
connect statsMR.outX -> kfMR.X;
connect statsMR.outErrCov -> kfMR.errCov;
connect y_toMarkingMR.out1 -> kfMR.Y;
connect kfMR.correctedX -> statsMR.X;
connect kfMR.correctedErrCov -> statsMR.errCov;
connect kfMR.predictedMeasurement -> affordanceUpdater.toMarkingMR;
connect affordanceUpdater.affordanceOut -> affordanceSmoothed;
}
package dp.subcomponents;
component KalmanFilter {
ports
in Q^{2, 1} X,
in Q^{1, 1} Y,
in Q^{2, 2} stateTrans, // state transition matrix
in Q^{1, 2} measM, // C: measurement matrix
in Q^{2, 2} procNoiseCov, // Q: covariance of process noise
in Q^{1, 1} measNoiseCov, // R: covariance of measurement noise
in Q^{2, 2} errCov,// P: estimate error covariance
out Q^{2, 1} correctedX,
out Q^{2, 2} correctedErrCov,
out Q predictedMeasurement;
implementation Math {
Q^{2, 2} I = ones(2, 2);
// Prediction step
X = stateTrans * X;
errCov = stateTrans * errCov * trans(stateTrans) + procNoiseCov;
// Correction step
Q^{2,1} kalmanGain = errCov * trans(measM) * inv(measM * errCov * trans(measM) + measNoiseCov);
correctedX = X + kalmanGain * (Y - measM * X);
correctedErrCov = (I - kalmanGain * measM) * errCov;
predictedMeasurement = correctedX(1);
}
}
package dp.subcomponents;
component KalmanStatsBuffer(Q startValue) {
ports in Q^{2, 1} X,
in Q^{2, 2} errCov,
out Q^{2, 1} outX,
out Q^{2, 2} outErrCov;
implementation Math {
static Q^{2, 1} bufferedX = [startValue; 0];
static Q^{2, 2} bufferedErrCov = [1000, 0; 0, 1000];
outX = bufferedX;
outErrCov = bufferedErrCov;
bufferedX = X;
bufferedErrCov = errCov;
}
}
package dp.subcomponents;
enum Lane {
CENTER_LANE | RIGHT_LANE | LEFT_LANE | OUTSIDE_LANE
}
package dp.subcomponents;
component Localization {
ports
in Affordance affordanceIn,
out B onLane,
out B onMarking,
out Z numLanes,
out Lane lane;
implementation Math {
onLane = false;
onMarking = false;
// Car is on lane
if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
onLane = true;
onMarking = false;
if (affordanceIn.toMarkingLL <= -7.5 && affordanceIn.toMarkingRR >= 7.5)
numLanes = 1;
lane = CENTER_LANE;
elseif (affordanceIn.toMarkingLL > -7.5 && affordanceIn.toMarkingRR < 7.5)
numLanes = 3;
lane = CENTER_LANE;
elseif (affordanceIn.toMarkingLL > -7.5 || affordanceIn.toMarkingRR < 7.5)
numLanes = 2;
if (affordanceIn.toMarkingRR >=7.5)
lane = RIGHT_LANE;
end
if (affordanceIn.toMarkingLL <= -7.5)
lane = LEFT_LANE;
end
else
numLanes = 0;
onLane = false;
lane = OUTSIDE_LANE;
end
else // Car is on marking
onLane = false;
onMarking = true;
// Car on central marking
if (affordanceIn.toMarkingR < 6 && affordanceIn.toMarkingL > -6)
numLanes = 2;
lane = CENTER_LANE;
// When on Outer lanes consider the road only 1 lane.
else
numLanes = 1;
if (affordanceIn.toMarkingL < -6)
lane = LEFT_LANE;
elseif (affordanceIn.toMarkingR > 6)
lane = RIGHT_LANE;
end
end
end
}
}
package dp.subcomponents;
component SteeringBuffer {
ports in Q inSteerCmd,
in Z timerLeftIn,
in Z timerRightIn,
in Z laneChangeIn,
out Z timerLeftOut,
out Z timerRightOut,
out Z laneChangeOut,
out Q^{5} outputBuffer;
implementation Math {
static Q^{5} buffer = [0; 0; 0; 0; 0];
static Z laneChangeBuffer = 0;
static Z timerLeftBuffer = 0;
static Z timerRightBuffer = 0;
timerLeftOut = timerLeftBuffer;
timerRightOut = timerRightBuffer;
timerLeftBuffer = timerLeftIn;
timerRightBuffer = timerRightIn;
laneChangeOut = laneChangeBuffer;
laneChangeBuffer = laneChangeIn;
buffer(5) = buffer(4);
buffer(4) = buffer(3);
buffer(3) = buffer(2);
buffer(2) = buffer(1);
buffer(1) = inSteerCmd;
outputBuffer(1) = buffer(1);
outputBuffer(2) = buffer(2);
outputBuffer(3) = buffer(3);
outputBuffer(4) = buffer(4);
outputBuffer(5) = buffer(5);
}
}
package dp.subcomponents;
import ChangeRange;
component VectorToAffordance {
ports in Q^{16} affordanceIndicators,
out Affordance affordance;
implementation Math {
affordance.fast = affordanceIndicators(1);
affordance.distL = affordanceIndicators(2);
affordance.distR = affordanceIndicators(3);
affordance.toMarkingL = affordanceIndicators(4);
affordance.toMarkingM = affordanceIndicators(5);
affordance.toMarkingR = affordanceIndicators(6);
affordance.distLL = affordanceIndicators(7);
affordance.distMM = affordanceIndicators(8);
affordance.distRR = affordanceIndicators(9);
affordance.toMarkingLL = affordanceIndicators(10);
affordance.toMarkingML = affordanceIndicators(11);
affordance.toMarkingMR = affordanceIndicators(12);
affordance.toMarkingRR = affordanceIndicators(13);
affordance.angle = affordanceIndicators(15);
}
}
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