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

Commit 3a4dc1d3 authored by Svetlana's avatar Svetlana

Real time visualization

parent 454a8bd7
......@@ -8,12 +8,9 @@
#include <cv.hpp>
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32MultiArray.h>
constexpr auto ROS_IMAGE_WIDTH = 280u;
constexpr auto ROS_IMAGE_HEIGHT = 210u;
constexpr auto ROS_IMAGE_SIZE = ROS_IMAGE_WIDTH*ROS_IMAGE_HEIGHT*3u;
torcs::TorcsClient client;
ros::Publisher* cameraPublisher;
ros::Publisher* speedPublisher;
......@@ -24,6 +21,11 @@ void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
client.sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2]));
}
void infoChatterCallback(const std_msgs::Int32::ConstPtr& msg)
{
client.setLanesCount(msg->data);
}
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));
......@@ -86,6 +88,13 @@ void clientRunner()
int main(int argc, char **argv)
{
// cvNamedWindow("Semantic Visualization",1);
// IplImage* background=cvLoadImage("TORCSComponent/assets/semantic_background_3lane.png");
// IplImage* semanticRGB=cvCreateImage(cvSize(torcs::SEMANTIC_WIDTH,torcs::SEMANTIC_HEIGHT),IPL_DEPTH_8U,3);
// cvCopy(background,semanticRGB);
// cvShowImage("Semantic Visualization",semanticRGB);
// cv::waitKey(0);
client.setPublishCallback(publishCallback);
ros::init(argc, argv, "TORCSComponent");
ros::NodeHandle n;
......@@ -97,18 +106,9 @@ int main(int argc, char **argv)
affordancePublisher = &affordancePub;
ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback);
ros::Subscriber infoSub = n.subscribe("/info", 1, infoChatterCallback);
ros::Rate loop_rate(10);
std::thread clientRunnerThread(clientRunner);
// int count = 0;
//
// while (ros::ok())
// {
//
// client->iterate();
// ros::spinOnce();
// loop_rate.sleep();
// ++count;
// }
ros::spin();
clientRunnerThread.join();
......
......@@ -7,9 +7,19 @@
#include <sys/shm.h>
#include <assert.h>
#include <ros/console.h>
#include <cv.hpp>
using namespace torcs;
////////////////////// set up opencv
IplImage* semanticRGB=nullptr;
IplImage* error_bar=nullptr;
IplImage* legend=nullptr;
IplImage* background=nullptr;
CvFont font;
char vi_buf[4];
////////////////////// END set up opencv
TorcsClient::TorcsClient()
{
gotCommand = false;
......@@ -33,6 +43,8 @@ TorcsClient::TorcsClient()
//std::cerr << "Failed to shmat()!" << std::endl;
return;
}
initVisualization();
connected = true;
......@@ -69,6 +81,28 @@ TorcsClient::TorcsClient()
shared->brakeCmd = 0.0;
}
TorcsClient::~TorcsClient()
{
cvDestroyWindow("Semantic Visualization");
cvDestroyWindow("Error Bar");
cvReleaseImage( &error_bar );
cvReleaseImage( &semanticRGB );
cvReleaseImage( &background );
cvReleaseImage( &legend );
}
void TorcsClient::initVisualization()
{
semanticRGB=cvCreateImage(cvSize(SEMANTIC_WIDTH,SEMANTIC_HEIGHT),IPL_DEPTH_8U,3);
error_bar=cvCreateImage(cvSize(640,180),IPL_DEPTH_8U,3);
legend=cvLoadImage("TORCSComponent/assets/Legend6.png");
background=cvLoadImage("TORCSComponent/assets/semantic_background_3lane.png");
cvNamedWindow("Semantic Visualization",1);
cvNamedWindow("Error Bar",1);
cvInitFont(&font, CV_FONT_HERSHEY_COMPLEX, 1, 1, 1, 2, 8);
}
void TorcsClient::setPublishCallback(std::function<void(std::shared_ptr<DataContainer>, Affordance)> callback)
{
publishCallback = callback;
......@@ -80,6 +114,25 @@ void TorcsClient::sendCommand(const Command& cmd)
command = cmd;
}
void TorcsClient::setLanesCount(int count)
{
lanesCount = count;
switch(lanesCount)
{
case 1:
cvReleaseImage( &background );
background=cvLoadImage("TORCSComponent/assets/semantic_background_1lane.png");
case 2:
cvReleaseImage( &background );
background=cvLoadImage("TORCSComponent/assets/semantic_background_2lane.png");
case 3:
cvReleaseImage( &background );
background=cvLoadImage("TORCSComponent/assets/semantic_background_3lane.png");
default:
ROS_ERROR("Unexpected lanes count! D:");
}
}
void TorcsClient::iterate()
{
assert(connected);
......@@ -94,42 +147,37 @@ void TorcsClient::iterate()
Affordance affordance;
affordance.populate(*shared);
publishCallback(screenshot, affordance);
// ROS_INFO("Affordance:\n"
// "dist_L: %f\n"
// "dist_R: %f\n"
// "dist_LL: %f\n"
// "dist_MM: %f\n"
// "dist_RR: %f\n"
// "toMarking_L: %f\n"
// "toMarking_M: %f\n"
// "toMarking_R: %f\n"
// "toMarking_LL: %f\n"
// "toMarking_ML: %f\n"
// "toMarking_MR: %f\n"
// "toMarking_RR: %f\n"
// "toMiddle: %f\n"
// "angle: %f\n"
// "fast: %f\n"
// "speed: %f\n",
// affordance.getDistL(),
// affordance.getDistR(),
// affordance.getDistLL(),
// affordance.getDistMM(),
// affordance.getDistRR(),
// affordance.getToMarkingL(),
// affordance.getToMarkingM(),
// affordance.getToMarkingR(),
// affordance.getToMarkingLL(),
// affordance.getToMarkingML(),
// affordance.getToMarkingMR(),
// affordance.getToMarkingRR(),
// affordance.getToMiddle(),
// affordance.getAngle(),
// affordance.getFast(),
// affordance.getSpeed());
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;
......@@ -139,15 +187,844 @@ void TorcsClient::iterate()
if (shared->control == 1) {
if (gotCommand) {
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;
} else {
//ROS_INFO("No command");
//shared->steerCmd = 0.0;
}
}
}
#pragma clang diagnostic pop
}
void TorcsClient::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)
{
////////////////////// visualization parameters
int marking_head=1;
int marking_st;
int marking_end;
int pace;
int car_pos;
int goto_lane=0;
int true_goto_lane=0;
float p1_x,p1_y,p2_x,p2_y,p3_x,p3_y,p4_x,p4_y;
CvPoint* pt = new CvPoint[4];
int visualize_angle=1;
float err_angle;
float err_toMarking_ML;
float err_toMarking_MR;
float err_toMarking_M;
int manual=0;
int counter=0;
////////////////////// END visualization parameters
if (lanesCount == 1)
{
//////////////////////////////////////////////// show legend and error bar
err_angle=(angle-true_angle)*343.8; // full scale +-12 degree
if (err_angle>72) err_angle=72;
if (err_angle<-72) err_angle=-72;
if (true_toMarking_ML>-5 && -toMarking_ML+toMarking_MR<5.5) {
err_toMarking_ML=(toMarking_ML-true_toMarking_ML)*72; // full scale +-1 meter
if (err_toMarking_ML>72) err_toMarking_ML=72;
if (err_toMarking_ML<-72) err_toMarking_ML=-72;
err_toMarking_MR=(toMarking_MR-true_toMarking_MR)*72; // full scale +-1 meter
if (err_toMarking_MR>72) err_toMarking_MR=72;
if (err_toMarking_MR<-72) err_toMarking_MR=-72;
} else {
err_toMarking_ML=0;
err_toMarking_MR=0;
}
if (true_toMarking_M<3 && toMarking_M<2) {
err_toMarking_M=(toMarking_M-true_toMarking_M)*72; // full scale +-1 meter
if (err_toMarking_M>72) err_toMarking_M=72;
if (err_toMarking_M<-72) err_toMarking_M=-72;
} else {
err_toMarking_M=0;
}
int bar_st=26;
cvCopy(legend,error_bar);
cvRectangle(error_bar,cvPoint(319,bar_st+0-10),cvPoint(319+err_angle,bar_st+0+10),cvScalar(127,127,127),-1);
cvRectangle(error_bar,cvPoint(319,bar_st+42-10),cvPoint(319+err_toMarking_ML,bar_st+42+10),cvScalar(190,146,122),-1);
cvRectangle(error_bar,cvPoint(319,bar_st+84-10),cvPoint(319+err_toMarking_MR,bar_st+84+10),cvScalar(0,121,0),-1);
cvRectangle(error_bar,cvPoint(319,bar_st+126-10),cvPoint(319+err_toMarking_M,bar_st+126+10),cvScalar(128,0,0),-1);
cvLine(error_bar,cvPoint(319,bar_st-15),cvPoint(319,bar_st+144),cvScalar(0,0,0),1);
cvShowImage("Error Bar",error_bar);
//////////////////////////////////////////////// END show legend and error bar
///////////////////////////// semantic visualization
cvCopy(background,semanticRGB);
sprintf(vi_buf,"%d",int(shared->speed*3.6));
cvPutText(semanticRGB,vi_buf,cvPoint(245,85),&font,cvScalar(255,255,255));
//////////////// visualize true_angle
if (visualize_angle==1) {
true_angle=-true_angle;
p1_x=-14*cos(true_angle)+28*sin(true_angle);
p1_y=14*sin(true_angle)+28*cos(true_angle);
p2_x=14*cos(true_angle)+28*sin(true_angle);
p2_y=-14*sin(true_angle)+28*cos(true_angle);
p3_x=14*cos(true_angle)-28*sin(true_angle);
p3_y=-14*sin(true_angle)-28*cos(true_angle);
p4_x=-14*cos(true_angle)-28*sin(true_angle);
p4_y=14*sin(true_angle)-28*cos(true_angle);
}
//////////////// END visualize true_angle
/////////////////// draw groundtruth data
if (true_toMarking_ML>-5) { // in the lane
if (true_toMarking_M<2 && true_toMarking_R>6.5)
car_pos=int((150-(true_toMarking_ML+true_toMarking_MR)*6+174-true_toMarking_M*12)/2);
else if (true_toMarking_M<2 && true_toMarking_L<-6.5)
car_pos=int((150-(true_toMarking_ML+true_toMarking_MR)*6+126-true_toMarking_M*12)/2);
else
car_pos=int(150-(true_toMarking_ML+true_toMarking_MR)*6);
if (visualize_angle==1) {
pt[0] = cvPoint(p1_x+car_pos,p1_y+600);
pt[1] = cvPoint(p2_x+car_pos,p2_y+600);
pt[2] = cvPoint(p3_x+car_pos,p3_y+600);
pt[3] = cvPoint(p4_x+car_pos,p4_y+600);
cvFillConvexPoly(semanticRGB,pt,4,cvScalar(0,0,255));
} else
cvRectangle(semanticRGB,cvPoint(car_pos-14,600-28),cvPoint(car_pos+14,600+28),cvScalar(0,0,255),-1);
if (true_dist_MM<60)
cvRectangle(semanticRGB,cvPoint(150-14,600-true_dist_MM*12-28),cvPoint(150+14,600-true_dist_MM*12+28),cvScalar(0,255,255),-1);
}
else if (true_toMarking_M<3) { // on the marking
if (true_toMarking_L<-6.5) { // left
car_pos=int(126-true_toMarking_M*12);
if (true_dist_R<60)
cvRectangle(semanticRGB,cvPoint(150-14,600-true_dist_R*12-28),cvPoint(150+14,600-true_dist_R*12+28),cvScalar(0,255,255),-1);
} else if (true_toMarking_R>6.5) { // right
car_pos=int(174-true_toMarking_M*12);
if (true_dist_L<60)
cvRectangle(semanticRGB,cvPoint(150-14,600-true_dist_L*12-28),cvPoint(150+14,600-true_dist_L*12+28),cvScalar(0,255,255),-1);
}
if (visualize_angle==1) {
pt[0] = cvPoint(p1_x+car_pos,p1_y+600);
pt[1] = cvPoint(p2_x+car_pos,p2_y+600);
pt[2] = cvPoint(p3_x+car_pos,p3_y+600);
pt[3] = cvPoint(p4_x+car_pos,p4_y+600);
cvFillConvexPoly(semanticRGB,pt,4,cvScalar(0,0,255));
} else
cvRectangle(semanticRGB,cvPoint(car_pos-14,600-28),cvPoint(car_pos+14,600+28),cvScalar(0,0,255),-1);
}
/////////////////// END draw groundtruth data
//////////////// visualize angle
if (visualize_angle==1) {
angle=-angle;
p1_x=-14*cos(angle)+28*sin(angle);
p1_y=14*sin(angle)+28*cos(angle);
p2_x=14*cos(angle)+28*sin(angle);
p2_y=-14*sin(angle)+28*cos(angle);
p3_x=14*cos(angle)-28*sin(angle);
p3_y=-14*sin(angle)-28*cos(angle);
p4_x=-14*cos(angle)-28*sin(angle);
p4_y=14*sin(angle)-28*cos(angle);
}
//////////////// END visualize angle
/////////////////// draw sensing data
if (-toMarking_ML+toMarking_MR<5.5) { // in the lane
if (toMarking_M<2 && toMarking_R>6)
car_pos=int((150-(toMarking_ML+toMarking_MR)*6+174-toMarking_M*12)/2);
else if (toMarking_M<2 && toMarking_L<-6)
car_pos=int((150-(toMarking_ML+toMarking_MR)*6+126-toMarking_M*12)/2);
else
car_pos=int(150-(toMarking_ML+toMarking_MR)*6);
if (visualize_angle==1) {
pt[0] = cvPoint(p1_x+car_pos,p1_y+600);
pt[1] = cvPoint(p2_x+car_pos,p2_y+600);
pt[2] = cvPoint(p3_x+car_pos,p3_y+600);
pt[3] = cvPoint(p4_x+car_pos,p4_y+600);
int npts=4;
cvPolyLine(semanticRGB,&pt,&npts,1,1,cvScalar(0,255,0),2,CV_AA);
} else
cvRectangle(semanticRGB,cvPoint(car_pos-14,600-28),cvPoint(car_pos+14,600+28),cvScalar(0,255,0),2);
if (dist_MM<50)
cvRectangle(semanticRGB,cvPoint(150-14,600-dist_MM*12-28),cvPoint(150+14,600-dist_MM*12+28),cvScalar(237,99,157),2);
}
else if (toMarking_M<2.5) { // on the marking
if (toMarking_L<-6) { // left
car_pos=int(126-toMarking_M*12);
if (dist_R<50)
cvRectangle(semanticRGB,cvPoint(150-14,600-dist_R*12-28),cvPoint(150+14,600-dist_R*12+28),cvScalar(237,99,157),2);
} else if (toMarking_R>6) { // right
car_pos=int(174-toMarking_M*12);
if (dist_L<50)
cvRectangle(semanticRGB,cvPoint(150-14,600-dist_L*12-28),cvPoint(150+14,600-dist_L*12+28),cvScalar(237,99,157),2);
}
if (visualize_angle==1) {
pt[0] = cvPoint(p1_x+car_pos,p1_y+600);
pt[1] = cvPoint(p2_x+car_pos,p2_y+600);
pt[2] = cvPoint(p3_x+car_pos,p3_y+600);
pt[3] = cvPoint(p4_x+car_pos,p4_y+600);
int npts=4;
cvPolyLine(semanticRGB,&pt,&npts,1,1,cvScalar(0,255,0),2,CV_AA);
} else
cvRectangle(semanticRGB,cvPoint(car_pos-14,600-28),cvPoint(car_pos+14,600+28),cvScalar(0,255,0),2);
}
/////////////////// END draw sensing data
if ((shared->control==0) || (manual==1)) {
for (int h = 0; h < SEMANTIC_HEIGHT; h++) {
for (int w = 0; w < SEMANTIC_WIDTH; w++) {
semanticRGB->imageData[(h*SEMANTIC_WIDTH+w)*3+1]=0;
semanticRGB->imageData[(h*SEMANTIC_WIDTH+w)*3+0]=0;
}
}
}
cvShowImage("Semantic Visualization",semanticRGB);
///////////////////////////// END semantic visualization
}
else if (lanesCount == 2)
{
//////////////////////////////////////////////// show legend and error bar
err_angle=(angle-true_angle)*343.8; // full scale +-12 degree
if (err_angle>72) err_angle=72;
if (err_angle<-72) err_angle=-72;
if (true_toMarking_ML>-5 && -toMarking_ML+toMarking_MR<5.5) {
err_toMarking_ML=(toMarking_ML-true_toMarking_ML)*72; // full scale +-1 meter
if (err_toMarking_ML>72) err_toMarking_ML=72;
if (err_toMarking_ML<-72) err_toMarking_ML=-72;
err_toMarking_MR=(toMarking_MR-true_toMarking_MR)*72; // full scale +-1 meter
if (err_toMarking_MR>72) err_toMarking_MR=72;
if (err_toMarking_MR<-72) err_toMarking_MR=-72;
} else {
err_toMarking_ML=0;
err_toMarking_MR=0;
}
if (true_toMarking_M<3 && toMarking_M<2) {
err_toMarking_M=(toMarking_M-true_toMarking_M)*72; // full scale +-1 meter
if (err_toMarking_M>72) err_toMarking_M=72;
if (err_toMarking_M<-72) err_toMarking_M=-72;
} else {
err_toMarking_M=0;
}
int bar_st=26;
cvCopy(legend,error_bar);
cvRectangle(error_bar,cvPoint(319,bar_st+0-10),cvPoint(319+err_angle,bar_st+0+10),cvScalar(127,127,127),-1);
cvRectangle(error_bar,cvPoint(319,bar_st+42-10),cvPoint(319+err_toMarking_ML,bar_st+42+10),cvScalar(190,146,122),-1);
cvRectangle(error_bar,cvPoint(319,bar_st+84-10),cvPoint(319+err_toMarking_MR,bar_st+84+10),cvScalar(0,121,0),-1);
cvRectangle(error_bar,cvPoint(319,bar_st+126-10),cvPoint(319+err_toMarking_M,bar_st+126+10),cvScalar(128,0,0),-1);
cvLine(error_bar,cvPoint(319,bar_st-15),cvPoint(319,bar_st+144),cvScalar(0,0,0),1);
cvShowImage("Error Bar",error_bar);
//////////////////////////////////////////////// END show legend and error bar
///////////////////////////// semantic visualization
cvCopy(background,semanticRGB);
pace=int(shared->speed*1.2);
if (pace>50) pace=50;
marking_head=marking_head+pace;
if (marking_head>0) marking_head=marking_head-110;
else if (marking_head<-110) marking_head=marking_head+110;
marking_st=marking_head;
marking_end=marking_head+55;
while (marking_st<=660) {
cvLine(semanticRGB,cvPoint(150,marking_st),cvPoint(150,marking_end),cvScalar(255,255,255),2);
marking_st=marking_st+110;
marking_end=marking_end+110;
}
sprintf(vi_buf,"%d",int(shared->speed*3.6));
cvPutText(semanticRGB,vi_buf,cvPoint(245,85),&font,cvScalar(255,255,255));
//////////////// visualize true_angle
if (visualize_angle==1) {
true_angle=-true_angle;
p1_x=-14*cos(true_angle)+28*sin(true_angle);
p1_y=14*sin(true_angle)+28*cos(true_angle);
p2_x=14*cos(true_angle)+28*sin(true_angle);
p2_y=-14*sin(true_angle)+28*cos(true_angle);
p3_x=14*cos(true_angle)-28*sin(true_angle);
p3_y=-14*sin(true_angle)-28*cos(true_angle);
p4_x=-14*cos(true_angle)-28*sin(true_angle);
p4_y=14*sin(true_angle)-28*cos(true_angle);
}
//////////////// END visualize true_angle
/////////////////// draw groundtruth data
if (true_toMarking_LL>-9) { // right lane
if (true_toMarking_M<2 && true_toMarking_R>6.5)
car_pos=int((174-(true_toMarking_ML+true_toMarking_MR)*6+198-true_toMarking_M*12)/2);
else if (true_toMarking_M<2 && true_toMarking_R<6.5)
car_pos=int((174-(true_toMarking_ML+true_toMarking_MR)*6+150-true_toMarking_M*12)/2);
else
car_pos=int(174-(true_toMarking_ML+true_toMarking_MR)*6);
if (visualize_angle==1) {
pt[0] = cvPoint(p1_x+car_pos,p1_y+600);
pt[1] = cvPoint(p2_x+car_pos,p2_y+600);
pt[2] = cvPoint(p3_x+car_pos,p3_y+600);
pt[3] = cvPoint(p4_x+car_pos,p4_y+600);
cvFillConvexPoly(semanticRGB,pt,4,cvScalar(0,0,255));
} else
cvRectangle(semanticRGB,cvPoint(car_pos-14,600-28),cvPoint(car_pos+14,600+28),cvScalar(0,0,255),-1);
if (true_dist_LL<60)
cvRectangle(semanticRGB,cvPoint(126-14,600-true_dist_LL*12-28),cvPoint(126+14,600-true_dist_LL*12+28),cvScalar(0,255,255),-1);
if (true_dist_MM<60)
cvRectangle(semanticRGB,cvPoint(174-14,600-true_dist_MM*12-28),cvPoint(174+14,600-true_dist_MM*12+28),cvScalar(0,255,255),-1);
}
else if (true_toMarking_RR<9) { // left lane
if (true_toMarking_M<2 && true_toMarking_L<-6.5)
car_pos=int((126-(true_toMarking_ML+true_toMarking_MR)*6+102-true_toMarking_M*12)/2);
else if (true_toMarking_M<2 && true_toMarking_L>-6.5)
car_pos=int((126-(true_toMarking_ML+true_toMarking_MR)*6+150-true_toMarking_M*12)/2);
else
car_pos=int(126-(true_toMarking_ML+true_toMarking_MR)*6);
if (visualize_angle==1) {
pt[0] = cvPoint(p1_x+car_pos,p1_y+600);
pt[1] = cvPoint(p2_x+car_pos,p2_y+600);
pt[2] = cvPoint(p3_x+car_pos,p3_y+600);
pt[3] = cvPoint(p4_x+car_pos,p4_y+600);
cvFillConvexPoly(semanticRGB,pt,4,cvScalar(0,0,255));
} else
cvRectangle(semanticRGB,cvPoint(car_pos-14,600-28),cvPoint(car_pos+14,600+28),cvScalar(0,0,255),-1);
if (true_dist_MM<60)
cvRectangle(semanticRGB,cvPoint(126-14,600-true_dist_MM*12-28),cvPoint(126+14,600-true_dist_MM*12+28),cvScalar(0,255,255),-1);
if (true_dist_RR<60)
cvRectangle(semanticRGB,cvPoint(174-14,600-true_dist_RR*12-28),cvPoint(174+14,600-true_dist_RR*12+28),cvScalar(0,255,255),-1);
}
else if (true_toMarking_M<3) {
if (true_toMarking_L<-6.5) { // left
car_pos=int(102-true_toMarking_M*12);
if (true_dist_R<60)
cvRectangle(semanticRGB,cvPoint(126-14,600-true_dist_R*12-28),cvPoint(126+14,600-true_dist_R*12+28),cvScalar(0,255,255),-1);