38 std::unordered_map<adore::env::TIndicatorHintID, adore::env::IndicatorHint>
known_hints;
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;
79 void update(
double const & egoX,
double const & egoY){
81 std::vector<adore::env::IndicatorHint> hints;
86 hints.push_back(limit_info);
90 for (
auto limit : hints)
97 std::set<adore::env::TIndicatorHintID> hint_ids_outside_radius;
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);
109 if (((abs1*abs1 + abs2*abs2) > visibilty_radius*visibilty_radius) and ((abs3*abs3 + abs4*abs4) > visibilty_radius*visibilty_radius))
111 hint_ids_outside_radius.insert(
id);
115 if (hint_ids_outside_radius.size() > 0)
119 if (hint_ids_outside_radius.find(it->first) != hint_ids_outside_radius.end())
144 centerSmoothed_fct.
limitHi(),0.0);
145 constraint_set_right.
insert(centerSmoothed_fct.
limitLo(),0.0,
146 centerSmoothed_fct.
limitHi(),0.0);
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))
158 constraint_set_left.
insert(startS,1.0,stopS,1.0);
162 constraint_set_right.
insert(startS,1.0,stopS,1.0);
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);
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 ¢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_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 ¢erSmoothed_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