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;
component AngleBuffer {
ports in Affordance inputAffordance,
out Q^{4} outputBuffer;
out Q^{5} outputBuffer;
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(3) = buffer(2);
buffer(2) = buffer(1);
......@@ -16,5 +17,6 @@ component AngleBuffer {
outputBuffer(2) = buffer(2);
outputBuffer(3) = buffer(3);
outputBuffer(4) = buffer(4);
outputBuffer(5) = buffer(5);
}
}
\ No newline at end of file
......@@ -2,7 +2,7 @@ package dp.subcomponents;
component Drivercontroller {
ports
in Q^{4} anglesIn,
in Q^{5} anglesIn,
in Affordance affordanceIn,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
out Q(0:1)^{3} commandsOut;
......@@ -16,8 +16,8 @@ component Drivercontroller {
if (affordanceIn.distMM < 15)
Q vMax = 20;
Q vC = 2.772;
Q vD = 0.693;
slowDown = vMax * (exp(-(vC/vMax) * affordanceIn.distMM - vD));
Q vD = -0.693;
slowDown = vMax * (1-exp(-(vC/vMax) * affordanceIn.distMM - vD));
if (slowDown < 0)
slowDown = 0;
......@@ -48,7 +48,16 @@ component Drivercontroller {
// steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
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)
desiredSpeed = slowDown;
end
......@@ -58,18 +67,17 @@ component Drivercontroller {
Q accelCmd = 0.2*(desiredSpeed - speedIn + 1);
if (accelCmd > 1)
accelCmd = 1;
else
commandsOut(1) = accelCmd;
end
commandsOut(1) = accelCmd;
commandsOut(3) = 0;
else
Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
if (brakeCmd > 1)
commandsOut(3) = 1;
else
commandsOut(3) = brakeCmd;
end
commandsOut(3) = brakeCmd;
commandsOut(1) = 0;
end
}
......
......@@ -13,12 +13,12 @@ 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.);
torcs::TorcsClient client;
std::unique_ptr<torcs::DataContainer> screenshot = std::make_unique<torcs::DataContainer>();
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)
......@@ -31,15 +31,13 @@ int main(int argc, char **argv)
ros::Rate loop_rate(10);
int count = 0;
torcs::TorcsClient client;
while (ros::ok())
{
float speed;
std::unique_ptr<torcs::DataContainer> screenshot = std::make_unique<torcs::DataContainer>();
client.iterate(screenshot, speed, cmd);
client.iterate(screenshot, speed);
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::resize(image, resizedImage, cv::Size(ROS_IMAGE_WIDTH, ROS_IMAGE_HEIGHT));
......
......@@ -38,78 +38,66 @@ TorcsClient::TorcsClient()
//std::cout << "Started shared memory at " << std::hex << shm << std::endl;
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)
{
assert(connected);
shared->toMarking_L = 0.0;
shared->toMarking_M = 0.0;
shared->toMarking_R = 0.0;
while(shared->written == 0)
{
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
shared->dist_LL = 0.0;
shared->dist_MM = 0.0;
shared->dist_RR = 0.0;
shared->pause = 1;
auto result = std::make_unique<DataContainer>();
memcpy(result->data(), shared->data, result->size());
speed = shared->speed;
shared->pause = 0;
shared->written = 0;
shared->toMarking_LL = 0.0;
shared->toMarking_ML = 0.0;
shared->toMarking_MR = 0.0;
shared->toMarking_RR = 0.0;
return result;
}
shared->toMiddle = 0.0;
shared->angle = 0.0;
shared->speed = 0.0;
void TorcsClient::sendCommand(Command cmd)
{
assert(connected);
shared->steerCmd = 0.0;
shared->accelCmd = 0.0;
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);
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)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wmissing-noreturn"
while (true) {
if (shared->written == 1) { // the new image data is ready to be read
memcpy(screenshot->data(), shared->data, screenshot->size());
speed = float(shared->speed);
if (gotCommand && shared->control == 1)
{
//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;
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->written = 0;
break;
}
}
#pragma clang diagnostic pop
}
......@@ -47,14 +47,15 @@ struct shared_use_st
struct Command
{
Command() = default;
Command(float accel, float steer, float brake)
: accelCmd(accel), steerCmd(steer), brakeCmd(brake)
{
}
float accelCmd;
float steerCmd;
float brakeCmd;
float accelCmd = 0.f;
float steerCmd = 0.f;
float brakeCmd = 0.f;
};
using DataContainer = std::array<uint8_t, IMAGE_SIZE_BYTES>;
......@@ -63,14 +64,16 @@ class TorcsClient
{
public:
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:
struct shared_use_st *shared;
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