76 double dx =
x_.
getX()-x_ref.getX();
77 double dy =
x_.
getY()-x_ref.getY();
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());
93 std::cout <<
"trajectory cannot be resumed: reset readius exceeded"<<std::endl;
94 combined_maneuver_.setPoints.clear();
99 if(combined_maneuver_.setPoints.size()>0)
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;
103 combined_maneuver_.setPoints.clear();
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