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/DriverController.emadl

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