ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
setpointrequest.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 API and implementation
13  ********************************************************************************/
14 
15 #pragma once
16 #include <adore/fun/setpoint.h>
17 #include <adore/mad/oderk4.h>
21 
22 
23 namespace adore
24 {
25  namespace fun
26  {
27 
35  {
36 
37  public:
38  std::vector<SetPoint> setPoints;
41  public:
43  {
45  }
46  virtual ~SetPointRequest()
47  {
48 
49  }
50  void push_back(const SetPoint& setPoint)
51  {
52  setPoints.push_back(setPoint);
53  }
54 
63  void append(const adoreMatrix<double,1,0>& T,const adoreMatrix<double,0,0>& X,int N,int maneuverID=0)
64  {
65  const int k = (std::ceil)((double)T.nc()/(double)N);
66  for(int i=0;i<T.nc();i+=k)
67  {
68  double ti = T(i);
69  double tj = i+k>=T.nc()?ti:T(i+k);
70  SetPoint sp;
71  sp.maneuverID = maneuverID;
72  sp.tStart = ti;
73  sp.tEnd = tj;
74  for(int n=0;n<(std::min)(X.nr(),sp.x0ref.data.nr());n++)
75  {
76  sp.x0ref.data(n) = X(n,i);
77  }
78  push_back(sp);
79  }
80  }
81 
86  void setStartTime(double new_t0)
87  {
88  if(setPoints.size()>0)
89  {
90  double delta_t = new_t0 - setPoints[0].tStart;
91  for(auto it=setPoints.begin();it!=setPoints.end();it++)
92  {
93  it->tStart += delta_t;
94  it->tEnd += delta_t;
95  }
96  }
97  }
98 
99  double getDuration()const
100  {
101  if(setPoints.size()==0)return 0.0;
102  else return setPoints.rbegin()->tEnd-setPoints.begin()->tStart;
103  }
104 
113  {
114  PlanarVehicleState10d vehiclestate;
115  vehiclestate.data = setPoints[setPoints.size()-1].x0ref.data;
117  adore::fun::VLB_OpenLoop model(p);
118  static const bool use_rk = false;
119  for (size_t i = 0; i < setPoints.size()-1; i++)
120  {
121  if (t >= setPoints[i].tStart && t < setPoints[i+1].tStart )
122  {
123  if(use_rk)
124  {
125  //integrate the reference trajectory to acquire current xref
126  auto T = adore::mad::sequence<double>(0, 0.01, t - setPoints[i].tStart);
127  auto result = rk4.solve(&model, T, setPoints[i].x0ref.data);
128  vehiclestate.data = colm(result, result.nc() - 1);
129  }
130  else
131  {
132  //linearly interpolate
133  const double ti = setPoints[i].tStart;
134  const double tj = setPoints[i+1].tStart;
135  const auto xi = setPoints[i].x0ref.data;
136  const auto xj = setPoints[i+1].x0ref.data;
137  vehiclestate.data = xi + (xj-xi) * (t-ti) / (tj-ti);
138  }
139 
140  break;
141  }
142  }
143  return vehiclestate;
144  }
146  {
147  SetPoint s;
148  s.tStart = t;
149  s.tEnd = t;
150  s.x0ref = interpolateReference(t,p);
151  return s;
152  }
153 
160  void relocate(double deltaX,double deltaY, double new_PSI0)
161  {
163  assert(setPoints.size()>0);
164  double delta_x1 = 0;//setPoints[0].x0ref.getX();
165  double delta_y1 = 0;//setPoints[0].x0ref.getY();
166  double delta_psi = new_PSI0 - setPoints[0].x0ref.getPSI();
167  double delta_x2 = deltaX;
168  double delta_y2 = deltaY;
169  double c = (std::cos)(delta_psi);
170  double s = (std::sin)(delta_psi);
171  for(auto it = setPoints.begin();it!=setPoints.end();it++)
172  {
173  double x = it->x0ref.getX()-delta_x1;
174  double y = it->x0ref.getY()-delta_y1;
175  it->x0ref.setX(c*x-s*y+delta_x2);
176  it->x0ref.setY(s*x+c*y+delta_y2);
177  it->x0ref.setPSI(it->x0ref.getPSI() + delta_psi);
178  }
179  }
180 
187  void relocateTo(double newX0,double newY0, double new_PSI0)
188  {
189  assert(setPoints.size()>0);
190  double delta_x1 = setPoints[0].x0ref.getX();
191  double delta_y1 = setPoints[0].x0ref.getY();
192  double delta_psi = new_PSI0 - setPoints[0].x0ref.getPSI();
193  double delta_x2 = newX0;
194  double delta_y2 = newY0;
195  double c = (std::cos)(delta_psi);
196  double s = (std::sin)(delta_psi);
197  for(auto it = setPoints.begin();it!=setPoints.end();it++)
198  {
199  double x = it->x0ref.getX()-delta_x1;
200  double y = it->x0ref.getY()-delta_y1;
201  it->x0ref.setX(c*x-s*y+delta_x2);
202  it->x0ref.setY(s*x+c*y+delta_y2);
203  it->x0ref.setPSI(it->x0ref.getPSI() + delta_psi);
204  }
205  }
206 
211  void removeAfter(double t)
212  {
213  while(setPoints.size()>0 && setPoints.back().tStart>t)
214  {
215  setPoints.pop_back();
216  }
217  }
218 
223  {
224  int mcount = 0;
225  int mid = -9999999;
226  for(auto it=setPoints.begin();it!=setPoints.end();it++)
227  {
228  if(it->maneuverID!=mid)
229  {
230  mcount++;
231  mid = it->maneuverID;
232  }
233  }
234  return mcount;
235  }
236 
240  void compress(int targetCount)
241  {
242  if(targetCount>(int)setPoints.size())return;
243  //count number of different maneuvers
244  int mcount = numberOfDistinctManeuvers();
245  if(targetCount<=mcount*2)return;
246  double dt_min = (setPoints.back().tEnd-setPoints.front().tStart)/((double)(targetCount-mcount*2));
247  std::vector<SetPoint> buffer;
248  buffer.push_back(setPoints.front());
249  double t = setPoints.front().tEnd;
250  for(int i=1;i<(int)setPoints.size()-1;i++)
251  {
252  if( setPoints[i].maneuverID!=setPoints[i-1].maneuverID
253  || setPoints[i].maneuverID!=setPoints[i+1].maneuverID )
254  {
255  buffer.push_back(setPoints[i]);
256  t = setPoints[i].tEnd;
257  }
258  else
259  {
260  if(setPoints[i].tStart>t+dt_min)
261  {
262  buffer.push_back(setPoints[i]);
263  t = setPoints[i].tEnd;
264  }
265  }
266  }
267  buffer.push_back(setPoints.back());
268  for(int i=0;i<(int)buffer.size()-1;i++)
269  {
270  buffer[i].tEnd = buffer[i+1].tStart;
271  }
272  setPoints.clear();
273  for(auto it=buffer.begin();it!=buffer.end();it++)setPoints.push_back(*it);
274  }
275 
279  void copySetPointInterval(double t0, double t1, SetPointRequest& destination) const
280  {
281  for(auto it = setPoints.begin();it!=setPoints.end();it++)
282  {
283  if( (t0 <= it->tStart && it->tStart < t1) || (it->tStart <= t0 && t0 < it->tEnd) ) //interval overlap
284  {
285  destination.setPoints.push_back(*it);
286  }
287  }
288  }
289 
293  void copyTo(SetPointRequest& destination) const
294  {
295  for(auto it = setPoints.begin();it!=setPoints.end();it++)
296  {
297  destination.setPoints.push_back(*it);
298  }
299  }
300 
304  void copySetPointInterval(double t0, double t1, SetPointRequest& destination, int maneuverID) const
305  {
306  for(auto it = setPoints.begin();it!=setPoints.end();it++)
307  {
308  if ((t0 <= it->tStart && it->tStart < t1)
309  || (it->tStart <= t0 && t0 < it->tEnd )) //interval overlap
310  {
311  destination.setPoints.push_back(*it);
312  destination.setPoints.back().maneuverID = maneuverID;
313  }
314  }
315  }
316 
320  void copyTo(SetPointRequest& destination, int maneuverID) const
321  {
322  for(auto it = setPoints.begin();it!=setPoints.end();it++)
323  {
324  destination.setPoints.push_back(*it);
325  destination.setPoints.back().maneuverID = maneuverID;
326  }
327  }
328 
332  bool isActive(double t) const
333  {
334  if(setPoints.size()==0)
335  {
336  return false;
337  }
338  else
339  {
340  return setPoints.front().tStart <= t && t < setPoints.back().tEnd;
341  }
342  }
346  bool isPending(double t)const
347  {
348  if(setPoints.size()<1)
349  {
350  return false;
351  }
352  else
353  {
354  return t<setPoints[0].tStart;
355  }
356  }
360  bool isDone(double t) const
361  {
362  if(setPoints.size()<1)
363  {
364  return false;
365  }
366  else
367  {
368  return t>=setPoints[setPoints.size()-1].tEnd;
369  }
370  }
374  int getActiveElementNumber(double t)const
375  {
376  assert(isActive(t));
377  int i=0;
378  for(auto it = setPoints.begin();it!=setPoints.end();it++)
379  {
380  if( it->tStart <= t && t < it->tEnd )
381  {
382  return i;
383  }
384  i++;
385  }
386  return 0;
387  }
388 
394  {
396  for(unsigned int i=0;i<setPoints.size();i++) // fix -Wsign-compare
397  {
398  tau.getData()(0,i) = setPoints[i].tStart;
399  tau.getData()(1,i) = setPoints[i].x0ref.getX();
400  tau.getData()(2,i) = setPoints[i].x0ref.getY();
401  tau.getData()(3,i) = setPoints[i].x0ref.getPSI();
402  tau.getData()(4,i) = setPoints[i].x0ref.getvx();
403  }
404  return tau;
405  }
406 
412  void cropAfterFirstStop(double vxslow)
413  {
414  if(setPoints.size()==0)return;
415  int crossdown = -1;
416  double vxi = setPoints[0].x0ref.getvx();
417  for(int j=1;j<setPoints.size();j++)
418  {
419  double vxj = setPoints[j].x0ref.getvx();
420  if(crossdown==-1 && vxi>vxslow && vxj<=vxslow)
421  {
422  crossdown = j+1;
423  }
424  }
425  if(crossdown!=-1)
426  {
427  while(setPoints.size()>crossdown)setPoints.pop_back();
428  }
429  setPoints.rbegin()->x0ref.setvx(0.0);
430  auto xend = *(setPoints).rbegin();
431  xend.tStart = xend.tEnd;
432  xend.tEnd = xend.tEnd + 5.0;
433  setPoints.push_back(xend);
434  }
435  };
436 
437  }
438 }
Definition: setpointrequest.h:35
void copySetPointInterval(double t0, double t1, SetPointRequest &destination) const
Definition: setpointrequest.h:279
void cropAfterFirstStop(double vxslow)
remove SetPoints after first stop Method looks for first downward zero crossing of vx and removes all...
Definition: setpointrequest.h:412
int setPointRequestID
Definition: setpointrequest.h:39
bool isDone(double t) const
Definition: setpointrequest.h:360
int getActiveElementNumber(double t) const
Definition: setpointrequest.h:374
SetPoint interpolateSetPoint(double t, adore::params::APVehicle *p) const
Definition: setpointrequest.h:145
double getDuration() const
Definition: setpointrequest.h:99
PlanarVehicleState10d interpolateReference(double t, adore::params::APVehicle *p) const
Definition: setpointrequest.h:112
void copySetPointInterval(double t0, double t1, SetPointRequest &destination, int maneuverID) const
Definition: setpointrequest.h:304
void relocateTo(double newX0, double newY0, double new_PSI0)
Definition: setpointrequest.h:187
virtual ~SetPointRequest()
Definition: setpointrequest.h:46
adore::mad::LLinearPiecewiseFunctionM< double, 4 > getTrajectory()
Definition: setpointrequest.h:393
void setStartTime(double new_t0)
Definition: setpointrequest.h:86
void compress(int targetCount)
Definition: setpointrequest.h:240
void push_back(const SetPoint &setPoint)
Definition: setpointrequest.h:50
void relocate(double deltaX, double deltaY, double new_PSI0)
Definition: setpointrequest.h:160
int numberOfDistinctManeuvers()
Definition: setpointrequest.h:222
void append(const adoreMatrix< double, 1, 0 > &T, const adoreMatrix< double, 0, 0 > &X, int N, int maneuverID=0)
Definition: setpointrequest.h:63
bool isPending(double t) const
Definition: setpointrequest.h:346
bool isActive(double t) const
Definition: setpointrequest.h:332
void removeAfter(double t)
Definition: setpointrequest.h:211
void copyTo(SetPointRequest &destination) const
Definition: setpointrequest.h:293
void copyTo(SetPointRequest &destination, int maneuverID) const
Definition: setpointrequest.h:320
SetPointRequest()
Definition: setpointrequest.h:42
std::vector< SetPoint > setPoints
Definition: setpointrequest.h:38
Definition: setpoint.h:30
PlanarVehicleState10d x0ref
Definition: setpoint.h:32
double tStart
Definition: setpoint.h:34
int maneuverID
Definition: setpoint.h:36
double tEnd
Definition: setpoint.h:35
Definition: vlb_openloop.h:32
virtual adoreMatrix< T > solve(AOdeModel< T > *model, const adoreMatrix< double, 1, 0 > &time, const adoreMatrix< double, 0, 1 > &x0) override
Definition: oderk4.h:37
abstract class for vehicle configuration related paremeters
Definition: ap_vehicle.h:29
double tau
Definition: platoonStateMachine.h:39
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
T min(T a, T b, T c, T d)
Definition: adoremath.h:663
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
x
Definition: adore_set_goal.py:30
y
Definition: adore_set_goal.py:31
Definition: areaofeffectconverter.h:20
Definition: planarvehiclestate10d.h:41
adoreMatrix< double, 10, 1 > data
Definition: planarvehiclestate10d.h:61