ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
connectionsonlane.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 
15 
16 #pragma once
17 #include <adore/view/alane.h>
20 #include <vector>
21 #include <algorithm>
22 
23 namespace adore
24 {
25 namespace env
26 {
27 
33  {
34  public:
36  {
37  private:
38  double s0_;
39  double s1_;
41  public:
42  MappedConnection(double s0,double s1,ControlledConnection* con):s0_(s0),s1_(s1),con_(con){}
43  double getS0()const {return s0_;}
44  double getS1()const {return s1_;}
46  };
47 
48  private:
51  std::vector<MappedConnection> mappedcons_;
52  public://methods from adore::view::ALimitLineEnRoute
53  virtual bool hasLimitLine(double s0)override
54  {
55  for(auto& con:mappedcons_)
56  {
57  if(con.getS1()>=s0)return true;
58  }
59  return false;
60  }
61  virtual adore::view::LimitLine getLimitLine(double t0,double s0)override
62  {
64  for(auto& con:mappedcons_)
65  {
66  if(con.getS1()>s0)
67  {
68  ll.setCurrentState(con.getConnection()->convert(con.getConnection()->getState_byMinTime(t0)));
69  ll.setProgress(con.getS0());
70  return ll;
71  }
72  }
73  return ll;
74  }
75 
76  public:
78  :lane_(lane),conset_(conset){}
82  void update()
83  {
84  mappedcons_.clear();
85  if(lane_->isValid())
86  {
87  //find all connections with start and end point in lane, add them to vector
88  for(auto itpair = conset_->getAllConnections();//@TODO: get bounding box of lane
89  itpair.current()!=itpair.end();
90  itpair.current()++)
91  {
92  ControlledConnection* con = itpair.current()->second;
93  auto from = con->getID().getFrom();
94  auto to = con->getID().getTo();
95  double s0,n0,s1,n1;
96  lane_->toRelativeCoordinates(from.get<0>(),from.get<1>(),s0,n0);
97  if( lane_->getOffsetOfRightBorder(s0)<= n0 && n0<=lane_->getOffsetOfLeftBorder(s0) )
98  {
99  lane_->toRelativeCoordinates(to.get<0>(),to.get<1>(),s1,n1);
100  if( lane_->getOffsetOfRightBorder(s1)<= n1 && n1<=lane_->getOffsetOfLeftBorder(s1) )
101  {
102  if((from.get<0>()==to.get<0>() && from.get<1>()==to.get<1>()) || s0<s1)
103  {
104  mappedcons_.push_back(MappedConnection(s0,s1,con));
105  }
106  }
107  }
108  }
109  //sort vector by start point
110  std::sort(mappedcons_.begin(), mappedcons_.end(), [](const MappedConnection& a,const MappedConnection& b) {return a.getS0()<=b.getS0();});
111  }
112  }
113  };
114 
115 }
116 }
Definition: connectionsonlane.h:33
adore::view::ALane * lane_
Definition: connectionsonlane.h:49
virtual bool hasLimitLine(double s0) override
Definition: connectionsonlane.h:53
virtual adore::view::LimitLine getLimitLine(double t0, double s0) override
Definition: connectionsonlane.h:61
void update()
Definition: connectionsonlane.h:82
ConnectionsOnLane(adore::view::ALane *lane, adore::env::ControlledConnectionSet *conset)
Definition: connectionsonlane.h:77
std::vector< MappedConnection > mappedcons_
Definition: connectionsonlane.h:51
ControlledConnectionSet * conset_
Definition: connectionsonlane.h:50
Definition: controlledconnection.h:147
TLocalBoxSet::itpair getAllConnections()
Definition: controlledconnection.h:222
Definition: controlledconnection.h:81
const TID & getID() const
Definition: controlledconnection.h:100
Definition: alane.h:28
virtual void toRelativeCoordinates(double xe, double ye, double &s, double &n)=0
virtual double getOffsetOfRightBorder(double s)=0
virtual bool isValid() const =0
Definition: alimitlineenroute.h:26
Definition: areaofeffectconverter.h:20
Definition: connectionsonlane.h:36
double getS1() const
Definition: connectionsonlane.h:44
double s1_
Definition: connectionsonlane.h:39
MappedConnection(double s0, double s1, ControlledConnection *con)
Definition: connectionsonlane.h:42
double s0_
Definition: connectionsonlane.h:38
ControlledConnection * con_
Definition: connectionsonlane.h:40
ControlledConnection * getConnection()
Definition: connectionsonlane.h:45
double getS0() const
Definition: connectionsonlane.h:43
Tboost_point getFrom() const
Definition: vectoridentifier.h:65
Tboost_point getTo() const
Definition: vectoridentifier.h:66
Definition: limitline.h:26
void setProgress(double value)
Definition: limitline.h:73
void setCurrentState(EState value)
Definition: limitline.h:61