ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
trafficqueueonalane.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 #pragma once
16 
17 #include <adore/view/alane.h>
19 #include <algorithm>
20 namespace adore
21 {
22  namespace env
23  {
24  namespace traffic
25  {
30  {
31  private:
33  public:
35  {
36  return queue_;
37  }
42  {
43  queue_.clear();
44  if(lane->isValid())
45  {
46  for( auto& par: *participantSet )
47  {
48  //@TODO: apply prediction/classification module to determine whether participant is moving
49  // with traffic flow or should be rated as cross traffic. Also determine potential exit time
50  // if participant is changing lanes, enter a prediction of exit time
51  // check neighboring lanes for entering vehicles
52  auto point = par.center_;
53  double s,n;
54  lane->toRelativeCoordinates(point(0),point(1),s,n);
55  bool inside = lane->getOffsetOfRightBorder(s)<=n
56  && n<=lane->getOffsetOfLeftBorder(s);
57  //classification, whether an object is matched to lane:
58  //based on center of object
59  // + vehicles in head-on adjacent lane might slightly stick into ego lane (as observed by sensor).
60  // using only center creates buffer and prevents false positives
61  // - late reaction, when vehicles in same direction parallel lanes execute a lane change
62 
63  if(!inside)continue;
64 
66  object.setEntranceProgress(s);
67  object.setEntranceTime(par.observation_time_);
68 
69  double cpsi = (std::cos)(par.yaw_);
70  double spsi = (std::sin)(par.yaw_);
71  double ctheta = (std::cos)(lane->getHeading(s));
72  double stheta = (std::sin)(lane->getHeading(s));
73  //project velocity to road
74  double ds = (ctheta*cpsi+stheta*spsi) * par.vx_
75  + (-ctheta*spsi+stheta*cpsi) * par.vy_;
76  object.setObservationTime(par.observation_time_);
77  object.setEntranceSpeed(ds);
78  object.setCurrentSpeed(ds);
79  object.setCurrentProgress(s);
80  object.setCurrentAcceleration(0.0);
81  object.setExitProgress(1.0e5);
82  object.setExitTime(par.observation_time_+1.0e5);
83  object.setTrackingID(par.trackingID_);
84  object.setV2XStationID(par.v2xStationID_);
85  object.setLength(par.length_);
86  queue_.push_back(object);
87  }
88  std::sort(queue_.begin(),queue_.end(),[](const adore::view::TrafficObject& a,
90  {return a.getCurrentProgress()
91  <b.getCurrentProgress();});
92  }
93  }
94  };
95  }
96  }
97 }
Definition: trafficqueueonalane.h:30
const adore::view::TrafficQueue & getQueue() const
Definition: trafficqueueonalane.h:34
adore::view::TrafficQueue queue_
Definition: trafficqueueonalane.h:32
void mapVehicles(adore::view::ALane *lane, adore::env::traffic::TParticipantSet *participantSet)
Map traffic unto lane.
Definition: trafficqueueonalane.h:41
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
virtual double getOffsetOfLeftBorder(double s)=0
virtual double getHeading(double s)=0
std::vector< Participant > TParticipantSet
Definition: participant.h:164
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
std::vector< TrafficObject > TrafficQueue
Definition: trafficobject.h:183
Definition: areaofeffectconverter.h:20
Definition: trafficobject.h:27
void setEntranceProgress(double value)
Definition: trafficobject.h:150