ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
setpointrequest_dispatcher.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2021 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 #pragma once
15 #include <adore/fun/afactory.h>
16 #include <adore/params/afactory.h>
19 
20 namespace adore
21 {
22 namespace fun
23 {
29  {
30  private:
46  bool reset_;
48  std::string status_msg_;
49  private:
51  {
53  {
54  reset_ = true;//while in manual control mode, always reset initial state to current vehicle state
55  status_msg_ = "reset: manual control";
56  return;
57  }
58 
60  {
62  double dx = x_odometry_.getX() - x_ref_.getX();
63  double dy = x_odometry_.getY() - x_ref_.getY();
64  double R = ptac_->getResetRadius();
65  if (dx * dx + dy * dy < R*R)
66  {
67  reset_ = false;
68  status_msg_ = "";
69  }
70  else
71  {
72  reset_ = true;
73  std::stringstream ss;
74  ss<<"reset: radius R="<<R<<" exceeded with dx="<<dx<<" and dy="<<dy;
75  status_msg_ = ss.str();
76  }
77  }
78  else
79  {
80  reset_ = true;
81  if(spr_localization_.setPoints.size()==0)
82  {
83  status_msg_ = "reset: spr empty";
84  }
85  else
86  {
87  std::stringstream ss;
88  ss<<"reset: spr["<< spr_localization_.setPoints.begin()->tStart
89  <<","<< spr_localization_.setPoints.rbegin()->tEnd
90  <<"]inactive at t="<<x_localization_.getTime();
91  }
92  }
93  }
94 
96  {
97  if(reset_)
98  {
101  if(x0_odometry_.getvx()<0.1)
102  {
103  x0_odometry_.setAx(0.0);
104  x0_localization_.setAx(0.0);
105  }
106  }
107  else
108  {
109  double t0 = x_localization_.getTime();
110  auto xref_odom = spr_odom_.interpolateReference(t0, pveh_);
111  x0_odometry_.setTime(t0);
112  x0_odometry_.setX(xref_odom.getX());
113  x0_odometry_.setY(xref_odom.getY());
114  x0_odometry_.setPSI(xref_odom.getPSI());
115  x0_odometry_.setvx(xref_odom.getvx());
116  x0_odometry_.setvy(xref_odom.getvy());
117  x0_odometry_.setOmega(xref_odom.getOmega());
118  x0_odometry_.setAx(xref_odom.getAx());
119  x0_odometry_.setDelta(xref_odom.getDelta());
121 
122  const double c0 = (std::cos)(x_odometry_.getPSI());
123  const double s0 = (std::sin)(x_odometry_.getPSI());
125  const double et = c0 * (x0_odometry_.getX()-x_odometry_.getX()) + s0 * (x0_odometry_.getY()-x_odometry_.getY());
126  const double en =-s0 * (x0_odometry_.getX()-x_odometry_.getX()) + c0 * (x0_odometry_.getY()-x_odometry_.getY());
127  const double epsit = c0 * (std::cos)(x0_odometry_.getPSI()) + s0 * (std::sin)(x0_odometry_.getPSI());
128  const double epsin =-s0 * (std::cos)(x0_odometry_.getPSI()) + c0 * (std::sin)(x0_odometry_.getPSI());
129  const double c1 = (std::cos)(x_localization_.getPSI());
130  const double s1 = (std::sin)(x_localization_.getPSI());
131  x0_localization_.setX(x_localization_.getX()+c1*et-s1*en);
132  x0_localization_.setY(x_localization_.getY()+s1*et+c1*en);
133  x0_localization_.setPSI(std::atan2(s1*epsit+c1*epsin,c1*epsit-s1*epsin));
134  }
135  }
139  void update()
140  {
141  status_msg_ = "";
142  has_vehicle_state_ = false;
143  //the localization/odometry signal are interpolated to the earlier time of both
145  if(localization_buffer_.size()==0)
146  {
147  status_msg_ = "no localization state";
148  return;
149  }
151  if(odometry_buffer_.size()==0)
152  {
153  status_msg_ = "no odometry state";
154  return;
155  }
159  if(!has_vehicle_state_)
160  {
161  status_msg_ = "can not interpolate vehicle states";
162  }
163  }
164  public:
166  : odometry_buffer_(FunFactoryInstance::get()->getVehicleOdometryMotionStateFeed()),
167  localization_buffer_(FunFactoryInstance::get()->getVehicleLocalizationMotionStateFeed())
168  {
170  setpointrequest_localization_writer_ = FunFactoryInstance::get()->getSetPointRequestWriter();
171  setpointrequest_odometry_writer_ = FunFactoryInstance::get()->getOdomSetPointRequestWriter();
172  nominal_trajectory_writer_ = FunFactoryInstance::get()->getNominalTrajectoryWriter();
175  has_vehicle_state_ = false;
176  }
178  {
183  delete ptac_;
184  delete pveh_;
185  }
192  {
193  update();
194  if(!has_vehicle_state_)return false;
197  result = x0_localization_;
198  return true;
199  }
200 
208  void dispatch(SetPointRequest& combined_trajectory, SetPointRequest& nominal_trajectory)
209  {
210  spr_localization_ = combined_trajectory;
211  spr_odom_ = combined_trajectory;
215  nominal_trajectory_writer_->write(nominal_trajectory);
216  }
217 
218  std::string getStatus()
219  {
220  return status_msg_;
221  }
222  };
223 
224 }
225 }
virtual TVehicleMotionStateReader * getVehicleMotionStateReader()=0
Definition: activationstateobserver.h:22
bool isAutomaticControlEnabled()
Definition: activationstateobserver.h:34
Utility class to simplify factory access.
Definition: afactory.h:154
static adore::fun::AFactory * get()
Definition: afactory.h:170
class helps to dispatch SetPointRequest to controller SetPointRequestDispatcher handles selection of ...
Definition: setpointrequest_dispatcher.h:29
std::string getStatus()
Definition: setpointrequest_dispatcher.h:218
VehicleMotionState9d x0_odometry_
Definition: setpointrequest_dispatcher.h:44
void computeResetCondition()
Definition: setpointrequest_dispatcher.h:50
AFactory::TSetPointRequestWriter * nominal_trajectory_writer_
Definition: setpointrequest_dispatcher.h:38
VehicleMotionState9d x_localization_
Definition: setpointrequest_dispatcher.h:41
SetPointRequest spr_localization_
Definition: setpointrequest_dispatcher.h:40
adore::params::APTacticalPlanner * ptac_
Definition: setpointrequest_dispatcher.h:31
VehicleMotionStateBuffer odometry_buffer_
Definition: setpointrequest_dispatcher.h:34
SetPointRequestDispatcher()
Definition: setpointrequest_dispatcher.h:165
VehicleMotionState9d x0_localization_
Definition: setpointrequest_dispatcher.h:43
ActivationStateObserver activation_observer_
Definition: setpointrequest_dispatcher.h:45
void update()
Definition: setpointrequest_dispatcher.h:139
AFactory::TSetPointRequestWriter * setpointrequest_localization_writer_
Definition: setpointrequest_dispatcher.h:36
std::string status_msg_
Definition: setpointrequest_dispatcher.h:48
SetPointRequest spr_odom_
Definition: setpointrequest_dispatcher.h:39
bool getInitialState(VehicleMotionState9d &result)
compute and return initial state for next planning iteration The according initial state in odometry ...
Definition: setpointrequest_dispatcher.h:191
AFactory::TMotionStateReader * localization_state_reader_
Definition: setpointrequest_dispatcher.h:33
void computePlanStartPoint()
Definition: setpointrequest_dispatcher.h:95
VehicleMotionStateBuffer localization_buffer_
Definition: setpointrequest_dispatcher.h:35
bool reset_
Definition: setpointrequest_dispatcher.h:46
bool has_vehicle_state_
Definition: setpointrequest_dispatcher.h:47
void dispatch(SetPointRequest &combined_trajectory, SetPointRequest &nominal_trajectory)
dispatch SetPointRequests computed in localization coordinates The combined trajectory will be conver...
Definition: setpointrequest_dispatcher.h:208
~SetPointRequestDispatcher()
Definition: setpointrequest_dispatcher.h:177
adore::params::APVehicle * pveh_
Definition: setpointrequest_dispatcher.h:32
VehicleMotionState9d x_odometry_
Definition: setpointrequest_dispatcher.h:42
AFactory::TSetPointRequestWriter * setpointrequest_odometry_writer_
Definition: setpointrequest_dispatcher.h:37
Definition: setpointrequest.h:35
PlanarVehicleState10d interpolateReference(double t, adore::params::APVehicle *p) const
Definition: setpointrequest.h:112
void relocateTo(double newX0, double newY0, double new_PSI0)
Definition: setpointrequest.h:187
bool isActive(double t) const
Definition: setpointrequest.h:332
std::vector< SetPoint > setPoints
Definition: setpointrequest.h:38
Buffers and interpolates vehicle positions.
Definition: vehiclemotionstatebuffer.h:28
double getTmax()
Definition: vehiclemotionstatebuffer.h:62
void update()
call update to check for new data
Definition: vehiclemotionstatebuffer.h:44
bool interpolate_or_latest(double t, VehicleMotionState9d &result)
get the latest, if
Definition: vehiclemotionstatebuffer.h:71
double size()
Definition: vehiclemotionstatebuffer.h:58
Definition: com_patterns.h:68
Definition: com_patterns.h:97
virtual void write(const T &value)=0
virtual APTacticalPlanner * getTacticalPlanner() const =0
virtual APVehicle * getVehicle() 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
abstract class for vehicle configuration related paremeters
Definition: ap_vehicle.h:29
static adore::params::AFactory * get()
Definition: afactory.h:103
interval< T > atan2(interval< T > y, interval< T > x)
Definition: intervalarithmetic.h:234
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
Definition: areaofeffectconverter.h:20
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
void setY(double value)
Set the y-coordinate.
Definition: vehiclemotionstate9d.h:121
double getX() const
Get the x-coordinate.
Definition: vehiclemotionstate9d.h:54
double getvx() const
Get the longitudinal velocity.
Definition: vehiclemotionstate9d.h:78
void setAx(double value)
Set the longitudinal acceleration.
Definition: vehiclemotionstate9d.h:157
void setvx(double value)
set the longitudinal velocity
Definition: vehiclemotionstate9d.h:139
double getY() const
Get the y-coordinate.
Definition: vehiclemotionstate9d.h:60
double getPSI() const
Get the heading.
Definition: vehiclemotionstate9d.h:72
void setOmega(double value)
Set the yaw rate.
Definition: vehiclemotionstate9d.h:151
void setX(double value)
Set the x-coordinate.
Definition: vehiclemotionstate9d.h:115
void setPSI(double value)
set the heading
Definition: vehiclemotionstate9d.h:133
void setvy(double value)
set the lateral velocity
Definition: vehiclemotionstate9d.h:145
void setDelta(double value)
Set the steering angle.
Definition: vehiclemotionstate9d.h:163
void setTime(double value)
Set the time.
Definition: vehiclemotionstate9d.h:109
double getTime() const
Get the time.
Definition: vehiclemotionstate9d.h:48