ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
initial_state_selector.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 implementation
13  ********************************************************************************/
14 
15 
16 #pragma once
17 
21 #include <adore/fun/afactory.h>
22 
23 namespace adore
24 {
25 namespace fun
26 {
32  {
33  private:
42  public:
44  {
45  hasValidInitialState_ = false;
47  xxreader_ = FunFactoryInstance::get()->getVehicleExtendedStateReader();
48  }
50  {
51  delete xreader_;
52  delete xxreader_;
53  }
55  {
56  spr_.setPoints.clear();
57  spr_ex.copyTo(spr_);
58  hasValidInitialState_ = false;
59  }
60  void update()
61  {
62  if(xreader_->hasData() && xxreader_->hasData())
63  {
66  x_replan_ = x_;
67  hasValidInitialState_ = true;
68  if( spr_.setPoints.size()>0 //maneuver exists
69  && xx_.getAutomaticControlOn() //< Freigabe im Fahrzeuginterface für Längs und Quer erhalten
70  && xx_.getAutomaticControlAccelerationActive())// Bestätigung der Freigabe durch Benutzer/Gaspedal erfolgt
71  {
72  double t = x_.getTime();
73  if( spr_.isActive(t) )
74  {
75  auto x_ref = spr_.interpolateReference(t,pvehicle_);
76  double dx = x_.getX()-x_ref.getX();
77  double dy = x_.getY()-x_ref.getY();
78  double R = pTacticalPlanner_->getResetRadius();
79  if(dx*dx+dy*dy<R*R)
80  {
81  x_replan.setX(x_ref.getX());
82  x_replan.setY(x_ref.getY());
83  x_replan.setPSI(x_ref.getPSI());
84  x_replan.setvx(x_ref.getvx());
85  x_replan.setvy(x_ref.getvy());
86  x_replan.setOmega(x_ref.getOmega());
87  x_replan.setAx(x_ref.getAx());
88  x_replan.setDelta(x_ref.getDelta());
89  reset = false;
90  }
91  else
92  {
93  std::cout <<"trajectory cannot be resumed: reset readius exceeded"<<std::endl;
94  combined_maneuver_.setPoints.clear();
95  }
96  }
97  else
98  {
99  if(combined_maneuver_.setPoints.size()>0)
100  {
101  std::cout <<"trajectory cannot be resumed due to timeout. t= "<<t<<", spr: ["<<combined_maneuver_.setPoints.front().tStart<<";"<<combined_maneuver_.setPoints.back().tEnd<<"]"<<std::endl;
102  }
103  combined_maneuver_.setPoints.clear();
104  }
105  }
106 
107  }
108  }
109 
110  };
111 }
112 }
virtual TVehicleMotionStateReader * getVehicleMotionStateReader()=0
static adore::fun::AFactory * get()
Definition: afactory.h:170
Helps to select initial state for motion planning. Considers previous plan, current vehicle state,...
Definition: initial_state_selector.h:32
void update()
Definition: initial_state_selector.h:60
adore::mad::AReader< adore::fun::VehicleMotionState9d > * xreader_
Definition: initial_state_selector.h:38
adore::params::APTacticalPlanner * pTacticalPlanner_
Definition: initial_state_selector.h:41
~InitialStateSelector()
Definition: initial_state_selector.h:49
bool hasValidInitialState_
Definition: initial_state_selector.h:40
adore::fun::VehicleMotionState9d x_replan_
Definition: initial_state_selector.h:35
SetPointRequest spr_
Definition: initial_state_selector.h:34
InitialStateSelector()
Definition: initial_state_selector.h:43
void setExecutedTrajectory(const SetPointRequest &spr_ex)
Definition: initial_state_selector.h:54
adore::fun::VehicleMotionState9d x_
Definition: initial_state_selector.h:36
adore::fun::VehicleExtendedState xx_
Definition: initial_state_selector.h:37
adore::mad::AReader< adore::fun::VehicleExtendedState > * xxreader_
Definition: initial_state_selector.h:39
Definition: setpointrequest.h:35
PlanarVehicleState10d interpolateReference(double t, adore::params::APVehicle *p) const
Definition: setpointrequest.h:112
bool isActive(double t) const
Definition: setpointrequest.h:332
void copyTo(SetPointRequest &destination) const
Definition: setpointrequest.h:293
std::vector< SetPoint > setPoints
Definition: setpointrequest.h:38
Definition: vehicleextendedstate.h:26
bool getAutomaticControlOn() const
Definition: vehicleextendedstate.h:98
bool getAutomaticControlAccelerationActive() const
Definition: vehicleextendedstate.h:83
virtual void getData(T &value)=0
virtual bool hasData() const =0
abstract classs containing parameters to configure aspects and constraints of the tactical planner
Definition: ap_tactical_planner.h:25
virtual double getResetRadius() const =0
Definition: areaofeffectconverter.h:20
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