Run TORCS client in a separate thread for correct driver controller functionality.

EMADL2TORCS/generate2ros.sh

@@ -11,8 +11,8 @@ cp -rf resources/scripts/* target/
 chmod u+x target/*.sh
 
 echo "Copying CNN weights..."
-cp -rf resources/torcs target/torcs
-chmod u+x resources/torcs/*.sh
+cp -rf resources/TORCSComponent target/TORCSComponent
+chmod u+x resources/TORCSComponent/*.sh
 cp -rf resources/dpnet target/model
 
 echo "Building generated code..."

EMADL2TORCS/resources/torcs/build.sh → EMADL2TORCS/resources/TORCSComponent/build.sh

@@ -2,9 +2,10 @@
 
 cd src
 
-cd camera
+cd TORCSComponent
 mkdir -p build
 cd build
 cmake ..
 make -j
 cd ../..
+

EMADL2TORCS/resources/torcs/src/camera/CMakeLists.txt → EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/CMakeLists.txt

 cmake_minimum_required(VERSION 3.5)
-project (ROSCamera)
+project (TORCSComponentProject)
 set (CXX_STANDARD_REQUIRED 14)
 
 find_package(roscpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(OpenCV REQUIRED)
 
-add_executable(camera src/main.cpp src/torcsclient.cpp)
+add_executable(TORCSComponent src/main.cpp src/torcsclient.cpp)
 
-set_property(TARGET camera PROPERTY CXX_STANDARD 14)
-target_link_libraries(camera ${roscpp_LIBRARIES} ${std_msgs_LIBRARIES} ${OpenCV_LIBS})
-target_include_directories(camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${roscpp_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS})
+set_property(TARGET TORCSComponent PROPERTY CXX_STANDARD 14)
+target_link_libraries(TORCSComponent ${roscpp_LIBRARIES} ${std_msgs_LIBRARIES} ${OpenCV_LIBS})
+target_include_directories(TORCSComponent PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${roscpp_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS})

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

+#include "torcsclient.h"
+
+#include <sstream>
+#include <thread>
+
+#include <ml.h>
+#include <highgui.h>
+#include <cv.hpp>
+#include <ros/ros.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Float32MultiArray.h>
+
+constexpr auto ROS_IMAGE_WIDTH = 280u;
+constexpr auto ROS_IMAGE_HEIGHT = 210u;
+constexpr auto ROS_IMAGE_SIZE = ROS_IMAGE_WIDTH*ROS_IMAGE_HEIGHT*3u;
+
+torcs::TorcsClient client;
+ros::Publisher* cameraPublisher;
+ros::Publisher* speedPublisher;
+ros::Publisher* affordancePublisher;
+
+void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
+{
+    client.sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2]));
+}
+
+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));
+    memcpy(image.data, screenshot->data(), torcs::IMAGE_SIZE_BYTES);
+
+    cv::Mat resizedImage(ROS_IMAGE_HEIGHT, ROS_IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
+    cv::resize(image, resizedImage, cv::Size(ROS_IMAGE_WIDTH, ROS_IMAGE_HEIGHT));
+
+    cv::Mat flippedImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
+    cv::flip(resizedImage, flippedImage, 0);
+
+    cv::Mat rgbImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
+    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 1", cv::WINDOW_AUTOSIZE );
+    //cv::imshow("Display Image 1", image);
+    //cv::waitKey(0);
+    //cv::destroyWindow("Display Image 1");
+
+
+    std::vector<float> data(0);
+    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);
+            for(size_t i=0; i<3; i++){
+                data.push_back((float) intensity[i]);
+            }
+        }
+    }
+
+    static_assert(sizeof(float) == sizeof(std_msgs::Float32), "Float is not 32 bit long!");
+
+    std_msgs::Float32MultiArray::Ptr msg(new std_msgs::Float32MultiArray);
+    msg->data.resize(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 = affordance.getSpeed();
+
+    std_msgs::Float32MultiArray::Ptr affordanceMsg(new std_msgs::Float32MultiArray);
+    affordanceMsg->data = affordance.getAsVector();
+
+    cameraPublisher->publish(msg);
+    speedPublisher->publish(speedMsg);
+    affordancePublisher->publish(affordanceMsg);
+}
+
+void clientRunner()
+{
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Wmissing-noreturn"
+    while(true)
+    {
+        client.iterate();
+    }
+#pragma clang diagnostic pop
+}
+
+int main(int argc, char **argv)
+{
+    client.setPublishCallback(publishCallback);
+    ros::init(argc, argv, "TORCSComponent");
+    ros::NodeHandle n;
+    ros::Publisher cameraPub = n.advertise<std_msgs::Float32MultiArray>("/camera", 1);
+    cameraPublisher = &cameraPub;
+    ros::Publisher speedPub = n.advertise<std_msgs::Float32>("/speed", 1);
+    speedPublisher = &speedPub;
+    ros::Publisher affordancePub = n.advertise<std_msgs::Float32MultiArray>("/affordance", 1);
+    affordancePublisher = &affordancePub;
+
+    ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback);
+    ros::Rate loop_rate(10);
+    std::thread clientRunnerThread(clientRunner);
+//    int count = 0;
+//
+//    while (ros::ok())
+//    {
+//
+//        client->iterate();
+//        ros::spinOnce();
+//        loop_rate.sleep();
+//        ++count;
+//    }
+
+    ros::spin();
+    clientRunnerThread.join();
+    return 0;
+}

EMADL2TORCS/resources/torcs/src/camera/src/torcsclient.cpp → EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.cpp

@@ -12,6 +12,7 @@ using namespace torcs;
 
 TorcsClient::TorcsClient()
 {
+    gotCommand = false;
     std::cout << "TORCS Client!" << std::endl;
 
     void *shm = NULL;
@@ -66,7 +67,11 @@ TorcsClient::TorcsClient()
     shared->steerCmd = 0.0;
     shared->accelCmd = 0.0;
     shared->brakeCmd = 0.0;
+}
 
+void TorcsClient::setPublishCallback(std::function<void(std::shared_ptr<DataContainer>, Affordance)> callback)
+{
+    publishCallback = callback;
 }
 
 void TorcsClient::sendCommand(const Command& cmd)
@@ -75,7 +80,7 @@ void TorcsClient::sendCommand(const Command& cmd)
     command = cmd;
 }
 
-void TorcsClient::iterate(std::unique_ptr<DataContainer>& screenshot, float& speed)
+void TorcsClient::iterate()
 {
     assert(connected);
 
@@ -83,25 +88,66 @@ void TorcsClient::iterate(std::unique_ptr<DataContainer>& screenshot, float& spe
 #pragma clang diagnostic ignored "-Wmissing-noreturn"
     while (true) {
         if (shared->written == 1) { // the new image data is ready to be read
+            auto screenshot = std::make_shared<DataContainer>();
             memcpy(screenshot->data(), shared->data, screenshot->size());
-            speed = float(shared->speed);
-
-            if (gotCommand && shared->control == 1) {
-                gotCommand = false;
-                ROS_INFO("Command acc: %f, brake: %f, steer: %f", command.accelCmd, command.brakeCmd, command.steerCmd);
-                shared->accelCmd = command.accelCmd;
-                shared->steerCmd = command.steerCmd;
-                shared->brakeCmd = command.brakeCmd;
-            }
+            shared->fast = 0.0;
+            Affordance affordance;
+            affordance.populate(*shared);
+            publishCallback(screenshot, affordance);
+//            ROS_INFO("Affordance:\n"
+//                     "dist_L: %f\n"
+//                     "dist_R: %f\n"
+//                     "dist_LL: %f\n"
+//                     "dist_MM: %f\n"
+//                     "dist_RR: %f\n"
+//                     "toMarking_L: %f\n"
+//                     "toMarking_M: %f\n"
+//                     "toMarking_R: %f\n"
+//                     "toMarking_LL: %f\n"
+//                     "toMarking_ML: %f\n"
+//                     "toMarking_MR: %f\n"
+//                     "toMarking_RR: %f\n"
+//                     "toMiddle: %f\n"
+//                     "angle: %f\n"
+//                     "fast: %f\n"
+//                     "speed: %f\n",
+//                     affordance.getDistL(),
+//                     affordance.getDistR(),
+//                     affordance.getDistLL(),
+//                     affordance.getDistMM(),
+//                     affordance.getDistRR(),
+//                     affordance.getToMarkingL(),
+//                     affordance.getToMarkingM(),
+//                     affordance.getToMarkingR(),
+//                     affordance.getToMarkingLL(),
+//                     affordance.getToMarkingML(),
+//                     affordance.getToMarkingMR(),
+//                     affordance.getToMarkingRR(),
+//                     affordance.getToMiddle(),
+//                     affordance.getAngle(),
+//                     affordance.getFast(),
+//                     affordance.getSpeed());
 
             shared->written = 0;
             shared->pause = 0;
-            break;
         } else {
             if (shared->pause != 1) {
                 shared->pause = 1;
             }
         }
+
+        if (shared->control == 1) {
+            if (gotCommand) {
+                gotCommand = false;
+                ROS_INFO("Command acc: %f, brake: %f, steer: %f", command.accelCmd, command.brakeCmd, command.steerCmd);
+                shared->accelCmd = command.accelCmd;
+                shared->steerCmd = command.steerCmd;
+                shared->brakeCmd = command.brakeCmd;
+            } else {
+                //ROS_INFO("No command");
+                //shared->steerCmd = 0.0;
+            }
+        }
     }
 #pragma clang diagnostic pop
 }

EMADL2TORCS/resources/torcs/src/camera/src/torcsclient.h → EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.h

@@ -3,6 +3,8 @@
 #include <cstdint>
 #include <array>
 #include <memory>
+#include <functional>
+#include <std_msgs/Float32MultiArray.h>
 
 namespace torcs
 {
@@ -60,20 +62,76 @@ struct Command
 
 using DataContainer = std::array<uint8_t, IMAGE_SIZE_BYTES>;
 
+class Affordance
+{
+public:
+    void populate(const shared_use_st& affordance)
+    {
+        affordance_[0] = float(affordance.fast);
+        affordance_[1] = float(affordance.dist_L);
+        affordance_[2] = float(affordance.dist_R);
+        affordance_[3] = float(affordance.toMarking_L);
+        affordance_[4] = float(affordance.toMarking_M);
+        affordance_[5] = float(affordance.toMarking_R);
+        affordance_[6] = float(affordance.dist_LL);
+        affordance_[7] = float(affordance.dist_MM);
+        affordance_[8] = float(affordance.dist_RR);
+        affordance_[9] = float(affordance.toMarking_LL);
+        affordance_[10] = float(affordance.toMarking_ML);
+        affordance_[11] = float(affordance.toMarking_MR);
+        affordance_[12] = float(affordance.toMarking_RR);
+        affordance_[13] = float(affordance.toMiddle); // not used
+        affordance_[14] = float(affordance.angle);
+        affordance_[15] = float(affordance.speed);
+    }
+
+    void populate(const std_msgs::Float32MultiArray& affordance)
+    {
+        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]; }
+
+    const std::vector<float>& getAsVector()
+    {
+        return affordance_;
+    }
+
+private:
+    std::vector<float> affordance_ = std::vector<float>(16);
+};
+
 class TorcsClient
 {
 public:
-    TorcsClient();
+    explicit TorcsClient();
 
-    void iterate(std::unique_ptr<DataContainer>& screenshot, float& speed);
+    void setPublishCallback(std::function<void(std::shared_ptr<DataContainer>, Affordance)> callback);
+    void iterate();
     void sendCommand(const Command& cmd);
 
 private:
 
     struct shared_use_st *shared;
     bool connected = false;
-    bool gotCommand = false;
+    std::atomic<bool> gotCommand;
     Command command;
+    std::function<void(std::shared_ptr<DataContainer>, Affordance)> publishCallback;
 };
 
 }

EMADL2TORCS/resources/scripts/build_all.sh

@@ -8,8 +8,8 @@ cmake ..
 make -j
 cd ../..
 
-echo "Building TORCS adapters"
-cd torcs
+echo "Building TORCSComponent"
+cd TORCSComponent
 ./build.sh
 
 echo

EMADL2TORCS/resources/scripts/run_all.sh

@@ -5,6 +5,6 @@ xterm -T ROSCORE -e roscore &
 echo "Waiting for ROSCORE to start..."
 sleep 2
 
-xterm -T TORCSCamera -e torcs/src/camera/build/camera &
+xterm -T TORCSComponent -e TORCSComponent/src/TORCSComponent/build/TORCSComponent &
 
-xterm -T Mastercomponent -e Mastercomponent/build/coordinator/Coordinator_dp_mastercomponent &
+xterm -T Mastercomponent -e Mastercomponent/build/dp_mastercomponent/coordinator/Coordinator_dp_mastercomponent &

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

-#include "torcsclient.h"
-
-#include <sstream>
-
-#include <ml.h>
-#include <highgui.h>
-#include <cv.hpp>
-#include <ros/ros.h>
-#include <std_msgs/Float32.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::TorcsClient client;
-std::unique_ptr<torcs::DataContainer> screenshot = std::make_unique<torcs::DataContainer>();
-
-void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
-{
-    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 cameraPub = n.advertise<std_msgs::Float32MultiArray>("/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;
-
-    while (ros::ok())
-    {
-        float speed;
-        client.iterate(screenshot, speed);
-
-        cv::Mat image(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
-        memcpy(image.data, screenshot->data(), torcs::IMAGE_SIZE_BYTES);
-
-        cv::Mat resizedImage(ROS_IMAGE_HEIGHT, ROS_IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
-        cv::resize(image, resizedImage, cv::Size(ROS_IMAGE_WIDTH, ROS_IMAGE_HEIGHT));
-
-        cv::Mat flippedImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
-        cv::flip(resizedImage, flippedImage, 0);
-
-        cv::Mat rgbImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
-        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 1", cv::WINDOW_AUTOSIZE );
-        //cv::imshow("Display Image 1", image);
-        //cv::waitKey(0);
-        //cv::destroyWindow("Display Image 1");
-
-
-        std::vector<float> data(0);
-        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);
-                for(size_t i=0; i<3; i++){
-                    data.push_back((float) intensity[i]);
-                }
-            }
-        }
-
-        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*sizeof(float));
-
-	    std_msgs::Float32::Ptr speedMsg(new std_msgs::Float32);
-	    speedMsg->data = speed;
-
-        cameraPub.publish(msg);
-        speedPub.publish(speedMsg);
-        ros::spinOnce();
-        loop_rate.sleep();
-        ++count;
-    }
-    return 0;
-}