Removed obsolete TORCS driver, fixed TORCS communication.

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

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

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

 #!/bin/bash
 
-#source devel/setup.bash#
-#catkin_make
-
 cd src
 
 cd camera
@@ -11,11 +8,3 @@ 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

@@ -49,10 +49,10 @@ 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 1", cv::WINDOW_AUTOSIZE );
-//        cv::imshow("Display Image 1", rgbImage);
-//        cv::waitKey(0);
-//        cv::destroyWindow("Display Image 1");
+        //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);

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

@@ -35,7 +35,7 @@ TorcsClient::TorcsClient()
     
     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;
     shared->written = 0; // Equals 1 when screenshot data is written and can bea read
@@ -86,8 +86,7 @@ void TorcsClient::iterate(std::unique_ptr<DataContainer>& screenshot, float& spe
             memcpy(screenshot->data(), shared->data, screenshot->size());
             speed = float(shared->speed);
 
-            if (gotCommand && shared->control == 1)
-            {
+            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;
@@ -96,7 +95,12 @@ void TorcsClient::iterate(std::unique_ptr<DataContainer>& screenshot, float& spe
             }
 
             shared->written = 0;
+            shared->pause = 0;
             break;
+        } else {
+            if (shared->pause != 1) {
+                shared->pause = 1;
+            }
         }
     }
 #pragma clang diagnostic pop

TorcsEMAMGenerator/src/main/resources/torcs/src/driver/CMakeLists.txt

-cmake_minimum_required(VERSION 3.5)
-project (ROSDriver)
-set (CXX_STANDARD_REQUIRED 11)
-
-find_package(roscpp REQUIRED)
-find_package(std_msgs REQUIRED)
-
-add_executable(driver src/main.cpp src/torcsclient.cpp)
-
-set_property(TARGET driver PROPERTY CXX_STANDARD 14)
-target_link_libraries(driver ${roscpp_LIBRARIES} ${std_msgs_LIBRARIES})
-target_include_directories(driver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${roscpp_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS})

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

-#include "ros/ros.h"
-#include "std_msgs/Float32MultiArray.h"
-#include "torcsclient.h"
-
-#include <memory>
-
-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]));
-}
-
-int main(int argc, char **argv)
-{
-    client = std::make_unique<torcs::TorcsClient>();
-    
-    ros::init(argc, argv, "driver");
-
-    ros::NodeHandle n;
-
-    ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback);
-    ros::spin();
-
-    return 0;
-}

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

-#include "torcsclient.h"
-
-#include <iostream>
-#include <chrono>
-#include <thread>
-#include <cstring>
-#include <sys/shm.h>
-#include <assert.h>
-#include <ros/console.h>
-
-using namespace torcs;
-
-TorcsClient::TorcsClient()
-{
-    std::cout << "TORCS Client!" << std::endl;
-
-    void *shm = NULL;
-    int shmid;
-
-    shmid = shmget((key_t)4567, sizeof(struct shared_use_st), 0666);
-    if(shmid < 0)
-    {
-        //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)
-    {
-        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;
-
-    shared = (struct shared_use_st*)shm;
-}
-
-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;
-}
-

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

-#pragma once
-
-#include <cstdint>
-#include <array>
-#include <memory>
-
-namespace torcs
-{
-
-constexpr auto IMAGE_WIDTH = 640;
-constexpr auto IMAGE_HEIGHT = 480;
-constexpr auto IMAGE_CHANNELS = 3;
-constexpr auto IMAGE_SIZE_BYTES = IMAGE_WIDTH*IMAGE_HEIGHT*IMAGE_CHANNELS;
-
-struct shared_use_st
-{
-    int written = 0;  //a label, if 1: available to read, if 0: available to write
-    uint8_t data[IMAGE_SIZE_BYTES];  // image data field
-    int control = 0;
-    int pause = 0;
-    double fast = 0.0;
-
-    double dist_L = 0.0;
-    double dist_R = 0.0;
-
-    double toMarking_L = 0.0;
-    double toMarking_M = 0.0;
-    double toMarking_R = 0.0;
-
-    double dist_LL = 0.0;
-    double dist_MM = 0.0;
-    double dist_RR = 0.0;
-
-    double toMarking_LL = 0.0;
-    double toMarking_ML = 0.0;
-    double toMarking_MR = 0.0;
-    double toMarking_RR = 0.0;
-
-    double toMiddle = 0.0;
-    double angle = 0.0;
-    double speed = 0.0;
-
-    double steerCmd = 0.0;
-    double accelCmd = 0.0;
-    double brakeCmd = 0.0;
-};
-
-struct Command
-{
-    Command(float accel, float steer, float brakec)
-        : accelCmd(accel), steerCmd(steer), brakeCmd(brakec)
-    {
-    }
-
-    float accelCmd;
-    float steerCmd;
-    float brakeCmd;
-};
-
-using DataContainer = std::array<uint8_t, IMAGE_SIZE_BYTES>;
-
-class TorcsClient
-{
-public:
-    TorcsClient();
-    void sendCommand(Command cmd);
-
-private:
-    struct shared_use_st *shared;
-    bool connected = false;
-};
-
-}
-