132 const double wheel_radius = 0.40;
133 const double wheel_width = 0.20;
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_);
141 plotPosition(
prefix_+
"/measuredPos",
vehicle_picture_,
position_.
getX(),
position_.
getY(),
position_.
getPSI(),delta,
position_.
getvx(),
position_.
getvy(),
position_.
getOmega(),L,c,d,
w,wheel_radius,wheel_width,
position_style_,
figure_);
146 plotPosition(
prefix_+
"/measuredPos",
vehicle_picture_,
position_.
getX(),
position_.
getY(),
position_.
getPSI(),delta,
position_.
getvx(),
position_.
getvy(),
position_.
getOmega(),L,c,d,
w,wheel_radius,wheel_width,
position_style_,
map_figure_);
163 double scale = 1.0/725.0 * 10.0;
184 static const int N=50;
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;
192 figure->
plot(name,X,Y,2.0,N,options);
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)
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);
225 auto rho = length*0.5-d;
229 if(vehicle_picture!=
"")
235 void attach_wheel_points(
double x,
double y,
double delta,
double wheel_radius,
double wheel_width,std::vector<std::pair<double,double>>& points)
239 const double w = wheel_width * 0.5;
240 const double r = wheel_radius;
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));
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)
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;
268 points.push_back(std::make_pair(
x,
y));
270 y + sin_psi * (0.0) + cos_psi * (-
w+ww+wing_mirror),
271 psi,wheel_radius,wheel_width,points);
273 y + sin_psi * (0.0) + cos_psi * (+
w-ww-wing_mirror),
274 psi,wheel_radius,wheel_width,points);
275 points.push_back(std::make_pair(
x,
y));
276 points.push_back(std::make_pair(
x + cos_psi * (L) - sin_psi * (0.0),
277 y + sin_psi * (L) + cos_psi * (0.0)));
279 y + sin_psi * (L) + cos_psi * (+
w-ww-wing_mirror),
280 psi+delta,wheel_radius,wheel_width,points);
282 y + sin_psi * (L) + cos_psi * (-
w+ww+wing_mirror),
283 psi+delta,wheel_radius,wheel_width,points);
284 points.push_back(std::make_pair(
x + cos_psi * (L) - sin_psi * (0.0),
285 y + sin_psi * (L) + cos_psi * (0.0)));
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)));
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)));
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)));
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)));
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)));
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)));
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;
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)));
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)));
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)));
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