ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
plot_traffic.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2023 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/env/afactory.h>
18 #include <plotlablib/afigurestub.h>
19 #include <plotlablib/plcommands.h>
20 #include <unordered_set>
21 
22 
23 namespace adore
24 {
25  namespace apps
26  {
32  {
33  private:
36 
37  std::unordered_set<std::string> plot_tags_old_;
38  std::unordered_set<std::string> plot_tags_current_;
39 
41  // adore::env::VehicleMotionState9d position_;
42 
43  std::string prefix_;
44  std::string app_tag_;
45 
46  public:
47  PlotTraffic(DLR_TS::PlotLab::AFigureStub* figure, std::string prefix, bool debug_plot_ids) :
48  figure_(figure), prefix_(prefix), debug_plot_ids_(debug_plot_ids)
49  {
51  app_tag_ = "/traffic/";
52  figure_->erase_similar(app_tag_); //remove plotted content from other instances of this app
53  }
54 
56  {
57  }
58 
59  void run()
60  {
61 
63  {
64 
66  plot_tags_current_.clear();
67 
68  // TODO is the feature to restrict plotting of traffic entities to a radius really useful?
69  // double R = pmap_->getVisibiltyRadius() + 100.0;//delete radius
71  trafficReader_->getData(tpset);
72  for(auto& tp:tpset)
73  {
74  auto pos = tp.getCenter();
75  auto psi = tp.getYaw();
76  const double abcd = tp.getLength();
77  const double c = 0.2 * abcd;
78  const double d = 0.2 * abcd;
79  const double L = abcd-c-d;
80  const double w = tp.getWidth();
81  pos(0) = pos(0) + cos(psi)*(-0.5*abcd+d);
82  pos(1) = pos(1) + sin(psi)*(-0.5*abcd+d);
83 
84  std::stringstream ss;
85  ss<<prefix_<<app_tag_<<tp.getTrackingID();
86  plotPosition(ss.str(),pos(0),pos(1),psi,L,c,d,w*0.5,tp.getVx(),tp.getVy(),"LineColor=0,0,0",tp.getClassification(),tp.getTrackingID(),tp.getStationID());
87  }
88 
89  for(auto s:plot_tags_old_)
90  {
91  if(plot_tags_current_.find(s)==plot_tags_current_.end())
92  {
93  figure_->erase(s);
94  }
95  }
96 
97 
98  }
99 
100  }
101 
115  void plotPosition(const std::string& name,double gX,double gY,double psi,double L,double c,double d,double w,double vx,double vy,const std::string& options,adore::env::traffic::Participant::EClassification participant_type, int id, int v2xid = 0)
116  {
117  auto length = L+c+d;
118  auto rho = length*0.5-d;
119  double a = L*0.5;
120  double b = L*0.5;
121  auto width = w*2;
122  auto z = 1.1;
123  auto m_targetZoom = 20.0;
124 
125  std::string s = name+"/texture";
126  std::stringstream image;
127  if(v2xid==111)image<<"../images/viewcar2.png";
128  else if(v2xid==222)image<<"../images/FASCarE.png";
129  else if(participant_type==adore::env::traffic::Participant::CAR)image<<"../images/car"<<std::setw(3) <<std::setfill('0')<<((id%18) + 1)<<".png";
130  else if(participant_type==adore::env::traffic::Participant::PEDESTRIAN)image<<"../images/person"<<std::setw(2) <<std::setfill('0')<<((id%10) + 1)<<".png";
131  else if(participant_type==adore::env::traffic::Participant::PEDESTRIAN_GROUP)image<<"../images/person_group.png";
132  else if(participant_type==adore::env::traffic::Participant::BUS)image<<"../images/bus.png";
133  else if(participant_type==adore::env::traffic::Participant::HEAVY_TRUCK)image<<"../images/bus.png";
134  else if(participant_type==adore::env::traffic::Participant::TRUCK)image<<"../images/truck.png";
135  if(image.str().size()>0)
136  {
137  figure_->plotTexture(s,image.str(),gX+std::cos(psi)*rho,gY+std::sin(psi)*rho,z,psi,width,length);
138  plot_tags_current_.insert(s);
139  }
140  double cpsi = std::cos(psi);
141  double spsi = std::sin(psi);
142  double X[14];
143  double Y[14];
144  double vscale = 1.0;
145  double v = std::sqrt(vx*vx+vy*vy);
146  double cv = v>0.1?vx/v:0.0;
147  double sv = v>0.1?vy/v:0.0;
148  double ca = 0.7071;
149  double sa = 0.7071;
150  double ascale = 0.2 * vscale;
151  double ax = -v * ca * ascale - v * sa * ascale;
152  double ay = -v * sa * ascale + v * ca * ascale;
153  ax= 1.0;
154  ay = 1.0;
155  X[0] = gX + std::cos(psi) * (b) - std::sin(psi) * (-w);
156  Y[0] = gY + std::sin(psi) * (b) + std::cos(psi) * (-w);
157  X[1] = gX + std::cos(psi) * (a+b+c) - std::sin(psi) * (0.0);
158  Y[1] = gY + std::sin(psi) * (a+b+c) + std::cos(psi) * (0.0);
159  X[2] = gX + std::cos(psi) * (b) - std::sin(psi) * (w);
160  Y[2] = gY + std::sin(psi) * (b) + std::cos(psi) * (w);
161  X[3] = gX + std::cos(psi) * (b) - std::sin(psi) * (-w);
162  Y[3] = gY + std::sin(psi) * (b) + std::cos(psi) * (-w);
163  X[4] = gX + std::cos(psi) * (a+b+c) - std::sin(psi) * (-w);
164  Y[4] = gY + std::sin(psi) * (a+b+c) + std::cos(psi) * (-w);
165  X[5] = gX + std::cos(psi) * (a+b+c) - std::sin(psi) * (w);
166  Y[5] = gY + std::sin(psi) * (a+b+c) + std::cos(psi) * (w);
167  X[6] = gX + std::cos(psi) * (-d) - std::sin(psi) * (w);
168  Y[6] = gY + std::sin(psi) * (-d) + std::cos(psi) * (w);
169  X[7] = gX + std::cos(psi) * (-d) - std::sin(psi) * (-w);
170  Y[7] = gY + std::sin(psi) * (-d) + std::cos(psi) * (-w);
171  X[8] = gX + std::cos(psi) * (b) - std::sin(psi) * (-w);
172  Y[8] = gY + std::sin(psi) * (b) + std::cos(psi) * (-w);
173  X[9] = gX + std::cos(psi) * (b) - std::sin(psi) * (0.0);
174  Y[9] = gY + std::sin(psi) * (b) + std::cos(psi) * (0.0);
175  X[10] = gX + std::cos(psi) * (b + vx*vscale) - std::sin(psi) * (vy*vscale);
176  Y[10] = gY + std::sin(psi) * (b + vx*vscale) + std::cos(psi) * (vy*vscale);
177  X[11] = gX + std::cos(psi) * (b + vx*vscale - cv * ax - sv * ay) - std::sin(psi) * (vy*vscale -sv * ax + cv * ay);
178  Y[11] = gY + std::sin(psi) * (b + vx*vscale - cv * ax - sv * ay) + std::cos(psi) * (vy*vscale -sv * ax + cv * ay);
179  X[12] = gX + std::cos(psi) * (b + vx*vscale) - std::sin(psi) * (vy*vscale);
180  Y[12] = gY + std::sin(psi) * (b + vx*vscale) + std::cos(psi) * (vy*vscale);
181  X[13] = gX + std::cos(psi) * (b + vx*vscale - cv * ax + sv * ay) - std::sin(psi) * (vy*vscale -sv * ax - cv * ay);
182  Y[13] = gY + std::sin(psi) * (b + vx*vscale - cv * ax + sv * ay) + std::cos(psi) * (vy*vscale -sv * ax - cv * ay);
183  std::string s2 = name+"/outline";
184  figure_->plot(s2,X,Y,z+0.5,14,"LineColor=1,0,0");
185  plot_tags_current_.insert(s2);
186  if (debug_plot_ids_)
187  {
188  figure_->plotText(name,gX+w+0.5,gY,name);
189  }
190  }
191  };
192  }
193 }
Definition: afigurestub.h:24
virtual void erase_similar(std::string hashtag)=0
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 erase(std::string hashtag)=0
virtual void plotText(std::string hashtag, double x, double y, std::string text)=0
a optimzed plotting application to plot other traffic
Definition: plot_traffic.h:32
PlotTraffic(DLR_TS::PlotLab::AFigureStub *figure, std::string prefix, bool debug_plot_ids)
Definition: plot_traffic.h:47
std::unordered_set< std::string > plot_tags_current_
Definition: plot_traffic.h:38
void plotPosition(const std::string &name, double gX, double gY, double psi, double L, double c, double d, double w, double vx, double vy, const std::string &options, adore::env::traffic::Participant::EClassification participant_type, int id, int v2xid=0)
plotting a vehicle
Definition: plot_traffic.h:115
bool debug_plot_ids_
Definition: plot_traffic.h:35
std::unordered_set< std::string > plot_tags_old_
Definition: plot_traffic.h:37
~PlotTraffic()
Definition: plot_traffic.h:55
std::string app_tag_
Definition: plot_traffic.h:44
adore::env::AFactory::TParticipantSetReader * trafficReader_
Definition: plot_traffic.h:34
DLR_TS::PlotLab::AFigureStub * figure_
Definition: plot_traffic.h:40
std::string prefix_
Definition: plot_traffic.h:43
void run()
Definition: plot_traffic.h:59
virtual TParticipantSetReader * getTrafficParticipantSetReader()=0
static adore::env::AFactory * get()
Definition: afactory.h:236
Definition: com_patterns.h:68
virtual void getData(T &value)=0
virtual bool hasUpdate() const =0
std::vector< Participant > TParticipantSet
Definition: participant.h:164
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
z
Definition: adore_set_goal.py:32
w
Definition: adore_set_pose.py:40
Definition: areaofeffectconverter.h:20
EClassification
Definition: participant.h:36
@ BUS
Definition: participant.h:49
@ PEDESTRIAN_GROUP
Definition: participant.h:56
@ TRUCK
Definition: participant.h:50
@ CAR
Definition: participant.h:47
@ HEAVY_TRUCK
Definition: participant.h:52
@ PEDESTRIAN
Definition: participant.h:42