Corrected way to get torcs image from shared memory

parent 60245204
......@@ -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!");
......
......@@ -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
......
......@@ -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,
......
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