63 void append(
const adoreMatrix<double,1,0>& T,
const adoreMatrix<double,0,0>& X,
int N,
int maneuverID=0)
65 const int k = (std::ceil)((
double)T.nc()/(double)N);
66 for(
int i=0;i<T.nc();i+=k)
69 double tj = i+k>=T.nc()?ti:T(i+k);
90 double delta_t = new_t0 -
setPoints[0].tStart;
93 it->tStart += delta_t;
118 static const bool use_rk =
false;
119 for (
size_t i = 0; i <
setPoints.size()-1; i++)
126 auto T = adore::mad::sequence<double>(0, 0.01, t -
setPoints[i].tStart);
128 vehiclestate.
data = colm(result, result.nc() - 1);
136 const auto xj =
setPoints[i+1].x0ref.data;
137 vehiclestate.
data = xi + (xj-xi) * (t-ti) / (tj-ti);
160 void relocate(
double deltaX,
double deltaY,
double new_PSI0)
166 double delta_psi = new_PSI0 -
setPoints[0].x0ref.getPSI();
167 double delta_x2 = deltaX;
168 double delta_y2 = deltaY;
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);
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;
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);
228 if(it->maneuverID!=mid)
231 mid = it->maneuverID;
242 if(targetCount>(
int)
setPoints.size())
return;
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;
250 for(
int i=1;i<(int)
setPoints.size()-1;i++)
268 for(
int i=0;i<(int)buffer.size()-1;i++)
270 buffer[i].tEnd = buffer[i+1].tStart;
273 for(
auto it=buffer.begin();it!=buffer.end();it++)
setPoints.push_back(*it);
283 if( (t0 <= it->tStart && it->tStart < t1) || (it->tStart <= t0 && t0 < it->tEnd) )
308 if ((t0 <= it->tStart && it->tStart < t1)
309 || (it->tStart <= t0 && t0 < it->tEnd ))
312 destination.
setPoints.back().maneuverID = maneuverID;
325 destination.
setPoints.back().maneuverID = maneuverID;
380 if( it->tStart <= t && t < it->tEnd )
396 for(
unsigned int i=0;i<
setPoints.size();i++)
420 if(crossdown==-1 && vxi>vxslow && vxj<=vxslow)
431 xend.tStart = xend.tEnd;
432 xend.tEnd = xend.tEnd + 5.0;
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