ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
lanefollowingviewproxy.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  * Thomas Lobig - initial API and implementation
13  ********************************************************************************/
14 
15 #pragma once
16 #include <adore/view/alane.h>
18 
19 // TODO dependency on traffic and conflictset should be removed once it is removed in alane.h
22 
23 namespace adore
24 {
25  namespace env
26  {
27  namespace BorderBased
28  {
34  {
35  private:
36  std::shared_ptr<LaneGeometryDataProxy> lane_;
37  double limitS(double s)
38  {
39  // std::cout << "s_min " << lane_->s_min << " - s_max " << lane_->s_max << std::flush;
40  return adore::mad::bound(lane_->centerSmoothed_fct.limitLo(),
41  s+lane_->centerSmoothed_fct.limitLo(),
42  lane_->centerSmoothed_fct.limitHi());
43  }
44 
45  public:
51  LaneFollowingViewProxy(std::shared_ptr<LaneGeometryDataProxy> lane) : lane_(lane)
52  {
53  }
54 
55  bool isValid() const override
56  {
57  return lane_->isValid;
58  };
59  virtual double getSMin()const override
60  {
61  return 0.0;
62  }
63  virtual double getSMax()const override
64  {
65  return lane_->centerSmoothed_fct.limitHi()-lane_->centerSmoothed_fct.limitLo();
66  }
67 
68  double getProgressOfWidthOpen() const override
69  {
70  //@TODO
71  return lane_->centerSmoothed_fct.limitLo();
72  }
73  double getProgressOfWidthClosed() const override
74  {
75  //@TODO
76  return lane_->centerSmoothed_fct.limitHi();
77  }
79  {
80  throw std::logic_error("Not implemented");
81  // adore::env::traffic::EgoLaneTraffic t(nullptr);
82  // return t.getQueue();
83  };
84 
85  const ConflictSet& getConflictSet() const override
86  {
87  throw std::logic_error("Not implemented");
88  // adore::env::BorderBased::ConflictSet cs_(nullptr);
89  // return cs_;
90  }
91 
92  double getSpeedLimit(double s) override
93  {
94  throw std::logic_error("Not implemented");
95  return 20.0;
96  }
97 
98  virtual double getLeftIndicatorHint(double s) override { throw std::logic_error("Not implemented"); return 0.0; }
99 
100  virtual double getRightIndicatorHint(double s) override { throw std::logic_error("Not implemented"); return 0.0; }
101 
102  bool hasSpeedRecommendation(double s) const override
103  {
104  throw std::logic_error("Not implemented");
105  return false;
106  }
107 
108  double getSpeedRecommendation(double s) const override
109  {
110  throw std::logic_error("Not implemented");
111  return 0.0;
112  }
113  double getNavigationCost(double s) override
114  {
115  return lane_->navigationCost_fct(limitS(s));
116  }
117  virtual void boundNavigationCost(double s0,double s1,double& cmin,double& cmax) override
118  {
119  adoreMatrix<double,1,1> y0,y1;
120  lane_->navigationCost_fct.bound(limitS(s0),limitS(s1),y0,y1);
121  cmin = y0;cmax = y1;
122  }
123 
124  double getHeading(double s) override
125  {
126  // return lane_->centerHeading_fct(limitS(s));
127  auto n = lane_->centerNormal_fct(limitS(s));
128  // return std::atan2(n(1),n(0))-M_PI*0.5;
129  return (std::atan2)(-n(0),n(1));
130  }
131 
132  double getCurvature(double s, int derivative) override
133  {
134  if (derivative <= 0)
135  {
136  return lane_->centerSmoothedCurvature_fct(limitS(s));
137  }
138  else if (derivative <= 1)
139  {
140  return lane_->centerSmoothedCurvatureDerivative_fct(limitS(s));
141  }
142  else
143  {
144  return 0.0;
145  }
146  }
147 
148  double getOffsetOfLeftBorder(double s) override
149  {
150  auto x = limitS(s);
151  // std::cout << " bound value: " << x << std::flush;
152  return lane_->leftDistance_fct(x);
153  }
154 
155  double getOffsetOfRightBorder(double s) override
156  {
157  return lane_->rightDistance_fct(limitS(s));
158  }
159 
160  void toRelativeCoordinates(double xe, double ye, double& s, double& n) override
161  {
162  if(!adore::mad::toRelativeWithNormalExtrapolation(xe,ye,&lane_->centerSmoothed_fct,&lane_->centerNormal_fct,s,n))
163  {
164  s = lane_->centerSmoothed_fct.getClosestParameter(xe, ye, 1, 2, n);
165  }
166  s -= lane_->centerSmoothed_fct.limitLo();
167  }
168 
169  void toEucledianCoordinates(double s, double n, double& xe, double& ye, double& ze) override
170  {
171  s = limitS(s);
172  adore::mad::fromRelative(s,n,&lane_->centerSmoothed_fct,&lane_->centerNormal_fct,xe,ye,ze);
173  ze=0.0;
174  }
175  };
176  } // namespace BorderBased
177  } // namespace env
178 } // namespace adore
Definition: conflictset.h:202
Proxy class to access ALane interfaces from preprocessed lane geometry received as data object.
Definition: lanefollowingviewproxy.h:34
double getOffsetOfLeftBorder(double s) override
Definition: lanefollowingviewproxy.h:148
const ConflictSet & getConflictSet() const override
Definition: lanefollowingviewproxy.h:85
virtual void boundNavigationCost(double s0, double s1, double &cmin, double &cmax) override
Definition: lanefollowingviewproxy.h:117
double getProgressOfWidthOpen() const override
Definition: lanefollowingviewproxy.h:68
bool hasSpeedRecommendation(double s) const override
Definition: lanefollowingviewproxy.h:102
virtual double getRightIndicatorHint(double s) override
Definition: lanefollowingviewproxy.h:100
std::shared_ptr< LaneGeometryDataProxy > lane_
Definition: lanefollowingviewproxy.h:36
bool isValid() const override
Definition: lanefollowingviewproxy.h:55
virtual double getLeftIndicatorHint(double s) override
Definition: lanefollowingviewproxy.h:98
LaneFollowingViewProxy(std::shared_ptr< LaneGeometryDataProxy > lane)
Construct a new Lane Following View Proxy object.
Definition: lanefollowingviewproxy.h:51
void toRelativeCoordinates(double xe, double ye, double &s, double &n) override
Definition: lanefollowingviewproxy.h:160
double getNavigationCost(double s) override
Definition: lanefollowingviewproxy.h:113
double getHeading(double s) override
Definition: lanefollowingviewproxy.h:124
virtual double getSMax() const override
Definition: lanefollowingviewproxy.h:63
double getSpeedLimit(double s) override
Definition: lanefollowingviewproxy.h:92
double getCurvature(double s, int derivative) override
Definition: lanefollowingviewproxy.h:132
double getProgressOfWidthClosed() const override
Definition: lanefollowingviewproxy.h:73
double getOffsetOfRightBorder(double s) override
Definition: lanefollowingviewproxy.h:155
double getSpeedRecommendation(double s) const override
Definition: lanefollowingviewproxy.h:108
virtual double getSMin() const override
Definition: lanefollowingviewproxy.h:59
const adore::view::TrafficQueue & getOnLaneTraffic() const override
Definition: lanefollowingviewproxy.h:78
void toEucledianCoordinates(double s, double n, double &xe, double &ye, double &ze) override
Definition: lanefollowingviewproxy.h:169
double limitS(double s)
Definition: lanefollowingviewproxy.h:37
Definition: alane.h:28
interval< T > atan2(interval< T > y, interval< T > x)
Definition: intervalarithmetic.h:234
T bound(T lb, T value, T ub)
Definition: adoremath.h:569
bool toRelativeWithNormalExtrapolation(double qX, double qY, const T1 pi, const T1 pj, const T2 ni, const T2 nj, double &s, double &t)
Transformation from Euclidean coordinate system to a relative coordinate system represented by linear...
Definition: adoremath.h:321
bool fromRelative(double s, double t, const T1 pi, const T1 pj, const T2 ni, const T2 nj, double &X, double &Y, double &Z)
Transform from relative coordinates to Euclidean coordinates.
Definition: adoremath.h:475
std::vector< TrafficObject > TrafficQueue
Definition: trafficobject.h:183
x
Definition: adore_set_goal.py:30
y0
Definition: adore_set_goal.py:26
y1
Definition: adore_set_pose.py:29
Definition: areaofeffectconverter.h:20