Working steering control

TorcsEMAMGenerator/src/main/java/de/monticore/lang/monticar/torcs_dl/Generator.java

@@ -54,7 +54,7 @@ public class Generator {
             FileUtils.copyDirectory(new File(RESOURCES_PATH+"torcs"), new File(TARGET_PATH_GENERATED+"torcs"));
             Runtime.getRuntime().exec("chmod u+x "+TARGET_PATH_GENERATED+"torcs/build.sh");
 
-            FileUtils.copyDirectory(new File(RESOURCES_PATH+"dpnet"), new File(TARGET_PATH_GENERATED+"model/dp_mastercomponent_dpnet"));
+            FileUtils.copyDirectory(new File(RESOURCES_PATH+"dpnet"), new File(TARGET_PATH_GENERATED+"model"));
         } catch (IOException e) {
             System.err.println("Failed to copy build and start scripts.");
             e.printStackTrace();

TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl

@@ -3,9 +3,9 @@ import dp.subcomponents.*;
 
 component Mastercomponent {
    ports
-        in   Z(0:255)^{3, 210, 280}   imageIn,
+        in   Q(0:255)^{3, 210, 280}   imageIn,
         in   Q(0 m/s:0.1 m/s:100 m/s) speedIn,
-        out  Q(0:1)^{3}           commandsOut;
+        out  Q(-1:1)^{3}           commandsOut;
 
    instance Dpnet dpnet;
    instance Drivercontroller driverController;

TorcsEMAMGenerator/src/main/models/dp/subcomponents/Dpnet.emadl

@@ -2,7 +2,7 @@
 package dp.subcomponents;
 
 component Dpnet{
-    ports in Z(0:255)^{3, 210, 280} image,
+    ports in Q(0:255)^{3, 210, 280} image,
          out Q(-oo:oo)^{14} predictions;
 
     implementation CNN {
@@ -26,6 +26,9 @@ component Dpnet{
         conv(kernel=(3,3), channels=256) ->
         fc() ->
         fc() ->
+        FullyConnected(units=256) ->
+        Relu() ->
+        Dropout() ->
         FullyConnected(units=14, no_bias=true) ->
         predictions
     }

TorcsEMAMGenerator/src/main/models/dp/subcomponents/Drivercontroller.emadl

@@ -5,7 +5,7 @@ component Drivercontroller {
         in Q^{5} anglesIn,
 	    in Affordance affordanceIn,
 	    in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
-	    out Q(0:1)^{3} commandsOut;
+	    out Q(-1:1)^{3} commandsOut;
 
 	implementation Math {
         Q roadWidth = 8;
@@ -45,13 +45,18 @@ component Drivercontroller {
             coeSteer = 0.3;
         end
 
+        Q steerCmd = 0;
         // steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
-	    commandsOut(2) = (affordanceIn.angle - centerLine/roadWidth) / 0.541052/coeSteer;
+	    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;
 
         Q desiredSpeed = 20;
-	    if (affordanceIn.fast != 1)
-	        desiredSpeed = 20 - abs(anglesIn(0) + anglesIn(1) + anglesIn(2) + anglesIn(3) + anglesIn(4)) * 4.5;
-	    end
+	    //if (affordanceIn.fast != 1)
+	    //    desiredSpeed = 20 - abs(anglesIn(0) + anglesIn(1) + anglesIn(2) + anglesIn(3) + anglesIn(4)) * 4.5;
+	    //end
 
 	    if (desiredSpeed < 10)
 	        desiredSpeed = 10;

TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/main.cpp

@@ -49,23 +49,14 @@ int main(int argc, char **argv)
         cv::cvtColor(flippedImage, rgbImage, cv::COLOR_RGB2BGR);
 
         //Code for debugging: displays screenshot in a window and waits for a key stroke to close it and get new one
-        //cv::namedWindow("Display Image", cv::WINDOW_AUTOSIZE );
-        //cv::imshow("Display Image", rgbImage);
-        //cv::waitKey(0);
-        //cv::destroyWindow("Display Image");
-
-//        std::vector<float> data(3*ROS_IMAGE_HEIGHT*ROS_IMAGE_WIDTH);
-//        for(size_t j=0; j<ROS_IMAGE_HEIGHT; j++){
-//            for(size_t k=0; k<ROS_IMAGE_WIDTH; k++){
-//                cv::Vec3b intensity = rgbImage.at<cv::Vec3b>(j, k);
-//                for(size_t i=0; i<3; i++){
-//                    data[i*ROS_IMAGE_HEIGHT*ROS_IMAGE_WIDTH + j*ROS_IMAGE_HEIGHT + k] = (float) intensity[i];
-//                }
-//            }
-//        }
+//        cv::namedWindow("Display Image 1", cv::WINDOW_AUTOSIZE );
+//        cv::imshow("Display Image 1", rgbImage);
+//        cv::waitKey(0);
+//        cv::destroyWindow("Display Image 1");
+
 
         std::vector<float> data(0);
-        data.reserve(3*ROS_IMAGE_HEIGHT*ROS_IMAGE_WIDTH);
+        data.reserve(ROS_IMAGE_SIZE);
         for(size_t j=0; j<ROS_IMAGE_HEIGHT; j++){
             for(size_t k=0; k<ROS_IMAGE_WIDTH; k++){
                 cv::Vec3b intensity = rgbImage.at<cv::Vec3b>(j, k);
@@ -75,10 +66,11 @@ int main(int argc, char **argv)
             }
         }
 
+        assert(sizeof(float) == sizeof(std_msgs::Float32));
 
         std_msgs::Float32MultiArray::Ptr msg(new std_msgs::Float32MultiArray);
 	    msg->data.resize(ROS_IMAGE_SIZE);
-	    memcpy(msg->data.data(), data.data(), ROS_IMAGE_SIZE);
+	    memcpy(msg->data.data(), data.data(), ROS_IMAGE_SIZE*sizeof(float));
 
 	    std_msgs::Float32::Ptr speedMsg(new std_msgs::Float32);
 	    speedMsg->data = speed;