Commit ea521acc authored by Svetlana's avatar Svetlana

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

parent 65bfd1fd
#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;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment