Aufgrund einer Wartung wird GitLab am 04.06. zwischen 8:00 und 10:00 Uhr kurzzeitig nicht zur Verfügung stehen. / Due to maintenance, GitLab will be temporarily unavailable on 04.06. between 8:00 and 10:00 am.

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"
This source diff could not be displayed because it is too large. You can view the blob instead.
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 DriverController {
ports
in Q^{5} steeringRecordIn,
in Affordance affordanceIn,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
in Z timerLeftIn,
in Z timerRightIn,
in Z laneChangeIn,
in Z lanesCountIn,
out Q(-1:1)^{3} commandsOut, // [accelCmd, steerCmd, brakeCmd]
out Q steerCmd,
out Z timerLeftOut,
out Z timerRightOut,
out Z laneChangeOut;
implementation Math {
Q roadWidth = 8;
Q centerLine;
Q coeSteer = 1.0;
Q preML;
Q preMR;
Q slowDown = 100; // Acceleration
Q preDistL = 60;
Q preDistR = 60;
B leftClear = false;
B rightClear = false;
Z timerSet = 60;
Z leftTimer = timerLeftIn;
Z rightTimer = timerRightIn;
Q steerTrend = steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4) + steeringRecordIn(5); // am I turning or not;
//parameters of car following modlel
Q vMax = 20;
Q vC = 2.772;
Q vD = -0.693;
Q carFollowingSlowDown = vMax * (1-exp(-(vC/vMax) * affordanceIn.distMM - vD)); // optimal velocity for car-following model
// CASE 1: one lane
if (lanesCountIn == 1)
if (affordanceIn.distMM < 20) // car in front of me, slow down
// optimal velocity car-following model
slowDown = carFollowingSlowDown;
if (slowDown < 0)
slowDown = 0;
end
end
if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
coeSteer = 1.5;
centerLine = (affordanceIn.toMarkingML + affordanceIn.toMarkingMR) / 2;
preML = affordanceIn.toMarkingML;
preMR = affordanceIn.toMarkingMR;
if (affordanceIn.toMarkingM < 1)
coeSteer = 0.4;
end
else
if (-preML > preMR)
centerLine = (affordanceIn.toMarkingL + affordanceIn.toMarkingM) / 2;
else
centerLine=(affordanceIn.toMarkingR + affordanceIn.toMarkingM) / 2;
end
coeSteer = 0.3;
end
// steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052 / coeSteer;
if (coeSteer > 1 && steerCmd > 0.1) // reshape the steering control curve
steerCmd = steerCmd * (2.5 * steerCmd + 0.75);
end
commandsOut(2) = steerCmd;
// CASE 2: two or three lanes
elseif (lanesCountIn == 2 || lanesCountIn == 3)
if (preDistL < 20 && affordanceIn.distLL < 20) // left lane is occupied or not
leftClear = false;
leftTimer = 0;
else
leftTimer += 1;
end
if (preDistR < 20 && affordanceIn.distRR < 20) // right lane is occupied or not
rightClear = false;
rightTimer = 0;
else
rightTimer += 1;
end
preDistL = affordanceIn.distLL;
preDistR = affordanceIn.distRR;
if (leftTimer > timerSet) // left lane is clear
leftTimer = timerSet;
leftClear = true;
end
if (rightTimer > timerSet) // right lane is clear
rightTimer = timerSet;
rightClear = true;
end
if (lanesCountIn == 2)
if (laneChangeIn == 0 && affordanceIn.distMM < 15) // if current lane is occupied
if (affordanceIn.toMarkingLL > -8 && leftClear == true && steerTrend >= 0) // move to left lane
laneChangeIn = -2;
coeSteer = 6;
rightClear = false;
rightTimer = 0;
leftClear = false;
leftTimer = 0;
timerSet = 30;
elseif (affordanceIn.toMarkingRR < 8 && rightClear == true && steerTrend <= 0) // move to right lane
laneChangeIn = 2;
coeSteer = 6;
rightClear = false;
rightTimer = 0;
leftClear = false;
leftTimer = 0;
timerSet = 30;
else // follow car in front
slowDown = carFollowingSlowDown;
if slowDown < 0
slowDown = 0;
end
end
elseif (laneChangeIn == 0 && affordanceIn.distMM >= 15) // prefer to stay in the right lane
if (affordanceIn.toMarkingLL < -8 && rightClear == true && steerTrend <= 0 && steerTrend > -0.2) // in right lane, so move to central lane
laneChangeIn = 2;
coeSteer = 6;
rightClear = false;
rightTimer = 20;
end
end
elseif (lanesCountIn == 3)
if (laneChangeIn == 0 && affordanceIn.distMM < 15) // current lane is occupied
if (affordanceIn.toMarkingLL > -8 && leftClear == true && steerTrend >= 0 && steerTrend < 0.2) // move to left lane
laneChangeIn = -2;
coeSteer = 6;
rightClear = false;
rightTimer = 0;
leftClear = false;
leftTimer = 30;
timerSet = 60;
elseif (affordanceIn.toMarkingRR < 8 && rightClear == true && steerTrend <= 0 && steerTrend > -0.2) // move to right lane
laneChangeIn = 2;
coeSteer = 6;
rightClear = false;
rightTimer = 30;
leftClear = false;
leftTimer = 0;
timerSet = 60;
else // car following
slowDown = carFollowingSlowDown;
if slowDown < 0
slowDown = 0;
end
end
elseif (laneChangeIn == 0 && affordanceIn.distMM >= 15) // prefer to stay in the central line
if (affordanceIn.toMarkingRR > 8 && leftClear == true && steerTrend >= 0 && steerTrend < 0.2) // in right lane, so move to central lane
laneChangeIn = -2;
coeSteer = 6;
leftClear = false;
leftTimer = 30;
elseif (affordanceIn.toMarkingLL < -8 && rightClear == true && steerTrend <= 0 && steerTrend > -0.2) // in left lane, so move to central lane
laneChangeIn = 2;
coeSteer = 6;
rightClear = false;
rightTimer = 30;
end
end
end
// go through all possible lane change maneuvers
if (laneChangeIn == 0) //car following
if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
coeSteer = 1.5;
centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
preML = affordanceIn.toMarkingML;
preMR = affordanceIn.toMarkingMR;
if (affordanceIn.toMarkingM < 1)
coeSteer = 0.4;
end
else
if (-preML > preMR)
centerLine = (affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2;
else
centerLine = (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2;
end
coeSteer = 0.3;
end
elseif (laneChangeIn == -2)
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (affordanceIn.toMarkingLL+affordanceIn.toMarkingML)/2;
if (affordanceIn.toMarkingL > -5 && affordanceIn.toMarkingM < 1.5)
centerLine = (centerLine+(affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2)/2;
end
else
centerLine = (affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2;
coeSteer = 20;
laneChangeIn = -1;
end
elseif (laneChangeIn == -1)
if (affordanceIn.toMarkingL > -5 && affordanceIn.toMarkingM < 1.5)
centerLine = (affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2;
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (centerLine+(affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2)/2;
end
else
centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
laneChangeIn = 0;
end
elseif (laneChangeIn == 2)
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (affordanceIn.toMarkingRR+affordanceIn.toMarkingMR)/2;
if (affordanceIn.toMarkingR < 5 && affordanceIn.toMarkingM < 1.5)
centerLine = (centerLine + (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2)/2;
end
else
centerLine = (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2;
coeSteer = 20;
laneChangeIn = 1;
end
elseif (laneChangeIn == 1)
if (affordanceIn.toMarkingR < 5 && affordanceIn.toMarkingM < 1.5)
centerLine = (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2;
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (centerLine+(affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2)/2;
else
centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
laneChangeIn = 0;
end
end
end
steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052 / coeSteer;
if (laneChangeIn == 0 && coeSteer > 1 && steerCmd > 0.1) // reshape the steering control curve
steerCmd = steerCmd * (2.5*steerCmd+0.75);
end
commandsOut(2) = steerCmd;
// DEFAULT CASE - no information on lanes number
else
commandsOut(2) = 0;
end
// compute desired speed
Q desiredSpeed = 20; // 20m/s = 72 kmh
if (affordanceIn.fast != 1)
desiredSpeed = 20 - abs(steerTrend) * 4.5;
end
if (desiredSpeed < 10)
desiredSpeed = 10;
end
if (desiredSpeed > slowDown)
desiredSpeed = slowDown;
end
// compute acceleration and brake
if (desiredSpeed >= speedIn)
Q accelCmd = 0.2*(desiredSpeed - speedIn + 1);
if (accelCmd > 1)
accelCmd = 1;
end
commandsOut(1) = accelCmd;
commandsOut(3) = 0; // brakeCmd
else
Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
if (brakeCmd > 1)
brakeCmd = 1;
end
commandsOut(3) = brakeCmd;
commandsOut(1) = 0; // accelCmd
end
timerLeftOut = leftTimer;
timerRightOut = rightTimer;
laneChangeOut = laneChangeIn;
}
}
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;