ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
indicator_hints_provider.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  * Thomas Lobig - initial implementation
13  ********************************************************************************/
14 
15 #pragma once
16 #include <adore/params/afactory.h>
19 #include <adore/env/afactory.h>
21 #include <adore/mad/com_patterns.h>
22 #include <string>
23 #include <sstream>
24 #include <vector>
25 #include <iostream>
26 #include <fstream>
27 #include <filesystem>
28 
29 #include "if_plotlab/plot_shape.h"
30 
31 namespace adore
32 {
33  namespace apps
34  {
40  {
41  //TODO vector only for quick prototyping, should be efficient geospatial data structure
43  std::string file_watch_path_;
46 
47  public:
48 
49  IndicatorHintsProvider(std::string file_watch_path) : file_watch_path_(file_watch_path)
50  {
53  auto read_result = adore::mad::CsvReader::get_data<std::string>(file_watch_path);
54 
55  if (read_result.size() % 5 == 0)
56  {
57  int index = 0;
58  int id = 0;
59  adore::env::IndicatorHint datapoint;
60  while(index < read_result.size())
61  {
62  datapoint.value = adore::env::IndicatorHint::indicatorSideFromString(read_result[index++]);
63  datapoint.startX = std::stod(read_result[index++]);
64  datapoint.startY = std::stod(read_result[index++]);
65  datapoint.stopX = std::stod(read_result[index++]);
66  datapoint.stopY = std::stod(read_result[index++]);
67  datapoint.id = id++;
68  hints.push_back(datapoint);
69  }
70  }
71  else
72  {
73  std::cerr << "ERROR speedlimit csv might be corrupted, the count of numbers is not divisible by 5";
74  }
75  }
76 
77  void run()
78  {
80 
81  motion_state_reader_->getData(motion_state);
83 
84  double x = motion_state.getX();
85  double y = motion_state.getY();
86 
87 
88  // TODO might think about caching which indicator hint data points were already sent and only send new ones and
89  // maybe resend other less frequently, similar to how the map provider handles borders
90 
91  for (auto item : hints)
92  {
93  double abs1 = std::abs(x - item.startX);
94  double abs2 = std::abs(y - item.startY);
95  double abs3 = std::abs(x - item.stopX);
96  double abs4 = std::abs(y - item.stopY);
97 
98  if (((abs1*abs1 + abs2*abs2) < visibilty_radius*visibilty_radius) or ((abs3*abs3 + abs4*abs4) < visibilty_radius*visibilty_radius))
99  {
101  {
103  }
104  else
105  {
106  std::cout << "indicator hint feed was full, data lost\n";
107  }
108 
109  }
110  }
111  }
112  };
113  }
114 }
A node to gather indicator hint data and provide it e.g. for laneview computation or tactical decisio...
Definition: indicator_hints_provider.h:40
adore::env::AFactory::TIndicatorHintWriter * indicator_hint_writer_
Definition: indicator_hints_provider.h:45
IndicatorHintsProvider(std::string file_watch_path)
Definition: indicator_hints_provider.h:49
std::string file_watch_path_
Definition: indicator_hints_provider.h:43
adore::env::AFactory::TVehicleMotionStateReader * motion_state_reader_
Definition: indicator_hints_provider.h:44
void run()
Definition: indicator_hints_provider.h:77
adore::env::TIndicatorHintList hints
Definition: indicator_hints_provider.h:42
virtual TVehicleMotionStateReader * getVehicleMotionStateReader()=0
virtual TIndicatorHintWriter * getIndicatorHintWriter()=0
static adore::env::AFactory * get()
Definition: afactory.h:236
Definition: com_patterns.h:68
virtual void getData(T &value)=0
Definition: com_patterns.h:97
virtual void write(const T &value)=0
virtual bool canWriteMore() const =0
virtual APMapProvider * getMapProvider() const =0
virtual double getVisibiltyRadius() const =0
static adore::params::AFactory * get()
Definition: afactory.h:103
std::vector< IndicatorHint > TIndicatorHintList
Definition: indicator_hint.h:101
x
Definition: adore_set_goal.py:30
y
Definition: adore_set_goal.py:31
Definition: areaofeffectconverter.h:20
Definition: indicator_hint.h:41
double stopX
Definition: indicator_hint.h:96
double stopY
Definition: indicator_hint.h:97
constexpr static adore::env::IndicatorSide indicatorSideFromString(std::string_view key)
Definition: indicator_hint.h:52
double startY
Definition: indicator_hint.h:95
TIndicatorHintID id
Definition: indicator_hint.h:98
IndicatorSide value
Definition: indicator_hint.h:93
double startX
Definition: indicator_hint.h:94
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