54 double const &
x,
double const &
y,
double &s)
59 if ( s < rightOffset_fct.
limitLo()
60 || s > rightOffset_fct.
limitHi()
62 || s > leftOffset_fct.
limitHi() )
69 return rightOffset_fct(s) < offset && leftOffset_fct(s) > offset;
80 void update(
double const & egoX,
double const & egoY){
87 bundle.push_back(limit_info);
91 for (
auto limit : bundle)
98 std::set<adore::env::TSpeedLimitID> speed_limit_ids_outside_radius;
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);
110 if (((abs1*abs1 + abs2*abs2) > visibilty_radius*visibilty_radius) and ((abs3*abs3 + abs4*abs4) > visibilty_radius*visibilty_radius))
112 speed_limit_ids_outside_radius.insert(
id);
116 if (speed_limit_ids_outside_radius.size() > 0)
120 if (speed_limit_ids_outside_radius.find(it->first) != speed_limit_ids_outside_radius.end())
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))
155 constraint_set.
insert(startS,item.value,stopS,item.value);
161 constraint_set.
bound(&(speedLimitOutput_fct),adore::mad::LinearConstraintSet::Direction::LOWER);
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 ¢erSmoothed_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 ¢erSmoothed_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