Commit a0f0db52 authored by Svetlana's avatar Svetlana
Browse files

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

parent b16a9a20
...@@ -4,6 +4,7 @@ import dp.subcomponents.*; ...@@ -4,6 +4,7 @@ import dp.subcomponents.*;
component Mastercomponent { component Mastercomponent {
ports ports
in Z(0:255)^{3, 210, 280} imageIn, in Z(0:255)^{3, 210, 280} imageIn,
in Q speedIn,
out Q(0:1)^{3,1,1} commandsOut; out Q(0:1)^{3,1,1} commandsOut;
instance Imagepreprocessing ip; instance Imagepreprocessing ip;
...@@ -13,6 +14,6 @@ component Mastercomponent { ...@@ -13,6 +14,6 @@ component Mastercomponent {
connect imageIn -> ip.imageIn; connect imageIn -> ip.imageIn;
connect ip.imageOut -> dpnet.image; connect ip.imageOut -> dpnet.image;
connect dpnet.predictions -> dc.affordanceIn; connect dpnet.predictions -> dc.affordanceIn;
connect speedIn -> dc.speedIn;
connect dc.commandsOut -> commandsOut; connect dc.commandsOut -> commandsOut;
} }
\ No newline at end of file
...@@ -3,5 +3,6 @@ conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema; ...@@ -3,5 +3,6 @@ conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema;
tags Mastercomponent { tags Mastercomponent {
tag mastercomponent.imageIn with RosConnection = {topic=(/camera,std_msgs/Int8MultiArray)}; 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)}; tag mastercomponent.commandsOut with RosConnection = {topic=(/commands,std_msgs/Float32MultiArray)};
} }
...@@ -28,6 +28,5 @@ component Dpnet{ ...@@ -28,6 +28,5 @@ component Dpnet{
FullyConnected(units=13) -> FullyConnected(units=13) ->
Sigmoid() -> Sigmoid() ->
predictions predictions
} }
} }
...@@ -3,6 +3,7 @@ package dp.subcomponents; ...@@ -3,6 +3,7 @@ package dp.subcomponents;
component Drivercontroller { component Drivercontroller {
ports ports
in Q(0:1)^{1,1,13} affordanceIn, in Q(0:1)^{1,1,13} affordanceIn,
in Q speedIn,
out Q(0:1)^{3,1,1} commandsOut; out Q(0:1)^{3,1,1} commandsOut;
implementation Math { implementation Math {
...@@ -13,15 +14,31 @@ component Drivercontroller { ...@@ -13,15 +14,31 @@ component Drivercontroller {
Q distToLaneCenter = abs(laneWidth/2 - affordanceIn(0,0,2)); Q distToLaneCenter = abs(laneWidth/2 - affordanceIn(0,0,2));
// Steering // Steering
commandsOut(2,1,1) = Constant * (angle-distToLaneCenter/roadWidth); // steering commandsOut(2,1,1) = 0;//Constant * (angle-distToLaneCenter/roadWidth); // steering
// Acceleration // Acceleration
Q vMax = 72; Q vMax = 25;
Q vC = 1; Q vC = 1;
Q vD = 0; Q vD = 0;
Q desiredSpeed = vMax * (exp(-(vC/vMax) * affordanceIn(0,0,6) - vD)); Q desiredSpeed = 15;//vMax * (exp(-(vC/vMax) * affordanceIn(0,0,6) - vD));
commandsOut(1,1,1) = desiredSpeed/vMax; // acceleration
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
...@@ -6,6 +6,6 @@ component Imagepreprocessing { ...@@ -6,6 +6,6 @@ component Imagepreprocessing {
out Z(0:255)^{3, 210, 280} imageOut; out Z(0:255)^{3, 210, 280} imageOut;
implementation Math { implementation Math {
imageOut=imageIn; imageOut = imageIn;
} }
} }
\ No newline at end of file
...@@ -5,7 +5,7 @@ xterm -T ROSCORE -e roscore & ...@@ -5,7 +5,7 @@ xterm -T ROSCORE -e roscore &
echo "Waiting for ROSCORE to start..." echo "Waiting for ROSCORE to start..."
sleep 2 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 & xterm -T TORCSCamera -e torcs/src/camera/build/camera &
......
...@@ -12,10 +12,10 @@ cmake .. ...@@ -12,10 +12,10 @@ cmake ..
make -j make -j
cd ../.. cd ../..
cd driver #cd driver
mkdir -p build #mkdir -p build
cd build #cd build
cmake .. #cmake ..
make -j #make -j
cd ../.. #cd ../..
#include "ros/ros.h" #include "ros/ros.h"
#include "std_msgs/Int8MultiArray.h" #include "std_msgs/Int8MultiArray.h"
#include "std_msgs/Float32.h"
#include "torcsclient.h" #include "torcsclient.h"
#include <sstream> #include <sstream>
...@@ -7,23 +8,37 @@ ...@@ -7,23 +8,37 @@
#include <ml.h> #include <ml.h>
#include <highgui.h> #include <highgui.h>
#include <cv.h> #include <cv.h>
#include <ros/console.h>
#include <std_msgs/Float32MultiArray.h>
constexpr auto ROS_IMAGE_WIDTH = 280; 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.);
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) int main(int argc, char **argv)
{ {
ros::init(argc, argv, "camera"); ros::init(argc, argv, "camera");
ros::NodeHandle n; 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); ros::Rate loop_rate(10);
int count = 0; int count = 0;
torcs::TorcsClient client; torcs::TorcsClient client;
while (ros::ok()) 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)); 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.get(), torcs::IMAGE_SIZE_BYTES);
...@@ -41,7 +56,11 @@ int main(int argc, char **argv) ...@@ -41,7 +56,11 @@ int main(int argc, char **argv)
msg->data.resize(ROS_IMAGE_SIZE); msg->data.resize(ROS_IMAGE_SIZE);
memcpy(msg->data.data(), resizedImage.data, 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(); ros::spinOnce();
loop_rate.sleep(); loop_rate.sleep();
++count; ++count;
......
...@@ -6,6 +6,7 @@ ...@@ -6,6 +6,7 @@
#include <cstring> #include <cstring>
#include <sys/shm.h> #include <sys/shm.h>
#include <assert.h> #include <assert.h>
#include <ros/console.h>
using namespace torcs; using namespace torcs;
...@@ -19,26 +20,28 @@ TorcsClient::TorcsClient() ...@@ -19,26 +20,28 @@ TorcsClient::TorcsClient()
shmid = shmget((key_t)4567, sizeof(struct shared_use_st), 0666); shmid = shmget((key_t)4567, sizeof(struct shared_use_st), 0666);
if(shmid < 0) 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; return;
} }
shm = shmat(shmid, 0, 0); shm = shmat(shmid, 0, 0);
if(shm == (void*)-1) if(shm == (void*)-1)
{ {
std::cerr << "Failed to shmat()!" << std::endl; ROS_ERROR("Failed to shmat()!");
//std::cerr << "Failed to shmat()!" << std::endl;
return; return;
} }
connected = true; 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; shared = (struct shared_use_st*)shm;
} }
std::unique_ptr<DataContainer> TorcsClient::getScreenshot() std::unique_ptr<DataContainer> TorcsClient::getScreenshot(float& speed)
{ {
assert(connected); assert(connected);
...@@ -48,12 +51,65 @@ std::unique_ptr<DataContainer> TorcsClient::getScreenshot() ...@@ -48,12 +51,65 @@ std::unique_ptr<DataContainer> TorcsClient::getScreenshot()
} }
shared->pause = 1; shared->pause = 1;
std::cout << "Got screenshot" << std::endl;
auto result = std::make_unique<DataContainer>(); auto result = std::make_unique<DataContainer>();
memcpy(result->data(), shared->data, result->size()); memcpy(result->data(), shared->data, result->size());
speed = shared->speed;
shared->pause = 0; shared->pause = 0;
shared->written = 0; shared->written = 0;
return result; 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;
}
}
}
}
...@@ -45,13 +45,28 @@ struct shared_use_st ...@@ -45,13 +45,28 @@ struct shared_use_st
double brakeCmd = 0.0; 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>; using DataContainer = std::array<uint8_t, IMAGE_SIZE_BYTES>;
class TorcsClient class TorcsClient
{ {
public: public:
TorcsClient(); 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: private:
struct shared_use_st *shared; struct shared_use_st *shared;
......
...@@ -8,6 +8,7 @@ std::unique_ptr<torcs::TorcsClient> client; ...@@ -8,6 +8,7 @@ std::unique_ptr<torcs::TorcsClient> client;
void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg) 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])); client->sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2]));
} }
...@@ -19,7 +20,7 @@ int main(int argc, char **argv) ...@@ -19,7 +20,7 @@ int main(int argc, char **argv)
ros::NodeHandle n; ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/commands", 100, chatterCallback); ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback);
ros::spin(); ros::spin();
return 0; return 0;
......
...@@ -6,6 +6,7 @@ ...@@ -6,6 +6,7 @@
#include <cstring> #include <cstring>
#include <sys/shm.h> #include <sys/shm.h>
#include <assert.h> #include <assert.h>
#include <ros/console.h>
using namespace torcs; using namespace torcs;
...@@ -19,20 +20,22 @@ TorcsClient::TorcsClient() ...@@ -19,20 +20,22 @@ TorcsClient::TorcsClient()
shmid = shmget((key_t)4567, sizeof(struct shared_use_st), 0666); shmid = shmget((key_t)4567, sizeof(struct shared_use_st), 0666);
if(shmid < 0) 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; return;
} }
shm = shmat(shmid, 0, 0); shm = shmat(shmid, 0, 0);
if(shm == (void*)-1) if(shm == (void*)-1)
{ {
std::cerr << "Failed to shmat()!" << std::endl; ROS_ERROR("Failed to shmat()!");
//std::cerr << "Failed to shmat()!" << std::endl;
return; return;
} }
connected = true; 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; shared = (struct shared_use_st*)shm;
} }
...@@ -45,7 +48,8 @@ void TorcsClient::sendCommand(Command cmd) ...@@ -45,7 +48,8 @@ void TorcsClient::sendCommand(Command cmd)
std::this_thread::sleep_for(std::chrono::milliseconds(100)); 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->accelCmd = cmd.accelCmd;
shared->steerCmd = cmd.steerCmd; shared->steerCmd = cmd.steerCmd;
shared->brakeCmd = cmd.brakeCmd; 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