ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
map_indicatorhint_management.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
13  ********************************************************************************/
14 
15 #pragma once
16 
17 #include "indicator_hint.h"
18 #include "adore/view/alane.h"
19 #include "adore/env/afactory.h"
20 #include "adore/params/afactory.h"
21 #include "adore/mad/com_patterns.h"
24 
25 namespace adore
26 {
27  namespace env
28  {
34  {
35  private:
36 
38  std::unordered_map<adore::env::TIndicatorHintID, adore::env::IndicatorHint> known_hints;
39 
40  //TODO: this function needs to be part of the alane class and should not be here
50  inline bool const isOnLane(
51  adore::mad::function_type_xyz const & centerSmoothed_fct,
52  adore::mad::function_type_scalar const & leftOffset_fct,
53  adore::mad::function_type_scalar const & rightOffset_fct,
54  double const &x, double const &y, double &s)
55  {
56  double offset = 0.0;
57  s = centerSmoothed_fct.getClosestParameter(x, y, 1, 2, offset);
58 
59  if ( s < rightOffset_fct.limitLo()
60  || s > rightOffset_fct.limitHi()
61  || s < leftOffset_fct.limitLo()
62  || s > leftOffset_fct.limitHi() )
63  {
64  // if s is outside the limits of the distance functions, assume it's not on lane
65  return false;
66  }
67  else
68  {
69  return rightOffset_fct(s) < offset && leftOffset_fct(s) > offset;
70  }
71  }
72 
73  public:
75  {
77  }
78  // not needed here void setDefaultValue(double value){default_speed_limit_ = value;}
79  void update(double const & egoX, double const & egoY){
80  // read updates from the speed limit feed
81  std::vector<adore::env::IndicatorHint> hints;
82  while (indicatorhint_feed_->hasNext())
83  {
84  adore::env::IndicatorHint limit_info;
85  indicatorhint_feed_->getNext(limit_info);
86  hints.push_back(limit_info);
87  }
88 
89  // if there are any updates, insert them to the known speed limits
90  for ( auto limit : hints)
91  {
92  known_hints.insert_or_assign(limit.id,limit);
93  }
94 
95  // see if any speed limit infos are completely outside the radius of interest
96  // TODO utility function available?
97  std::set<adore::env::TIndicatorHintID> hint_ids_outside_radius;
99  double carX = egoX;
100  double carY = egoY;
101  for (auto [id,item] : known_hints)
102  {
103 
104  double abs1 = std::abs(carX - item.startX);
105  double abs2 = std::abs(carY - item.startY);
106  double abs3 = std::abs(carX - item.stopX);
107  double abs4 = std::abs(carY - item.stopY);
108 
109  if (((abs1*abs1 + abs2*abs2) > visibilty_radius*visibilty_radius) and ((abs3*abs3 + abs4*abs4) > visibilty_radius*visibilty_radius))
110  {
111  hint_ids_outside_radius.insert(id);
112  }
113  }
114 
115  if (hint_ids_outside_radius.size() > 0)
116  {
117  for ( auto it = known_hints.begin(); it != known_hints.end();)
118  {
119  if (hint_ids_outside_radius.find(it->first) != hint_ids_outside_radius.end())
120  {
121  it = known_hints.erase(it);
122  }
123  else
124  {
125  it++;
126  }
127 
128  }
129  }
130  }
131 
132  void const getFunctions(
133  adore::mad::function_type_xyz & centerSmoothed_fct,
134  adore::mad::function_type_scalar & leftOffset_fct,
135  adore::mad::function_type_scalar & rightOffset_fct,
136  adore::mad::function_type_scalar & left_indicator_hint_fct,
137  adore::mad::function_type_scalar & right_indicator_hint_fct
138  )
139 
140  {
141  adore::mad::LinearConstraintSet constraint_set_left;
142  adore::mad::LinearConstraintSet constraint_set_right;
143  constraint_set_left.insert(centerSmoothed_fct.limitLo(),0.0,
144  centerSmoothed_fct.limitHi(),0.0);
145  constraint_set_right.insert(centerSmoothed_fct.limitLo(),0.0,
146  centerSmoothed_fct.limitHi(),0.0);
147 
148 
149  for (auto const & [id,item] : known_hints)
150  {
151  double startS;
152  double stopS;
153  if (isOnLane(centerSmoothed_fct,leftOffset_fct,rightOffset_fct,item.startX,item.startY,startS)
154  && isOnLane(centerSmoothed_fct,leftOffset_fct,rightOffset_fct,item.stopX,item.stopY,stopS))
155  {
156  if (startS != stopS && (item.value == adore::env::IndicatorSide::left || item.value == adore::env::IndicatorSide::both))
157  {
158  constraint_set_left.insert(startS,1.0,stopS,1.0);
159  }
160  if (startS != stopS && (item.value == adore::env::IndicatorSide::right || item.value == adore::env::IndicatorSide::both))
161  {
162  constraint_set_right.insert(startS,1.0,stopS,1.0);
163  }
164  }
165  }
166 
167  // TODO currently this does not allow to e.g. drive at 70 kmph if base speed is 50 kmph
168  constraint_set_left.bound(&(left_indicator_hint_fct),adore::mad::LinearConstraintSet::Direction::UPPER);
169  constraint_set_right.bound(&(right_indicator_hint_fct),adore::mad::LinearConstraintSet::Direction::UPPER);
170  }
171  };
172  } // namespace env
173 } // namespace adore
virtual TIndicatorHintFeed * getIndicatorHintFeed()=0
static adore::env::AFactory * get()
Definition: afactory.h:236
automatically manage indicator hints based on current vehicle position
Definition: map_indicatorhint_management.h:34
bool const isOnLane(adore::mad::function_type_xyz const &centerSmoothed_fct, adore::mad::function_type_scalar const &leftOffset_fct, adore::mad::function_type_scalar const &rightOffset_fct, double const &x, double const &y, double &s)
helper function to determine if a eucledian (x,y) point is on the lane or outside,...
Definition: map_indicatorhint_management.h:50
adore::env::AFactory::TIndicatorHintFeed * indicatorhint_feed_
Definition: map_indicatorhint_management.h:37
IndicatorHintManagement()
Definition: map_indicatorhint_management.h:74
void update(double const &egoX, double const &egoY)
Definition: map_indicatorhint_management.h:79
void const getFunctions(adore::mad::function_type_xyz &centerSmoothed_fct, adore::mad::function_type_scalar &leftOffset_fct, adore::mad::function_type_scalar &rightOffset_fct, adore::mad::function_type_scalar &left_indicator_hint_fct, adore::mad::function_type_scalar &right_indicator_hint_fct)
Definition: map_indicatorhint_management.h:132
std::unordered_map< adore::env::TIndicatorHintID, adore::env::IndicatorHint > known_hints
Definition: map_indicatorhint_management.h:38
Definition: com_patterns.h:29
virtual void getNext(T &value)=0
virtual bool hasNext() const =0
double getClosestParameter(T px, T py, int d1, int d2, T &n_min) const
Definition: llinearpiecewisefunction.h:1014
virtual DT limitLo() const override
Definition: llinearpiecewisefunction.h:264
virtual DT limitHi() const override
Definition: llinearpiecewisefunction.h:259
Definition: linearconstraintset.h:30
void bound(function_type_scalar *fun, Direction direction)
Definition: linearconstraintset.h:85
void insert(double x0, double y0, double x1, double y1)
Definition: linearconstraintset.h:69
virtual APMapProvider * getMapProvider() const =0
virtual double getVisibiltyRadius() const =0
static adore::params::AFactory * get()
Definition: afactory.h:103
@ right
Definition: indicator_hint.h:36
@ both
Definition: indicator_hint.h:37
@ left
Definition: indicator_hint.h:35
x
Definition: adore_set_goal.py:30
y
Definition: adore_set_goal.py:31
Definition: areaofeffectconverter.h:20
Definition: indicator_hint.h:41