ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
map_speedlimit_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 "speedlimit.h"
18 #include "adore/view/alane.h"
19 #include "adore/env/afactory.h"
20 #include "adore/mad/com_patterns.h"
23 
24 namespace adore
25 {
26  namespace env
27  {
33  {
34  private:
35 
37  std::unordered_map<adore::env::TSpeedLimitID, adore::env::SpeedLimit> known_speed_limits;
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  {
78  }
79  void setDefaultValue(double value){default_speed_limit_ = value;}
80  void update(double const & egoX, double const & egoY){
81  // read updates from the speed limit feed
83  while (speedlimit_feed_->hasNext())
84  {
85  adore::env::SpeedLimit limit_info;
86  speedlimit_feed_->getNext(limit_info);
87  bundle.push_back(limit_info);
88  }
89 
90  // if there are any updates, insert them to the known speed limits
91  for ( auto limit : bundle)
92  {
93  known_speed_limits.insert_or_assign(limit.id,limit);
94  }
95 
96  // see if any speed limit infos are completely outside the radius of interest
97  // TODO utility function available?
98  std::set<adore::env::TSpeedLimitID> speed_limit_ids_outside_radius;
100  double carX = egoX;
101  double carY = egoY;
102  for (auto [id,item] : known_speed_limits)
103  {
104 
105  double abs1 = std::abs(carX - item.startX);
106  double abs2 = std::abs(carY - item.startY);
107  double abs3 = std::abs(carX - item.stopX);
108  double abs4 = std::abs(carY - item.stopY);
109 
110  if (((abs1*abs1 + abs2*abs2) > visibilty_radius*visibilty_radius) and ((abs3*abs3 + abs4*abs4) > visibilty_radius*visibilty_radius))
111  {
112  speed_limit_ids_outside_radius.insert(id);
113  }
114  }
115 
116  if (speed_limit_ids_outside_radius.size() > 0)
117  {
118  for ( auto it = known_speed_limits.begin(); it != known_speed_limits.end();)
119  {
120  if (speed_limit_ids_outside_radius.find(it->first) != speed_limit_ids_outside_radius.end())
121  {
122  it = known_speed_limits.erase(it);
123  }
124  else
125  {
126  it++;
127  }
128 
129  }
130  }
131  }
132 
133  void const getFunction(
134  adore::mad::function_type_xyz & centerSmoothed_fct,
135  adore::mad::function_type_scalar & leftOffset_fct,
136  adore::mad::function_type_scalar & rightOffset_fct,
137  adore::mad::function_type_scalar & speedLimitOutput_fct
138  )
139 
140  {
141  adore::mad::LinearConstraintSet constraint_set;
142  constraint_set.insert(centerSmoothed_fct.limitLo(),default_speed_limit_,
143  centerSmoothed_fct.limitHi(),default_speed_limit_);
144 
145 
146  for (auto const & [id,item] : known_speed_limits)
147  {
148  double startS;
149  double stopS;
150  if (isOnLane(centerSmoothed_fct,leftOffset_fct,rightOffset_fct,item.startX,item.startY,startS)
151  && isOnLane(centerSmoothed_fct,leftOffset_fct,rightOffset_fct,item.stopX,item.stopY,stopS))
152  {
153  if (startS != stopS)
154  {
155  constraint_set.insert(startS,item.value,stopS,item.value);
156  }
157  }
158  }
159 
160  // TODO currently this does not allow to e.g. drive at 70 kmph if base speed is 50 kmph
161  constraint_set.bound(&(speedLimitOutput_fct),adore::mad::LinearConstraintSet::Direction::LOWER);
162  }
163  };
164  } // namespace env
165 } // namespace adore
virtual TSpeedLimitFeed * getSpeedLimitFeed()=0
static adore::env::AFactory * get()
Definition: afactory.h:236
automatically manage speed limit information based on current vehicle position
Definition: map_speedlimit_management.h:33
void update(double const &egoX, double const &egoY)
Definition: map_speedlimit_management.h:80
std::unordered_map< adore::env::TSpeedLimitID, adore::env::SpeedLimit > known_speed_limits
Definition: map_speedlimit_management.h:37
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_speedlimit_management.h:50
void setDefaultValue(double value)
Definition: map_speedlimit_management.h:79
SpeedLimitManagement()
Definition: map_speedlimit_management.h:74
void const getFunction(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 &speedLimitOutput_fct)
Definition: map_speedlimit_management.h:133
adore::env::AFactory::TSpeedLimitFeed * speedlimit_feed_
Definition: map_speedlimit_management.h:36
double default_speed_limit_
Definition: map_speedlimit_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
std::vector< SpeedLimit > TSpeedLimitBundle
Definition: speedlimit.h:49
x
Definition: adore_set_goal.py:30
y
Definition: adore_set_goal.py:31
Definition: areaofeffectconverter.h:20
Definition: speedlimit.h:31