From 8ef40f258a7dc8cde3b3de82d38a0e80c2a5ba6e Mon Sep 17 00:00:00 2001
From: Svetlana Pavlitskaya <svetlana.pavlitskaya@rwth-aachen.de>
Date: Mon, 25 Jun 2018 01:05:59 +0200
Subject: [PATCH] Implemented screenshot resize on the cpp side. Fixed TORCS
 screenshot querying. Fixed TORCS screenshot&command coordination.

---
 .../src/main/models/dp/Mastercomponent.emadl  |  2 +-
 .../resources/torcs/src/camera/CMakeLists.txt |  3 +-
 .../resources/torcs/src/camera/src/main.cpp   | 31 ++++++++++++----
 .../torcs/src/camera/src/torcsclient.cpp      | 30 ++--------------
 .../torcs/src/camera/src/torcsclient.h        | 18 ++--------
 .../resources/torcs/src/driver/src/main.cpp   |  1 -
 .../torcs/src/driver/src/torcsclient.cpp      | 35 +++----------------
 7 files changed, 37 insertions(+), 83 deletions(-)

diff --git a/TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl b/TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl
index e0c8b09..c3fc685 100644
--- a/TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl
+++ b/TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl
@@ -3,7 +3,7 @@ import dp.subcomponents.*;
 
 component Mastercomponent {
    ports
-        in   Z(0:255)^{3, 240, 320}  imageIn,
+        in   Z(0:255)^{3, 210, 280}  imageIn,
         out  Q(0:1)^{3,1,1}        commandsOut;
 
    instance Imagepreprocessing ip;
diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/CMakeLists.txt b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/CMakeLists.txt
index 50ac0d9..55edd96 100644
--- a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/CMakeLists.txt
+++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/CMakeLists.txt
@@ -4,9 +4,10 @@ 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)
 
 set_property(TARGET camera PROPERTY CXX_STANDARD 14)
-target_link_libraries(camera ${roscpp_LIBRARIES} ${std_msgs_LIBRARIES})
+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})
diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/main.cpp b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/main.cpp
index 70b545f..67ab0d8 100644
--- a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/main.cpp
+++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/main.cpp
@@ -4,6 +4,14 @@
 
 #include <sstream>
 
+#include <ml.h>
+#include <highgui.h>
+#include <cv.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;
+
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "camera");
@@ -13,17 +21,26 @@ int main(int argc, char **argv)
     int count = 0;
 
     torcs::TorcsClient client;
-
     while (ros::ok())
     {
+        const auto screenshot = client.getScreenshot();
+
+        cv::Mat image(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
+        memcpy(image.data, screenshot.get(), 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));
+
+//      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", resizedImage);
+//        cv::waitKey(0);
+//        cv::destroyWindow("Display Image");
+
         std_msgs::Int8MultiArray::Ptr msg(new std_msgs::Int8MultiArray);
-	    msg->data.resize(torcs::IMAGE_SIZE_BYTES);
+	    msg->data.resize(ROS_IMAGE_SIZE);
+	    memcpy(msg->data.data(), resizedImage.data, ROS_IMAGE_SIZE);
 
-        constexpr auto SCALED_IMAGE_SIZE_BYTES = 280*210*3u;
-        const auto imageData = client.getScreenshot();
-        memcpy(msg->data.data(), imageData->data(), SCALED_IMAGE_SIZE_BYTES);
-        
-        ROS_INFO("%f", msg->data[0]);
         chatter_pub.publish(msg);
         ros::spinOnce();
         loop_rate.sleep();
diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.cpp b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.cpp
index 7e3d09a..8cbd026 100644
--- a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.cpp
+++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.cpp
@@ -35,34 +35,7 @@ TorcsClient::TorcsClient()
     std::cout << "Started shared memory at " << std::hex << shm << std::endl;
 
     shared = (struct shared_use_st*)shm;
-    shared->written = 0;
-    shared->control = 0;
-    shared->pause = 0;
-    shared->fast = 0.0;
-
-    shared->dist_L = 0.0;
-    shared->dist_R = 0.0;
-
-    shared->toMarking_L = 0.0;
-    shared->toMarking_M = 0.0;
-    shared->toMarking_R = 0.0;
 
-    shared->dist_LL = 0.0;
-    shared->dist_MM = 0.0;
-    shared->dist_RR = 0.0;
-
-    shared->toMarking_LL = 0.0;
-    shared->toMarking_ML = 0.0;
-    shared->toMarking_MR = 0.0;
-    shared->toMarking_RR = 0.0;
-
-    shared->toMiddle = 0.0;
-    shared->angle = 0.0;
-    shared->speed = 0.0;
-
-    shared->steerCmd = 0.0;
-    shared->accelCmd = 0.0;
-    shared->brakeCmd = 0.0;
 }
 
 std::unique_ptr<DataContainer> TorcsClient::getScreenshot()
@@ -74,8 +47,11 @@ std::unique_ptr<DataContainer> TorcsClient::getScreenshot()
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
     }
 
+    shared->pause = 1;
+    std::cout << "Got screenshot" << std::endl;
     auto result = std::make_unique<DataContainer>();
     memcpy(result->data(), shared->data, result->size());
+    shared->pause = 0;
     shared->written = 0;
 
     return result;
diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.h b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.h
index 7827d83..ef47160 100644
--- a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.h
+++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.h
@@ -7,9 +7,9 @@
 namespace torcs
 {
 
-constexpr auto IMAGE_WIDTH = 640;
-constexpr auto IMAGE_HEIGHT = 480;
-constexpr auto IMAGE_CHANNELS = 3;
+constexpr auto IMAGE_WIDTH = 640u;
+constexpr auto IMAGE_HEIGHT = 480u;
+constexpr auto IMAGE_CHANNELS = 3u;
 constexpr auto IMAGE_SIZE_BYTES = IMAGE_WIDTH*IMAGE_HEIGHT*IMAGE_CHANNELS;
 
 struct shared_use_st
@@ -45,18 +45,6 @@ struct shared_use_st
     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
diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/main.cpp b/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/main.cpp
index 2e81277..b21c193 100644
--- a/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/main.cpp
+++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/main.cpp
@@ -8,7 +8,6 @@ std::unique_ptr<torcs::TorcsClient> client;
 
 void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
 {
-    ROS_INFO("Received command. accel: %f, steer: %f, break: %f", msg->data[0], msg->data[1], msg->data[2]);
     client->sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2]));
 }
 
diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.cpp b/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.cpp
index a24a88a..e162499 100644
--- a/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.cpp
+++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.cpp
@@ -35,48 +35,21 @@ TorcsClient::TorcsClient()
     std::cout << "Started shared memory at " << std::hex << shm << std::endl;
 
     shared = (struct shared_use_st*)shm;
-    shared->written = 0;
-    shared->control = 0;
-    shared->pause = 0;
-    shared->fast = 0.0;
-
-    shared->dist_L = 0.0;
-    shared->dist_R = 0.0;
-
-    shared->toMarking_L = 0.0;
-    shared->toMarking_M = 0.0;
-    shared->toMarking_R = 0.0;
-
-    shared->dist_LL = 0.0;
-    shared->dist_MM = 0.0;
-    shared->dist_RR = 0.0;
-
-    shared->toMarking_LL = 0.0;
-    shared->toMarking_ML = 0.0;
-    shared->toMarking_MR = 0.0;
-    shared->toMarking_RR = 0.0;
-
-    shared->toMiddle = 0.0;
-    shared->angle = 0.0;
-    shared->speed = 0.0;
-
-    shared->steerCmd = 0.0;
-    shared->accelCmd = 0.0;
-    shared->brakeCmd = 0.0;
 }
 
 void TorcsClient::sendCommand(Command cmd)
 {
     assert(connected);
 
-    while (shared->control == 1) {
+    while (shared->written == 1) {
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
     }
 
+    std::cout << "Sent command" << std::endl;
     shared->accelCmd = cmd.accelCmd;
     shared->steerCmd = cmd.steerCmd;
     shared->brakeCmd = cmd.brakeCmd;
-
-    shared->control = 0;
+    shared->control = 1;
+    shared->written = 1;
 }
 
-- 
GitLab