Aufgrund einer Wartung wird GitLab am 17.08. zwischen 8:30 und 9:00 Uhr kurzzeitig nicht zur Verfügung stehen. / Due to maintenance, GitLab will be temporarily unavailable on 17.08. between 8:30 and 9:00 am.

Commit 0a2262df authored by Svetlana's avatar Svetlana

Fixed TORCS communication. Expanded angles buffer. Fixed mistakes in driver cntroller component.

parent 88391930
...@@ -2,11 +2,12 @@ package dp.subcomponents; ...@@ -2,11 +2,12 @@ package dp.subcomponents;
component AngleBuffer { component AngleBuffer {
ports in Affordance inputAffordance, ports in Affordance inputAffordance,
out Q^{4} outputBuffer; out Q^{5} outputBuffer;
implementation Math { implementation Math {
static Q^{4} buffer = [0; 0; 0; 0]; static Q^{5} buffer = [0; 0; 0; 0; 0];
buffer(5) = buffer(4);
buffer(4) = buffer(3); buffer(4) = buffer(3);
buffer(3) = buffer(2); buffer(3) = buffer(2);
buffer(2) = buffer(1); buffer(2) = buffer(1);
...@@ -16,5 +17,6 @@ component AngleBuffer { ...@@ -16,5 +17,6 @@ component AngleBuffer {
outputBuffer(2) = buffer(2); outputBuffer(2) = buffer(2);
outputBuffer(3) = buffer(3); outputBuffer(3) = buffer(3);
outputBuffer(4) = buffer(4); outputBuffer(4) = buffer(4);
outputBuffer(5) = buffer(5);
} }
} }
\ No newline at end of file
...@@ -2,7 +2,7 @@ package dp.subcomponents; ...@@ -2,7 +2,7 @@ package dp.subcomponents;
component Drivercontroller { component Drivercontroller {
ports ports
in Q^{4} anglesIn, in Q^{5} anglesIn,
in Affordance affordanceIn, in Affordance affordanceIn,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn, in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
out Q(0:1)^{3} commandsOut; out Q(0:1)^{3} commandsOut;
...@@ -16,8 +16,8 @@ component Drivercontroller { ...@@ -16,8 +16,8 @@ component Drivercontroller {
if (affordanceIn.distMM < 15) if (affordanceIn.distMM < 15)
Q vMax = 20; Q vMax = 20;
Q vC = 2.772; Q vC = 2.772;
Q vD = 0.693; Q vD = -0.693;
slowDown = vMax * (exp(-(vC/vMax) * affordanceIn.distMM - vD)); slowDown = vMax * (1-exp(-(vC/vMax) * affordanceIn.distMM - vD));
if (slowDown < 0) if (slowDown < 0)
slowDown = 0; slowDown = 0;
...@@ -48,7 +48,16 @@ component Drivercontroller { ...@@ -48,7 +48,16 @@ component Drivercontroller {
// steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS // steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
commandsOut(2) = (affordanceIn.angle - centerLine/roadWidth) / 0.541052/coeSteer; commandsOut(2) = (affordanceIn.angle - centerLine/roadWidth) / 0.541052/coeSteer;
Q desiredSpeed = 20; Q desiredSpeed = 20;
if (affordanceIn.fast != 1)
desiredSpeed = 20 - abs(anglesIn(0) + anglesIn(1) + anglesIn(2) + anglesIn(3) + anglesIn(4)) * 4.5;
end
if (desiredSpeed < 10)
desiredSpeed = 10;
end
if (desiredSpeed > slowDown) if (desiredSpeed > slowDown)
desiredSpeed = slowDown; desiredSpeed = slowDown;
end end
...@@ -58,18 +67,17 @@ component Drivercontroller { ...@@ -58,18 +67,17 @@ component Drivercontroller {
Q accelCmd = 0.2*(desiredSpeed - speedIn + 1); Q accelCmd = 0.2*(desiredSpeed - speedIn + 1);
if (accelCmd > 1) if (accelCmd > 1)
accelCmd = 1; accelCmd = 1;
else
commandsOut(1) = accelCmd;
end end
commandsOut(1) = accelCmd;
commandsOut(3) = 0; commandsOut(3) = 0;
else else
Q brakeCmd = 0.1 * (speedIn - desiredSpeed); Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
if (brakeCmd > 1) if (brakeCmd > 1)
commandsOut(3) = 1; commandsOut(3) = 1;
else
commandsOut(3) = brakeCmd;
end end
commandsOut(3) = brakeCmd;
commandsOut(1) = 0; commandsOut(1) = 0;
end end
} }
......
...@@ -13,12 +13,12 @@ constexpr auto ROS_IMAGE_WIDTH = 280; ...@@ -13,12 +13,12 @@ constexpr auto ROS_IMAGE_WIDTH = 280;
constexpr auto ROS_IMAGE_HEIGHT = 210; constexpr auto ROS_IMAGE_HEIGHT = 210;
constexpr auto ROS_IMAGE_SIZE = ROS_IMAGE_WIDTH*ROS_IMAGE_HEIGHT*3; constexpr auto ROS_IMAGE_SIZE = ROS_IMAGE_WIDTH*ROS_IMAGE_HEIGHT*3;
torcs::Command cmd(1., 0., 0.); torcs::TorcsClient client;
std::unique_ptr<torcs::DataContainer> screenshot = std::make_unique<torcs::DataContainer>();
void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg) 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]));
//client->sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2]));
} }
int main(int argc, char **argv) int main(int argc, char **argv)
...@@ -31,15 +31,13 @@ int main(int argc, char **argv) ...@@ -31,15 +31,13 @@ int main(int argc, char **argv)
ros::Rate loop_rate(10); ros::Rate loop_rate(10);
int count = 0; int count = 0;
torcs::TorcsClient client;
while (ros::ok()) while (ros::ok())
{ {
float speed; float speed;
std::unique_ptr<torcs::DataContainer> screenshot = std::make_unique<torcs::DataContainer>(); client.iterate(screenshot, speed);
client.iterate(screenshot, speed, cmd);
cv::Mat image(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS)); cv::Mat image(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
memcpy(image.data, screenshot.get(), torcs::IMAGE_SIZE_BYTES); memcpy(image.data, screenshot->data(), torcs::IMAGE_SIZE_BYTES);
cv::Mat resizedImage(ROS_IMAGE_HEIGHT, ROS_IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS)); 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::resize(image, resizedImage, cv::Size(ROS_IMAGE_WIDTH, ROS_IMAGE_HEIGHT));
......
...@@ -38,78 +38,66 @@ TorcsClient::TorcsClient() ...@@ -38,78 +38,66 @@ TorcsClient::TorcsClient()
//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; 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->fast = 0.0;
} shared->dist_L = 0.0;
shared->dist_R = 0.0;
std::unique_ptr<DataContainer> TorcsClient::getScreenshot(float& speed) shared->toMarking_L = 0.0;
{ shared->toMarking_M = 0.0;
assert(connected); shared->toMarking_R = 0.0;
while(shared->written == 0) shared->dist_LL = 0.0;
{ shared->dist_MM = 0.0;
std::this_thread::sleep_for(std::chrono::milliseconds(100)); shared->dist_RR = 0.0;
}
shared->pause = 1; shared->toMarking_LL = 0.0;
auto result = std::make_unique<DataContainer>(); shared->toMarking_ML = 0.0;
memcpy(result->data(), shared->data, result->size()); shared->toMarking_MR = 0.0;
speed = shared->speed; shared->toMarking_RR = 0.0;
shared->pause = 0;
shared->written = 0;
return result; shared->toMiddle = 0.0;
} shared->angle = 0.0;
shared->speed = 0.0;
void TorcsClient::sendCommand(Command cmd) shared->steerCmd = 0.0;
{ shared->accelCmd = 0.0;
assert(connected); shared->brakeCmd = 0.0;
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) void TorcsClient::sendCommand(const Command& cmd)
{ {
//ROS_INFO("Iteration"); gotCommand = true;
command = cmd;
}
void TorcsClient::iterate(std::unique_ptr<DataContainer>& screenshot, float& speed)
{
assert(connected); assert(connected);
bool commandSent = false;
bool screenshotRead = false;
while (!(commandSent && screenshotRead)) { #pragma clang diagnostic push
if (shared->written == 0) { #pragma clang diagnostic ignored "-Wmissing-noreturn"
//ROS_INFO("Commanding"); while (true) {
if (!commandSent) if (shared->written == 1) { // the new image data is ready to be read
{ memcpy(screenshot->data(), shared->data, screenshot->size());
//ROS_INFO("Command done"); speed = float(shared->speed);
shared->accelCmd = cmd.accelCmd;
shared->steerCmd = cmd.steerCmd; if (gotCommand && shared->control == 1)
shared->brakeCmd = cmd.brakeCmd;
shared->control = 1;
shared->written = 1;
commandSent = true;
}
} else {
//ROS_INFO("Reading");
if (!screenshotRead)
{ {
//ROS_INFO("Reading done"); gotCommand = false;
shared->pause = 1; ROS_INFO("Command acc: %f, brake: %f, steer: %f", command.accelCmd, command.brakeCmd, command.steerCmd);
memcpy(screenshot->data(), shared->data, screenshot->size()); shared->accelCmd = command.accelCmd;
speed = float(shared->speed); shared->steerCmd = command.steerCmd;
shared->pause = 0; shared->brakeCmd = command.brakeCmd;
shared->written = 0;
screenshotRead = true;
} }
shared->written = 0;
break;
} }
} }
#pragma clang diagnostic pop
} }
...@@ -47,14 +47,15 @@ struct shared_use_st ...@@ -47,14 +47,15 @@ struct shared_use_st
struct Command struct Command
{ {
Command() = default;
Command(float accel, float steer, float brake) Command(float accel, float steer, float brake)
: accelCmd(accel), steerCmd(steer), brakeCmd(brake) : accelCmd(accel), steerCmd(steer), brakeCmd(brake)
{ {
} }
float accelCmd; float accelCmd = 0.f;
float steerCmd; float steerCmd = 0.f;
float brakeCmd; float brakeCmd = 0.f;
}; };
using DataContainer = std::array<uint8_t, IMAGE_SIZE_BYTES>; using DataContainer = std::array<uint8_t, IMAGE_SIZE_BYTES>;
...@@ -63,14 +64,16 @@ class TorcsClient ...@@ -63,14 +64,16 @@ class TorcsClient
{ {
public: public:
TorcsClient(); TorcsClient();
std::unique_ptr<DataContainer> getScreenshot(float& speed);
void sendCommand(Command cmd);
void iterate(std::unique_ptr<DataContainer>& screenshot, float& speed, const Command& cmd); void iterate(std::unique_ptr<DataContainer>& screenshot, float& speed);
void sendCommand(const Command& cmd);
private: private:
struct shared_use_st *shared; struct shared_use_st *shared;
bool connected = false; bool connected = false;
bool gotCommand = false;
Command command;
}; };
} }
......
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