ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
checkpoint_controller.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ß - Virtual traffic lights, switched according to test driver
13  ********************************************************************************/
14 #pragma once
15 #include <adore/env/afactory.h>
16 #include <adore/fun/afactory.h>
17 #include <adore/mad/adoremath.h>
18 #include <adore/params/afactory.h>
20 #include <vector>
21 #include <string>
22 
23 namespace adore
24 {
25 namespace apps
26 {
35 {
36 private:
41  std::vector<adore::env::ControlledConnection> checkpoints_;
43 public:
45  {
47  xx_reader_ = adore::fun::FunFactoryInstance::get()->getVehicleExtendedStateReader();
50  }
52  {
53  delete x_reader_;
54  delete xx_reader_;
55  delete cc_writer_;
56  delete pcheckpoints_;
57  }
58 
59  void run()
60  {
63  if(!x_reader_->hasData())return;
64  if(!xx_reader_->hasData())return;
66  xx_reader_->getData(xx);
67  lv_.update();
68 
69  double max_distance = pcheckpoints_->getClearDistance();
71  bool button_pressed = xx.getCheckpointClearance();
72  double t0 = x.getTime()+timeout;
73  double t1 = x.getTime()+timeout + 24.0*60.0*60.0;
74 
75  if(!lv_.getCurrentLane()->isValid())return;
76  double s_ego,n;
77  lv_.getCurrentLane()->toRelativeCoordinates(x.getX(),x.getY(),s_ego,n);
79  {
80  bool checkpoint_in_view = true;
81  bool checkpoint_in_clear_distance = true;
82  double s_con;
83  lv_.getCurrentLane()->toRelativeCoordinates(con.getID().getFrom().get<0>(),con.getID().getFrom().get<1>(),s_con,n);
84  if(s_con<lv_.getCurrentLane()->getSMin()||s_con>=lv_.getCurrentLane()->getSMax())checkpoint_in_view = false;
85  if(checkpoint_in_view &&
87  ||lv_.getCurrentLane()->getOffsetOfRightBorder(s_con)>n))checkpoint_in_view = false;
88  if(!checkpoint_in_view || s_con-s_ego>max_distance)checkpoint_in_clear_distance = false;
89  if(checkpoint_in_view)
90  {
91  if(button_pressed && checkpoint_in_clear_distance)
92  {
93  con.clear();
94  con.insertStateEvent(adore::env::ConnectionStateEvent(
96  t0,true,true,t0,t0
97  ));
98  con.insertStateEvent(adore::env::ConnectionStateEvent(
100  t1,false,false,t1,t1
101  ));
102  }
103  else
104  {
105  if(con.data_.size()==0)
106  {
107  con.insertStateEvent(adore::env::ConnectionStateEvent(
109  t1,false,false,t1,t1
110  ));
111  }
112  }
113  cc_writer_->write(con);
114  }
115  }
116  }
117 
127  bool loadFromFile(std::string filename)
128  {
129  std::string line;
130  std::ifstream file(filename);
131  if(!file.is_open())
132  {
133  std::cout<<"failed to open file: "<<filename<<std::endl;
134  return false;
135  }
136  while(std::getline(file,line))
137  {
138  size_t p0,p1,p2,p3;
139  double X = 0.0,Y = 0.0,Z = 0.0;
140 
141  p3 = line.find("#");
142  if(p3==std::string::npos)p3 = line.length();
143  p0 = line.find(";");
144  if(p0==std::string::npos)
145  {
146  std::cout<<"error in file: "<<filename<<std::endl;
147  return false;
148  }
149  else
150  {
151  std::stringstream ss0;
152  ss0<<line.substr(0,p0);
153  ss0>>X;
154 
155  p1 = line.find(";",p0+1);
156  if(p1==std::string::npos)p1 = p3;
157  else p2 = p3;
158 
159  {
160  std::stringstream ss0;
161  ss0<<line.substr(p0+1,p1-1);
162  ss0>>Y;
163  }
164 
165  if(p1<p3)
166  {
167  std::stringstream ss0;
168  ss0<<line.substr(p1+1,p2-1);
169  ss0>>Z;
170  }
171  }
172  checkpoints_.push_back(adore::env::ControlledConnection(X,Y,Z,X,Y,Z));
173  }
174  return true;
175  }
176 
177 };
178 }
179 }
A set of virtual traffic lights are switched according to test driver input. CheckpointController is ...
Definition: checkpoint_controller.h:35
adore::params::APCheckpoints * pcheckpoints_
Definition: checkpoint_controller.h:42
std::vector< adore::env::ControlledConnection > checkpoints_
Definition: checkpoint_controller.h:41
adore::mad::AReader< adore::fun::VehicleExtendedState > * xx_reader_
Definition: checkpoint_controller.h:38
adore::env::ThreeLaneViewDecoupled lv_
Definition: checkpoint_controller.h:40
bool loadFromFile(std::string filename)
read a set of 2d/3d coordinates from a csv file and add these to checkpoints_
Definition: checkpoint_controller.h:127
adore::mad::AReader< adore::fun::VehicleMotionState9d > * x_reader_
Definition: checkpoint_controller.h:37
void run()
Definition: checkpoint_controller.h:59
CheckpointController()
Definition: checkpoint_controller.h:44
adore::env::AFactory::TControlledConnectionWriter * cc_writer_
Definition: checkpoint_controller.h:39
~CheckpointController()
Definition: checkpoint_controller.h:51
virtual TControlledConnectionWriter * getCheckPointWriter()=0
virtual TVehicleMotionStateReader * getVehicleMotionStateReader()=0
Definition: controlledconnection.h:81
static adore::env::AFactory * get()
Definition: afactory.h:236
Definition: threelaneviewdecoupled.h:32
virtual adore::view::ALane * getCurrentLane()
Definition: threelaneviewdecoupled.h:405
void update()
Definition: threelaneviewdecoupled.h:373
static adore::fun::AFactory * get()
Definition: afactory.h:170
Definition: vehicleextendedstate.h:26
bool getCheckpointClearance() const
Definition: vehicleextendedstate.h:103
virtual void getData(T &value)=0
virtual bool hasData() const =0
Definition: com_patterns.h:97
virtual void write(const T &value)=0
virtual APCheckpoints * getCheckpoints() const =0
abstract class containing parameters for checkpoint handling
Definition: ap_checkpoints.h:25
virtual double getClearDistance() const =0
returns distance value for resetting current checkpoint
virtual double getClearTimeout() const =0
returns timeout for resetting current checkpoint
static adore::params::AFactory * get()
Definition: afactory.h:103
virtual void toRelativeCoordinates(double xe, double ye, double &s, double &n)=0
virtual double getSMin() const =0
virtual double getOffsetOfRightBorder(double s)=0
virtual double getSMax() const =0
virtual bool isValid() const =0
virtual double getOffsetOfLeftBorder(double s)=0
@ STOP___AND___REMAIN
Definition: controlledconnection.h:40
@ PERMISSIVE___MOVEMENT___ALLOWED
Definition: controlledconnection.h:42
int timeout
Definition: adore_ci_terminator_node.py:25
x
Definition: adore_set_goal.py:30
p0
Definition: adore_set_pose.py:32
string file
Definition: adore_suppress_lanechanges.py:181
Definition: areaofeffectconverter.h:20
Definition: controlledconnection.h:55
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39