ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
egolanetraffic.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 
20 #include <adore/view/alane.h>
21 #include <algorithm>
22 #include "trafficmap.h"
23 
24 namespace adore
25 {
26  namespace env
27  {
28  namespace traffic
29  {
34  {
35  private:
38  public:
44  EgoLaneTraffic(TrafficMap* trafficMap):trafficMap_(trafficMap){}
51  {
52  return queue_;
53  }
59  const TrafficMap* getTrafficMap()const
60  {
61  return trafficMap_;
62  }
71  {
72  queue_.clear();
73  if(lfv->isValid())
74  {
75  std::unordered_set<Participant::TTrackingID> idset;
76  for( auto pborder: borders )
77  {
78  auto range = trafficMap_->getBorderToParticipant().equal_range(pborder->m_id);
79  for(auto it = range.first; it != range.second; ++it)
80  {
81  // TODO investigate if the following unused variable is actually a bug
82  // auto pos = it->second.first; // fixed Wunused-but-set-variable
83  Participant::TTrackingID id = it->second.second;
84  if(idset.find(id)!=idset.end())continue;//don't duplicate
85  auto it2 = trafficMap_->getTrackingIDToParticipant().find(id);
86  if(it2==trafficMap_->getTrackingIDToParticipant().end())continue;
87  idset.emplace(id);
88  const Participant& par = it2->second;
89 
90  //@TODO: apply prediction/classification module to determine whether participant is moving
91  // with traffic flow or should be rated as cross traffic. Also determine potential exit time
92  // if participant is changing lanes, enter a prediction of exit time
93  // check neighboring lanes for entering vehicles
94  auto& center = par.center_;
95  double s,n;
96  lfv->toRelativeCoordinates(center(0),center(1),s,n);
98  object.setEntranceProgress(s);
99  object.setEntranceTime(par.observation_time_);
100  double cpsi = (std::cos)(par.yaw_);
101  double spsi = (std::sin)(par.yaw_);
102  double ctheta = (std::cos)(lfv->getHeading(s));
103  double stheta = (std::sin)(lfv->getHeading(s));
104  //project velocity to road
105  double ds = (ctheta*cpsi+stheta*spsi) * par.vx_
106  + (-ctheta*spsi+stheta*cpsi) * par.vy_;
107  object.setObservationTime(par.observation_time_);
108  object.setEntranceSpeed(ds);
109  object.setCurrentSpeed(ds);
110  object.setCurrentProgress(s);
111  object.setCurrentAcceleration(0.0);
112  object.setExitProgress(1.0e5);
113  object.setExitTime(par.observation_time_+1.0e5);
114  object.setTrackingID(par.trackingID_);
115  object.setV2XStationID(par.v2xStationID_);
116  object.setLength(par.length_);
117  queue_.push_back(object);
118  }
119  }
120  std::sort(queue_.begin(),queue_.end(),[](const adore::view::TrafficObject& a,
122  {return a.getCurrentProgress()
123  <b.getCurrentProgress();});
124  }
125  }
126  };
127  }
128  }
129 }
Definition: egolanetraffic.h:34
const TrafficMap * getTrafficMap() const
Get the traffic map.
Definition: egolanetraffic.h:59
const adore::view::TrafficQueue & getQueue() const
Get the traffic queue.
Definition: egolanetraffic.h:50
TrafficMap * trafficMap_
Definition: egolanetraffic.h:37
EgoLaneTraffic(TrafficMap *trafficMap)
Construct a new EgoLaneTraffic object.
Definition: egolanetraffic.h:44
void mapVehiclesOnBorders(adore::view::ALane *lfv, const adore::env::BorderBased::BorderSubSet &borders)
Map vehicles on borders.
Definition: egolanetraffic.h:69
adore::view::TrafficQueue queue_
Definition: egolanetraffic.h:36
Definition: trafficmap.h:36
const TBorderToParticipant & getBorderToParticipant() const
Get the border to participant map.
Definition: trafficmap.h:72
const TTrackingIDToParticipant & getTrackingIDToParticipant() const
Get the tracking id to participant map.
Definition: trafficmap.h:90
Definition: alane.h:28
virtual void toRelativeCoordinates(double xe, double ye, double &s, double &n)=0
virtual bool isValid() const =0
virtual double getHeading(double s)=0
std::vector< Border * > BorderSubSet
Definition: borderset.h:92
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
Struct for representing a participant in traffic.
Definition: participant.h:30
double vy_
Definition: participant.h:104
double yaw_
Definition: participant.h:99
double vx_
Definition: participant.h:103
adoreMatrix< double, 3, 1 > center_
Definition: participant.h:98
TTrackingID trackingID_
Definition: participant.h:78
int TTrackingID
Definition: participant.h:32
TV2XStationID v2xStationID_
Definition: participant.h:79
double length_
Definition: participant.h:100
double observation_time_
Definition: participant.h:81
Definition: trafficobject.h:27
void setEntranceProgress(double value)
Definition: trafficobject.h:150