From 8ef40f258a7dc8cde3b3de82d38a0e80c2a5ba6e Mon Sep 17 00:00:00 2001
From: Svetlana Pavlitskaya <svetlana.pavlitskaya@rwth-aachen.de>
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 <sstream>
+#include <ml.h>
+#include <highgui.h>
+#include <cv.h>
+
+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<DataContainer> TorcsClient::getScreenshot()
@@ -74,8 +47,11 @@ std::unique_ptr<DataContainer> 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<DataContainer>();
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<uint8_t, IMAGE_SIZE_BYTES>;
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<torcs::TorcsClient> 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;
}
--
GitLab