Commit c4ae23eb authored by Svetlana Pavlitskaya's avatar Svetlana Pavlitskaya

Corrected interaction with TORCS. Used uint8 to send screenshot to ROS.

parent ef7bb6f9
......@@ -4,7 +4,7 @@ echo "Cleaning target directory..."
rm -rf target/*
echo "Generating code..."
java -cp embedded-montiarc-math-middleware-generator-0.0.9-SNAPSHOT-jar-with-dependencies.jar de.monticore.lang.monticar.generator.middleware.DistributedTargetGeneratorCli --models-dir=src/main/ --root-model=dp.mastercomponent --output-dir=target/Mastercomponent --generators=emadlcpp,roscpp --backend=MXNET
java -cp embedded-montiarc-math-middleware-generator-0.0.11-SNAPSHOT-jar-with-dependencies.jar de.monticore.lang.monticar.generator.middleware.DistributedTargetGeneratorCli --models-dir=src/main/ --root-model=dp.mastercomponent --output-dir=target/Mastercomponent --generators=emadlcpp,roscpp --backend=MXNET
echo "Copying build and run scripts..."
cp -rf resources/scripts/* target/
......
......@@ -2,6 +2,7 @@
#include <sstream>
#include <thread>
#include <fstream>
#include <ml.h>
#include <highgui.h>
......@@ -10,6 +11,7 @@
#include <std_msgs/Float32.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/UInt8MultiArray.h>
torcs::TorcsClient client;
ros::Publisher* cameraPublisher;
......@@ -35,41 +37,43 @@ void predictionsChatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg
void publishCallback(std::shared_ptr<torcs::DataContainer> screenshot, torcs::Affordance affordance)
{
cv::Mat image(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
cv::Mat image(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC3);
memcpy(image.data, screenshot->data(), 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));
cv::Mat flippedImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC3);
cv::flip(image, flippedImage, 0);
cv::Mat flippedImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
cv::flip(resizedImage, flippedImage, 0);
cv::Mat resizedImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC3);
cv::resize(flippedImage, resizedImage, cv::Size(ROS_IMAGE_WIDTH, ROS_IMAGE_HEIGHT));
cv::Mat rgbImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
cv::cvtColor(flippedImage, rgbImage, cv::COLOR_RGB2BGR);
cv::Mat rgbImage(ROS_IMAGE_HEIGHT, ROS_IMAGE_WIDTH, CV_8UC3);
cv::cvtColor(resizedImage, rgbImage, cv::COLOR_BGR2RGB);
//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 1", cv::WINDOW_AUTOSIZE );
//cv::imshow("Display Image 1", image);
//cv::waitKey(0);
//cv::destroyWindow("Display Image 1");
std::vector<float> data(0);
data.reserve(ROS_IMAGE_SIZE);
for(size_t j=0; j<ROS_IMAGE_HEIGHT; j++){
for(size_t k=0; k<ROS_IMAGE_WIDTH; k++){
cv::Vec3b intensity = rgbImage.at<cv::Vec3b>(j, k);
for(size_t i=0; i<3; i++){
data.push_back((float) intensity[i]);
}
cv::namedWindow("Screenshot", cv::WINDOW_AUTOSIZE );
cv::imshow("Screenshot", rgbImage);
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];
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!");
std_msgs::Float32MultiArray::Ptr msg(new std_msgs::Float32MultiArray);
std_msgs::UInt8MultiArray::Ptr msg(new std_msgs::UInt8MultiArray);
msg->data.resize(ROS_IMAGE_SIZE);
memcpy(msg->data.data(), data.data(), ROS_IMAGE_SIZE*sizeof(float));
memcpy(msg->data.data(), data.data(), data.size());
std_msgs::Float32::Ptr speedMsg(new std_msgs::Float32);
speedMsg->data = affordance.getSpeed();
......@@ -105,7 +109,7 @@ int main(int argc, char **argv)
client.setPublishCallback(publishCallback);
ros::init(argc, argv, "TORCSComponent");
ros::NodeHandle n;
ros::Publisher cameraPub = n.advertise<std_msgs::Float32MultiArray>("/camera", 1);
ros::Publisher cameraPub = n.advertise<std_msgs::UInt8MultiArray>("/camera", 1);
cameraPublisher = &cameraPub;
ros::Publisher speedPub = n.advertise<std_msgs::Float32>("/speed", 1);
speedPublisher = &speedPub;
......
......@@ -52,8 +52,8 @@ TorcsClient::TorcsClient()
shared = (struct shared_use_st*)shm;
shared->written = 0; // Equals 1 when screenshot data is written and can bea read
shared->control = 1; // Seems to be not used by TORCS - was only needed by training framework of the original paper
shared->pause = 0; // Set to one to pause execution - was used in original paper
shared->control = 0; // Seems to be not used by TORCS - was only needed by training framework of the original paper
shared->pause = 1; // Set to one to pause execution - was used in original paper
shared->fast = 0.0;
shared->dist_L = 0.0;
......@@ -170,31 +170,36 @@ void TorcsClient::iterate()
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wmissing-noreturn"
bool firstRun = true;
shared->written = 0;
shared->pause = 1;
shared->control = 1;
while (true) {
if (shared->written == 1) { // the new image data is ready to be read
auto screenshot = std::make_shared<DataContainer>();
memcpy(screenshot->data(), shared->data, screenshot->size());
shared->fast = 0.0;
Affordance affordance;
affordance.populate(*shared);
publishCallback(screenshot, affordance);
shared->written = 0;
shared->pause = 0;
} else {
if (shared->pause != 1) {
shared->pause = 1;
}
while(shared->written == 0) {
shared->pause = 1;
using namespace std::chrono_literals;
std::this_thread::sleep_for(1ms);
}
auto screenshot = std::make_shared<DataContainer>();
memcpy(screenshot->data(), shared->data, screenshot->size());
shared->written = 0;
Affordance affordance;
affordance.populate(*shared); // ground truth affordance
publishCallback(screenshot, affordance);
if (shared->control == 1) {
if (gotCommand) {
gotCommand = false;
shared->accelCmd = command.accelCmd;
shared->steerCmd = command.steerCmd;
shared->brakeCmd = command.brakeCmd;
shared->pause = 1;
while(!gotCommand) {
using namespace std::chrono_literals;
std::this_thread::sleep_for(5ms);
}
gotCommand = false;
shared->accelCmd = command.accelCmd;
shared->steerCmd = command.steerCmd;
shared->brakeCmd = command.brakeCmd;
}
shared->pause = 0;
}
#pragma clang diagnostic pop
}
......@@ -228,6 +233,22 @@ void TorcsClient::visualize(float angle, float true_angle, float toMarking_ML, f
int counter=0;
////////////////////// END visualization parameters
#if DEBUG_LOG
std::cout << "angle: " << angle << " should be " << true_angle << std::endl;
std::cout << "toMarking_ML: " << toMarking_ML << " should be " << true_toMarking_ML << std::endl;
std::cout << "toMarking_MR: " << toMarking_MR << " should be " << true_toMarking_MR << std::endl;
std::cout << "toMarking_M: " << toMarking_M << " should be " << true_toMarking_M << std::endl;
std::cout << "toMarking_LL: " << toMarking_LL << " should be " << true_toMarking_LL << std::endl;
std::cout << "toMarking_R: " << toMarking_R << " should be " << true_toMarking_R << std::endl;
std::cout << "toMarking_L: " << toMarking_L << " should be " << true_toMarking_L << std::endl;
std::cout << "toMarking_RR: " << toMarking_RR << " should be " << true_toMarking_RR << std::endl;
std::cout << "dist_LL: " << dist_LL << " should be " << true_dist_LL << std::endl;
std::cout << "dist_RR: " << dist_RR << " should be " << true_dist_RR << std::endl;
std::cout << "dist_MM: " << dist_MM << " should be " << true_dist_MM << std::endl;
std::cout << "dist_L: " << dist_L << " should be " << true_dist_L << std::endl;
std::cout << "dist_R: " << dist_R << " should be " << true_dist_R << std::endl;
#endif
if (lanesCount == 1)
{
//////////////////////////////////////////////// show legend and error bar
......
......@@ -6,6 +6,8 @@
#include <functional>
#include <std_msgs/Float32MultiArray.h>
#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;
......
......@@ -2,9 +2,8 @@ package dp;
conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema;
tags Mastercomponent {
tag mastercomponent.imageIn with RosConnection = {topic=(/camera,std_msgs/Float32MultiArray)};
tag mastercomponent.imageIn with RosConnection = {topic=(/camera,std_msgs/UInt8MultiArray)};
tag mastercomponent.speedIn with RosConnection = {topic=(/speed,std_msgs/Float32)};
tag mastercomponent.groundTruthAffordance with RosConnection = {topic=(/affordance,std_msgs/Float32MultiArray)};
tag mastercomponent.commandsOut with RosConnection = {topic=(/commands,std_msgs/Float32MultiArray)};
tag mastercomponent.predictedAffordanceOut with RosConnection = {topic=(/predictions,std_msgs/Float32MultiArray)};
}
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