Skip to content
GitLab
Commit 15490e1f authored by Svetlana's avatar Svetlana
Browse files

Run TORCS client in a separate thread for correct driver controller functionality.

parent f05e7021
Hide whitespace changes
Inline Side-by-side
EMADL2TORCS/generate2ros.sh
View file @ 15490e1f
......@@ -11,8 +11,8 @@ cp -rf resources/scripts/* target/
chmod u+x target/*.sh
echo "Copying CNN weights..."
cp -rf resources/torcs target/torcs
chmod u+x resources/torcs/*.sh
cp -rf resources/TORCSComponent target/TORCSComponent
chmod u+x resources/TORCSComponent/*.sh
cp -rf resources/dpnet target/model
echo "Building generated code..."
......
EMADL2TORCS/resources/torcs/build.sh → EMADL2TORCS/resources/TORCSComponent/build.sh
View file @ 15490e1f
......@@ -2,9 +2,10 @@
cd src
cd camera
cd TORCSComponent
mkdir -p build
cd build
cmake ..
make -j
cd ../..
EMADL2TORCS/resources/torcs/src/camera/CMakeLists.txt → EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/CMakeLists.txt
View file @ 15490e1f
cmake_minimum_required(VERSION 3.5)
project (ROSCamera)
project (TORCSComponentProject)
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)
add_executable(TORCSComponent src/main.cpp src/torcsclient.cpp)
set_property(TARGET camera PROPERTY CXX_STANDARD 14)
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})
set_property(TARGET TORCSComponent PROPERTY CXX_STANDARD 14)
target_link_libraries(TORCSComponent ${roscpp_LIBRARIES} ${std_msgs_LIBRARIES} ${OpenCV_LIBS})
target_include_directories(TORCSComponent PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${roscpp_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS})
EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/main.cpp 0 → 100644
View file @ 15490e1f
#include "torcsclient.h"
#include <sstream>
#include <thread>
#include <ml.h>
#include <highgui.h>
#include <cv.hpp>
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>
constexpr auto ROS_IMAGE_WIDTH = 280u;
constexpr auto ROS_IMAGE_HEIGHT = 210u;
constexpr auto ROS_IMAGE_SIZE = ROS_IMAGE_WIDTH*ROS_IMAGE_HEIGHT*3u;
torcs::TorcsClient client;
ros::Publisher* cameraPublisher;
ros::Publisher* speedPublisher;
ros::Publisher* affordancePublisher;
void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
client.sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2]));
}
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));
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_8UC(torcs::IMAGE_CHANNELS));
cv::flip(resizedImage, flippedImage, 0);
cv::Mat rgbImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
cv::cvtColor(flippedImage, rgbImage, cv::COLOR_RGB2BGR);
//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]);
}
}
}
static_assert(sizeof(float) == sizeof(std_msgs::Float32), "Float is not 32 bit long!");
std_msgs::Float32MultiArray::Ptr msg(new std_msgs::Float32MultiArray);
msg->data.resize(ROS_IMAGE_SIZE);
memcpy(msg->data.data(), data.data(), ROS_IMAGE_SIZE*sizeof(float));
std_msgs::Float32::Ptr speedMsg(new std_msgs::Float32);
speedMsg->data = affordance.getSpeed();
std_msgs::Float32MultiArray::Ptr affordanceMsg(new std_msgs::Float32MultiArray);
affordanceMsg->data = affordance.getAsVector();
cameraPublisher->publish(msg);
speedPublisher->publish(speedMsg);
affordancePublisher->publish(affordanceMsg);
}
void clientRunner()
{
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wmissing-noreturn"
while(true)
{
client.iterate();
}
#pragma clang diagnostic pop
}
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);
cameraPublisher = &cameraPub;
ros::Publisher speedPub = n.advertise<std_msgs::Float32>("/speed", 1);
speedPublisher = &speedPub;
ros::Publisher affordancePub = n.advertise<std_msgs::Float32MultiArray>("/affordance", 1);
affordancePublisher = &affordancePub;
ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback);
ros::Rate loop_rate(10);
std::thread clientRunnerThread(clientRunner);
// int count = 0;
//
// while (ros::ok())
// {
//
// client->iterate();
// ros::spinOnce();
// loop_rate.sleep();
// ++count;
// }
ros::spin();
clientRunnerThread.join();
return 0;
}
EMADL2TORCS/resources/torcs/src/camera/src/torcsclient.cpp → EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.cpp
View file @ 15490e1f
......@@ -12,6 +12,7 @@ using namespace torcs;
TorcsClient::TorcsClient()
{
gotCommand = false;
std::cout << "TORCS Client!" << std::endl;
void *shm = NULL;
......@@ -66,7 +67,11 @@ TorcsClient::TorcsClient()
shared->steerCmd = 0.0;
shared->accelCmd = 0.0;
shared->brakeCmd = 0.0;
}
void TorcsClient::setPublishCallback(std::function<void(std::shared_ptr<DataContainer>, Affordance)> callback)
{
publishCallback = callback;
}
void TorcsClient::sendCommand(const Command& cmd)
......@@ -75,7 +80,7 @@ void TorcsClient::sendCommand(const Command& cmd)
command = cmd;
}
void TorcsClient::iterate(std::unique_ptr<DataContainer>& screenshot, float& speed)
void TorcsClient::iterate()
{
assert(connected);
......@@ -83,25 +88,66 @@ void TorcsClient::iterate(std::unique_ptr<DataContainer>& screenshot, float& spe
#pragma clang diagnostic ignored "-Wmissing-noreturn"
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());
speed = float(shared->speed);
if (gotCommand && shared->control == 1) {
gotCommand = false;
ROS_INFO("Command acc: %f, brake: %f, steer: %f", command.accelCmd, command.brakeCmd, command.steerCmd);
shared->accelCmd = command.accelCmd;
shared->steerCmd = command.steerCmd;
shared->brakeCmd = command.brakeCmd;
}
shared->fast = 0.0;
Affordance affordance;
affordance.populate(*shared);
publishCallback(screenshot, affordance);
// ROS_INFO("Affordance:\n"
// "dist_L: %f\n"
// "dist_R: %f\n"
// "dist_LL: %f\n"
// "dist_MM: %f\n"
// "dist_RR: %f\n"
// "toMarking_L: %f\n"
// "toMarking_M: %f\n"
// "toMarking_R: %f\n"
// "toMarking_LL: %f\n"
// "toMarking_ML: %f\n"
// "toMarking_MR: %f\n"
// "toMarking_RR: %f\n"
// "toMiddle: %f\n"
// "angle: %f\n"
// "fast: %f\n"
// "speed: %f\n",
// affordance.getDistL(),
// affordance.getDistR(),
// affordance.getDistLL(),
// affordance.getDistMM(),
// affordance.getDistRR(),
// affordance.getToMarkingL(),
// affordance.getToMarkingM(),
// affordance.getToMarkingR(),
// affordance.getToMarkingLL(),
// affordance.getToMarkingML(),
// affordance.getToMarkingMR(),
// affordance.getToMarkingRR(),
// affordance.getToMiddle(),
// affordance.getAngle(),
// affordance.getFast(),
// affordance.getSpeed());
shared->written = 0;
shared->pause = 0;
break;
} else {
if (shared->pause != 1) {
shared->pause = 1;
}
}
if (shared->control == 1) {
if (gotCommand) {
gotCommand = false;
ROS_INFO("Command acc: %f, brake: %f, steer: %f", command.accelCmd, command.brakeCmd, command.steerCmd);
shared->accelCmd = command.accelCmd;
shared->steerCmd = command.steerCmd;
shared->brakeCmd = command.brakeCmd;
} else {
//ROS_INFO("No command");
//shared->steerCmd = 0.0;
}
}
}
#pragma clang diagnostic pop
}
EMADL2TORCS/resources/torcs/src/camera/src/torcsclient.h → EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.h
View file @ 15490e1f
......@@ -3,6 +3,8 @@
#include <cstdint>
#include <array>
#include <memory>
#include <functional>
#include <std_msgs/Float32MultiArray.h>
namespace torcs
{
......@@ -60,20 +62,76 @@ struct Command
using DataContainer = std::array<uint8_t, IMAGE_SIZE_BYTES>;
class Affordance
{
public:
void populate(const shared_use_st& affordance)
{
affordance_[0] = float(affordance.fast);
affordance_[1] = float(affordance.dist_L);
affordance_[2] = float(affordance.dist_R);
affordance_[3] = float(affordance.toMarking_L);
affordance_[4] = float(affordance.toMarking_M);
affordance_[5] = float(affordance.toMarking_R);
affordance_[6] = float(affordance.dist_LL);
affordance_[7] = float(affordance.dist_MM);
affordance_[8] = float(affordance.dist_RR);
affordance_[9] = float(affordance.toMarking_LL);
affordance_[10] = float(affordance.toMarking_ML);
affordance_[11] = float(affordance.toMarking_MR);
affordance_[12] = float(affordance.toMarking_RR);
affordance_[13] = float(affordance.toMiddle); // not used
affordance_[14] = float(affordance.angle);
affordance_[15] = float(affordance.speed);
}
void populate(const std_msgs::Float32MultiArray& affordance)
{
affordance_ = affordance.data;
}
float getFast() { return affordance_[0]; }
float getDistL() { return affordance_[1]; }
float getDistR() { return affordance_[2]; }
float getToMarkingL() { return affordance_[3]; }
float getToMarkingM() { return affordance_[4]; }
float getToMarkingR() { return affordance_[5]; }
float getDistLL() { return affordance_[6]; }
float getDistMM() { return affordance_[7]; }
float getDistRR() { return affordance_[8]; }
float getToMarkingLL() { return affordance_[9]; }
float getToMarkingML() { return affordance_[10]; }
float getToMarkingMR() { return affordance_[11]; }
float getToMarkingRR() { return affordance_[12]; }
float getToMiddle() { return affordance_[13]; }
float getAngle() { return affordance_[14]; }
float getSpeed() { return affordance_[15]; }
const std::vector<float>& getAsVector()
{
return affordance_;
}
private:
std::vector<float> affordance_ = std::vector<float>(16);
};
class TorcsClient
{
public:
TorcsClient();
explicit TorcsClient();
void iterate(std::unique_ptr<DataContainer>& screenshot, float& speed);
void setPublishCallback(std::function<void(std::shared_ptr<DataContainer>, Affordance)> callback);
void iterate();
void sendCommand(const Command& cmd);
private:
struct shared_use_st *shared;
bool connected = false;
bool gotCommand = false;
std::atomic<bool> gotCommand;
Command command;
std::function<void(std::shared_ptr<DataContainer>, Affordance)> publishCallback;
};
}
......
EMADL2TORCS/resources/scripts/build_all.sh
View file @ 15490e1f
......@@ -8,8 +8,8 @@ cmake ..
make -j
cd ../..
echo "Building TORCS adapters"
cd torcs
echo "Building TORCSComponent"
cd TORCSComponent
./build.sh
echo
......
EMADL2TORCS/resources/scripts/run_all.sh
View file @ 15490e1f
......@@ -5,6 +5,6 @@ xterm -T ROSCORE -e roscore &
echo "Waiting for ROSCORE to start..."
sleep 2
xterm -T TORCSCamera -e torcs/src/camera/build/camera &
xterm -T TORCSComponent -e TORCSComponent/src/TORCSComponent/build/TORCSComponent &
xterm -T Mastercomponent -e Mastercomponent/build/coordinator/Coordinator_dp_mastercomponent &
xterm -T Mastercomponent -e Mastercomponent/build/dp_mastercomponent/coordinator/Coordinator_dp_mastercomponent &
EMADL2TORCS/resources/torcs/src/camera/src/main.cpp deleted 100644 → 0
View file @ f05e7021
#include "torcsclient.h"
#include <sstream>
#include <ml.h>
#include <highgui.h>
#include <cv.hpp>
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.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;
torcs::TorcsClient client;
std::unique_ptr<torcs::DataContainer> screenshot = std::make_unique<torcs::DataContainer>();
void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
client.sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2]));
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "camera");
ros::NodeHandle n;
ros::Publisher cameraPub = n.advertise<std_msgs::Float32MultiArray>("/camera", 1);
ros::Publisher speedPub = n.advertise<std_msgs::Float32>("/speed", 1);
ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback);
ros::Rate loop_rate(10);
int count = 0;
while (ros::ok())
{
float speed;
client.iterate(screenshot, speed);
cv::Mat image(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
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_8UC(torcs::IMAGE_CHANNELS));
cv::flip(resizedImage, flippedImage, 0);
cv::Mat rgbImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
cv::cvtColor(flippedImage, rgbImage, cv::COLOR_RGB2BGR);
//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]);
}
}
}
assert(sizeof(float) == sizeof(std_msgs::Float32));
std_msgs::Float32MultiArray::Ptr msg(new std_msgs::Float32MultiArray);
msg->data.resize(ROS_IMAGE_SIZE);
memcpy(msg->data.data(), data.data(), ROS_IMAGE_SIZE*sizeof(float));
std_msgs::Float32::Ptr speedMsg(new std_msgs::Float32);
speedMsg->data = speed;
cameraPub.publish(msg);
speedPub.publish(speedMsg);
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}
Supports Markdown
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