Use CNN predictions for driving

EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/main.cpp

@@ -26,6 +26,13 @@ void infoChatterCallback(const std_msgs::Int32::ConstPtr& msg)
     client.setLanesCount(msg->data);
 }
 
+void predictionsChatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
+{
+    torcs::Affordance affordance;
+    affordance.populate(*msg);
+    client.visualizePredictions(affordance);
+}
+
 void publishCallback(std::shared_ptr<torcs::DataContainer> screenshot, torcs::Affordance affordance)
 {
     cv::Mat image(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
@@ -107,6 +114,7 @@ int main(int argc, char **argv)
 
     ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback);
     ros::Subscriber infoSub = n.subscribe("/info", 1, infoChatterCallback);
+    ros::Subscriber predictionsSub = n.subscribe("/predictions", 1, predictionsChatterCallback);
     ros::Rate loop_rate(10);
     std::thread clientRunnerThread(clientRunner);
 

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

@@ -103,6 +103,37 @@ void TorcsClient::initVisualization()
     cvInitFont(&font, CV_FONT_HERSHEY_COMPLEX, 1, 1, 1, 2, 8);
 }
 
+void TorcsClient::visualizePredictions(const torcs::Affordance &affordance)
+{
+    float true_angle = float(shared->angle);
+    float true_dist_L = float(shared->dist_L);
+    float true_dist_R = float(shared->dist_R);
+    float true_toMarking_L = float(shared->toMarking_L);
+    float true_toMarking_M = float(shared->toMarking_M);
+    float true_toMarking_R = float(shared->toMarking_R);
+    float true_dist_LL = float(shared->dist_LL);
+    float true_dist_MM = float(shared->dist_MM);
+    float true_dist_RR = float(shared->dist_RR);
+    float true_toMarking_LL = float(shared->toMarking_LL);
+    float true_toMarking_ML = float(shared->toMarking_ML);
+    float true_toMarking_MR = float(shared->toMarking_MR);
+    float true_toMarking_RR = float(shared->toMarking_RR);
+
+    visualize(affordance.getAngle(), true_angle,
+              affordance.getToMarkingML(), true_toMarking_ML,
+              affordance.getToMarkingMR(), true_toMarking_MR,
+              affordance.getToMarkingM(), true_toMarking_M,
+              affordance.getToMarkingLL(), true_toMarking_LL,
+              affordance.getToMarkingR(), true_toMarking_R,
+              affordance.getToMarkingL(), true_toMarking_L,
+              affordance.getToMarkingRR(), true_toMarking_RR,
+              affordance.getDistLL(), true_dist_LL,
+              affordance.getDistRR(), true_dist_RR,
+              affordance.getDistMM(), true_dist_MM,
+              affordance.getDistL(), true_dist_L,
+              affordance.getDistR(), true_dist_R);
+}
+
 void TorcsClient::setPublishCallback(std::function<void(std::shared_ptr<DataContainer>, Affordance)> callback)
 {
     publishCallback = callback;
@@ -150,34 +181,6 @@ void TorcsClient::iterate()
 
             shared->written = 0;
             shared->pause = 0;
-
-            float true_angle = float(shared->angle);
-            float true_dist_L = float(shared->dist_L);
-            float true_dist_R = float(shared->dist_R);
-            float true_toMarking_L = float(shared->toMarking_L);
-            float true_toMarking_M = float(shared->toMarking_M);
-            float true_toMarking_R = float(shared->toMarking_R);
-            float true_dist_LL = float(shared->dist_LL);
-            float true_dist_MM = float(shared->dist_MM);
-            float true_dist_RR = float(shared->dist_RR);
-            float true_toMarking_LL = float(shared->toMarking_LL);
-            float true_toMarking_ML = float(shared->toMarking_ML);
-            float true_toMarking_MR = float(shared->toMarking_MR);
-            float true_toMarking_RR = float(shared->toMarking_RR);
-
-            visualize(affordance.getAngle(), true_angle,
-                      affordance.getToMarkingML(), true_toMarking_ML,
-                      affordance.getToMarkingMR(), true_toMarking_MR,
-                      affordance.getToMarkingM(), true_toMarking_M,
-                      affordance.getToMarkingLL(), true_toMarking_LL,
-                      affordance.getToMarkingR(), true_toMarking_R,
-                      affordance.getToMarkingL(), true_toMarking_L,
-                      affordance.getToMarkingRR(), true_toMarking_RR,
-                      affordance.getDistLL(), true_dist_LL,
-                      affordance.getDistRR(), true_dist_RR,
-                      affordance.getDistMM(), true_dist_MM,
-                      affordance.getDistL(), true_dist_L,
-                      affordance.getDistR(), true_dist_R);
         } else {
             if (shared->pause != 1) {
                 shared->pause = 1;

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

@@ -97,22 +97,22 @@ public:
         affordance_ = affordance.data;
     }
 
-    float getFast() { return affordance_[0]; }
-    float getDistL() { return affordance_[1]; }
-    float getDistR() { return affordance_[2]; }
-    float getToMarkingL() { return affordance_[3]; }
-    float getToMarkingM() { return affordance_[4]; }
-    float getToMarkingR() { return affordance_[5]; }
-    float getDistLL() { return affordance_[6]; }
-    float getDistMM() { return affordance_[7]; }
-    float getDistRR() { return affordance_[8]; }
-    float getToMarkingLL() { return affordance_[9]; }
-    float getToMarkingML() { return affordance_[10]; }
-    float getToMarkingMR() { return affordance_[11]; }
-    float getToMarkingRR() { return affordance_[12]; }
-    float getToMiddle() { return affordance_[13]; }
-    float getAngle() { return affordance_[14]; }
-    float getSpeed() { return affordance_[15]; }
+    float getFast() const { return affordance_[0]; }
+    float getDistL() const { return affordance_[1]; }
+    float getDistR() const { return affordance_[2]; }
+    float getToMarkingL() const { return affordance_[3]; }
+    float getToMarkingM() const { return affordance_[4]; }
+    float getToMarkingR() const { return affordance_[5]; }
+    float getDistLL() const { return affordance_[6]; }
+    float getDistMM() const { return affordance_[7]; }
+    float getDistRR() const { return affordance_[8]; }
+    float getToMarkingLL() const { return affordance_[9]; }
+    float getToMarkingML() const { return affordance_[10]; }
+    float getToMarkingMR() const { return affordance_[11]; }
+    float getToMarkingRR() const { return affordance_[12]; }
+    float getToMiddle() const { return affordance_[13]; }
+    float getAngle() const { return affordance_[14]; }
+    float getSpeed() const { return affordance_[15]; }
 
     const std::vector<float>& getAsVector()
     {
@@ -133,12 +133,7 @@ public:
     void iterate();
     void sendCommand(const Command& cmd);
     void setLanesCount(int count);
-    void visualize(float angle, float true_angle, float toMarking_ML, float true_toMarking_ML,
-                   float toMarking_MR, float true_toMarking_MR, float toMarking_M, float true_toMarking_M,
-                   float toMarking_LL, float true_toMarking_LL, float toMarking_R, float true_toMarking_R,
-                   float toMarking_L, float true_toMarking_L, float toMarking_RR, float true_toMarking_RR,
-                   float dist_LL, float true_dist_LL, float dist_RR, float true_dist_RR, float dist_MM, float true_dist_MM,
-                   float dist_L, float true_dist_L, float dist_R, float true_dist_R);
+    void visualizePredictions(const Affordance& affordance);
 
 private:
 
@@ -149,6 +144,12 @@ private:
     std::function<void(std::shared_ptr<DataContainer>, Affordance)> publishCallback;
     int lanesCount = 3; // Use three lanes configuration by default
 
+    void visualize(float angle, float true_angle, float toMarking_ML, float true_toMarking_ML,
+                   float toMarking_MR, float true_toMarking_MR, float toMarking_M, float true_toMarking_M,
+                   float toMarking_LL, float true_toMarking_LL, float toMarking_R, float true_toMarking_R,
+                   float toMarking_L, float true_toMarking_L, float toMarking_RR, float true_toMarking_RR,
+                   float dist_LL, float true_dist_LL, float dist_RR, float true_dist_RR, float dist_MM, float true_dist_MM,
+                   float dist_L, float true_dist_L, float dist_R, float true_dist_R);
     void initVisualization();
 
 };

EMADL2TORCS/src/main/dp/Mastercomponent.emadl

@@ -3,32 +3,36 @@ import dp.subcomponents.*;
 
 component Mastercomponent {
    ports
-        //in   Z(0:255)^{3, 210, 280}   imageIn,
-        in Q^{16} groundTruthAffordance,
+        in   Z(0:255)^{3, 210, 280}   imageIn,
+        //in Q^{16} groundTruthAffordance,
         in   Q(0 m/s:0.1 m/s:100 m/s) speedIn,
-        out  Q(-1:1)^{3}           commandsOut;
+        out  Q^{16} predictedAffordanceOut,
+        out  Q(-1:1)^{3} commandsOut;
 
-   //instance Dpnet dpnet;
+   instance Dpnet dpnet;
    instance DriverController driverController;
-   //instance Denormalizer denorm;
+   instance Denormalizer denorm;
    //instance KFMastercomponent kfm;
    instance SteeringBuffer steeringBuffer;
    instance VectorToAffordance vecToAffordance;
    instance LocalizationController locController;
+   instance AffordanceToVector affToVector;
 
-   //connect imageIn -> dpnet.data;
-   //connect dpnet.predictions -> denorm.normalizedPredictions;
+   connect imageIn -> dpnet.data;
+   connect dpnet.predictions -> denorm.normalizedPredictions;
 
    //connect denorm.affordance -> kfm.affordanceIn;
    //connect kfm.affordanceSmoothed -> driverController.affordanceIn;
-   //connect denorm.affordance -> driverController.affordanceIn;
-   connect groundTruthAffordance -> vecToAffordance.affordanceIndicators;
-   connect vecToAffordance.affordance -> driverController.affordanceIn;
+   connect denorm.affordance -> driverController.affordanceIn, affToVector.affordanceIn;
+   connect affToVector.affordanceOut -> predictedAffordanceOut;
+   //connect groundTruthAffordance -> vecToAffordance.affordanceIndicators;
+   //connect vecToAffordance.affordance -> driverController.affordanceIn;
    //connect vecToAffordance.affordance -> locController.affordanceIn;
 
    connect steeringBuffer.outputBuffer -> driverController.steeringRecordIn;
    connect speedIn -> driverController.speedIn;
 
+   connect locController.lanesCount -> driverController.lanesCountIn;
    connect steeringBuffer.timerLeftOut -> driverController.timerLeftIn;
    connect steeringBuffer.timerRightOut -> driverController.timerRightIn;
    connect steeringBuffer.laneChangeOut -> driverController.laneChangeIn;

EMADL2TORCS/src/main/dp/Mastercomponent.tag

@@ -4,6 +4,7 @@ conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema;
 tags Mastercomponent {
     tag mastercomponent.imageIn with RosConnection = {topic=(/camera,std_msgs/Float32MultiArray)};
     tag mastercomponent.speedIn with RosConnection = {topic=(/speed,std_msgs/Float32)};
+    tag mastercomponent.groundTruthAffordance with RosConnection = {topic=(/affordance,std_msgs/Float32MultiArray)};
     tag mastercomponent.commandsOut with RosConnection = {topic=(/commands,std_msgs/Float32MultiArray)};
-    tag mastercomponent.lanesCountOut with RosConnection = {topic=(/info,std_msgs/Int32)};
+    tag mastercomponent.predictedAffordanceOut with RosConnection = {topic=(/predictions,std_msgs/Float32MultiArray)};
 }

EMADL2TORCS/src/main/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/main/dp/subcomponents/SteeringBuffer.emadl

@@ -12,20 +12,17 @@ component SteeringBuffer {
 
     implementation Math {
             static Q^{5} buffer = [0; 0; 0; 0; 0];
-            static Q^{2} laneChangeBuffer = [0;0];
-            static Z^{2} timerLeftBuffer = [0; 0];
-            static Z^{2} timerRightBuffer = [0; 0];
+            static Q laneChangeBuffer = 0;
+            static Z timerLeftBuffer = 0;
+            static Z timerRightBuffer = 0;
 
-            timerLeftBuffer(1) = timerLeftBuffer(2);
-            timerRightBuffer(1) = timerRightBuffer(2);
-            timerLeftOut = timerLeftBuffer(1);
-            timerRightOut = timerRightBuffer(1);
-            timerLeftBuffer(2) = timerLeftIn;
-            timerRightBuffer(2) = timerRightIn;
+            timerLeftOut = timerLeftBuffer;
+            timerRightOut = timerRightBuffer;
+            timerLeftBuffer = timerLeftIn;
+            timerRightBuffer = timerRightIn;
 
-            laneChangeBuffer(1) = laneChangeBuffer(2);
-            laneChangeOut = laneChangeBuffer(1);
-            laneChangeBuffer(2) = laneChangeIn;
+            laneChangeOut = laneChangeBuffer;
+            laneChangeBuffer = laneChangeIn;
 
             buffer(5) = buffer(4);
             buffer(4) = buffer(3);