ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
controlledconnection.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  * Daniel Heß - initial implementation and API
13  ********************************************************************************/
14 #pragma once
15 
16 #include <adore/mad/com_patterns.h>
17 #include <adore/mad/adoremath.h>
18 #include <boost/geometry.hpp>
19 #include <boost/geometry/geometries/point.hpp>
20 #include <boost/geometry/geometries/box.hpp>
23 #include <adore/view/limitline.h>
25 
26 namespace adore
27 {
28  namespace env
29  {
33  namespace ConnectionState
34  {
36  {
38  DARK = 1,
47  };
48  }
49 
55  {
56  private:
57  double minEndTime_;
58  double maxEndTime_;
59  double likelyTime_;
63  public:
64  ConnectionStateEvent(ConnectionState::EConnectionState state,double minEndTime,bool maxEndTime_present,bool likelyTime_present,double maxEndTime,double likelyTime)
65  :minEndTime_(minEndTime),maxEndTime_(maxEndTime),likelyTime_(likelyTime),maxEndTime_present_(maxEndTime_present),likelyTime_present_(likelyTime_present),state_(state){}
66  double getMinEndTime()const {return minEndTime_;}
67  double getMaxEndTime()const {return maxEndTime_;}
69  double getLikelyTime()const {return likelyTime_;}
70  bool hasMaxEndTime()const {return maxEndTime_present_;}
71  bool hasLikelyTime()const {return likelyTime_present_;}
73  };
74 
75 
81  {
82  public:
84  private:
86  public:
87  std::vector<ConnectionStateEvent> data_;
89  :id_(0.0,0.0,0.0,0.0,0.0,0.0)
90  {
91  }
92  ControlledConnection(double x0,double y0,double z0,double x1,double y1,double z1)
93  :id_(x0,y0,z0,x1,y1,z1)
94  {
95  }
96  void setID(double x0,double y0,double z0,double x1,double y1,double z1)
97  {
99  }
100  const TID& getID()const{return id_;}
105  {
106  data_.push_back(e);
107  }
111  void clear()
112  {
113  data_.clear();
114  }
119  {
121  for( ConnectionStateEvent e: data_)
122  {
123  if( t < e.getMinEndTime() )
124  {
125  state = e.getState();
126  break;
127  }
128  }
129  return state;
130  }
131  //@TODO: add additional state evaluation functions for max/likely
136  {
137  return (adore::view::LimitLine::EState)(int)s;
138  }
139 
140 
141  };
142 
147  {
148  private:
149  struct Comparator
150  {
152  {
153  return a->getID().equals(b->getID());
154  }
155  };
156  struct BoxGen
157  {
158  typedef boost::geometry::model::point<double,3,boost::geometry::cs::cartesian> Tboost_point;
159  typedef boost::geometry::model::box<Tboost_point> Tboost_box;
161  {
162  const VectorIdentifier::Tboost_vectors& vectors = con->getID().getID();
163  const VectorIdentifier::Tboost_vectors& precision = con->getID().getPrecision();
164  Tboost_box box;
165  box.min_corner().set<0>((std::min)(vectors.get<0>()-precision.get<0>(),vectors.get<3>()-precision.get<3>()));
166  box.min_corner().set<1>((std::min)(vectors.get<1>()-precision.get<1>(),vectors.get<4>()-precision.get<4>()));
167  box.min_corner().set<2>((std::min)(vectors.get<2>()-precision.get<2>(),vectors.get<5>()-precision.get<5>()));
168  box.max_corner().set<0>((std::max)(vectors.get<0>()+precision.get<0>(),vectors.get<3>()+precision.get<3>()));
169  box.max_corner().set<1>((std::max)(vectors.get<1>()+precision.get<1>(),vectors.get<4>()+precision.get<4>()));
170  box.max_corner().set<2>((std::max)(vectors.get<2>()+precision.get<2>(),vectors.get<5>()+precision.get<5>()));
171  return box;
172  }
173  };
174  public:
177 
178  private:
179  std::vector<TConFeed*> feeds_;
181  double max_value_;
182  public:
188  ControlledConnectionSet(TConFeed* connection_feed,TConFeed* checkpoint_feed=nullptr)
189  {
190  max_value_ = 1e10;
191  feeds_.push_back(connection_feed);
192  if(checkpoint_feed!=nullptr)feeds_.push_back(checkpoint_feed);
193  }
195  {
196  for(TConFeed* feed:feeds_)delete feed;
197  feeds_.clear();
198  }
204  virtual void update(double radius, double X,double Y)
205  {
206  for(auto feed:feeds_)
207  {
208  while(feed->hasNext())
209  {
211  feed->getNext(con);
212  data_.insert(con);
213  }
214  }
215  data_.refocus(X-radius,Y-radius,X+radius,Y+radius);
216  }
217  TLocalBoxSet::itpair getConnectionsInRegion(double x0,double y0,double x1,double y1)
218  {
219  return data_.getObjectsInRegion(x0,y0,x1,y1);
220  }
221  int size()const{return data_.size();}
222  TLocalBoxSet::itpair getAllConnections()
223  {
225  }
226  };
227 
232  {
233  public:
235  private:
238  public:
239  ControlledConnectionSet4Ego(TVehicleMotionStateReader* xreader,TConFeed* connection_feed,TConFeed* checkpoint_feed=nullptr)
240  :ControlledConnectionSet(connection_feed,checkpoint_feed),xreader_(xreader)
241  {
242  }
243  virtual void update()
244  {
245  xreader_->getData(x_);
246  ControlledConnectionSet::update(200.0,x_.getX(),x_.getY());//@TODO read parameter r
247  }
248  };
249  }
250 }
Specialization of ControlledConnectionSet: Filters connections in range of ego.
Definition: controlledconnection.h:232
ControlledConnectionSet4Ego(TVehicleMotionStateReader *xreader, TConFeed *connection_feed, TConFeed *checkpoint_feed=nullptr)
Definition: controlledconnection.h:239
virtual void update()
Definition: controlledconnection.h:243
TVehicleMotionStateReader * xreader_
Definition: controlledconnection.h:236
adore::mad::AReader< VehicleMotionState9d > TVehicleMotionStateReader
Definition: controlledconnection.h:234
VehicleMotionState9d x_
Definition: controlledconnection.h:237
Definition: controlledconnection.h:147
TLocalBoxSet data_
Definition: controlledconnection.h:180
TLocalBoxSet::itpair getAllConnections()
Definition: controlledconnection.h:222
virtual void update(double radius, double X, double Y)
Definition: controlledconnection.h:204
~ControlledConnectionSet()
Definition: controlledconnection.h:194
double max_value_
Definition: controlledconnection.h:181
ControlledConnectionSet(TConFeed *connection_feed, TConFeed *checkpoint_feed=nullptr)
Constructor for ControlledConnectionSet stores pointer to update feeds.
Definition: controlledconnection.h:188
adore::mad::AFeed< ControlledConnection > TConFeed
Definition: controlledconnection.h:176
LocalBoxSet< ControlledConnection, BoxGen, Comparator > TLocalBoxSet
Definition: controlledconnection.h:175
std::vector< TConFeed * > feeds_
Definition: controlledconnection.h:179
TLocalBoxSet::itpair getConnectionsInRegion(double x0, double y0, double x1, double y1)
Definition: controlledconnection.h:217
int size() const
Definition: controlledconnection.h:221
Definition: controlledconnection.h:81
void insertStateEvent(const ConnectionStateEvent &e)
Definition: controlledconnection.h:104
adore::view::LimitLine::EState convert(ConnectionState::EConnectionState s)
Definition: controlledconnection.h:135
void setID(double x0, double y0, double z0, double x1, double y1, double z1)
Definition: controlledconnection.h:96
TID id_
Definition: controlledconnection.h:85
std::vector< ConnectionStateEvent > data_
Definition: controlledconnection.h:87
const TID & getID() const
Definition: controlledconnection.h:100
ControlledConnection()
Definition: controlledconnection.h:88
ControlledConnection(double x0, double y0, double z0, double x1, double y1, double z1)
Definition: controlledconnection.h:92
ConnectionState::EConnectionState getState_byMinTime(double t) const
Definition: controlledconnection.h:118
adore::env::VectorIdentifier TID
Definition: controlledconnection.h:83
void clear()
Definition: controlledconnection.h:111
itpair getObjectsInRegion(double x0, double y0, double x1, double y1)
Definition: localboxset.h:121
void insert(const TObject &object)
Definition: localboxset.h:70
int size() const
Definition: localboxset.h:115
void refocus(double x0, double y0, double x1, double y1)
Definition: localboxset.h:150
Definition: com_patterns.h:29
Definition: com_patterns.h:68
virtual void getData(T &value)=0
EConnectionState
Definition: controlledconnection.h:36
@ PROTECTED___MOVEMENT___ALLOWED
Definition: controlledconnection.h:43
@ STOP___AND___REMAIN
Definition: controlledconnection.h:40
@ PERMISSIVE___MOVEMENT___ALLOWED
Definition: controlledconnection.h:42
@ DARK
Definition: controlledconnection.h:38
@ STOP___THEN___PROCEED
Definition: controlledconnection.h:39
@ PERMISSIVE__CLEARANCE
Definition: controlledconnection.h:44
@ CAUTION___CONFLICTING___TRAFFIC
Definition: controlledconnection.h:46
@ UNAVAILABLE
Definition: controlledconnection.h:37
@ PRE___MOVEMENT
Definition: controlledconnection.h:41
@ PROTECTED__CLEARANCE
Definition: controlledconnection.h:45
T min(T a, T b, T c, T d)
Definition: adoremath.h:663
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
x0
Definition: adore_set_goal.py:25
y0
Definition: adore_set_goal.py:26
z0
Definition: adore_set_goal.py:27
y1
Definition: adore_set_pose.py:29
x1
Definition: adore_set_pose.py:28
z1
Definition: adore_set_pose.py:30
Definition: areaofeffectconverter.h:20
Definition: controlledconnection.h:55
ConnectionState::EConnectionState getState() const
Definition: controlledconnection.h:72
double likelyTime_
Definition: controlledconnection.h:59
bool likelyTime_present_
Definition: controlledconnection.h:61
double minEndTime_
Definition: controlledconnection.h:57
bool hasMaxEndTime() const
Definition: controlledconnection.h:70
double getLikelyTime() const
Definition: controlledconnection.h:69
double getMaxOrMinEndTime() const
Definition: controlledconnection.h:68
bool maxEndTime_present_
Definition: controlledconnection.h:60
bool hasLikelyTime() const
Definition: controlledconnection.h:71
ConnectionStateEvent(ConnectionState::EConnectionState state, double minEndTime, bool maxEndTime_present, bool likelyTime_present, double maxEndTime, double likelyTime)
Definition: controlledconnection.h:64
double maxEndTime_
Definition: controlledconnection.h:58
double getMinEndTime() const
Definition: controlledconnection.h:66
ConnectionState::EConnectionState state_
Definition: controlledconnection.h:62
double getMaxEndTime() const
Definition: controlledconnection.h:67
Definition: controlledconnection.h:157
Tboost_box operator()(const ControlledConnection *con) const
Definition: controlledconnection.h:160
boost::geometry::model::box< Tboost_point > Tboost_box
Definition: controlledconnection.h:159
boost::geometry::model::point< double, 3, boost::geometry::cs::cartesian > Tboost_point
Definition: controlledconnection.h:158
Definition: controlledconnection.h:150
bool operator()(const ControlledConnection *a, const ControlledConnection *b) const
Definition: controlledconnection.h:151
Definition: vectoridentifier.h:30
boost::geometry::model::point< double, 6, boost::geometry::cs::cartesian > Tboost_vectors
Definition: vectoridentifier.h:32
bool equals(const VectorIdentifier &other) const
Definition: vectoridentifier.h:53
const Tboost_vectors & getPrecision() const
Definition: vectoridentifier.h:63
const Tboost_vectors & getID() const
Definition: vectoridentifier.h:64
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
double getX() const
Get the x-coordinate.
Definition: vehiclemotionstate9d.h:54
double getY() const
Get the y-coordinate.
Definition: vehiclemotionstate9d.h:60
EState
Definition: limitline.h:32