ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
plot_ego.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2020 German Aerospace Center (DLR).
3  * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
4  *
5  * This program and the accompanying materials are made available under the
6  * terms of the Eclipse Public License 2.0 which is available at
7  * http://www.eclipse.org/legal/epl-2.0.
8  *
9  * SPDX-License-Identifier: EPL-2.0
10  *
11  * Contributors:
12  * Daniel Heß - initial implementation
13  * Thomas Lobig
14  ********************************************************************************/
15 
16 #pragma once
17 #include <adore/params/afactory.h>
19 #include <adore/env/afactory.h>
20 #include <adore/fun/afactory.h>
21 #include <adore/sim/afactory.h>
22 #include <plotlablib/afigurestub.h>
23 #include <plotlablib/plcommands.h>
24 #include <string>
25 #include <vector>
26 
27 
28 
29 namespace adore
30 {
31  namespace apps
32  {
37  class PlotEgo
38  {
39  private:
48 
49 
52 
53  std::string prefix_;
54  std::string vehicle_picture_;
55  std::string position_style_;
56  std::string target_picture_;
57 
58  double fov_width_;
59  double fov_height_;
60 
62 
63  struct info
64  {
65  std::string name_;
66  bool visible_;
67  info(bool visible,std::string name):name_(name),visible_(visible){}
68  };
69  public:
70  PlotEgo(DLR_TS::PlotLab::AFigureStub* figure,std::string prefix, int followMode)
71  {
73  simPositionReader_ = adore::sim::SimFactoryInstance::get()->getVehicleMotionStateReader();//this is the true vehicle state, displayed in simulation
75  xxReader_ = adore::fun::FunFactoryInstance::get()->getVehicleExtendedStateReader();
77  figure_ = figure;
78  map_figure_ = nullptr;
79  prefix_ = prefix;
80  fov_width_ = 640.0;
81  fov_height_ = 480.0;
82  followMode_ = followMode != 0;
83  vehicle_picture_ = "../images/ego.png";
84  target_picture_ = "../images/target.png";
85  position_style_ = "LineColor=0,0,0;LineWidth=2.0";
86  }
87  void setPicture(std::string value)
88  {
89  vehicle_picture_ = value;
90  }
92  {
93  map_figure_ = value;
94  }
95 
97  {
98  }
99 
100  void run()
101  {
102  bool sim_mode = simPositionReader_->hasData();
104  {
107 
108  //automation state
109  if(xxReader_->hasData())
110  {
115  {
116  position_style_ = "LineColor=0,0,1;LineWidth=2.0";
117  }
118  else
119  {
120  position_style_ = "LineColor=0,0.8,0;LineWidth=2.0";
121  }
122  }
123 
124  //plot the vehicle
125  vehicle_picture_ = "../images/"+pvehicle_->get_vehicle_id()+".png";
126  const double L = pvehicle_->get_a()+pvehicle_->get_b();
127  const double c = pvehicle_->get_c();
128  const double d = pvehicle_->get_d();
129  // const double w = std::max(pvehicle_->get_wf(),pvehicle_->get_wr())*0.5;
130  const double w = pvehicle_->get_bodyWidth()*0.5;
131  const double delta = position_.getDelta();
132  const double wheel_radius = 0.40;
133  const double wheel_width = 0.20;
134  if(sim_mode)
135  {
136  plotPosition(prefix_+"/truePos",vehicle_picture_,simPosition_.getX(),simPosition_.getY(),simPosition_.getPSI(),delta,simPosition_.getvx(),simPosition_.getvy(),simPosition_.getOmega(),L,c,d,w,wheel_radius,wheel_width,"LineColor=1,0,0",figure_);
137  plotPosition(prefix_+"/measuredPos","",position_.getX(),position_.getY(),position_.getPSI(),delta,position_.getvx(),position_.getvy(),position_.getOmega(),L,c,d,w,wheel_radius,wheel_width,position_style_,figure_);
138  }
139  else
140  {
142  }
143 
144  if(map_figure_!=nullptr)
145  {
147  plotCircle(prefix_+"/ego_c1",position_.getX(),position_.getY(),20,"LineWidth=3.0;LineColor=0,0,1",map_figure_);
148  plotCircle(prefix_+"/ego_c2",position_.getX(),position_.getY(),100,"LineWidth=3.0;LineColor=0,0,1",map_figure_);
149  plotCircle(prefix_+"/ego_c3",position_.getX(),position_.getY(),300,"LineWidth=3.0;LineColor=0,0,1",map_figure_);
150  }
151 
152  if ( followMode_ != 0 )
153  {
154  double rho = position_.getvx() * 1.5;
155  double gX = position_.getX() + rho * (std::cos)(position_.getPSI());
156  double gY = position_.getY() + rho * (std::sin)(position_.getPSI());;
157  auto zoom = 50.0 / std::max(5.0,std::abs(position_.getvx())*2.0);
158  figure_->setViewPortOffsets(gX,gY,360.0*(position_.getPSI() / (2.0 *3.14159))-90.0,zoom);
159  }
160 
161  }
162  {//plot the goal marker
163  double scale = 1.0/725.0 * 10.0;
164  double length=378;
165  double width=1107;
166  if(goalReader_->hasData())
167  {//plot goal marker in local map with vehicle orientation
169  goalReader_->getData(goal);
170  figure_->plotTexture(prefix_+"/target",target_picture_,goal.target_.x_,goal.target_.y_,2.0,position_.getPSI(),width*scale,length*scale);
171  if(map_figure_!=nullptr)
172  {
173  map_figure_->plotTexture(prefix_+"/target",target_picture_,goal.target_.x_,goal.target_.y_,2.0,0.0,width*scale,length*scale);
174  plotCircle(prefix_+"/target_c1",goal.target_.x_,goal.target_.y_,20,"LineWidth=3.0;LineColor=1,0,0",map_figure_);
175  plotCircle(prefix_+"/target_c2",goal.target_.x_,goal.target_.y_,100,"LineWidth=3.0;LineColor=1,0,0",map_figure_);
176  plotCircle(prefix_+"/target_c3",goal.target_.x_,goal.target_.y_,300,"LineWidth=3.0;LineColor=1,0,0",map_figure_);
177  }
178  }
179  }
180  }
181 
182  void plotCircle(const std::string& name,double gX,double gY,double R,const std::string& options,DLR_TS::PlotLab::AFigureStub* figure)
183  {
184  static const int N=50;
185  double X[N];
186  double Y[N];
187  for(int i=0;i<N;i++)
188  {
189  X[i] = gX + std::cos(M_PI*2.0*(double)i/(double)(N-1))*R;
190  Y[i] = gY + std::sin(M_PI*2.0*(double)i/(double)(N-1))*R;
191  }
192  figure->plot(name,X,Y,2.0,N,options);
193  }
194 
208  void plotPosition(const std::string& name,const std::string& vehicle_picture,double gX,double gY,double psi,double delta,double vx,double vy,double omega,double L,double c,double d,double w,double wheel_radius,double wheel_width,const std::string& options,DLR_TS::PlotLab::AFigureStub* figure)
209  {
210  double X[200];
211  double Y[200];
212  std::vector<std::pair<double,double>> points;
213  attach_vehicle_points2(gX,gY,psi,delta,vx,vy+(-d+(L+c+d)*0.5)*omega,L,c,d,w,wheel_radius,wheel_width,points);
214  for(int i=0;i<points.size();i++){X[i]=points[i].first;Y[i]=points[i].second;}
215  figure->plot(name,X,Y,1.5,points.size(),options);
216 
217 
218  double xmin = gX-fov_width_*0.5;
219  double xmax = gX+fov_width_*0.5;
220  double ymin = gY-fov_height_*0.5;
221  double ymax = gY+fov_height_*0.5;
222 
224  auto length = L+c+d;
225  auto rho = length*0.5-d;
226  auto width = w*2;
227  auto z = 1.1;
228 
229  if(vehicle_picture!="")
230  {
231  figure->plotTexture(name+"vehiclealphablend",vehicle_picture,gX+std::cos(psi)*rho,gY+std::sin(psi)*rho,z,psi,width,length);
232  }
233  }
234 
235  void attach_wheel_points(double x,double y, double delta, double wheel_radius,double wheel_width,std::vector<std::pair<double,double>>& points)
236  {
237  const double c = (std::cos)(delta);
238  const double s = (std::sin)(delta);
239  const double w = wheel_width * 0.5;
240  const double r = wheel_radius;
241 
242  points.push_back(std::make_pair(x,y));
243  points.push_back(std::make_pair(x + c * 0.0 - s * w,
244  y + s * 0.0 + c * w));
245  points.push_back(std::make_pair(x + c * (-r) - s * w,
246  y + s * (-r) + c * w));
247  points.push_back(std::make_pair(x + c * (-r) - s * (-w),
248  y + s * (-r) + c * (-w)));
249  points.push_back(std::make_pair(x + c * (+r) - s * (-w),
250  y + s * (+r) + c * (-w)));
251  points.push_back(std::make_pair(x + c * (+r) - s * (+w),
252  y + s * (+r) + c * (+w)));
253  points.push_back(std::make_pair(x + c * 0.0 - s * (+w),
254  y + s * 0.0 + c * (+w)));
255  points.push_back(std::make_pair(x + c * 0.0 - s * (-w),
256  y + s * 0.0 + c * (-w)));
257  points.push_back(std::make_pair(x,y));
258  }
259 
260  void attach_vehicle_points2(double x,double y,double psi, double delta, double vx, double vy, double L,double c,double d,double w,double wheel_radius,double wheel_width,std::vector<std::pair<double,double>>& points)
261  {
262  const double cos_psi = (std::cos)(psi);
263  const double sin_psi = (std::sin)(psi);
264  const double ww = wheel_width*0.5;
265  const double wing_mirror = 0.17;
266  const double vscale = 1.0;
267 
268  points.push_back(std::make_pair(x,y));//COR
269  attach_wheel_points(x + cos_psi * (0.0) - sin_psi * (-w+ww+wing_mirror),
270  y + sin_psi * (0.0) + cos_psi * (-w+ww+wing_mirror),
271  psi,wheel_radius,wheel_width,points);//RR
272  attach_wheel_points(x + cos_psi * (0.0) - sin_psi * (+w-ww-wing_mirror),
273  y + sin_psi * (0.0) + cos_psi * (+w-ww-wing_mirror),
274  psi,wheel_radius,wheel_width,points);//RL
275  points.push_back(std::make_pair(x,y));//COR
276  points.push_back(std::make_pair(x + cos_psi * (L) - sin_psi * (0.0),
277  y + sin_psi * (L) + cos_psi * (0.0)));//COF
278  attach_wheel_points(x + cos_psi * (L) - sin_psi * (+w-ww-wing_mirror),
279  y + sin_psi * (L) + cos_psi * (+w-ww-wing_mirror),
280  psi+delta,wheel_radius,wheel_width,points);//FL
281  attach_wheel_points(x + cos_psi * (L) - sin_psi * (-w+ww+wing_mirror),
282  y + sin_psi * (L) + cos_psi * (-w+ww+wing_mirror),
283  psi+delta,wheel_radius,wheel_width,points);//FR
284  points.push_back(std::make_pair(x + cos_psi * (L) - sin_psi * (0.0),
285  y + sin_psi * (L) + cos_psi * (0.0)));//COF
286  points.push_back(std::make_pair(x + cos_psi * (L+c) - sin_psi * (0.0),
287  y + sin_psi * (L+c) + cos_psi * (0.0)));//COFB
288  points.push_back(std::make_pair(x + cos_psi * (L+c*0.5) - sin_psi * (w*0.5),
289  y + sin_psi * (L+c*0.5) + cos_psi * (w*0.5)));
290  points.push_back(std::make_pair(x + cos_psi * (L+c) - sin_psi * (0.0),
291  y + sin_psi * (L+c) + cos_psi * (0.0)));//COFB
292  points.push_back(std::make_pair(x + cos_psi * (L+c*0.5) - sin_psi * (-w*0.5),
293  y + sin_psi * (L+c*0.5) + cos_psi * (-w*0.5)));
294  points.push_back(std::make_pair(x + cos_psi * (L+c) - sin_psi * (0.0),
295  y + sin_psi * (L+c) + cos_psi * (0.0)));//COFB
296  points.push_back(std::make_pair(x + cos_psi * (L+c) - sin_psi * (w),
297  y + sin_psi * (L+c) + cos_psi * (w)));
298  points.push_back(std::make_pair(x + cos_psi * (-d) - sin_psi * (w),
299  y + sin_psi * (-d) + cos_psi * (w)));
300  points.push_back(std::make_pair(x + cos_psi * (-d) - sin_psi * (-w),
301  y + sin_psi * (-d) + cos_psi * (-w)));
302  points.push_back(std::make_pair(x + cos_psi * (L+c) - sin_psi * (-w),
303  y + sin_psi * (L+c) + cos_psi * (-w)));
304  points.push_back(std::make_pair(x + cos_psi * (L+c) - sin_psi * (0.0),
305  y + sin_psi * (L+c) + cos_psi * (0.0)));//COFB
306 
307  points.push_back(std::make_pair(x + cos_psi * (-d+(L+c+d)*0.5) - sin_psi * (0.0),
308  y + sin_psi * (-d+(L+c+d)*0.5) + cos_psi * (0.0)));//C
309  points.push_back(std::make_pair(x + cos_psi * (-d+(L+c+d)*0.5 + vx*vscale) - sin_psi * (vy*vscale),
310  y + sin_psi * (-d+(L+c+d)*0.5 + vx*vscale) + cos_psi * (vy*vscale)));//v-arrow-tip
311  const double v = std::sqrt(vx*vx+vy*vy);
312  const double cv = v>0.1?vx/v:0.0;
313  const double sv = v>0.1?vy/v:0.0;
314  const double ax = 1.0;
315  const double ay = 1.0;
316 
317  points.push_back(std::make_pair(x + cos_psi * (-d+(L+c+d)*0.5 + vx*vscale - cv *ax - sv*ay ) - sin_psi * (vy*vscale - sv *ax + cv*ay),
318  y + sin_psi * (-d+(L+c+d)*0.5 + vx*vscale - cv *ax - sv*ay ) + cos_psi * (vy*vscale - sv *ax + cv*ay)));//v-arrow-left
319  points.push_back(std::make_pair(x + cos_psi * (-d+(L+c+d)*0.5 + vx*vscale) - sin_psi * (vy*vscale),
320  y + sin_psi * (-d+(L+c+d)*0.5 + vx*vscale) + cos_psi * (vy*vscale)));//v-arrow-tip
321  points.push_back(std::make_pair(x + cos_psi * (-d+(L+c+d)*0.5 + vx*vscale - cv *ax + sv*ay ) - sin_psi * (vy*vscale - sv *ax - cv*ay),
322  y + sin_psi * (-d+(L+c+d)*0.5 + vx*vscale - cv *ax + sv*ay ) + cos_psi * (vy*vscale - sv *ax - cv*ay)));//v-arrow-right
323 
324 
325  }
326  };
327  }
328 }
#define M_PI
Definition: arraymatrixtools.h:24
Definition: afigurestub.h:24
virtual void plotTexture(std::string hashtag, std::string url, double x, double y, double z, double psi, double w, double l)=0
virtual void plot(std::string hashtag, double *X, double *Y, double *Z, int size, std::string options)=0
virtual void setViewPortOffsets(double targetX, double targetY, double orientDeg, double zoom)=0
a optimzed plotting application to plot map borders, vehicles and environment information and backgro...
Definition: plot_ego.h:38
adore::mad::AReader< adore::fun::VehicleExtendedState > * xxReader_
Definition: plot_ego.h:43
void setMapFigure(DLR_TS::PlotLab::AFigureStub *value)
Definition: plot_ego.h:91
~PlotEgo()
Definition: plot_ego.h:96
std::string position_style_
Definition: plot_ego.h:55
bool followMode_
Definition: plot_ego.h:61
adore::fun::VehicleMotionState9d position_
Definition: plot_ego.h:45
void setPicture(std::string value)
Definition: plot_ego.h:87
std::string prefix_
Definition: plot_ego.h:53
void plotCircle(const std::string &name, double gX, double gY, double R, const std::string &options, DLR_TS::PlotLab::AFigureStub *figure)
Definition: plot_ego.h:182
std::string vehicle_picture_
Definition: plot_ego.h:54
void attach_wheel_points(double x, double y, double delta, double wheel_radius, double wheel_width, std::vector< std::pair< double, double >> &points)
Definition: plot_ego.h:235
DLR_TS::PlotLab::AFigureStub * figure_
Definition: plot_ego.h:50
void run()
Definition: plot_ego.h:100
void attach_vehicle_points2(double x, double y, double psi, double delta, double vx, double vy, double L, double c, double d, double w, double wheel_radius, double wheel_width, std::vector< std::pair< double, double >> &points)
Definition: plot_ego.h:260
adore::mad::AReader< adore::fun::VehicleMotionState9d > * simPositionReader_
Definition: plot_ego.h:42
double fov_height_
Definition: plot_ego.h:59
adore::fun::VehicleExtendedState extendedState_
Definition: plot_ego.h:47
PlotEgo(DLR_TS::PlotLab::AFigureStub *figure, std::string prefix, int followMode)
Definition: plot_ego.h:70
adore::params::APVehicle * pvehicle_
Definition: plot_ego.h:40
std::string target_picture_
Definition: plot_ego.h:56
void plotPosition(const std::string &name, const std::string &vehicle_picture, double gX, double gY, double psi, double delta, double vx, double vy, double omega, double L, double c, double d, double w, double wheel_radius, double wheel_width, const std::string &options, DLR_TS::PlotLab::AFigureStub *figure)
plotting a vehicle
Definition: plot_ego.h:208
adore::fun::VehicleMotionState9d simPosition_
Definition: plot_ego.h:46
double fov_width_
Definition: plot_ego.h:58
adore::mad::AReader< adore::fun::VehicleMotionState9d > * positionReader_
Definition: plot_ego.h:41
DLR_TS::PlotLab::AFigureStub * map_figure_
Definition: plot_ego.h:51
adore::env::AFactory::TNavigationGoalReader * goalReader_
Definition: plot_ego.h:44
virtual TNavigationGoalReader * getNavigationGoalReader()=0
virtual TVehicleMotionStateReader * getVehicleMotionStateReader()=0
static adore::env::AFactory * get()
Definition: afactory.h:236
static adore::fun::AFactory * get()
Definition: afactory.h:170
Definition: vehicleextendedstate.h:26
bool getAutomaticControlAccelerationOn() const
Definition: vehicleextendedstate.h:75
bool getAutomaticControlSteeringOn() const
Definition: vehicleextendedstate.h:91
bool getAutomaticControlAccelerationActive() const
Definition: vehicleextendedstate.h:83
virtual void getData(T &value)=0
virtual bool hasData() const =0
virtual bool hasUpdate() const =0
virtual APVehicle * getVehicle() const =0
abstract class for vehicle configuration related paremeters
Definition: ap_vehicle.h:29
virtual double get_b() const =0
rear axle to cog
virtual std::string get_vehicle_id() const =0
ID of current vehicle.
virtual double get_bodyWidth() const =0
virtual double get_d() const =0
rear border to rear axle
virtual double get_a() const =0
cog to front axle
virtual double get_c() const =0
front axle to front border
static adore::params::AFactory * get()
Definition: afactory.h:103
virtual TVehicleMotionStateReader * getVehicleMotionStateReader()=0
read updates on the true vehicle motion state
static adore::sim::AFactory * get()
Definition: afactory.h:145
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
x
Definition: adore_set_goal.py:30
y
Definition: adore_set_goal.py:31
z
Definition: adore_set_goal.py:32
w
Definition: adore_set_pose.py:40
r
Definition: adore_suppress_lanechanges.py:209
Definition: areaofeffectconverter.h:20
Definition: plot_ego.h:64
info(bool visible, std::string name)
Definition: plot_ego.h:67
bool visible_
Definition: plot_ego.h:66
std::string name_
Definition: plot_ego.h:65
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
double getX() const
Get the x-coordinate.
Definition: vehiclemotionstate9d.h:54
double getvx() const
Get the longitudinal velocity.
Definition: vehiclemotionstate9d.h:78
double getOmega() const
Get the yaw rate.
Definition: vehiclemotionstate9d.h:90
double getY() const
Get the y-coordinate.
Definition: vehiclemotionstate9d.h:60
double getPSI() const
Get the heading.
Definition: vehiclemotionstate9d.h:72
double getvy() const
Get the lateral velocity.
Definition: vehiclemotionstate9d.h:84
double getDelta() const
Get the steering angle.
Definition: vehiclemotionstate9d.h:102
Definition: navigationgoal.h:26
adore::mad::GlobalPosition target_
Definition: navigationgoal.h:28
double x_
Definition: globalposition.h:23
double y_
Definition: globalposition.h:23