Merge branch 'corrected_get_torcs_image' into 'master'

Corrected way to get torcs image from shared memory

See merge request !3

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

@@ -35,39 +35,42 @@ void predictionsChatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg
     client.visualizePredictions(affordance);
 }
 
-void publishCallback(std::shared_ptr<torcs::DataContainer> screenshot, torcs::Affordance affordance)
+void publishCallback(std::vector<uint8_t> screenshot, torcs::Affordance affordance)
 {
-    cv::Mat image(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC3);
-    memcpy(image.data, screenshot->data(), torcs::IMAGE_SIZE_BYTES);
 
-    cv::Mat flippedImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC3);
-    cv::flip(image, flippedImage, 0);
+    cv::Mat screenRGB(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC3);
+    cv::Mat resizeRGB(ROS_IMAGE_HEIGHT, ROS_IMAGE_WIDTH, CV_8UC3);
 
-    cv::Mat resizedImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC3);
-    cv::resize(flippedImage, resizedImage, cv::Size(ROS_IMAGE_WIDTH, ROS_IMAGE_HEIGHT));
+    uchar* ptr = screenRGB.data;
+    for (int h = 0; h < torcs::IMAGE_HEIGHT; h++) {
+      for (int w = 0; w < torcs::IMAGE_WIDTH; w++) {
+        ptr[(h*torcs::IMAGE_WIDTH+w)*3+2]=screenshot.data()[((torcs::IMAGE_HEIGHT-h-1)*torcs::IMAGE_WIDTH+w)*3+0];
+        ptr[(h*torcs::IMAGE_WIDTH+w)*3+1]=screenshot.data()[((torcs::IMAGE_HEIGHT-h-1)*torcs::IMAGE_WIDTH+w)*3+1];
+        ptr[(h*torcs::IMAGE_WIDTH+w)*3+0]=screenshot.data()[((torcs::IMAGE_HEIGHT-h-1)*torcs::IMAGE_WIDTH+w)*3+2];
+      }
+    }
+
+    //Code for debugging: displays screenshot in a window
+    //cv::namedWindow("Screenshot", cv::WINDOW_AUTOSIZE);
+    //cv::imshow("Screenshot", screenRGB);
 
-    cv::Mat rgbImage(ROS_IMAGE_HEIGHT, ROS_IMAGE_WIDTH, CV_8UC3);
-    cv::cvtColor(resizedImage, rgbImage, cv::COLOR_BGR2RGB);
+    cv::resize(screenRGB, resizeRGB, resizeRGB.size(), 0, 0);
 
-    //Code for debugging: displays screenshot in a window and waits for a key stroke to close it and get new one
-    cv::namedWindow("Screenshot", cv::WINDOW_AUTOSIZE );
-    cv::imshow("Screenshot", rgbImage);
+    //Code for debugging: displays screenshot in a window
+    cv::namedWindow("Screenshot", cv::WINDOW_AUTOSIZE);
+    cv::imshow("Screenshot", resizeRGB);
 
     std::vector<uint8_t> data(ROS_IMAGE_SIZE);
     size_t i = 0;
     for (int c = 0; c < 3; ++c) {
       for (int h = 0; h < ROS_IMAGE_HEIGHT; ++h) {
         for (int w = 0; w < ROS_IMAGE_WIDTH; ++w) {
-          data[i] = rgbImage.data[(h*ROS_IMAGE_WIDTH+w)*3+c];
+          data[i] = resizeRGB.data[(h*ROS_IMAGE_WIDTH+w)*3+c];
           i++;
         }
       }
     }
 
-    cv::Mat imageForRos(ROS_IMAGE_HEIGHT, ROS_IMAGE_WIDTH, CV_8UC3);
-    memcpy(imageForRos.data, data.data(), data.size());
-    cv::namedWindow("Image for ROS", cv::WINDOW_AUTOSIZE );
-    cv::imshow("Image for ROS", imageForRos);
 
     static_assert(sizeof(float) == sizeof(std_msgs::Float32), "Float is not 32 bit long!");
 

EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.cpp

@@ -135,7 +135,7 @@ void TorcsClient::visualizePredictions(const torcs::Affordance &affordance)
               affordance.getDistR(), true_dist_R);
 }
 
-void TorcsClient::setPublishCallback(std::function<void(std::shared_ptr<DataContainer>, Affordance)> callback)
+void TorcsClient::setPublishCallback(std::function<void(std::vector<uint8_t>, Affordance)> callback)
 {
     publishCallback = callback;
 }
@@ -189,8 +189,8 @@ void TorcsClient::iterate()
             else
             {
                 readyForNewShot = false;
-                auto screenshot = std::make_shared<DataContainer>();
-                memcpy(screenshot->data(), shared->data, screenshot->size());
+                std::vector<uint8_t> screenshot(torcs::IMAGE_SIZE_BYTES);
+                memcpy(screenshot.data(), shared->data, screenshot.size());
                 shared->written = 0;
                 Affordance affordance;
                 affordance.populate(*shared); // ground truth affordance

EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.h

@@ -9,16 +9,16 @@
 
 #define DEBUG_LOG false
 
-constexpr auto ROS_IMAGE_WIDTH = 280u;
-constexpr auto ROS_IMAGE_HEIGHT = 210u;
-constexpr auto ROS_IMAGE_SIZE = ROS_IMAGE_WIDTH*ROS_IMAGE_HEIGHT*3u;
+constexpr auto ROS_IMAGE_WIDTH = 280;
+constexpr auto ROS_IMAGE_HEIGHT = 210;
+constexpr auto ROS_IMAGE_SIZE = ROS_IMAGE_WIDTH*ROS_IMAGE_HEIGHT*3;
 
 namespace torcs
 {
 
-constexpr auto IMAGE_WIDTH = 640u;
-constexpr auto IMAGE_HEIGHT = 480u;
-constexpr auto IMAGE_CHANNELS = 3u;
+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;
 
 constexpr auto SEMANTIC_WIDTH = 320;
@@ -132,7 +132,7 @@ public:
     explicit TorcsClient();
     ~TorcsClient();
 
-    void setPublishCallback(std::function<void(std::shared_ptr<DataContainer>, Affordance)> callback);
+    void setPublishCallback(std::function<void(std::vector<uint8_t>, Affordance)> callback);
     void iterate();
     void sendCommand(const Command& cmd);
     void setLanesCount(int count);
@@ -144,7 +144,7 @@ private:
     bool connected = false;
     std::atomic<bool> gotCommand;
     Command command;
-    std::function<void(std::shared_ptr<DataContainer>, Affordance)> publishCallback;
+    std::function<void(std::vector<uint8_t>, Affordance)> publishCallback;
     int lanesCount = 3; // Use three lanes configuration by default
 
     void visualize(float angle, float true_angle, float toMarking_ML, float true_toMarking_ML,