More image preprocessing: flip, translate BGR to RGB, pack into vector.

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

-#include "ros/ros.h"
-#include "std_msgs/Int8MultiArray.h"
-#include "std_msgs/Float32.h"
 #include "torcsclient.h"
 
 #include <sstream>
@@ -8,7 +5,8 @@
 #include <ml.h>
 #include <highgui.h>
 #include <cv.h>
-#include <ros/console.h>
+#include <ros/ros.h>
+#include <std_msgs/Float32.h>
 #include <std_msgs/Float32MultiArray.h>
 
 constexpr auto ROS_IMAGE_WIDTH = 280;
@@ -27,7 +25,7 @@ int main(int argc, char **argv)
 {
     ros::init(argc, argv, "camera");
     ros::NodeHandle n;
-    ros::Publisher cameraPub = n.advertise<std_msgs::Int8MultiArray>("/camera", 1);
+    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);
@@ -46,15 +44,43 @@ int main(int argc, char **argv)
         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");
+        cv::Mat flippedImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
+        cv::flip(resizedImage, flippedImage, 0);
 
-        std_msgs::Int8MultiArray::Ptr msg(new std_msgs::Int8MultiArray);
+        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", cv::WINDOW_AUTOSIZE );
+        //cv::imshow("Display Image", rgbImage);
+        //cv::waitKey(0);
+        //cv::destroyWindow("Display Image");
+
+//        std::vector<float> data(3*ROS_IMAGE_HEIGHT*ROS_IMAGE_WIDTH);
+//        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[i*ROS_IMAGE_HEIGHT*ROS_IMAGE_WIDTH + j*ROS_IMAGE_HEIGHT + k] = (float) intensity[i];
+//                }
+//            }
+//        }
+
+        std::vector<float> data(0);
+        data.reserve(3*ROS_IMAGE_HEIGHT*ROS_IMAGE_WIDTH);
+        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]);
+                }
+            }
+        }
+
+
+        std_msgs::Float32MultiArray::Ptr msg(new std_msgs::Float32MultiArray);
 	    msg->data.resize(ROS_IMAGE_SIZE);
-	    memcpy(msg->data.data(), resizedImage.data, ROS_IMAGE_SIZE);
+	    memcpy(msg->data.data(), data.data(), ROS_IMAGE_SIZE);
 
 	    std_msgs::Float32::Ptr speedMsg(new std_msgs::Float32);
 	    speedMsg->data = speed;