ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
navigationgoalobserver.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 and API
13  ********************************************************************************/
14 
15 
16 #pragma once
17 
18 #include <adore/env/afactory.h>
20 #include <adore/params/afactory.h>
21 
22 namespace adore
23 {
24  namespace env
25  {
31  {
32  private:
39  bool inview_;
43  double s_;
44  double n_;
46 
47 
48  public:
50  :lfv_(lfv),lcl_(lcl),lcr_(lcr)
51  {
53  initialized_ = false;
55  }
56  void update()
57  {
58  inview_ = false;
59  oncurrentlane_ = false;
60  onlaneleft_ = false;
61  onlaneright_ = false;
62  const double lateral_tolerance = p_nav_->getNagivationGoalStopToleranceLateral();
63  if(goalReader_->hasData())
64  {
66  initialized_ = true;
67  }
68  if(initialized_ && lfv_!=0 && lfv_->isValid())
69  {
71  if( lfv_->getSMin()<s_ && s_<lfv_->getSMax()-1.0
72  && -lateral_tolerance + lfv_->getOffsetOfRightBorder(s_) < n_
73  && n_ < lateral_tolerance + lfv_->getOffsetOfLeftBorder(s_))
74  {
75  inview_ =true;
76  if( lfv_->getOffsetOfRightBorder(s_)<= n_ && n_ <= lfv_->getOffsetOfLeftBorder(s_) )
77  {
78  oncurrentlane_ = true;
79  }
80  else
81  {
82  if(lcl_!=0 && lcl_->getTargetLane()->isValid() && lcl_->getTargetLane()->inSRange(s_))
83  {
84  if( lcl_->getOffsetOfSeparatingBorder(s_) <= n_ && n_ <=lcl_->getOffsetOfDestinationOuterBorder(s_) )
85  {
86  onlaneleft_=true;
87  }
88  }
89  if(lcr_!=0 && lcr_->getTargetLane()->isValid() && lcr_->getTargetLane()->inSRange(s_))
90  {
91  if( lcr_->getOffsetOfDestinationOuterBorder(s_) <= n_ && n_ <= lcr_->getOffsetOfSeparatingBorder(s_) )
92  {
93  onlaneright_=true;
94  }
95  }
96  }
97 
98  }
99  }
100 
101  }
107  virtual const bool isNextGoalPointFinal()const{return true;/*@TODO: Make data available*/}
111  virtual const bool isNextGoalPointInView()const{return inview_;}
115  virtual const bool isNextGoalPointOnCurrentLane()const{return oncurrentlane_;}
119  virtual const bool isNextGoalPointOnLaneToTheLeft()const{return onlaneleft_;}
123  virtual const bool isNextGoalPointOnLaneToTheRight()const{return onlaneright_;}
127  virtual const double getProgress()const{return s_;}
128  };
129  }
130 }
abstract factory for adore::env communication
Definition: afactory.h:41
virtual TNavigationGoalReader * getNavigationGoalReader()=0
Definition: navigationgoalobserver.h:31
adore::fun::NavigationGoal goal_
Definition: navigationgoalobserver.h:38
adore::view::ALaneChangeView * lcr_
Definition: navigationgoalobserver.h:36
adore::params::APNavigation * p_nav_
Definition: navigationgoalobserver.h:45
double n_
Definition: navigationgoalobserver.h:44
virtual const bool isNextGoalPointOnCurrentLane() const
Definition: navigationgoalobserver.h:115
bool oncurrentlane_
Definition: navigationgoalobserver.h:40
void update()
Definition: navigationgoalobserver.h:56
NavigationGoalObserver(AFactory *afactory, adore::view::ALane *lfv, adore::view::ALaneChangeView *lcl, adore::view::ALaneChangeView *lcr)
Definition: navigationgoalobserver.h:49
bool inview_
Definition: navigationgoalobserver.h:39
adore::view::ALaneChangeView * lcl_
Definition: navigationgoalobserver.h:35
AFactory::TNavigationGoalReader * goalReader_
Definition: navigationgoalobserver.h:33
virtual const bool isNextGoalPointOnLaneToTheLeft() const
Definition: navigationgoalobserver.h:119
virtual const bool isNextGoalPointInView() const
Definition: navigationgoalobserver.h:111
double s_
Definition: navigationgoalobserver.h:43
bool initialized_
Definition: navigationgoalobserver.h:37
adore::view::ALane * lfv_
Definition: navigationgoalobserver.h:34
bool onlaneright_
Definition: navigationgoalobserver.h:42
bool onlaneleft_
Definition: navigationgoalobserver.h:41
virtual const bool isNextGoalPointFinal() const
Definition: navigationgoalobserver.h:107
virtual const bool isNextGoalPointOnLaneToTheRight() const
Definition: navigationgoalobserver.h:123
virtual const double getProgress() const
Definition: navigationgoalobserver.h:127
virtual void getData(T &value)=0
virtual bool hasData() const =0
virtual APNavigation * getNavigation() const =0
abstract class containing parameters which configure navigation behaviour
Definition: ap_navigation.h:25
virtual double getNagivationGoalStopToleranceLateral() const =0
static adore::params::AFactory * get()
Definition: afactory.h:103
Definition: alanechangeview.h:27
virtual double getOffsetOfDestinationOuterBorder(double s)=0
virtual ALane * getTargetLane()=0
virtual double getOffsetOfSeparatingBorder(double s)=0
Definition: alane.h:28
virtual void toRelativeCoordinates(double xe, double ye, double &s, double &n)=0
virtual double getSMin() const =0
virtual double getOffsetOfRightBorder(double s)=0
virtual bool isValid() const =0
bool inSRange(double s) const
Definition: alane.h:45
Definition: anavigationgoalview.h:25
Definition: areaofeffectconverter.h:20
Definition: navigationgoal.h:26
adore::mad::GlobalPosition target_
Definition: navigationgoal.h:28
double x_
Definition: globalposition.h:23
double y_
Definition: globalposition.h:23