From 8ef40f258a7dc8cde3b3de82d38a0e80c2a5ba6e Mon Sep 17 00:00:00 2001 From: Svetlana Pavlitskaya Date: Mon, 25 Jun 2018 01:05:59 +0200 Subject: [PATCH] Implemented screenshot resize on the cpp side. Fixed TORCS screenshot querying. Fixed TORCS screenshot&command coordination. --- .../src/main/models/dp/Mastercomponent.emadl | 2 +- .../resources/torcs/src/camera/CMakeLists.txt | 3 +- .../resources/torcs/src/camera/src/main.cpp | 31 ++++++++++++---- .../torcs/src/camera/src/torcsclient.cpp | 30 ++-------------- .../torcs/src/camera/src/torcsclient.h | 18 ++-------- .../resources/torcs/src/driver/src/main.cpp | 1 - .../torcs/src/driver/src/torcsclient.cpp | 35 +++---------------- 7 files changed, 37 insertions(+), 83 deletions(-) diff --git a/TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl b/TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl index e0c8b09..c3fc685 100644 --- a/TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl +++ b/TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl @@ -3,7 +3,7 @@ import dp.subcomponents.*; component Mastercomponent { ports - in Z(0:255)^{3, 240, 320} imageIn, + in Z(0:255)^{3, 210, 280} imageIn, out Q(0:1)^{3,1,1} commandsOut; instance Imagepreprocessing ip; diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/CMakeLists.txt b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/CMakeLists.txt index 50ac0d9..55edd96 100644 --- a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/CMakeLists.txt +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/CMakeLists.txt @@ -4,9 +4,10 @@ set (CXX_STANDARD_REQUIRED 14) find_package(roscpp REQUIRED) find_package(std_msgs REQUIRED) +find_package(OpenCV REQUIRED) add_executable(camera src/main.cpp src/torcsclient.cpp) set_property(TARGET camera PROPERTY CXX_STANDARD 14) -target_link_libraries(camera ${roscpp_LIBRARIES} ${std_msgs_LIBRARIES}) +target_link_libraries(camera ${roscpp_LIBRARIES} ${std_msgs_LIBRARIES} ${OpenCV_LIBS}) target_include_directories(camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${roscpp_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS}) diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/main.cpp b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/main.cpp index 70b545f..67ab0d8 100644 --- a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/main.cpp +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/main.cpp @@ -4,6 +4,14 @@ #include +#include +#include +#include + +constexpr auto ROS_IMAGE_WIDTH = 280; +constexpr auto ROS_IMAGE_HEIGHT = 210; +constexpr auto ROS_IMAGE_SIZE = ROS_IMAGE_WIDTH*ROS_IMAGE_HEIGHT*3; + int main(int argc, char **argv) { ros::init(argc, argv, "camera"); @@ -13,17 +21,26 @@ int main(int argc, char **argv) int count = 0; torcs::TorcsClient client; - while (ros::ok()) { + const auto screenshot = client.getScreenshot(); + + cv::Mat image(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS)); + memcpy(image.data, screenshot.get(), torcs::IMAGE_SIZE_BYTES); + + 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"); + std_msgs::Int8MultiArray::Ptr msg(new std_msgs::Int8MultiArray); - msg->data.resize(torcs::IMAGE_SIZE_BYTES); + msg->data.resize(ROS_IMAGE_SIZE); + memcpy(msg->data.data(), resizedImage.data, ROS_IMAGE_SIZE); - constexpr auto SCALED_IMAGE_SIZE_BYTES = 280*210*3u; - const auto imageData = client.getScreenshot(); - memcpy(msg->data.data(), imageData->data(), SCALED_IMAGE_SIZE_BYTES); - - ROS_INFO("%f", msg->data[0]); chatter_pub.publish(msg); ros::spinOnce(); loop_rate.sleep(); diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.cpp b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.cpp index 7e3d09a..8cbd026 100644 --- a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.cpp +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.cpp @@ -35,34 +35,7 @@ TorcsClient::TorcsClient() std::cout << "Started shared memory at " << std::hex << shm << std::endl; shared = (struct shared_use_st*)shm; - shared->written = 0; - shared->control = 0; - shared->pause = 0; - shared->fast = 0.0; - - shared->dist_L = 0.0; - shared->dist_R = 0.0; - - shared->toMarking_L = 0.0; - shared->toMarking_M = 0.0; - shared->toMarking_R = 0.0; - shared->dist_LL = 0.0; - shared->dist_MM = 0.0; - shared->dist_RR = 0.0; - - shared->toMarking_LL = 0.0; - shared->toMarking_ML = 0.0; - shared->toMarking_MR = 0.0; - shared->toMarking_RR = 0.0; - - shared->toMiddle = 0.0; - shared->angle = 0.0; - shared->speed = 0.0; - - shared->steerCmd = 0.0; - shared->accelCmd = 0.0; - shared->brakeCmd = 0.0; } std::unique_ptr TorcsClient::getScreenshot() @@ -74,8 +47,11 @@ std::unique_ptr TorcsClient::getScreenshot() std::this_thread::sleep_for(std::chrono::milliseconds(100)); } + shared->pause = 1; + std::cout << "Got screenshot" << std::endl; auto result = std::make_unique(); memcpy(result->data(), shared->data, result->size()); + shared->pause = 0; shared->written = 0; return result; diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.h b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.h index 7827d83..ef47160 100644 --- a/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.h +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.h @@ -7,9 +7,9 @@ namespace torcs { -constexpr auto IMAGE_WIDTH = 640; -constexpr auto IMAGE_HEIGHT = 480; -constexpr auto IMAGE_CHANNELS = 3; +constexpr auto IMAGE_WIDTH = 640u; +constexpr auto IMAGE_HEIGHT = 480u; +constexpr auto IMAGE_CHANNELS = 3u; constexpr auto IMAGE_SIZE_BYTES = IMAGE_WIDTH*IMAGE_HEIGHT*IMAGE_CHANNELS; struct shared_use_st @@ -45,18 +45,6 @@ struct shared_use_st double brakeCmd = 0.0; }; -struct Command -{ - Command(float accel, float steer, float brakec) - : accelCmd(accel), steerCmd(steer), brakeCmd(brakec) - { - } - - float accelCmd; - float steerCmd; - float brakeCmd; -}; - using DataContainer = std::array; class TorcsClient diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/main.cpp b/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/main.cpp index 2e81277..b21c193 100644 --- a/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/main.cpp +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/main.cpp @@ -8,7 +8,6 @@ std::unique_ptr client; void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg) { - ROS_INFO("Received command. accel: %f, steer: %f, break: %f", msg->data[0], msg->data[1], msg->data[2]); client->sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2])); } diff --git a/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.cpp b/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.cpp index a24a88a..e162499 100644 --- a/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.cpp +++ b/TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.cpp @@ -35,48 +35,21 @@ TorcsClient::TorcsClient() std::cout << "Started shared memory at " << std::hex << shm << std::endl; shared = (struct shared_use_st*)shm; - shared->written = 0; - shared->control = 0; - shared->pause = 0; - shared->fast = 0.0; - - shared->dist_L = 0.0; - shared->dist_R = 0.0; - - shared->toMarking_L = 0.0; - shared->toMarking_M = 0.0; - shared->toMarking_R = 0.0; - - shared->dist_LL = 0.0; - shared->dist_MM = 0.0; - shared->dist_RR = 0.0; - - shared->toMarking_LL = 0.0; - shared->toMarking_ML = 0.0; - shared->toMarking_MR = 0.0; - shared->toMarking_RR = 0.0; - - shared->toMiddle = 0.0; - shared->angle = 0.0; - shared->speed = 0.0; - - shared->steerCmd = 0.0; - shared->accelCmd = 0.0; - shared->brakeCmd = 0.0; } void TorcsClient::sendCommand(Command cmd) { assert(connected); - while (shared->control == 1) { + while (shared->written == 1) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } + std::cout << "Sent command" << std::endl; shared->accelCmd = cmd.accelCmd; shared->steerCmd = cmd.steerCmd; shared->brakeCmd = cmd.brakeCmd; - - shared->control = 0; + shared->control = 1; + shared->written = 1; } -- 2.24.1