18 #include <boost/geometry.hpp>
19 #include <boost/geometry/geometries/point.hpp>
20 #include <boost/geometry/geometries/box.hpp>
33 namespace ConnectionState
87 std::vector<ConnectionStateEvent>
data_;
89 :
id_(0.0,0.0,0.0,0.0,0.0,0.0)
123 if( t < e.getMinEndTime() )
125 state = e.getState();
158 typedef boost::geometry::model::point<double,3,boost::geometry::cs::cartesian>
Tboost_point;
159 typedef boost::geometry::model::box<Tboost_point>
Tboost_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>()));
191 feeds_.push_back(connection_feed);
192 if(checkpoint_feed!=
nullptr)
feeds_.push_back(checkpoint_feed);
204 virtual void update(
double radius,
double X,
double Y)
208 while(feed->hasNext())
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