Merge branch 'multi_variant_model' into 'develop'

Multi variant model

See merge request !1

EMADL2TORCS/generate2ros.sh → EMADL2TORCS/generate2ros_dpnet_variant.sh

@@ -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/

EMADL2TORCS/generate2ros_groundTruthLabels_variant.sh

+#!/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"

EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.cpp

@@ -8,6 +8,7 @@
 #include <assert.h>
 #include <ros/console.h>
 #include <cv.hpp>
+#include <highgui.h>
 
 using namespace torcs;
 

EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.h

@@ -5,6 +5,7 @@
 #include <memory>
 #include <functional>
 #include <std_msgs/Float32MultiArray.h>
+#include <atomic>
 
 #define DEBUG_LOG false
 
@@ -157,4 +158,3 @@ private:
 };
 
 }
-

EMADL2TORCS/src/groundTruthLabels_variant/dp/Mastercomponent.emadl

+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;
+}

EMADL2TORCS/src/groundTruthLabels_variant/dp/Mastercomponent.tag

+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)};    
+}

EMADL2TORCS/src/groundTruthLabels_variant/dp/subcomponents/Affordance.struct

+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;
+}

EMADL2TORCS/src/groundTruthLabels_variant/dp/subcomponents/AffordanceToVector.emadl

+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;
+        }
+
+}

EMADL2TORCS/src/groundTruthLabels_variant/dp/subcomponents/ConstantMatrix.emadl

+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;
+    }
+}

EMADL2TORCS/src/groundTruthLabels_variant/dp/subcomponents/Denormalizer.emadl

+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;
+        }
+
+}

EMADL2TORCS/src/groundTruthLabels_variant/dp/subcomponents/Dpnet.cnnt

+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
+
+     }
+}

EMADL2TORCS/src/groundTruthLabels_variant/dp/subcomponents/Dpnet.emadl

+
+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
+    }
+}

EMADL2TORCS/src/groundTruthLabels_variant/dp/subcomponents/KFMastercomponent.emadl

+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;
+}

EMADL2TORCS/src/groundTruthLabels_variant/dp/subcomponents/KalmanFilter.emadl

+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);
+	
+    }
+}

EMADL2TORCS/src/groundTruthLabels_variant/dp/subcomponents/KalmanStatsBuffer.emadl

+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;
+        }
+}

EMADL2TORCS/src/groundTruthLabels_variant/dp/subcomponents/Lane.enum

+package dp.subcomponents;
+
+enum Lane {
+	CENTER_LANE |  RIGHT_LANE |  LEFT_LANE | OUTSIDE_LANE
+}
+

EMADL2TORCS/src/groundTruthLabels_variant/dp/subcomponents/Localization.emadl

+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
+	}
+}

EMADL2TORCS/src/groundTruthLabels_variant/dp/subcomponents/SteeringBuffer.emadl

+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);
+        }
+}

EMADL2TORCS/src/groundTruthLabels_variant/dp/subcomponents/VectorToAffordance.emadl

+package dp.subcomponents;
+
+import ChangeRange;
+