Commit 8ef40f25 authored by Svetlana Pavlitskaya's avatar Svetlana Pavlitskaya

Implemented screenshot resize on the cpp side. Fixed TORCS screenshot...

Implemented screenshot resize on the cpp side. Fixed TORCS screenshot querying. Fixed TORCS screenshot&command coordination.
parent 299c1090
......@@ -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;
......
......@@ -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})
......@@ -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())
{
std_msgs::Int8MultiArray::Ptr msg(new std_msgs::Int8MultiArray);
msg->data.resize(torcs::IMAGE_SIZE_BYTES);
const auto screenshot = client.getScreenshot();
constexpr auto SCALED_IMAGE_SIZE_BYTES = 280*210*3u;
const auto imageData = client.getScreenshot();
memcpy(msg->data.data(), imageData->data(), SCALED_IMAGE_SIZE_BYTES);
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(ROS_IMAGE_SIZE);
memcpy(msg->data.data(), resizedImage.data, ROS_IMAGE_SIZE);
ROS_INFO("%f", msg->data[0]);
chatter_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
......
......@@ -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;
......
......@@ -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
......
......@@ -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]));
}
......
......@@ -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;
}
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