ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
test_lc_trajectory_planner.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  * Matthias Nichting - initial implementation
13  ********************************************************************************/
14 
15 #pragma once
16 
18 #include <adore/fun/afactory.h>
22 #include <adore/env/afactory.h>
23 #include <adore/params/afactory.h>
29 #include <iostream>
30 
31 namespace adore
32 {
33 namespace apps
34 {
40 {
41 private:
60  double last_n_;
62 
63 public:
65  : envFactory_(adore::env::EnvFactoryInstance::get()), funFactory_(adore::fun::FunFactoryInstance::get()), paramsFactory_(adore::params::ParamsFactoryInstance::get()),
70  {
71 
73  lfplanner_ = new TPlanner(&lfv_,nullptr,nullptr,nullptr,
79  lcplanner_l_ = new adore::fun::BasicLaneChangePlanner<20, 5>(&lcvl_, nullptr,nullptr,nullptr,nullptr,nullptr,
85  lcplanner_r_ = new adore::fun::BasicLaneChangePlanner<20, 5>(&lcvr_, nullptr,nullptr,nullptr,nullptr,nullptr,
92  wwriter_ = funFactory_->getSetPointRequestWriter();
95  }
96  void run()
97  {
98  roadmap_.update();
100  lfv_.update();
103  xreader_->getData(x_);
104  auto x_replan = x_;
105  bool reset = true;
107  {
108  auto spr = active_planner_->getSetPointRequest();
109  double t = x_.getTime();
110  if (spr->isActive(t))
111  {
112  auto x_ref = spr->interpolateReference(t, pvehicle_);
113  double dx = x_.getX() - x_ref.getX();
114  double dy = x_.getY() - x_ref.getY();
115  if (dx * dx + dy * dy < pTacticalPlanner_->getResetRadius())
116  {
117  x_replan.setX(x_ref.getX());
118  x_replan.setY(x_ref.getY());
119  x_replan.setPSI(x_ref.getPSI());
120  x_replan.setvx(x_ref.getvx());
121  x_replan.setvy(x_ref.getvy());
122  x_replan.setOmega(x_ref.getOmega());
123  x_replan.setAx(x_ref.getAx());
124  x_replan.setDelta(x_ref.getDelta());
125  reset = false;
126  }
127  }
128  }
129  if (reset)
130  std::cout << "TestTrajectoryPlanner: Reset initial state.\n";
131  double s, n;
132  lfv_.toRelativeCoordinates(x_replan.getX(), x_replan.getY(), s, n);
134  {
135  auto olt = lfv_.getOnLaneTraffic();
136  for (auto &obj : olt)
137  {
138  if (obj.getCurrentProgress() > s && obj.getCurrentProgress() < s + 30.0 && obj.getCurrentSpeed() < 0.8 * lfv_.getSpeedLimit(s) && lcvl_.getTargetLane()->isValid())
139  {
140  perform_lc_left_ = true;
141  break;
142  }
143  }
145  {
146  auto oltr = lcvr_.getTargetLane()->getOnLaneTraffic();
147  perform_lc_right_ = true;
148  for (auto &obj : oltr)
149  {
150  if (obj.getCurrentProgress() < s+35 && obj.getCurrentProgress() > s-2 && obj.getCurrentSpeed() < 0.95 * lfv_.getSpeedLimit(s))
151  {
152  perform_lc_right_ = false;
153  }
154  }
155  }
156  }
157 
158  if (last_n_ > 0.5 && n < 0.0 || last_n_ < -0.5 && n > 0.0) // lane change complete
159  {
160  perform_lc_left_ = false;
161  perform_lc_right_ = false;
162  }
163  last_n_ = n;
164  lfplanner_->compute(x_replan);
166  {
168  {
169  // lcplanner_r_->setGapCoordinate(s);
170  }
171  lcplanner_r_->compute(x_replan);
172  }
174  {
176  {
177  // lcplanner_l_->setGapCoordinate(s);
178  }
179  lcplanner_l_->compute(x_replan);
180  }
182  {
184  }
186  {
188  }
189  else
190  {
191  perform_lc_right_ = false;
192  perform_lc_left_ = false;
194  }
197  }
199  {
200  return active_planner_->hasValidPlan();
201  }
203  {
204  perform_lc_left_ = true;
205  perform_lc_right_ = false;
206  }
208  {
209  perform_lc_left_ = false;
210  perform_lc_right_ = true;
211  }
213  {
215  }
217  {
219  }
221  {
222  return active_planner_;
223  }
225  {
227  }
229  {
231  }
232 
234  {
235  auto cornerpoints = lfv_.getCornerPoints();
236  std::stringstream ss;
237  std::string options_point = "LineColor=1.0,0.0,0.0;LineWidth=0;LineStyle=none;PointSize=5";
238  int c = 0;
239  //{
240  // lfv_.toEucledianCoordinates(i->first,i->second,x[c],y[c],z[c]);
241  //++c;
242  //}
243  //fig->plot(ss.str(),x,y,1.0,c,options_point);
244  std::cout << "cornerpoitns size" << cornerpoints->size() << std::endl;
245  double xc[cornerpoints->size()];
246  double yc[cornerpoints->size()];
247  double zc[cornerpoints->size()];
248  c = 0;
249  for (auto i = cornerpoints->begin(); i != cornerpoints->end(); ++i)
250  {
251  xc[c] = i->m_X;
252  yc[c] = i->m_Y;
253  ++c;
254  }
255  options_point = "LineColor=1.0,0.0,1.0;LineWidth=0;LineStyle=none;PointSize=6";
256  fig->plot("cornerpoints", xc, yc, 2.0, c, options_point);
257  std::cout << "now:conflictset" << std::endl;
258  auto plotdata = lfv_.getConflictSetPlotData();
259  std::string options;
260  std::string options_normal = "LineColor=0.0,1.0,0.0;LineWidth=3";
261  std::string options_highlighted = "LineColor=1.0,1.0,0.0;LineWidth=6";
262  std::string cz_str = "cz_subgroup";
263  std::string bo_str = "bo_number";
264  int bo_int;
265  int cz_int = 0;
266  std::cout << "\n plot evaluated borders";
267  for (auto it = plotdata->begin(); it != plotdata->end(); ++it)
268  {
269  bo_int = 0;
270  std::cout << "number of cz:" << plotdata->size() << std::endl;
271  for (auto ij = it->second.begin(); ij != it->second.end(); ++ij)
272  {
273  std::cout << " path of this cz" << std::endl;
274  options = (bo_int == 0 ? options_highlighted : options_normal);
275  PLOT::plotPath(cz_str + std::to_string(cz_int) + bo_str + std::to_string(bo_int), roadmap_.getBorderSet()->getCenterline((*ij)->m_id).getData(), 0.18, options, fig);
276  ++bo_int;
277  }
278  ++cz_int;
279  }
280  }
282  {
283  static const int nplot = 100;
284  double X[nplot];
285  double Y[nplot];
286  double Xn[nplot];
287  double Yn[nplot];
288  int n = right->m_path->getData().nc();
289  right->m_path->writePointsToArray(0, n - 1, 0, X);
290  right->m_path->writePointsToArray(0, n - 1, 1, Y);
291  if (left != 0)
292  {
293  int m = left->m_path->getData().nc();
294  left->m_path->writePointsToArray(0, m - 1, 0, Xn);
295  left->m_path->writePointsToArray(0, m - 1, 1, Yn);
296  if (right->getNeighborDirection() == adore::env::BorderBased::Border::SAME_DIRECTION)
297  {
298  for (int i = 0; i < m; i++)
299  {
300  X[n + m - i - 1] = Xn[i];
301  Y[n + m - i - 1] = Yn[i];
302  }
303  }
304  else
305  {
306  for (int i = 0; i < m; i++)
307  {
308  X[n + i] = Xn[i];
309  Y[n + i] = Yn[i];
310  }
311  }
312  figure_->patch(name + "/h", X, Y, z, n + m, options);
313  }
314  figure_->plot(name, X, Y, z + 0.1, n, options);
315  X[1] = X[n - 1];
316  Y[1] = Y[n - 1];
317  //figure_->plot(name+"/p",X,Y,z+0.1,2,"LineStyle=none;PointSize=5");
318  }
319 
321  {
322  std::cout << "plot border overlaps" << std::endl;
323  auto overlapsets = lfv_.getOverlapSet();
324  int os = -1;
325  int count = 0;
326  for (auto overlapset = overlapsets->begin(); overlapset != overlapsets->end(); ++overlapset)
327  {
328  ++os;
329  int p = 0;
330  for (auto overlap = overlapset->borderoverlaps_.begin(); overlap != overlapset->borderoverlaps_.end(); ++overlap)
331  {
332  p++;
333  auto cpv = (*overlap)->getCornerPointVector();
334  count += cpv->size();
335  double x[cpv->size()];
336  double y[cpv->size()];
337  double z[cpv->size()];
338  int i = 0;
339  for (auto coordinate = cpv->begin(); coordinate != cpv->end(); ++coordinate)
340  {
341  x[i] = coordinate->m_X;
342  y[i] = coordinate->m_Y;
343  ++i;
344  }
345  double red = 1.0 - os / 6.1;
346  double green = os / 6.1;
347  std::string options_point = "LineColor=" + std::to_string(red) + "," + std::to_string(green) + ",1.0;LineWidth=0;LineStyle=none;PointSize=11";
348  fig->plot("cornerpointsos" + std::to_string(p) + std::to_string(os), x, y, 2.0 + os, i, options_point);
349  std::string optionsy = "LineColor=1.0,1.0,0.0;LineWidth=2;PointSize=1";
350  std::string options = "LineColor=1.0,0.0,0.0;LineWidth=2;PointSize=1";
351  std::string optionsg = "LineColor=0.0,1.0,0.0;LineWidth=2;PointSize=1";
352  plotBorder("namelfvl" + std::to_string(p) + std::to_string(os), (*overlap)->m_first_left, 0, 5.0, optionsy, fig);
353  plotBorder("namelfvr" + std::to_string(p) + std::to_string(os), (*overlap)->m_first_right, 0, 5.0, optionsy, fig);
354  plotBorder("namesl" + std::to_string(p) + std::to_string(os), (*overlap)->m_second_left, 0, 10.0, optionsg, fig);
355  plotBorder("namesr" + std::to_string(p) + std::to_string(os), (*overlap)->m_second_right, 0, 10.0, options, fig);
356  }
357  int counteri = 0;
358  std::vector<env::BorderBased::Border *> borderlist;
359  std::vector<int> ids;
360  lfv_.getBordersToPrint(borderlist, ids);
361  std::string optionsmagenta = "LineColor=1.0,0.0,1.0;LineWidth=2;PointSize=2";
362  std::string optionsmagenta2 = "LineColor=1.0,0.0,1.0;LineWidth=8;PointSize=8";
363  for (auto &b : borderlist)
364  {
365  if (std::find(ids.begin(), ids.end(), counteri) != ids.end())
366  {
367  plotBorder("borderpathC" + std::to_string(counteri), b, 0, 30.0, optionsmagenta2, fig);
368  }
369  else
370  {
371  plotBorder("borderpathC" + std::to_string(counteri), b, 0, 30.0, optionsmagenta, fig);
372  }
373 
374  ++counteri;
375  }
376  }
377  }
379  {
380  auto plotdata = lfv_.getConflictSetPlotData();
381  if (i > plotdata->size() - 1)
382  i = 0;
383  std::string options;
384  std::string options_normal = "LineColor=0.0,1.0,0.0;LineWidth=3";
385  std::string options_highlighted = "LineColor=1.0,1.0,0.0;LineWidth=6";
386  std::string cz_str = "cz_subgroup";
387  std::string bo_str = "bo_number";
388  int bo_int;
389  int cz_int = 0;
390  std::cout << "\n plot evaluated borders";
391  bool plot = false;
392  for (auto it = plotdata->begin(); it != plotdata->end(); ++it)
393  {
394  plot = i == cz_int ? true : false;
395  bo_int = 0;
396  std::cout << "number of cz:" << plotdata->size() << std::endl;
397  for (auto ij = it->second.begin(); ij != it->second.end(); ++ij)
398  {
399  std::cout << " path of this cz" << std::endl;
400  options = (bo_int == 0 ? options_highlighted : options_normal);
401  if (plot)
402  PLOT::plotPath(cz_str + std::to_string(cz_int) + bo_str + std::to_string(bo_int), roadmap_.getBorderSet()->getCenterline((*ij)->m_id).getData(), 0.18, options, fig);
403  else
404  fig->erase(cz_str + std::to_string(cz_int) + bo_str + std::to_string(bo_int));
405  ++bo_int;
406  }
407  ++cz_int;
408  }
409  ++i;
410  }
411 };
412 } // namespace apps
413 } // namespace adore
Definition: afigurestub.h:24
virtual void patch(std::string hashtag, double *X, double *Y, double *Z, int size, std::string options)=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
test implementation of a lane change trajectory planner
Definition: test_lc_trajectory_planner.h:40
adore::env::BorderBased::LaneChangeView lcvl_
Definition: test_lc_trajectory_planner.h:50
void plotCZ(DLR_TS::PlotLab::AFigureStub *fig, int &i)
Definition: test_lc_trajectory_planner.h:378
adore::fun::BasicLaneFollowingPlanner< 20, 5 > TPlanner
Definition: test_lc_trajectory_planner.h:42
bool perform_lc_right_
Definition: test_lc_trajectory_planner.h:61
adore::fun::AFactory * funFactory_
Definition: test_lc_trajectory_planner.h:44
bool hasValidPlan()
Definition: test_lc_trajectory_planner.h:198
adore::fun::DecoupledLFLCPlanner< 20, 5 > * getPlanner()
Definition: test_lc_trajectory_planner.h:220
adore::env::traffic::TrafficMap trafficMap_
Definition: test_lc_trajectory_planner.h:53
void plotConflictSet(DLR_TS::PlotLab::AFigureStub *fig)
Definition: test_lc_trajectory_planner.h:233
bool perform_lc_left_
Definition: test_lc_trajectory_planner.h:61
adore::env::BorderBased::LaneFollowingView lfv_
Definition: test_lc_trajectory_planner.h:48
adore::params::APTacticalPlanner * pTacticalPlanner_
Definition: test_lc_trajectory_planner.h:46
adore::params::AFactory * paramsFactory_
Definition: test_lc_trajectory_planner.h:45
adore::params::APVehicle * pvehicle_
Definition: test_lc_trajectory_planner.h:59
TPlanner * lfplanner_
Definition: test_lc_trajectory_planner.h:55
adore::fun::VehicleMotionState9d x_
Definition: test_lc_trajectory_planner.h:54
TestLCTrajectoryPlanner()
Definition: test_lc_trajectory_planner.h:64
adore::fun::DecoupledLFLCPlanner< 20, 5 >::TProgressSolver & getProgressSolver()
Definition: test_lc_trajectory_planner.h:212
void performLCRight()
Definition: test_lc_trajectory_planner.h:207
adore::fun::DecoupledLFLCPlanner< 20, 5 > * active_planner_
Definition: test_lc_trajectory_planner.h:58
double last_n_
Definition: test_lc_trajectory_planner.h:60
adore::fun::BasicLaneChangePlanner< 20, 5 > * lcplanner_l_
Definition: test_lc_trajectory_planner.h:56
adore::fun::RoadCoordinateConverter & getRoadCoordinateConverter()
Definition: test_lc_trajectory_planner.h:228
adore::env::BorderBased::LocalRoadMap roadmap_
Definition: test_lc_trajectory_planner.h:47
adore::mad::AWriter< adore::fun::SetPointRequest > * wwriter_
Definition: test_lc_trajectory_planner.h:52
adore::env::BorderBased::LaneChangeView lcvr_
Definition: test_lc_trajectory_planner.h:49
void plotBorder(std::string name, adore::env::BorderBased::Border *right, adore::env::BorderBased::Border *left, double z, std::string options, DLR_TS::PlotLab::AFigureStub *figure_)
Definition: test_lc_trajectory_planner.h:281
adore::mad::AReader< adore::fun::VehicleMotionState9d > * xreader_
Definition: test_lc_trajectory_planner.h:51
void run()
Definition: test_lc_trajectory_planner.h:96
void performLCLeft()
Definition: test_lc_trajectory_planner.h:202
const adore::fun::SetPointRequest * getSetPointRequest()
Definition: test_lc_trajectory_planner.h:224
adore::fun::DecoupledLFLCPlanner< 20, 5 >::TOffsetSolver & getOffsetSolver()
Definition: test_lc_trajectory_planner.h:216
adore::fun::BasicLaneChangePlanner< 20, 5 > * lcplanner_r_
Definition: test_lc_trajectory_planner.h:57
void plotBorderOverlapSet(DLR_TS::PlotLab::AFigureStub *fig)
Definition: test_lc_trajectory_planner.h:320
adore::env::AFactory * envFactory_
Definition: test_lc_trajectory_planner.h:43
abstract factory for adore::env communication
Definition: afactory.h:41
virtual TVehicleMotionStateReader * getVehicleMotionStateReader()=0
adore::mad::LLinearPiecewiseFunctionM< double, 4 > getCenterline(const BorderID &id)
get the linear piecewise description of the centerline:
Definition: borderset.h:647
LaneChangeView provides traffic related information for an adjacent lane.
Definition: lanechangeview.h:182
virtual adore::view::ALane * getTargetLane() override
Definition: lanechangeview.h:243
void update(adore::view::ALaneChangeView::direction direction)
Update the LaneChangeView.
Definition: lanechangeview.h:222
LaneFollowingview provides traffic related information for the current lane.
Definition: lanefollowingview.h:42
virtual const adore::view::TrafficQueue & getOnLaneTraffic() const override
Definition: lanefollowingview.h:140
virtual double getSpeedLimit(double s) override
Definition: lanefollowingview.h:157
void update()
update the LaneFollowingView
Definition: lanefollowingview.h:87
void getBordersToPrint(std::vector< Border * > &result, std::vector< int > &ids)
Definition: lanefollowingview.h:99
std::unordered_multimap< view::ConflictZone *, std::vector< Border * > > * getConflictSetPlotData()
Definition: lanefollowingview.h:78
std::vector< BorderOverlapSet > * getOverlapSet()
Definition: lanefollowingview.h:233
virtual void toRelativeCoordinates(double xe, double ye, double &s, double &n) override
Definition: lanefollowingview.h:217
std::vector< Coordinate > * getCornerPoints()
Definition: lanefollowingview.h:232
Definition: localroadmap.h:38
void update(bool matched_lane_proposal_valid=false, BorderID matched_lane_proposal_id=BorderID())
update the local road map
Definition: localroadmap.h:256
BorderSet * getBorderSet()
Get the BorderSet object.
Definition: localroadmap.h:123
Definition: trafficmap.h:36
void update()
Update the TrafficMap.
Definition: trafficmap.h:126
Definition: basiclanefollowingplanner.h:37
virtual const SetPointRequest * getSetPointRequest() const override
Definition: decoupled_lflc_planner.h:491
TProgressSolver & getProgressSolver()
Definition: decoupled_lflc_planner.h:335
virtual bool hasValidPlan() const override
Definition: decoupled_lflc_planner.h:484
TOffsetSolver & getOffsetSolver()
Definition: decoupled_lflc_planner.h:339
RoadCoordinateConverter & getRoadCoordinateConverter()
Definition: decoupled_lflc_planner.h:343
virtual void compute(const VehicleMotionState9d &initial_state) override
Definition: decoupled_lflc_planner.h:372
Definition: roadcoordinates.h:46
Definition: setpointrequest.h:35
virtual void getData(T &value)=0
virtual void write(const T &value)=0
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
Definition: lq_oc_single_shooting.h:61
abstract factory for adore::params classes
Definition: afactory.h:54
virtual APLateralPlanner * getLateralPlanner() const =0
virtual APTacticalPlanner * getTacticalPlanner() const =0
virtual APTrajectoryGeneration * getTrajectoryGeneration() const =0
virtual APVehicle * getVehicle() const =0
virtual APLongitudinalPlanner * getLongitudinalPlanner() const =0
abstract classs containing parameters to configure aspects and constraints of the tactical planner
Definition: ap_tactical_planner.h:25
abstract class for vehicle configuration related paremeters
Definition: ap_vehicle.h:29
@ LEFT
Definition: alanechangeview.h:43
@ RIGHT
Definition: alanechangeview.h:43
virtual const TrafficQueue & getOnLaneTraffic() const =0
virtual bool isValid() const =0
void plotPath(std::string name, const adoreMatrix< double > &data, double z, std::string options, DLR_TS::PlotLab::AFigureStub *figure)
Definition: plot_shape.h:262
@ right
Definition: indicator_hint.h:36
@ left
Definition: indicator_hint.h:35
x
Definition: adore_set_goal.py:30
y
Definition: adore_set_goal.py:31
z
Definition: adore_set_goal.py:32
Definition: areaofeffectconverter.h:20
The border struct contains data of the smallest.
Definition: border.h:62
@ SAME_DIRECTION
Definition: border.h:507
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 getY() const
Get the y-coordinate.
Definition: vehiclemotionstate9d.h:60
double getTime() const
Get the time.
Definition: vehiclemotionstate9d.h:48
void plot(adore::fun::DecoupledLFLCPlanner< 20, 5 > *testplanner)
Definition: test_lc_trajectory_planner_node.cpp:97