Commit 3d9fbed8 authored by Svetlana's avatar Svetlana

Minor changes in torcs client

parent e2c29491
......@@ -37,3 +37,4 @@ torcs/src/tools/package/specfiles/torcs-robot-base.spec
torcs/src/tools/package/specfiles/torcs.spec
torcs/src/tools/texmapper/texmapper-bin
torcs/src/tools/trackgen/trackgen-bin
/TorcsClient/build/
\ No newline at end of file
......@@ -5,12 +5,15 @@ project(TorcsClient)
SET(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
SET(CMAKE_CXX_STANDARD 14)
message(${CMAKE_MODULE_PATH})
find_package(roscpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(FreeImage)
#find_package(FreeImage)
INCLUDE_DIRECTORIES("${CMAKE_SOURCE_DIR}/../experiments/output/cpp")
add_executable(${PROJECT_NAME} "main.cpp")
add_executable(${PROJECT_NAME} torcs.cpp torcsclient.cpp)
target_link_libraries( TorcsClient freeimage)
target_link_libraries(TorcsClient freeimage ${roscpp_LIBRARIES} ${std_msgs_LIBRARIES})
target_include_directories(TorcsClient PUBLIC ${roscpp_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS})
set_target_properties(TorcsClient PROPERTIES LINKER_LANGUAGE CXX)
This diff is collapsed.
# Install script for directory: /home/alex/MA_sveta/torcs_dl/TorcsClient
# Set the install prefix
if(NOT DEFINED CMAKE_INSTALL_PREFIX)
set(CMAKE_INSTALL_PREFIX "/usr/local")
endif()
string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}")
# Set the install configuration name.
if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
if(BUILD_TYPE)
string(REGEX REPLACE "^[^A-Za-z0-9_]+" ""
CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}")
else()
set(CMAKE_INSTALL_CONFIG_NAME "Debug")
endif()
message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"")
endif()
# Set the component getting installed.
if(NOT CMAKE_INSTALL_COMPONENT)
if(COMPONENT)
message(STATUS "Install component: \"${COMPONENT}\"")
set(CMAKE_INSTALL_COMPONENT "${COMPONENT}")
else()
set(CMAKE_INSTALL_COMPONENT)
endif()
endif()
# Install shared libraries without execute permission?
if(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
set(CMAKE_INSTALL_SO_NO_EXE "1")
endif()
if(CMAKE_INSTALL_COMPONENT)
set(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt")
else()
set(CMAKE_INSTALL_MANIFEST "install_manifest.txt")
endif()
string(REPLACE ";" "\n" CMAKE_INSTALL_MANIFEST_CONTENT
"${CMAKE_INSTALL_MANIFEST_FILES}")
file(WRITE "/home/alex/MA_sveta/torcs_dl/TorcsClient/${CMAKE_INSTALL_MANIFEST}"
"${CMAKE_INSTALL_MANIFEST_CONTENT}")
[
{
"directory": "/home/alex/MA_sveta/torcs_dl/TorcsClient",
"command": "/usr/bin/g++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_PACKAGE_NAME=\\\"TorcsClient\\\" -I/home/alex/MA_sveta/torcs_dl/TorcsClient/../experiments/output/cpp -I/opt/ros/lunar/include -I/opt/ros/lunar/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -g3 -gdwarf-2 -std=gnu++14 -o CMakeFiles/TorcsClient.dir/main.cpp.o -c /home/alex/MA_sveta/torcs_dl/TorcsClient/main.cpp",
"file": "/home/alex/MA_sveta/torcs_dl/TorcsClient/main.cpp"
}
]
\ No newline at end of file
#include <ros/ros.h>
#include <std_msgs/Int8MultiArray.h>
#include <std_msgs/Float32MultiArray.h>
#include "torcsclient.h"
#include <sstream>
torcs::TorcsClient client;
void commandMsgCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
torcs::Command cmd;
cmd.accelCmd = msg->data[0];
cmd.steerCmd = msg->data[1];
cmd.brakeCmd = msg->data[2];
ROS_INFO("Received command.");
ROS_INFO(" accel: %f", cmd.accelCmd);
ROS_INFO(" steer: %f", cmd.steerCmd);
ROS_INFO(" break: %f", cmd.brakeCmd);
client.sendCommand(cmd);
}
int main(int argc, char **argv) {
ros::init(argc, argv, "torcs");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/commands", 1000, commandMsgCallback);
ros::Publisher pub = n.advertise<std_msgs::Int8MultiArray>("/camera", 1000);
ros::Rate loop_rate(100);
while (ros::ok()) {
std_msgs::Int8MultiArray msg;
msg.data.clear();
msg.data.resize(torcs::IMAGE_SIZE_BYTES);
const auto imageData = client.getScreenshot();
memcpy(msg.data.data(), imageData->data(), imageData->size());
pub.publish(msg);
loop_rate.sleep();
}
return 0;
}
#include "torcsclient.h"
#include <iostream>
#include <chrono>
#include <thread>
#include <cstring>
#include <sys/shm.h>
#include <assert.h>
#include <FreeImage.h>
constexpr auto IMAGE_WIDTH = 640;
constexpr auto IMAGE_HEIGHT = 480;
constexpr auto IMAGE_CHANNELS = 3;
using namespace std::chrono_literals;
struct shared_use_st
{
int written = 0; //a label, if 1: available to read, if 0: available to write
uint8_t data[IMAGE_WIDTH*IMAGE_HEIGHT*IMAGE_CHANNELS]; // image data field
int control = 0;
int pause = 0;
double fast = 0.0;
double dist_L = 0.0;
double dist_R = 0.0;
double toMarking_L = 0.0;
double toMarking_M = 0.0;
double toMarking_R = 0.0;
double dist_LL = 0.0;
double dist_MM = 0.0;
double dist_RR = 0.0;
double toMarking_LL = 0.0;
double toMarking_ML = 0.0;
double toMarking_MR = 0.0;
double toMarking_RR = 0.0;
double toMiddle = 0.0;
double angle = 0.0;
double speed = 0.0;
double steerCmd = 0.0;
double accelCmd = 0.0;
double brakeCmd = 0.0;
};
void writeScreenshot(shared_use_st* shared, unsigned int& num)
{
std::cout << "Read image." << std::endl;
FIBITMAP* newShot = FreeImage_Allocate(IMAGE_WIDTH, IMAGE_HEIGHT, 24);
if (!newShot)
{
std::cerr << "Failed to allocate freeimage file." << std::endl;
FreeImage_Unload(newShot);
return;
}
BYTE *bits = (BYTE*)FreeImage_GetBits(newShot);
for (int h = 0; h < IMAGE_HEIGHT; h++) {
BYTE *pixel = (BYTE*)bits;
for (int w = 0; w < IMAGE_WIDTH; w++) {
pixel[FI_RGBA_RED] = shared->data[((h)*IMAGE_WIDTH+w)*3+0];
pixel[FI_RGBA_GREEN] = shared->data[((h)*IMAGE_WIDTH+w)*3+1];
pixel[FI_RGBA_BLUE] = shared->data[((h)*IMAGE_WIDTH+w)*3+2];
pixel += 3;
}
bits += 3*IMAGE_WIDTH;
}
FreeImage_Save(FIF_BMP, newShot, (std::string("shot-")+std::to_string(num)).c_str());
FreeImage_Unload(newShot);
shared->control = 0;
shared->written = 0;
std::this_thread::sleep_for(std::chrono::seconds(2));std::cout << "Read image." << std::endl;
using namespace torcs;
shared->control = 0;
shared->written = 0;
}
int main()
TorcsClient::TorcsClient()
{
std::cout << "TORCS Client!" << std::endl;
FreeImage_Initialise();
void *shm = NULL;
struct shared_use_st *shared;
int shmid;
shmid = shmget((key_t)4567, sizeof(struct shared_use_st), 0666);
if(shmid < 0)
{
std::cerr << "Failed to get shared memory!" << std::endl;
exit(EXIT_FAILURE);
return;
}
shm = shmat(shmid, 0, 0);
if(shm == (void*)-1)
{
std::cerr << "Failed to shmat()!" << std::endl;
exit(EXIT_FAILURE);
return;
}
connected = true;
std::cout << "Started shared memory at " << std::hex << shm << std::endl;
......@@ -136,36 +63,69 @@ int main()
shared->steerCmd = 0.0;
shared->accelCmd = 0.0;
shared->brakeCmd = 0.0;
}
unsigned num = 0;
while (1) {
if (shared->control == 1) {
std::cout << "Control == 1!" << std::endl;
shared->control = 1;
shared->accelCmd = 10.0;
shared->steerCmd = 1.0;
std::this_thread::sleep_for(std::chrono::milliseconds(500));
} else
{
std::cout << "Control == 0!" << std::endl;
shared->control = 1;
shared->accelCmd = 10.0;
shared->steerCmd = 1.0;
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
if (shared->written == 1) {
//writeScreenshot(shared, num);
std::unique_ptr<DataContainer> TorcsClient::getScreenshot()
{
assert(connected);
while(shared->written == 0)
{
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
auto result = std::make_unique<DataContainer>();
memcpy(result->data(), shared->data, result->size());
shared->written = 0;
return result;
}
void TorcsClient::sendCommand(Command cmd)
{
assert(connected);
while (shared->control == 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
shared->accelCmd = cmd.accelCmd;
shared->steerCmd = cmd.steerCmd;
shared->brakeCmd = cmd.brakeCmd;
shared->control = 0;
}
std::cout << "Speed: " << shared->speed << std::endl;
std::cout << "Steer: " << shared->angle << std::endl;
std::cout << "ToMiddle: " << shared->toMiddle << std::endl;
/*void writeScreenshot(shared_use_st* shared, unsigned int& num)
{
std::cout << "Read image." << std::endl;
std::this_thread::sleep_for(std::chrono::seconds(1));
num++;
FIBITMAP* newShot = FreeImage_Allocate(IMAGE_WIDTH, IMAGE_HEIGHT, 24);
if (!newShot)
{
std::cerr << "Failed to allocate freeimage file." << std::endl;
FreeImage_Unload(newShot);
return;
}
BYTE *bits = (BYTE*)FreeImage_GetBits(newShot);
for (int h = 0; h < IMAGE_HEIGHT; h++) {
BYTE *pixel = (BYTE*)bits;
for (int w = 0; w < IMAGE_WIDTH; w++) {
pixel[FI_RGBA_RED] = shared->data[((h)*IMAGE_WIDTH+w)*3+0];
pixel[FI_RGBA_GREEN] = shared->data[((h)*IMAGE_WIDTH+w)*3+1];
pixel[FI_RGBA_BLUE] = shared->data[((h)*IMAGE_WIDTH+w)*3+2];
pixel += 3;
}
bits += 3*IMAGE_WIDTH;
}
FreeImage_DeInitialise();
FreeImage_Save(FIF_BMP, newShot, (std::string("shot-")+std::to_string(num)).c_str());
FreeImage_Unload(newShot);
shared->control = 0;
shared->written = 0;
std::this_thread::sleep_for(std::chrono::seconds(2));std::cout << "Read image." << std::endl;
}*/
return 0;
}
#pragma once
#include <cstdint>
#include <array>
#include <memory>
namespace torcs
{
constexpr auto IMAGE_WIDTH = 640;
constexpr auto IMAGE_HEIGHT = 480;
constexpr auto IMAGE_CHANNELS = 1;
constexpr auto IMAGE_SIZE_BYTES = IMAGE_WIDTH*IMAGE_HEIGHT*IMAGE_CHANNELS;
struct shared_use_st
{
int written = 0; //a label, if 1: available to read, if 0: available to write
uint8_t data[IMAGE_SIZE_BYTES]; // image data field
int control = 0;
int pause = 0;
double fast = 0.0;
double dist_L = 0.0;
double dist_R = 0.0;
double toMarking_L = 0.0;
double toMarking_M = 0.0;
double toMarking_R = 0.0;
double dist_LL = 0.0;
double dist_MM = 0.0;
double dist_RR = 0.0;
double toMarking_LL = 0.0;
double toMarking_ML = 0.0;
double toMarking_MR = 0.0;
double toMarking_RR = 0.0;
double toMiddle = 0.0;
double angle = 0.0;
double speed = 0.0;
double steerCmd = 0.0;
double accelCmd = 0.0;
double brakeCmd = 0.0;
};
struct Command
{
double steerCmd = 0.0;
double accelCmd = 0.0;
double brakeCmd = 0.0;
};
using DataContainer = std::array<uint8_t, IMAGE_SIZE_BYTES>;
class TorcsClient
{
public:
TorcsClient();
std::unique_ptr<DataContainer> getScreenshot();
void sendCommand(Command cmd);
private:
struct shared_use_st *shared;
bool connected = false;
};
}
<component name="libraryTable">
<library name="Maven: de.monticore.lang:NumberUnit:0.0.10-SNAPSHOT">
<CLASSES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/NumberUnit/0.0.10-SNAPSHOT/NumberUnit-0.0.10-20180410.013402-1.jar!/" />
</CLASSES>
<JAVADOC>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/NumberUnit/0.0.10-SNAPSHOT/NumberUnit-0.0.10-20180410.013402-1-javadoc.jar!/" />
</JAVADOC>
<SOURCES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/NumberUnit/0.0.10-SNAPSHOT/NumberUnit-0.0.10-20180410.013402-1-sources.jar!/" />
</SOURCES>
</library>
</component>
\ No newline at end of file
<component name="libraryTable">
<library name="Maven: de.monticore.lang.monticar:embedded-montiarc-math:0.0.10-SNAPSHOT">
<CLASSES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/monticar/embedded-montiarc-math/0.0.10-SNAPSHOT/embedded-montiarc-math-0.0.10-SNAPSHOT.jar!/" />
</CLASSES>
<JAVADOC>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/monticar/embedded-montiarc-math/0.0.10-SNAPSHOT/embedded-montiarc-math-0.0.10-SNAPSHOT-javadoc.jar!/" />
</JAVADOC>
<SOURCES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/monticar/embedded-montiarc-math/0.0.10-SNAPSHOT/embedded-montiarc-math-0.0.10-SNAPSHOT-sources.jar!/" />
</SOURCES>
</library>
</component>
\ No newline at end of file
This diff is collapsed.
......@@ -2,7 +2,7 @@ package dp;
component Featureextraction {
ports
in Z(0:255)^{512,3} imageIn,
in Z(0:255)^{640,480} imageIn,
out Q(0:1)^{13,1} affordanceOut;
}
......@@ -2,6 +2,6 @@ package dp;
conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema;
tags Featureextraction {
tag featureextraction.imageIn with RosConnection = {topic=(/images,std_msgs/Float64MultiArray)};
tag featureextraction.affordanceOut with RosConnection = {topic=(/affordance,std_msgs/Float64MultiArray)};
tag featureextraction.imageIn with RosConnection = {topic=(/images,std_msgs/Int8MultiArray)};
tag featureextraction.affordanceOut with RosConnection = {topic=(/affordance,std_msgs/Int8MultiArray)};
}
......@@ -2,8 +2,8 @@ package dp;
component Imagepreprocessing {
ports
in Z(0:255)^{512,1} imageIn,
out Z(0:255)^{512,1} imageOut;
in Z(0:255)^{640,480} imageIn,
out Z(0:255)^{640,480} imageOut;
implementation Math {
imageOut=imageIn;
......
......@@ -2,6 +2,6 @@ package dp;
conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema;
tags Imagepreprocessing {
tag imagepreprocessing.imageIn with RosConnection = {topic=(/camera,std_msgs/Float64MultiArray)};
tag imagepreprocessing.imageOut with RosConnection = {topic=(/images,std_msgs/Float64MultiArray)};
tag imagepreprocessing.imageIn with RosConnection = {topic=(/camera,std_msgs/Int8MultiArray)};
tag imagepreprocessing.imageOut with RosConnection = {topic=(/images,std_msgs/Int8MultiArray)};
}
......@@ -2,7 +2,7 @@ package dp;
component Safetycontroller {
ports
in Z(0:255)^{512,1} imageIn,
in Z(0:255)^{640,480} imageIn,
in Q(0:1)^{13,1} affordanceIn,
out Q(0:1)^{1,1} safetyLevelOut;
......
......@@ -2,7 +2,7 @@ package dp;
conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema;
tags Safetycontroller {
tag safetycontroller.imageIn with RosConnection = {topic=(/images,std_msgs/Float64MultiArray)};
tag safetycontroller.imageIn with RosConnection = {topic=(/images,std_msgs/Int8MultiArray)};
tag safetycontroller.affordanceIn with RosConnection = {topic=(/affordance,std_msgs/Float64MultiArray)};
tag safetycontroller.safetyLevelOut with RosConnection = {topic=(/safety,std_msgs/Float64MultiArray)};
}
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