Fixed TORCS communication sync, added speed messages, implemented within-lane acceleration control.

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

@@ -3,8 +3,9 @@ import dp.subcomponents.*;
 
 component Mastercomponent {
    ports
-        in   Z(0:255)^{3, 210, 280}  imageIn,
-        out  Q(0:1)^{3,1,1}        commandsOut;
+        in   Z(0:255)^{3, 210, 280} imageIn,
+        in   Q                      speedIn,
+        out  Q(0:1)^{3,1,1}         commandsOut;
 
    instance Imagepreprocessing ip;
    instance Dpnet dpnet;
@@ -13,6 +14,6 @@ component Mastercomponent {
    connect imageIn -> ip.imageIn;
    connect ip.imageOut -> dpnet.image;
    connect dpnet.predictions -> dc.affordanceIn;
+   connect speedIn -> dc.speedIn;
    connect dc.commandsOut -> commandsOut;
-
 }
\ No newline at end of file

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

@@ -3,5 +3,6 @@ conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema;
 
 tags Mastercomponent {
     tag mastercomponent.imageIn with RosConnection = {topic=(/camera,std_msgs/Int8MultiArray)};
+    tag mastercomponent.speedIn with RosConnection = {topic=(/speed,std_msgs/Float32)};
     tag mastercomponent.commandsOut with RosConnection = {topic=(/commands,std_msgs/Float32MultiArray)};
 }

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

@@ -28,6 +28,5 @@ component Dpnet{
         FullyConnected(units=13) ->
         Sigmoid() ->
         predictions
-
     }
 }

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

@@ -3,6 +3,7 @@ package dp.subcomponents;
 component Drivercontroller {
     ports
 	    in Q(0:1)^{1,1,13} affordanceIn,
+	    in Q speedIn,
 	    out Q(0:1)^{3,1,1} commandsOut;
 
 	implementation Math {
@@ -13,15 +14,31 @@ component Drivercontroller {
 	    Q distToLaneCenter = abs(laneWidth/2 - affordanceIn(0,0,2));
 
 	    // Steering
-	    commandsOut(2,1,1) = Constant * (angle-distToLaneCenter/roadWidth);  // steering
+	    commandsOut(2,1,1) = 0;//Constant * (angle-distToLaneCenter/roadWidth);  // steering
 
 	    // Acceleration
-	    Q vMax = 72;
+	    Q vMax = 25;
 	    Q vC = 1;
 	    Q vD = 0;
-	    Q desiredSpeed = vMax * (exp(-(vC/vMax) * affordanceIn(0,0,6) - vD));
-	    commandsOut(1,1,1) = desiredSpeed/vMax;  // acceleration
+	    Q desiredSpeed = 15;//vMax * (exp(-(vC/vMax) * affordanceIn(0,0,6) - vD));
 
-        commandsOut(3,1,1)=0.0;  // brake
+	    Q accelCmd = 0.1 * (desiredSpeed - speedIn + 1);
+	    if accelCmd > 1
+            accelCmd = 1.0;
+        end
+
+        Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
+        if brakeCmd > 1
+            brakeCmd = 1;
+        end
+
+        // acceleration and brake
+	    if speedIn < desiredSpeed
+	        commandsOut(1,1,1) = accelCmd;
+	        commandsOut(3,1,1) = 0;
+	    else
+            commandsOut(1,1,1) = 0;
+            commandsOut(3,1,1) = brakeCmd;
+        end
     }
 }
\ No newline at end of file

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

@@ -6,6 +6,6 @@ component Imagepreprocessing {
         out Z(0:255)^{3, 210, 280} imageOut;
 
     implementation Math {
-        imageOut=imageIn;
+        imageOut = imageIn;
     }
 }
\ No newline at end of file

TorcsEMAMGenerator/src/main/resources/scripts/run_all.sh

@@ -5,7 +5,7 @@ xterm -T ROSCORE -e roscore &
 echo "Waiting for ROSCORE to start..."
 sleep 2
 
-xterm -T TORCSDriver -e torcs/src/driver/build/driver &
+#xterm -T TORCSDriver -e torcs/src/driver/build/driver &
 
 xterm -T TORCSCamera -e torcs/src/camera/build/camera &
 

TorcsEMAMGenerator/src/main/resources/torcs/build.sh

@@ -12,10 +12,10 @@ cmake ..
 make -j
 cd ../..
 
-cd driver
-mkdir -p build
-cd build
-cmake ..
-make -j
-cd ../..
+#cd driver
+#mkdir -p build
+#cd build
+#cmake ..
+#make -j
+#cd ../..
 

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

 #include "ros/ros.h"
 #include "std_msgs/Int8MultiArray.h"
+#include "std_msgs/Float32.h"
 #include "torcsclient.h"
 
 #include <sstream>
@@ -7,23 +8,37 @@
 #include <ml.h>
 #include <highgui.h>
 #include <cv.h>
+#include <ros/console.h>
+#include <std_msgs/Float32MultiArray.h>
 
 constexpr auto ROS_IMAGE_WIDTH = 280;
 constexpr auto ROS_IMAGE_HEIGHT = 210;
 constexpr auto ROS_IMAGE_SIZE = ROS_IMAGE_WIDTH*ROS_IMAGE_HEIGHT*3;
 
+torcs::Command cmd(1., 0., 0.);
+
+void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
+{
+    cmd = torcs::Command(msg->data[0], msg->data[1], msg->data[2]);
+    //client->sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2]));
+}
+
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "camera");
     ros::NodeHandle n;
-    ros::Publisher chatter_pub = n.advertise<std_msgs::Int8MultiArray>("/camera", 1000);
+    ros::Publisher cameraPub = n.advertise<std_msgs::Int8MultiArray>("/camera", 1);
+    ros::Publisher speedPub = n.advertise<std_msgs::Float32>("/speed", 1);
+    ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback);
     ros::Rate loop_rate(10);
     int count = 0;
 
     torcs::TorcsClient client;
     while (ros::ok())
     {
-        const auto screenshot = client.getScreenshot();
+        float speed;
+        std::unique_ptr<torcs::DataContainer> screenshot = std::make_unique<torcs::DataContainer>();
+        client.iterate(screenshot, speed, cmd);
 
         cv::Mat image(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
         memcpy(image.data, screenshot.get(), torcs::IMAGE_SIZE_BYTES);
@@ -41,7 +56,11 @@ int main(int argc, char **argv)
 	    msg->data.resize(ROS_IMAGE_SIZE);
 	    memcpy(msg->data.data(), resizedImage.data, ROS_IMAGE_SIZE);
 
-        chatter_pub.publish(msg);
+	    std_msgs::Float32::Ptr speedMsg(new std_msgs::Float32);
+	    speedMsg->data = speed;
+
+        cameraPub.publish(msg);
+        speedPub.publish(speedMsg);
         ros::spinOnce();
         loop_rate.sleep();
         ++count;

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

@@ -6,6 +6,7 @@
 #include <cstring>
 #include <sys/shm.h>
 #include <assert.h>
+#include <ros/console.h>
 
 using namespace torcs;
 
@@ -19,26 +20,28 @@ TorcsClient::TorcsClient()
     shmid = shmget((key_t)4567, sizeof(struct shared_use_st), 0666);
     if(shmid < 0)
     {
-        std::cerr << "Failed to get shared memory!" << std::endl;
-	return;
+        //std::cerr << "Failed to get shared memory!" << std::endl;
+        ROS_ERROR("Failed to get shared memory!");
+    return;
     }
 
     shm = shmat(shmid, 0, 0);
     if(shm == (void*)-1)
     {
-        std::cerr << "Failed to shmat()!" << std::endl;
+        ROS_ERROR("Failed to shmat()!");
+        //std::cerr << "Failed to shmat()!" << std::endl;
         return;
     }
     
     connected = true;
 
-    std::cout << "Started shared memory at " << std::hex << shm << std::endl;
+    //std::cout << "Started shared memory at " << std::hex << shm << std::endl;
 
     shared = (struct shared_use_st*)shm;
 
 }
 
-std::unique_ptr<DataContainer> TorcsClient::getScreenshot()
+std::unique_ptr<DataContainer> TorcsClient::getScreenshot(float& speed)
 {
     assert(connected);
 
@@ -48,12 +51,65 @@ std::unique_ptr<DataContainer> TorcsClient::getScreenshot()
     }
 
     shared->pause = 1;
-    std::cout << "Got screenshot" << std::endl;
     auto result = std::make_unique<DataContainer>();
     memcpy(result->data(), shared->data, result->size());
+    speed = shared->speed;
     shared->pause = 0;
     shared->written = 0;
 
     return result;
 }
 
+void TorcsClient::sendCommand(Command cmd)
+{
+    assert(connected);
+
+    while (shared->written == 1) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+
+    //ROS_INFO("Command acc: %f, brake: %f, steer: %f", cmd.accelCmd, cmd.brakeCmd, cmd.steerCmd);
+
+    shared->accelCmd = cmd.accelCmd;
+    shared->steerCmd = cmd.steerCmd;
+    shared->brakeCmd = cmd.brakeCmd;
+    shared->control = 1;
+    shared->written = 1;
+}
+
+void TorcsClient::iterate(std::unique_ptr<DataContainer>& screenshot, float& speed, const Command& cmd)
+{
+    //ROS_INFO("Iteration");
+
+    assert(connected);
+    bool commandSent = false;
+    bool screenshotRead = false;
+
+    while (!(commandSent && screenshotRead)) {
+        if (shared->written == 0) {
+            //ROS_INFO("Commanding");
+            if (!commandSent)
+            {
+                //ROS_INFO("Command done");
+                shared->accelCmd = cmd.accelCmd;
+                shared->steerCmd = cmd.steerCmd;
+                shared->brakeCmd = cmd.brakeCmd;
+                shared->control = 1;
+                shared->written = 1;
+                commandSent = true;
+            }
+        } else {
+            //ROS_INFO("Reading");
+            if (!screenshotRead)
+            {
+                //ROS_INFO("Reading done");
+                shared->pause = 1;
+                memcpy(screenshot->data(), shared->data, screenshot->size());
+                speed = float(shared->speed);
+                shared->pause = 0;
+                shared->written = 0;
+                screenshotRead = true;
+            }
+        }
+    }
+}

TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.h

@@ -45,13 +45,28 @@ struct shared_use_st
     double brakeCmd = 0.0;
 };
 
+struct Command
+{
+    Command(float accel, float steer, float brake)
+        : accelCmd(accel), steerCmd(steer), brakeCmd(brake)
+    {
+    }
+
+    float accelCmd;
+    float steerCmd;
+    float brakeCmd;
+};
+
 using DataContainer = std::array<uint8_t, IMAGE_SIZE_BYTES>;
 
 class TorcsClient
 {
 public:
     TorcsClient();
-    std::unique_ptr<DataContainer> getScreenshot();
+    std::unique_ptr<DataContainer> getScreenshot(float& speed);
+    void sendCommand(Command cmd);
+
+    void iterate(std::unique_ptr<DataContainer>& screenshot, float& speed, const Command& cmd);
 
 private:
     struct shared_use_st *shared;

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

@@ -8,6 +8,7 @@ std::unique_ptr<torcs::TorcsClient> client;
 
 void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
 {
+    ROS_INFO("Received data");
     client->sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2]));
 }
 
@@ -19,7 +20,7 @@ int main(int argc, char **argv)
 
     ros::NodeHandle n;
 
-    ros::Subscriber sub = n.subscribe("/commands", 100, chatterCallback);
+    ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback);
     ros::spin();
 
     return 0;

TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.cpp

@@ -6,6 +6,7 @@
 #include <cstring>
 #include <sys/shm.h>
 #include <assert.h>
+#include <ros/console.h>
 
 using namespace torcs;
 
@@ -19,20 +20,22 @@ TorcsClient::TorcsClient()
     shmid = shmget((key_t)4567, sizeof(struct shared_use_st), 0666);
     if(shmid < 0)
     {
-        std::cerr << "Failed to get shared memory!" << std::endl;
+        //std::cerr << "Failed to get shared memory!" << std::endl;
+        ROS_ERROR("Failed to get shared memory!");
 	return;
     }
 
     shm = shmat(shmid, 0, 0);
     if(shm == (void*)-1)
     {
-        std::cerr << "Failed to shmat()!" << std::endl;
+        ROS_ERROR("Failed to shmat()!");
+        //std::cerr << "Failed to shmat()!" << std::endl;
         return;
     }
     
     connected = true;
 
-    std::cout << "Started shared memory at " << std::hex << shm << std::endl;
+    //std::cout << "Started shared memory at " << std::hex << shm << std::endl;
 
     shared = (struct shared_use_st*)shm;
 }
@@ -45,7 +48,8 @@ void TorcsClient::sendCommand(Command cmd)
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
     }
 
-    std::cout << "Sent command" << std::endl;
+    ROS_INFO("Command acc: %f, brake: %f, steer: %f", cmd.accelCmd, cmd.brakeCmd, cmd.steerCmd);
+
     shared->accelCmd = cmd.accelCmd;
     shared->steerCmd = cmd.steerCmd;
     shared->brakeCmd = cmd.brakeCmd;