Skip to content
GitLab
Commit 28fc37d5 authored by Svetlana's avatar Svetlana
Browse files

Use CNN predictions for driving

parent 3a4dc1d3
Hide whitespace changes
Inline Side-by-side
EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/main.cpp
View file @ 28fc37d5
......@@ -26,6 +26,13 @@ void infoChatterCallback(const std_msgs::Int32::ConstPtr& msg)
client.setLanesCount(msg->data);
}
void predictionsChatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
torcs::Affordance affordance;
affordance.populate(*msg);
client.visualizePredictions(affordance);
}
void publishCallback(std::shared_ptr<torcs::DataContainer> screenshot, torcs::Affordance affordance)
{
cv::Mat image(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
......@@ -107,6 +114,7 @@ int main(int argc, char **argv)
ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback);
ros::Subscriber infoSub = n.subscribe("/info", 1, infoChatterCallback);
ros::Subscriber predictionsSub = n.subscribe("/predictions", 1, predictionsChatterCallback);
ros::Rate loop_rate(10);
std::thread clientRunnerThread(clientRunner);
......
EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.cpp
View file @ 28fc37d5
......@@ -103,6 +103,37 @@ void TorcsClient::initVisualization()
cvInitFont(&font, CV_FONT_HERSHEY_COMPLEX, 1, 1, 1, 2, 8);
}
void TorcsClient::visualizePredictions(const torcs::Affordance &affordance)
{
float true_angle = float(shared->angle);
float true_dist_L = float(shared->dist_L);
float true_dist_R = float(shared->dist_R);
float true_toMarking_L = float(shared->toMarking_L);
float true_toMarking_M = float(shared->toMarking_M);
float true_toMarking_R = float(shared->toMarking_R);
float true_dist_LL = float(shared->dist_LL);
float true_dist_MM = float(shared->dist_MM);
float true_dist_RR = float(shared->dist_RR);
float true_toMarking_LL = float(shared->toMarking_LL);
float true_toMarking_ML = float(shared->toMarking_ML);
float true_toMarking_MR = float(shared->toMarking_MR);
float true_toMarking_RR = float(shared->toMarking_RR);
visualize(affordance.getAngle(), true_angle,
affordance.getToMarkingML(), true_toMarking_ML,
affordance.getToMarkingMR(), true_toMarking_MR,
affordance.getToMarkingM(), true_toMarking_M,
affordance.getToMarkingLL(), true_toMarking_LL,
affordance.getToMarkingR(), true_toMarking_R,
affordance.getToMarkingL(), true_toMarking_L,
affordance.getToMarkingRR(), true_toMarking_RR,
affordance.getDistLL(), true_dist_LL,
affordance.getDistRR(), true_dist_RR,
affordance.getDistMM(), true_dist_MM,
affordance.getDistL(), true_dist_L,
affordance.getDistR(), true_dist_R);
}
void TorcsClient::setPublishCallback(std::function<void(std::shared_ptr<DataContainer>, Affordance)> callback)
{
publishCallback = callback;
......@@ -150,34 +181,6 @@ void TorcsClient::iterate()
shared->written = 0;
shared->pause = 0;
float true_angle = float(shared->angle);
float true_dist_L = float(shared->dist_L);
float true_dist_R = float(shared->dist_R);
float true_toMarking_L = float(shared->toMarking_L);
float true_toMarking_M = float(shared->toMarking_M);
float true_toMarking_R = float(shared->toMarking_R);
float true_dist_LL = float(shared->dist_LL);
float true_dist_MM = float(shared->dist_MM);
float true_dist_RR = float(shared->dist_RR);
float true_toMarking_LL = float(shared->toMarking_LL);
float true_toMarking_ML = float(shared->toMarking_ML);
float true_toMarking_MR = float(shared->toMarking_MR);
float true_toMarking_RR = float(shared->toMarking_RR);
visualize(affordance.getAngle(), true_angle,
affordance.getToMarkingML(), true_toMarking_ML,
affordance.getToMarkingMR(), true_toMarking_MR,
affordance.getToMarkingM(), true_toMarking_M,
affordance.getToMarkingLL(), true_toMarking_LL,
affordance.getToMarkingR(), true_toMarking_R,
affordance.getToMarkingL(), true_toMarking_L,
affordance.getToMarkingRR(), true_toMarking_RR,
affordance.getDistLL(), true_dist_LL,
affordance.getDistRR(), true_dist_RR,
affordance.getDistMM(), true_dist_MM,
affordance.getDistL(), true_dist_L,
affordance.getDistR(), true_dist_R);
} else {
if (shared->pause != 1) {
shared->pause = 1;
......
EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.h
View file @ 28fc37d5
......@@ -97,22 +97,22 @@ public:
affordance_ = affordance.data;
}
float getFast() { return affordance_[0]; }
float getDistL() { return affordance_[1]; }
float getDistR() { return affordance_[2]; }
float getToMarkingL() { return affordance_[3]; }
float getToMarkingM() { return affordance_[4]; }
float getToMarkingR() { return affordance_[5]; }
float getDistLL() { return affordance_[6]; }
float getDistMM() { return affordance_[7]; }
float getDistRR() { return affordance_[8]; }
float getToMarkingLL() { return affordance_[9]; }
float getToMarkingML() { return affordance_[10]; }
float getToMarkingMR() { return affordance_[11]; }
float getToMarkingRR() { return affordance_[12]; }
float getToMiddle() { return affordance_[13]; }
float getAngle() { return affordance_[14]; }
float getSpeed() { return affordance_[15]; }
float getFast() const { return affordance_[0]; }
float getDistL() const { return affordance_[1]; }
float getDistR() const { return affordance_[2]; }
float getToMarkingL() const { return affordance_[3]; }
float getToMarkingM() const { return affordance_[4]; }
float getToMarkingR() const { return affordance_[5]; }
float getDistLL() const { return affordance_[6]; }
float getDistMM() const { return affordance_[7]; }
float getDistRR() const { return affordance_[8]; }
float getToMarkingLL() const { return affordance_[9]; }
float getToMarkingML() const { return affordance_[10]; }
float getToMarkingMR() const { return affordance_[11]; }
float getToMarkingRR() const { return affordance_[12]; }
float getToMiddle() const { return affordance_[13]; }
float getAngle() const { return affordance_[14]; }
float getSpeed() const { return affordance_[15]; }
const std::vector<float>& getAsVector()
{
......@@ -133,12 +133,7 @@ public:
void iterate();
void sendCommand(const Command& cmd);
void setLanesCount(int count);
void visualize(float angle, float true_angle, float toMarking_ML, float true_toMarking_ML,
float toMarking_MR, float true_toMarking_MR, float toMarking_M, float true_toMarking_M,
float toMarking_LL, float true_toMarking_LL, float toMarking_R, float true_toMarking_R,
float toMarking_L, float true_toMarking_L, float toMarking_RR, float true_toMarking_RR,
float dist_LL, float true_dist_LL, float dist_RR, float true_dist_RR, float dist_MM, float true_dist_MM,
float dist_L, float true_dist_L, float dist_R, float true_dist_R);
void visualizePredictions(const Affordance& affordance);
private:
......@@ -149,6 +144,12 @@ private:
std::function<void(std::shared_ptr<DataContainer>, Affordance)> publishCallback;
int lanesCount = 3; // Use three lanes configuration by default
void visualize(float angle, float true_angle, float toMarking_ML, float true_toMarking_ML,
float toMarking_MR, float true_toMarking_MR, float toMarking_M, float true_toMarking_M,
float toMarking_LL, float true_toMarking_LL, float toMarking_R, float true_toMarking_R,
float toMarking_L, float true_toMarking_L, float toMarking_RR, float true_toMarking_RR,
float dist_LL, float true_dist_LL, float dist_RR, float true_dist_RR, float dist_MM, float true_dist_MM,
float dist_L, float true_dist_L, float dist_R, float true_dist_R);
void initVisualization();
};
......
EMADL2TORCS/src/main/dp/Mastercomponent.emadl
View file @ 28fc37d5
......@@ -3,32 +3,36 @@ import dp.subcomponents.*;
component Mastercomponent {
ports
//in Z(0:255)^{3, 210, 280} imageIn,
in Q^{16} groundTruthAffordance,
in Z(0:255)^{3, 210, 280} imageIn,
//in Q^{16} groundTruthAffordance,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
out Q(-1:1)^{3} commandsOut;
out Q^{16} predictedAffordanceOut,
out Q(-1:1)^{3} commandsOut;
//instance Dpnet dpnet;
instance Dpnet dpnet;
instance DriverController driverController;
//instance Denormalizer denorm;
instance Denormalizer denorm;
//instance KFMastercomponent kfm;
instance SteeringBuffer steeringBuffer;
instance VectorToAffordance vecToAffordance;
instance LocalizationController locController;
instance AffordanceToVector affToVector;
//connect imageIn -> dpnet.data;
//connect dpnet.predictions -> denorm.normalizedPredictions;
connect imageIn -> dpnet.data;
connect dpnet.predictions -> denorm.normalizedPredictions;
//connect denorm.affordance -> kfm.affordanceIn;
//connect kfm.affordanceSmoothed -> driverController.affordanceIn;
//connect denorm.affordance -> driverController.affordanceIn;
connect groundTruthAffordance -> vecToAffordance.affordanceIndicators;
connect vecToAffordance.affordance -> driverController.affordanceIn;
connect denorm.affordance -> driverController.affordanceIn, affToVector.affordanceIn;
connect affToVector.affordanceOut -> predictedAffordanceOut;
//connect groundTruthAffordance -> vecToAffordance.affordanceIndicators;
//connect vecToAffordance.affordance -> driverController.affordanceIn;
//connect vecToAffordance.affordance -> locController.affordanceIn;
connect steeringBuffer.outputBuffer -> driverController.steeringRecordIn;
connect speedIn -> driverController.speedIn;
connect locController.lanesCount -> driverController.lanesCountIn;
connect steeringBuffer.timerLeftOut -> driverController.timerLeftIn;
connect steeringBuffer.timerRightOut -> driverController.timerRightIn;
connect steeringBuffer.laneChangeOut -> driverController.laneChangeIn;
......
EMADL2TORCS/src/main/dp/Mastercomponent.tag
View file @ 28fc37d5
......@@ -4,6 +4,7 @@ conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema;
tags Mastercomponent {
tag mastercomponent.imageIn with RosConnection = {topic=(/camera,std_msgs/Float32MultiArray)};
tag mastercomponent.speedIn with RosConnection = {topic=(/speed,std_msgs/Float32)};
tag mastercomponent.groundTruthAffordance with RosConnection = {topic=(/affordance,std_msgs/Float32MultiArray)};
tag mastercomponent.commandsOut with RosConnection = {topic=(/commands,std_msgs/Float32MultiArray)};
tag mastercomponent.lanesCountOut with RosConnection = {topic=(/info,std_msgs/Int32)};
tag mastercomponent.predictedAffordanceOut with RosConnection = {topic=(/predictions,std_msgs/Float32MultiArray)};
}
EMADL2TORCS/src/main/dp/subcomponents/AffordanceToVector.emadl 0 → 100644
View file @ 28fc37d5
package dp.subcomponents;
import ChangeRange;
component AffordanceToVector {
ports in Affordance affordanceIn,
out Q^{16} affordanceOut;
implementation Math {
affordanceOut(1) = affordanceIn.fast;
affordanceOut(2) = affordanceIn.distL;
affordanceOut(3) = affordanceIn.distR;
affordanceOut(4) = affordanceIn.toMarkingL;
affordanceOut(5) = affordanceIn.toMarkingM;
affordanceOut(6) = affordanceIn.toMarkingR;
affordanceOut(7) = affordanceIn.distLL;
affordanceOut(8) = affordanceIn.distMM;
affordanceOut(9) = affordanceIn.distRR;
affordanceOut(10) = affordanceIn.toMarkingLL;
affordanceOut(11) = affordanceIn.toMarkingML;
affordanceOut(12) = affordanceIn.toMarkingMR;
affordanceOut(13) = affordanceIn.toMarkingRR;
affordanceOut(15) = affordanceIn.angle;
}
}
EMADL2TORCS/src/main/dp/subcomponents/SteeringBuffer.emadl
View file @ 28fc37d5
......@@ -12,20 +12,17 @@ component SteeringBuffer {
implementation Math {
static Q^{5} buffer = [0; 0; 0; 0; 0];
static Q^{2} laneChangeBuffer = [0;0];
static Z^{2} timerLeftBuffer = [0; 0];
static Z^{2} timerRightBuffer = [0; 0];
static Q laneChangeBuffer = 0;
static Z timerLeftBuffer = 0;
static Z timerRightBuffer = 0;
timerLeftBuffer(1) = timerLeftBuffer(2);
timerRightBuffer(1) = timerRightBuffer(2);
timerLeftOut = timerLeftBuffer(1);
timerRightOut = timerRightBuffer(1);
timerLeftBuffer(2) = timerLeftIn;
timerRightBuffer(2) = timerRightIn;
timerLeftOut = timerLeftBuffer;
timerRightOut = timerRightBuffer;
timerLeftBuffer = timerLeftIn;
timerRightBuffer = timerRightIn;
laneChangeBuffer(1) = laneChangeBuffer(2);
laneChangeOut = laneChangeBuffer(1);
laneChangeBuffer(2) = laneChangeIn;
laneChangeOut = laneChangeBuffer;
laneChangeBuffer = laneChangeIn;
buffer(5) = buffer(4);
buffer(4) = buffer(3);
......
Supports Markdown
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