ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
objectdetectionmodel.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
13  ********************************************************************************/
14 
15 #pragma once
16 #include <adore/sim/afactory.h>
17 #include <adore/params/afactory.h>
18 #include <adore/env/afactory.h>
19 #include <unordered_map>
20 
21 namespace adore
22 {
23  namespace apps
24  {
29  {
30  private:
32  adoreMatrix<double,3,1> ego_location_;
35  std::unordered_map<int,adore::env::traffic::Participant> latest_data_;
40 
41  public:
47  ObjectDetectionModel(int simulationID)
48  {
49  simulationID_ = simulationID;
56  }
61  virtual void run()
62  {
63  if(!timer_->hasData())return;
64  double t_now;
65  timer_->getData(t_now);
66 
67  //update parameters
68  double sensor_range;
69  double discard_age;
70  sensor_range = psensor_model_->get_objectDetectionRange();
71  discard_age = psensor_model_->get_objectDiscardAge();
72 
73  adore::env::VehicleMotionState9d x_localization;
74  localization_state_reader_->getData(x_localization);
75 
78 
79  adoreMatrix<double, 3, 1> position;
80  // update ego position
81  position(0, 0) = x.getX();
82  position(1, 0) = x.getY();
83  position(2, 0) = 0.0;
84  ego_location_ = position;
85 
86  //retrieve state updates
87  while( participant_feed_->hasNext() )
88  {
91  if( p.getTrackingID() != simulationID_ )
92  {
93  if( adore::mad::norm2((adoreMatrix<double,3,1>)(p.getCenter()-ego_location_)) < sensor_range )
94  {
95  //transform the participant to local coordinates, then to localization coordinates
96  const double ct = std::cos(x.getPSI());
97  const double st = std::sin(x.getPSI());
98  const double ce = std::cos(x_localization.getPSI());
99  const double se = std::sin(x_localization.getPSI());
100  const double X_l = ct*(p.getCenter()(0)-x.getX())+st*(p.getCenter()(1)-x.getY());
101  const double Y_l =-st*(p.getCenter()(0)-x.getX())+ct*(p.getCenter()(1)-x.getY());
102  const double Z_l = p.getCenter()(2)-x.getZ();
103  const double X_e = ct*X_l-st*Y_l + x_localization.getX();
104  const double Y_e = st*X_l+ct*Y_l + x_localization.getY();
105  const double Z_e = Z_l + x_localization.getZ();
106  p.center_(0) = X_e;
107  p.center_(1) = Y_e;
108  p.center_(2) = Z_e;
109  const double cy = std::cos(p.getYaw());
110  const double sy = std::sin(p.getYaw());
111  const double cy_l = ct * cy + st * sy;
112  const double sy_l =-st * cy + ct * sy;
113  const double cy_e = ce * cy_l - se * sy_l;
114  const double sy_e = se * cy_l + ce * sy_l;
115  p.yaw_ = std::atan2(sy_e,cy_e);
116  latest_data_.emplace(p.getTrackingID(), p).first->second = p;
117  }
118  }
119 
120  }
121 
122  //collect latest known state updates
124  for( auto& pair: latest_data_ )
125  {
126  auto& p = pair.second;
127 
128  if( t_now - p.getObservationTime() < discard_age )
129  {
130  p.existance_certainty_ = 100.0;
131  pset.push_back(p);
132  //std::cout<<"traffic id="<<p.getTrackingID()<<", x="<<p.getCenter()(0)<<", y="<<p.getCenter()(1)<<"\n";
133  }
134  else
135  {
136  //TODO: do nothing or eventually discard from latest_data_
137  }
138  }
139  //send set of observations
141  }
142 
143  };
144  }
145 }
Definition: objectdetectionmodel.h:29
adore::mad::AReader< double > * timer_
Definition: objectdetectionmodel.h:36
virtual void run()
publish updates on the detection of traffic participants
Definition: objectdetectionmodel.h:61
adoreMatrix< double, 3, 1 > ego_location_
Definition: objectdetectionmodel.h:32
adore::params::APSensorModel * psensor_model_
Definition: objectdetectionmodel.h:37
ObjectDetectionModel(int simulationID)
Definition: objectdetectionmodel.h:47
adore::env::AFactory::TVehicleMotionStateReader * localization_state_reader_
Definition: objectdetectionmodel.h:38
std::unordered_map< int, adore::env::traffic::Participant > latest_data_
Definition: objectdetectionmodel.h:35
adore::sim::AFactory::TParticipantSetWriter * participant_set_writer_
Definition: objectdetectionmodel.h:34
adore::sim::AFactory::TParticipantFeed * participant_feed_
Definition: objectdetectionmodel.h:33
int simulationID_
Definition: objectdetectionmodel.h:31
adore::sim::AFactory::TVehicleMotionStateReader * true_state_reader_
Definition: objectdetectionmodel.h:39
virtual TVehicleMotionStateReader * getVehicleMotionStateReader()=0
static adore::env::AFactory * get()
Definition: afactory.h:236
virtual void getNext(T &value)=0
virtual bool hasNext() const =0
virtual void getData(T &value)=0
virtual bool hasData() const =0
Definition: com_patterns.h:97
virtual void write(const T &value)=0
virtual APSensorModel * getSensorModel() const =0
abstract class for vehicle sensor model parameters
Definition: ap_sensor_model.h:27
virtual double get_objectDetectionRange() const =0
maximum sensor range for object detection in a generalized sensor setting
virtual double get_objectDiscardAge() const =0
time after which object detections are discarded
static adore::params::AFactory * get()
Definition: afactory.h:103
virtual TVehicleMotionStateReader * getVehicleMotionStateReader()=0
read updates on the true vehicle motion state
virtual TParticipantFeed * getParticipantFeed()=0
get state updates from all vehicles
virtual TParticipantSetWriter * getParticipantSetWriter()=0
send simulated sensor data
virtual TSimulationTimeReader * getSimulationTimeReader()=0
read the simulation time
static adore::sim::AFactory * get()
Definition: afactory.h:145
std::vector< Participant > TParticipantSet
Definition: participant.h:164
interval< T > atan2(interval< T > y, interval< T > x)
Definition: intervalarithmetic.h:234
T norm2(const adoreMatrix< T, N, 1 > &x)
Definition: adoremath.h:186
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
x
Definition: adore_set_goal.py:30
Definition: areaofeffectconverter.h:20
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
double getPSI() const
Get the heading.
Definition: vehiclemotionstate9d.h:72
double getZ() const
Get the z-coordinate.
Definition: vehiclemotionstate9d.h:66
Struct for representing a participant in traffic.
Definition: participant.h:30
TTrackingID getTrackingID() const
Definition: participant.h:109
double yaw_
Definition: participant.h:99
adoreMatrix< double, 3, 1 > center_
Definition: participant.h:98
const adoreMatrix< double, 3, 1 > & getCenter() const
Definition: participant.h:124
double getYaw() const
Definition: participant.h:154
double existance_certainty_
Definition: participant.h:80