Skip to content

GitLab

Aufgrund einer Störung des s3 Storage, könnten in nächster Zeit folgende GitLab Funktionen nicht zur Verfügung stehen: LFS, Container Registry, Job Artifacs, Uploads (Wiki, Bilder, Projekt-Exporte). Wir bitten um Verständnis. Es wird mit Hochdruck an der Behebung des Problems gearbeitet. Weitere Informationen zur Störung des Object Storage finden Sie hier: https://maintenance.itc.rwth-aachen.de/ticket/status/messages/59-object-storage-pilot

Commit a0f0db52 authored by Svetlana's avatar Svetlana
Browse files

Fixed TORCS communication sync, added speed messages, implemented within-lane acceleration control.

parent b16a9a20
Hide whitespace changes
Inline Side-by-side
Showing with 145 additions and 32 deletions
TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl
View file @ a0f0db52
......@@ -3,8 +3,9 @@ import dp.subcomponents.*;
component Mastercomponent {
ports
in Z(0:255)^{3, 210, 280} imageIn,
out Q(0:1)^{3,1,1} commandsOut;
in Z(0:255)^{3, 210, 280} imageIn,
in Q speedIn,
out Q(0:1)^{3,1,1} commandsOut;
instance Imagepreprocessing ip;
instance Dpnet dpnet;
......@@ -13,6 +14,6 @@ component Mastercomponent {
connect imageIn -> ip.imageIn;
connect ip.imageOut -> dpnet.image;
connect dpnet.predictions -> dc.affordanceIn;
connect speedIn -> dc.speedIn;
connect dc.commandsOut -> commandsOut;
}
\ No newline at end of file
TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.tag
View file @ a0f0db52
......@@ -3,5 +3,6 @@ conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema;
tags Mastercomponent {
tag mastercomponent.imageIn with RosConnection = {topic=(/camera,std_msgs/Int8MultiArray)};
tag mastercomponent.speedIn with RosConnection = {topic=(/speed,std_msgs/Float32)};
tag mastercomponent.commandsOut with RosConnection = {topic=(/commands,std_msgs/Float32MultiArray)};
}
TorcsEMAMGenerator/src/main/models/dp/subcomponents/Dpnet.emadl
View file @ a0f0db52
......@@ -28,6 +28,5 @@ component Dpnet{
FullyConnected(units=13) ->
Sigmoid() ->
predictions
}
}
TorcsEMAMGenerator/src/main/models/dp/subcomponents/Drivercontroller.emadl
View file @ a0f0db52
......@@ -3,6 +3,7 @@ package dp.subcomponents;
component Drivercontroller {
ports
in Q(0:1)^{1,1,13} affordanceIn,
in Q speedIn,
out Q(0:1)^{3,1,1} commandsOut;
implementation Math {
......@@ -13,15 +14,31 @@ component Drivercontroller {
Q distToLaneCenter = abs(laneWidth/2 - affordanceIn(0,0,2));
// Steering
commandsOut(2,1,1) = Constant * (angle-distToLaneCenter/roadWidth); // steering
commandsOut(2,1,1) = 0;//Constant * (angle-distToLaneCenter/roadWidth); // steering
// Acceleration
Q vMax = 72;
Q vMax = 25;
Q vC = 1;
Q vD = 0;
Q desiredSpeed = vMax * (exp(-(vC/vMax) * affordanceIn(0,0,6) - vD));
commandsOut(1,1,1) = desiredSpeed/vMax; // acceleration
Q desiredSpeed = 15;//vMax * (exp(-(vC/vMax) * affordanceIn(0,0,6) - vD));
commandsOut(3,1,1)=0.0; // brake
Q accelCmd = 0.1 * (desiredSpeed - speedIn + 1);
if accelCmd > 1
accelCmd = 1.0;
end
Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
if brakeCmd > 1
brakeCmd = 1;
end
// acceleration and brake
if speedIn < desiredSpeed
commandsOut(1,1,1) = accelCmd;
commandsOut(3,1,1) = 0;
else
commandsOut(1,1,1) = 0;
commandsOut(3,1,1) = brakeCmd;
end
}
}
\ No newline at end of file
TorcsEMAMGenerator/src/main/models/dp/subcomponents/Imagepreprocessing.emadl
View file @ a0f0db52
......@@ -6,6 +6,6 @@ component Imagepreprocessing {
out Z(0:255)^{3, 210, 280} imageOut;
implementation Math {
imageOut=imageIn;
imageOut = imageIn;
}
}
\ No newline at end of file
TorcsEMAMGenerator/src/main/resources/scripts/run_all.sh
View file @ a0f0db52
......@@ -5,7 +5,7 @@ xterm -T ROSCORE -e roscore &
echo "Waiting for ROSCORE to start..."
sleep 2
xterm -T TORCSDriver -e torcs/src/driver/build/driver &
#xterm -T TORCSDriver -e torcs/src/driver/build/driver &
xterm -T TORCSCamera -e torcs/src/camera/build/camera &
......
TorcsEMAMGenerator/src/main/resources/torcs/build.sh
View file @ a0f0db52
......@@ -12,10 +12,10 @@ cmake ..
make -j
cd ../..
cd driver
mkdir -p build
cd build
cmake ..
make -j
cd ../..
#cd driver
#mkdir -p build
#cd build
#cmake ..
#make -j
#cd ../..
TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/main.cpp
View file @ a0f0db52
#include "ros/ros.h"
#include "std_msgs/Int8MultiArray.h"
#include "std_msgs/Float32.h"
#include "torcsclient.h"
#include <sstream>
......@@ -7,23 +8,37 @@
#include <ml.h>
#include <highgui.h>
#include <cv.h>
#include <ros/console.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::Command cmd(1., 0., 0.);
void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
cmd = torcs::Command(msg->data[0], msg->data[1], msg->data[2]);
//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 chatter_pub = n.advertise<std_msgs::Int8MultiArray>("/camera", 1000);
ros::Publisher cameraPub = n.advertise<std_msgs::Int8MultiArray>("/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;
torcs::TorcsClient client;
while (ros::ok())
{
const auto screenshot = client.getScreenshot();
float speed;
std::unique_ptr<torcs::DataContainer> screenshot = std::make_unique<torcs::DataContainer>();
client.iterate(screenshot, speed, cmd);
cv::Mat image(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
memcpy(image.data, screenshot.get(), torcs::IMAGE_SIZE_BYTES);
......@@ -41,7 +56,11 @@ int main(int argc, char **argv)
msg->data.resize(ROS_IMAGE_SIZE);
memcpy(msg->data.data(), resizedImage.data, ROS_IMAGE_SIZE);
chatter_pub.publish(msg);
std_msgs::Float32::Ptr speedMsg(new std_msgs::Float32);
speedMsg->data = speed;
cameraPub.publish(msg);
speedPub.publish(speedMsg);
ros::spinOnce();
loop_rate.sleep();
++count;
......
TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.cpp
View file @ a0f0db52
......@@ -6,6 +6,7 @@
#include <cstring>
#include <sys/shm.h>
#include <assert.h>
#include <ros/console.h>
using namespace torcs;
......@@ -19,26 +20,28 @@ TorcsClient::TorcsClient()
shmid = shmget((key_t)4567, sizeof(struct shared_use_st), 0666);
if(shmid < 0)
{
std::cerr << "Failed to get shared memory!" << std::endl;
return;
//std::cerr << "Failed to get shared memory!" << std::endl;
ROS_ERROR("Failed to get shared memory!");
return;
}
shm = shmat(shmid, 0, 0);
if(shm == (void*)-1)
{
std::cerr << "Failed to shmat()!" << std::endl;
ROS_ERROR("Failed to shmat()!");
//std::cerr << "Failed to shmat()!" << std::endl;
return;
}
connected = true;
std::cout << "Started shared memory at " << std::hex << shm << std::endl;
//std::cout << "Started shared memory at " << std::hex << shm << std::endl;
shared = (struct shared_use_st*)shm;
}
std::unique_ptr<DataContainer> TorcsClient::getScreenshot()
std::unique_ptr<DataContainer> TorcsClient::getScreenshot(float& speed)
{
assert(connected);
......@@ -48,12 +51,65 @@ std::unique_ptr<DataContainer> TorcsClient::getScreenshot()
}
shared->pause = 1;
std::cout << "Got screenshot" << std::endl;
auto result = std::make_unique<DataContainer>();
memcpy(result->data(), shared->data, result->size());
speed = shared->speed;
shared->pause = 0;
shared->written = 0;
return result;
}
void TorcsClient::sendCommand(Command cmd)
{
assert(connected);
while (shared->written == 1) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
//ROS_INFO("Command acc: %f, brake: %f, steer: %f", cmd.accelCmd, cmd.brakeCmd, cmd.steerCmd);
shared->accelCmd = cmd.accelCmd;
shared->steerCmd = cmd.steerCmd;
shared->brakeCmd = cmd.brakeCmd;
shared->control = 1;
shared->written = 1;
}
void TorcsClient::iterate(std::unique_ptr<DataContainer>& screenshot, float& speed, const Command& cmd)
{
//ROS_INFO("Iteration");
assert(connected);
bool commandSent = false;
bool screenshotRead = false;
while (!(commandSent && screenshotRead)) {
if (shared->written == 0) {
//ROS_INFO("Commanding");
if (!commandSent)
{
//ROS_INFO("Command done");
shared->accelCmd = cmd.accelCmd;
shared->steerCmd = cmd.steerCmd;
shared->brakeCmd = cmd.brakeCmd;
shared->control = 1;
shared->written = 1;
commandSent = true;
}
} else {
//ROS_INFO("Reading");
if (!screenshotRead)
{
//ROS_INFO("Reading done");
shared->pause = 1;
memcpy(screenshot->data(), shared->data, screenshot->size());
speed = float(shared->speed);
shared->pause = 0;
shared->written = 0;
screenshotRead = true;
}
}
}
}
TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.h
View file @ a0f0db52
......@@ -45,13 +45,28 @@ struct shared_use_st
double brakeCmd = 0.0;
};
struct Command
{
Command(float accel, float steer, float brake)
: accelCmd(accel), steerCmd(steer), brakeCmd(brake)
{
}
float accelCmd;
float steerCmd;
float brakeCmd;
};
using DataContainer = std::array<uint8_t, IMAGE_SIZE_BYTES>;
class TorcsClient
{
public:
TorcsClient();
std::unique_ptr<DataContainer> getScreenshot();
std::unique_ptr<DataContainer> getScreenshot(float& speed);
void sendCommand(Command cmd);
void iterate(std::unique_ptr<DataContainer>& screenshot, float& speed, const Command& cmd);
private:
struct shared_use_st *shared;
......
TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/main.cpp
View file @ a0f0db52
......@@ -8,6 +8,7 @@ std::unique_ptr<torcs::TorcsClient> client;
void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
ROS_INFO("Received data");
client->sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2]));
}
......@@ -19,7 +20,7 @@ int main(int argc, char **argv)
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/commands", 100, chatterCallback);
ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback);
ros::spin();
return 0;
......
TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.cpp
View file @ a0f0db52
......@@ -6,6 +6,7 @@
#include <cstring>
#include <sys/shm.h>
#include <assert.h>
#include <ros/console.h>
using namespace torcs;
......@@ -19,20 +20,22 @@ TorcsClient::TorcsClient()
shmid = shmget((key_t)4567, sizeof(struct shared_use_st), 0666);
if(shmid < 0)
{
std::cerr << "Failed to get shared memory!" << std::endl;
//std::cerr << "Failed to get shared memory!" << std::endl;
ROS_ERROR("Failed to get shared memory!");
return;
}
shm = shmat(shmid, 0, 0);
if(shm == (void*)-1)
{
std::cerr << "Failed to shmat()!" << std::endl;
ROS_ERROR("Failed to shmat()!");
//std::cerr << "Failed to shmat()!" << std::endl;
return;
}
connected = true;
std::cout << "Started shared memory at " << std::hex << shm << std::endl;
//std::cout << "Started shared memory at " << std::hex << shm << std::endl;
shared = (struct shared_use_st*)shm;
}
......@@ -45,7 +48,8 @@ void TorcsClient::sendCommand(Command cmd)
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
std::cout << "Sent command" << std::endl;
ROS_INFO("Command acc: %f, brake: %f, steer: %f", cmd.accelCmd, cmd.brakeCmd, cmd.steerCmd);
shared->accelCmd = cmd.accelCmd;
shared->steerCmd = cmd.steerCmd;
shared->brakeCmd = cmd.brakeCmd;
......
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