diff --git a/EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/main.cpp b/EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/main.cpp index cdd136bfe08a547244c5ef2e0be524154332775c..da53d24d9b921395e9f5200bf095c507dc21ec23 100644 --- a/EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/main.cpp +++ b/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 screenshot, torcs::Affordance affordance) +void publishCallback(std::vector 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 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!"); diff --git a/EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.cpp b/EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.cpp index 67ec659d7fce20cdb98dbc3d2db61d1b3fe52abb..949f98760cbe623d5a959235225ed6b90b6e01b4 100644 --- a/EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.cpp +++ b/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, Affordance)> callback) +void TorcsClient::setPublishCallback(std::function, Affordance)> callback) { publishCallback = callback; } @@ -189,8 +189,8 @@ void TorcsClient::iterate() else { readyForNewShot = false; - auto screenshot = std::make_shared(); - memcpy(screenshot->data(), shared->data, screenshot->size()); + std::vector screenshot(torcs::IMAGE_SIZE_BYTES); + memcpy(screenshot.data(), shared->data, screenshot.size()); shared->written = 0; Affordance affordance; affordance.populate(*shared); // ground truth affordance diff --git a/EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.h b/EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.h index da0b1d884edaae881a7e443c9d9e4b35af87cb7c..59fab9d0b485ce796f647e89b8d9dc410ad2112f 100644 --- a/EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.h +++ b/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, Affordance)> callback); + void setPublishCallback(std::function, Affordance)> callback); void iterate(); void sendCommand(const Command& cmd); void setLanesCount(int count); @@ -144,7 +144,7 @@ private: bool connected = false; std::atomic gotCommand; Command command; - std::function, Affordance)> publishCallback; + std::function, 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,