diff --git a/.gitignore b/.gitignore index 1ecb03ee6623c5c559eeb88dbb9ae3a4b74b075d..2ff85ff999588590160bd62c3f7f875b1eeaa953 100644 --- a/.gitignore +++ b/.gitignore @@ -37,3 +37,4 @@ torcs/src/tools/package/specfiles/torcs-robot-base.spec torcs/src/tools/package/specfiles/torcs.spec torcs/src/tools/texmapper/texmapper-bin torcs/src/tools/trackgen/trackgen-bin +/TorcsClient/build/ \ No newline at end of file diff --git a/TorcsClient/CMakeLists.txt b/TorcsClient/CMakeLists.txt index c1e1b3035f7853de0e11f24510971894c5352d5b..6bc925a73016b2ceeaf35d7968abbed2a643eda9 100644 --- a/TorcsClient/CMakeLists.txt +++ b/TorcsClient/CMakeLists.txt @@ -5,12 +5,15 @@ project(TorcsClient) SET(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") SET(CMAKE_CXX_STANDARD 14) -message(${CMAKE_MODULE_PATH}) +find_package(roscpp REQUIRED) +find_package(std_msgs REQUIRED) -find_package(FreeImage) +#find_package(FreeImage) INCLUDE_DIRECTORIES("${CMAKE_SOURCE_DIR}/../experiments/output/cpp") -add_executable(${PROJECT_NAME} "main.cpp") +add_executable(${PROJECT_NAME} torcs.cpp torcsclient.cpp) -target_link_libraries( TorcsClient freeimage) +target_link_libraries(TorcsClient freeimage ${roscpp_LIBRARIES} ${std_msgs_LIBRARIES}) +target_include_directories(TorcsClient PUBLIC ${roscpp_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS}) +set_target_properties(TorcsClient PROPERTIES LINKER_LANGUAGE CXX) diff --git a/TorcsClient/CMakeLists.txt.user b/TorcsClient/CMakeLists.txt.user deleted file mode 100644 index 811f9b53e1b094b08695c56f7f5b85d8581c68de..0000000000000000000000000000000000000000 --- a/TorcsClient/CMakeLists.txt.user +++ /dev/null @@ -1,251 +0,0 @@ - - - - - - EnvironmentId - {a358912b-8071-419f-839b-8836ba6c9080} - - - ProjectExplorer.Project.ActiveTarget - 0 - - - ProjectExplorer.Project.EditorSettings - - true - false - true - - Cpp - - CppGlobal - - - - QmlJS - - QmlJSGlobal - - - 2 - UTF-8 - false - 4 - false - 80 - true - true - 1 - true - false - 0 - true - true - 0 - 8 - true - 1 - true - true - true - false - - - - ProjectExplorer.Project.PluginSettings - - - - ProjectExplorer.Project.Target.0 - - Desktop - Desktop - {ec686e2a-51dd-4f8d-9605-fe4379ba79aa} - 0 - 0 - 0 - - - CMAKE_BUILD_TYPE:STRING=Debug - CMAKE_CXX_COMPILER:STRING=%{Compiler:Executable:Cxx} - CMAKE_C_COMPILER:STRING=%{Compiler:Executable:C} - CMAKE_EXE_LINKER_FLAGS:STRING= - CMAKE_MODULE_LINKER_FLAGS:STRING=-lfreeimage - CMAKE_PREFIX_PATH:STRING=%{Qt:QT_INSTALL_PREFIX} - CMAKE_SHARED_LINKER_FLAGS:STRING= - CMAKE_STATIC_LINKER_FLAGS:STRING= - FreeImage_DIR:PATH=cmake - QT_QMAKE_EXECUTABLE:STRING=%{Qt:qmakeExecutable} - - /home/alex/MA_sveta/build-TorcsClient-Desktop-Debug - - - - - all - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Build - - ProjectExplorer.BuildSteps.Build - - - - - - clean - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Clean - - ProjectExplorer.BuildSteps.Clean - - 2 - false - - Debug - Debug - CMakeProjectManager.CMakeBuildConfiguration - - - - CMAKE_BUILD_TYPE:STRING=Release - - /home/alex/MA_sveta/build-TorcsClient-Desktop-Release - - - - - all - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Build - - ProjectExplorer.BuildSteps.Build - - - - - - clean - - true - CMake Build - - CMakeProjectManager.MakeStep - - 1 - Clean - - ProjectExplorer.BuildSteps.Clean - - 2 - false - - Release - Release - CMakeProjectManager.CMakeBuildConfiguration - - 2 - - - 0 - Deploy - - ProjectExplorer.BuildSteps.Deploy - - 1 - Deploy locally - - ProjectExplorer.DefaultDeployConfiguration - - 1 - - - false - false - 1000 - - true - - false - false - false - false - true - 0.01 - 10 - true - 1 - 25 - - 1 - true - false - true - valgrind - - 0 - 1 - 2 - 3 - 4 - 5 - 6 - 7 - 8 - 9 - 10 - 11 - 12 - 13 - 14 - - TorcsClient - - - /tmp/QtCreator-ViQ1rd/qtc-cmake-XXpJzi57 - 2 - - TorcsClient - - CMakeProjectManager.CMakeRunConfiguration.TorcsClient - 3768 - false - true - false - false - true - - 1 - - - - ProjectExplorer.Project.TargetCount - 1 - - - ProjectExplorer.Project.Updater.FileVersion - 18 - - - Version - 18 - - diff --git a/TorcsClient/cmake_install.cmake b/TorcsClient/cmake_install.cmake new file mode 100644 index 0000000000000000000000000000000000000000..28b847dc9e9b9aefb4d9dcc29e484f19332ad281 --- /dev/null +++ b/TorcsClient/cmake_install.cmake @@ -0,0 +1,44 @@ +# Install script for directory: /home/alex/MA_sveta/torcs_dl/TorcsClient + +# Set the install prefix +if(NOT DEFINED CMAKE_INSTALL_PREFIX) + set(CMAKE_INSTALL_PREFIX "/usr/local") +endif() +string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") + +# Set the install configuration name. +if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) + if(BUILD_TYPE) + string(REGEX REPLACE "^[^A-Za-z0-9_]+" "" + CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") + else() + set(CMAKE_INSTALL_CONFIG_NAME "Debug") + endif() + message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") +endif() + +# Set the component getting installed. +if(NOT CMAKE_INSTALL_COMPONENT) + if(COMPONENT) + message(STATUS "Install component: \"${COMPONENT}\"") + set(CMAKE_INSTALL_COMPONENT "${COMPONENT}") + else() + set(CMAKE_INSTALL_COMPONENT) + endif() +endif() + +# Install shared libraries without execute permission? +if(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) + set(CMAKE_INSTALL_SO_NO_EXE "1") +endif() + +if(CMAKE_INSTALL_COMPONENT) + set(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt") +else() + set(CMAKE_INSTALL_MANIFEST "install_manifest.txt") +endif() + +string(REPLACE ";" "\n" CMAKE_INSTALL_MANIFEST_CONTENT + "${CMAKE_INSTALL_MANIFEST_FILES}") +file(WRITE "/home/alex/MA_sveta/torcs_dl/TorcsClient/${CMAKE_INSTALL_MANIFEST}" + "${CMAKE_INSTALL_MANIFEST_CONTENT}") diff --git a/TorcsClient/compile_commands.json b/TorcsClient/compile_commands.json new file mode 100644 index 0000000000000000000000000000000000000000..970799ecf106c02f15c6cfe3d9e8397b3cf78079 --- /dev/null +++ b/TorcsClient/compile_commands.json @@ -0,0 +1,7 @@ +[ +{ + "directory": "/home/alex/MA_sveta/torcs_dl/TorcsClient", + "command": "/usr/bin/g++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_PACKAGE_NAME=\\\"TorcsClient\\\" -I/home/alex/MA_sveta/torcs_dl/TorcsClient/../experiments/output/cpp -I/opt/ros/lunar/include -I/opt/ros/lunar/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -g3 -gdwarf-2 -std=gnu++14 -o CMakeFiles/TorcsClient.dir/main.cpp.o -c /home/alex/MA_sveta/torcs_dl/TorcsClient/main.cpp", + "file": "/home/alex/MA_sveta/torcs_dl/TorcsClient/main.cpp" +} +] \ No newline at end of file diff --git a/TorcsClient/torcs.cpp b/TorcsClient/torcs.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0b22698413aff906b401c9c018e6dceab02b7d71 --- /dev/null +++ b/TorcsClient/torcs.cpp @@ -0,0 +1,47 @@ +#include +#include +#include + +#include "torcsclient.h" + +#include + +torcs::TorcsClient client; + +void commandMsgCallback(const std_msgs::Float32MultiArray::ConstPtr& msg) +{ + torcs::Command cmd; + cmd.accelCmd = msg->data[0]; + cmd.steerCmd = msg->data[1]; + cmd.brakeCmd = msg->data[2]; + ROS_INFO("Received command."); + ROS_INFO(" accel: %f", cmd.accelCmd); + ROS_INFO(" steer: %f", cmd.steerCmd); + ROS_INFO(" break: %f", cmd.brakeCmd); + + client.sendCommand(cmd); +} + +int main(int argc, char **argv) { + + ros::init(argc, argv, "torcs"); + + ros::NodeHandle n; + ros::Subscriber sub = n.subscribe("/commands", 1000, commandMsgCallback); + ros::Publisher pub = n.advertise("/camera", 1000); + ros::Rate loop_rate(100); + + while (ros::ok()) { + std_msgs::Int8MultiArray msg; + + msg.data.clear(); + msg.data.resize(torcs::IMAGE_SIZE_BYTES); + const auto imageData = client.getScreenshot(); + memcpy(msg.data.data(), imageData->data(), imageData->size()); + pub.publish(msg); + loop_rate.sleep(); + } + + + return 0; +} diff --git a/TorcsClient/main.cpp b/TorcsClient/torcsclient.cpp similarity index 52% rename from TorcsClient/main.cpp rename to TorcsClient/torcsclient.cpp index 2621ac63cbb04769fcc646ff352d8b2aa015fa6e..83e4e96a2167365f560021136c02eda7d8e27fa4 100644 --- a/TorcsClient/main.cpp +++ b/TorcsClient/torcsclient.cpp @@ -1,109 +1,36 @@ +#include "torcsclient.h" + #include #include #include +#include #include +#include -#include - -constexpr auto IMAGE_WIDTH = 640; -constexpr auto IMAGE_HEIGHT = 480; -constexpr auto IMAGE_CHANNELS = 3; - -using namespace std::chrono_literals; - -struct shared_use_st -{ - int written = 0; //a label, if 1: available to read, if 0: available to write - uint8_t data[IMAGE_WIDTH*IMAGE_HEIGHT*IMAGE_CHANNELS]; // 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; -}; - -void writeScreenshot(shared_use_st* shared, unsigned int& num) -{ - std::cout << "Read image." << std::endl; - - FIBITMAP* newShot = FreeImage_Allocate(IMAGE_WIDTH, IMAGE_HEIGHT, 24); - if (!newShot) - { - std::cerr << "Failed to allocate freeimage file." << std::endl; - FreeImage_Unload(newShot); - return; - } - - BYTE *bits = (BYTE*)FreeImage_GetBits(newShot); - - for (int h = 0; h < IMAGE_HEIGHT; h++) { - BYTE *pixel = (BYTE*)bits; - - for (int w = 0; w < IMAGE_WIDTH; w++) { - pixel[FI_RGBA_RED] = shared->data[((h)*IMAGE_WIDTH+w)*3+0]; - pixel[FI_RGBA_GREEN] = shared->data[((h)*IMAGE_WIDTH+w)*3+1]; - pixel[FI_RGBA_BLUE] = shared->data[((h)*IMAGE_WIDTH+w)*3+2]; - pixel += 3; - } - bits += 3*IMAGE_WIDTH; - } - - FreeImage_Save(FIF_BMP, newShot, (std::string("shot-")+std::to_string(num)).c_str()); - FreeImage_Unload(newShot); - - shared->control = 0; - shared->written = 0; - std::this_thread::sleep_for(std::chrono::seconds(2));std::cout << "Read image." << std::endl; +using namespace torcs; - shared->control = 0; - shared->written = 0; -} - -int main() +TorcsClient::TorcsClient() { std::cout << "TORCS Client!" << std::endl; - FreeImage_Initialise(); - void *shm = NULL; - struct shared_use_st *shared; 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; - exit(EXIT_FAILURE); + return; } shm = shmat(shmid, 0, 0); if(shm == (void*)-1) { std::cerr << "Failed to shmat()!" << std::endl; - exit(EXIT_FAILURE); + return; } + + connected = true; std::cout << "Started shared memory at " << std::hex << shm << std::endl; @@ -136,36 +63,69 @@ int main() shared->steerCmd = 0.0; shared->accelCmd = 0.0; shared->brakeCmd = 0.0; +} - unsigned num = 0; - while (1) { - if (shared->control == 1) { - std::cout << "Control == 1!" << std::endl; - shared->control = 1; - shared->accelCmd = 10.0; - shared->steerCmd = 1.0; - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - } else - { - std::cout << "Control == 0!" << std::endl; - shared->control = 1; - shared->accelCmd = 10.0; - shared->steerCmd = 1.0; - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - } - if (shared->written == 1) { - //writeScreenshot(shared, num); +std::unique_ptr TorcsClient::getScreenshot() +{ + assert(connected); + + while(shared->written == 0) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + auto result = std::make_unique(); + memcpy(result->data(), shared->data, result->size()); + shared->written = 0; + + return result; +} + +void TorcsClient::sendCommand(Command cmd) +{ + assert(connected); + + while (shared->control == 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + shared->accelCmd = cmd.accelCmd; + shared->steerCmd = cmd.steerCmd; + shared->brakeCmd = cmd.brakeCmd; + shared->control = 0; +} - std::cout << "Speed: " << shared->speed << std::endl; - std::cout << "Steer: " << shared->angle << std::endl; - std::cout << "ToMiddle: " << shared->toMiddle << std::endl; +/*void writeScreenshot(shared_use_st* shared, unsigned int& num) +{ + std::cout << "Read image." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(1)); - num++; + FIBITMAP* newShot = FreeImage_Allocate(IMAGE_WIDTH, IMAGE_HEIGHT, 24); + if (!newShot) + { + std::cerr << "Failed to allocate freeimage file." << std::endl; + FreeImage_Unload(newShot); + return; + } + + BYTE *bits = (BYTE*)FreeImage_GetBits(newShot); + + for (int h = 0; h < IMAGE_HEIGHT; h++) { + BYTE *pixel = (BYTE*)bits; + + for (int w = 0; w < IMAGE_WIDTH; w++) { + pixel[FI_RGBA_RED] = shared->data[((h)*IMAGE_WIDTH+w)*3+0]; + pixel[FI_RGBA_GREEN] = shared->data[((h)*IMAGE_WIDTH+w)*3+1]; + pixel[FI_RGBA_BLUE] = shared->data[((h)*IMAGE_WIDTH+w)*3+2]; + pixel += 3; } + bits += 3*IMAGE_WIDTH; } - FreeImage_DeInitialise(); + FreeImage_Save(FIF_BMP, newShot, (std::string("shot-")+std::to_string(num)).c_str()); + FreeImage_Unload(newShot); + + shared->control = 0; + shared->written = 0; + std::this_thread::sleep_for(std::chrono::seconds(2));std::cout << "Read image." << std::endl; +}*/ - return 0; -} diff --git a/TorcsClient/torcsclient.h b/TorcsClient/torcsclient.h new file mode 100644 index 0000000000000000000000000000000000000000..566acae5795a20f5f7a63bc700e18ca159a09981 --- /dev/null +++ b/TorcsClient/torcsclient.h @@ -0,0 +1,70 @@ +#pragma once + +#include +#include +#include + +namespace torcs +{ + +constexpr auto IMAGE_WIDTH = 640; +constexpr auto IMAGE_HEIGHT = 480; +constexpr auto IMAGE_CHANNELS = 1; +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 +{ + double steerCmd = 0.0; + double accelCmd = 0.0; + double brakeCmd = 0.0; +}; + +using DataContainer = std::array; + +class TorcsClient +{ +public: + TorcsClient(); + std::unique_ptr getScreenshot(); + void sendCommand(Command cmd); + +private: + struct shared_use_st *shared; + bool connected = false; +}; + +} + diff --git a/TorcsEMAMGenerator/.idea/libraries/Maven__de_monticore_lang_NumberUnit_0_0_10_SNAPSHOT.xml b/TorcsEMAMGenerator/.idea/libraries/Maven__de_monticore_lang_NumberUnit_0_0_10_SNAPSHOT.xml deleted file mode 100644 index 906fed965a615132ddfc615d9cc4c78c6fe56b79..0000000000000000000000000000000000000000 --- a/TorcsEMAMGenerator/.idea/libraries/Maven__de_monticore_lang_NumberUnit_0_0_10_SNAPSHOT.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - \ No newline at end of file diff --git a/TorcsEMAMGenerator/.idea/libraries/Maven__de_monticore_lang_monticar_embedded_montiarc_math_0_0_10_SNAPSHOT.xml b/TorcsEMAMGenerator/.idea/libraries/Maven__de_monticore_lang_monticar_embedded_montiarc_math_0_0_10_SNAPSHOT.xml deleted file mode 100644 index 674d52fbd1d74bd91d0251aa0dbf7e1129bcf25f..0000000000000000000000000000000000000000 --- a/TorcsEMAMGenerator/.idea/libraries/Maven__de_monticore_lang_monticar_embedded_montiarc_math_0_0_10_SNAPSHOT.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - \ No newline at end of file diff --git a/TorcsEMAMGenerator/.idea/workspace.xml b/TorcsEMAMGenerator/.idea/workspace.xml deleted file mode 100644 index 4b85ead829bfefc9708a1391d3e5d226bdb3c95c..0000000000000000000000000000000000000000 --- a/TorcsEMAMGenerator/.idea/workspace.xml +++ /dev/null @@ -1,1043 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - -