ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
plot_roadannotations.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/mad/com_patterns.h>
18 #include <adore/params/afactory.h>
19 #include <adore/env/afactory.h>
23 // #include <cstdio>
24 #include <sstream>
25 #include <map>
26 
27 namespace adore
28 {
29  namespace apps
30  {
36  {
37  private:
40 
43  std::string prefix_;
44  std::unordered_map<adore::env::TSpeedLimitID,adore::env::SpeedLimit> limits_;
45 
46  public:
48  {
51  figure_ = figure;
52  prefix_ = prefix;
53  }
54 
56  {
57  }
58 
59  void run()
60  {
62  {
64  }
65  adore::env::TSpeedLimitBundle limits_update;
66 
67  while (speedlimit_feed_->hasNext())
68  {
70  speedlimit_feed_->getNext(limit);
71  limits_update.push_back(limit);
72  }
73 
74  for (auto limit : limits_update)
75  {
76  limits_.insert_or_assign(limit.id,limit);
77  }
78 
80 
81  }
82 
83  virtual void plotSpeedLimits()
84  {
85  // adore::env::VehicleMotionState9d vehicle_state;
86  // motion_state_reader_->getData(vehicle_state);
87  auto position = vehicle_state_.getTime();
88  auto time = vehicle_state_.getTime();
89 
90  std::stringstream buffer1;
91  std::stringstream buffer2;
92 
93  for( auto [key, limit] : limits_)
94  {
95  buffer1.clear();
96  buffer2.clear();
97  buffer1 << "speedlimit_start/id" << key;
98  buffer2 << "X-" << limit.value << " - id:" << key;
99  // std::string hash
100  // figure_->plotText(buffer1.str(),limit.startX, limit.startY,buffer2.str());
101 
102  buffer1.clear();
103  buffer1 << "speedlimit_line/id" << limit.id;
104  adore::PLOT::plotLine(buffer1.str(),limit.startX,limit.startY,limit.stopX,limit.stopY,0.5,"LineColor=0,0,1",figure_);
105  // adore::PLOT::plotLine(buffer1.str(),limit.startX,limit.startY,limit.stopX,limit.stopY,0.5,"LineWidth=2;LineColor=0,0.2,1",figure_);
106 
107  // buffer1.clear();
108  // buffer1 << "speedlimit_stop/id" << limit.id;
109  // figure_->plotText(buffer1.str(),limit.startX, limit.startY,buffer2.str());
110  }
111 
112  // removeDistanceBased(limits_,plot_distance);
113 
114  // while(tl_controller_feed_->hasNext())
115  // {
116  // adore::PLOT::plotLine(ss_hashtag.str(),x_from,y_from,x_to,y_to,2,ss_color.str(),figure_);
117  // }
118  }
119 
120  void removeDistanceBased(std::unordered_map<std::string, std::pair<double,double>> &visible_objects, int distance)
121  {
122  // for (auto [key, limit] : limits_)
123  // {
124  // double x = vehicle_state_.getX();
125  // double y = vehicle_state_.getY();
126  // if ((limit.startX - x)*(limit.startX - x)
127  // }
128  // //remove tiles which are no longer visible
129  // std::vector<std::string> remove;
130 
131  // adore::env::VehicleMotionState9d vehicle_state;
132  // positionReader_->getData(vehicle_state);
133  // double v_pos_x = vehicle_state.getX();
134  // double v_pos_y = vehicle_state.getY();
135 
136  // for(auto it = visible_objects.begin();it!=visible_objects.end();it++)
137  // {
138  // auto coord = (*it).second;
139  // double distance_to_object = getDistanceToPoint(coord);
140  // if(distance_to_object > distance) remove.push_back((*it).first);
141  // }
142 
143  // for(auto it = remove.begin();it!=remove.end();it++)
144  // {
145  // figure_->erase((*it));
146  // visible_objects.erase((*it));
147  // }
148 
149  }
150 
151  };
152  }
153 }
Definition: afigurestub.h:24
an optimzed plotting application to plot annotations like speed limits
Definition: plot_roadannotations.h:36
void run()
Definition: plot_roadannotations.h:59
std::unordered_map< adore::env::TSpeedLimitID, adore::env::SpeedLimit > limits_
Definition: plot_roadannotations.h:44
virtual void plotSpeedLimits()
Definition: plot_roadannotations.h:83
void removeDistanceBased(std::unordered_map< std::string, std::pair< double, double >> &visible_objects, int distance)
Definition: plot_roadannotations.h:120
PlotRoadAnnotations(DLR_TS::PlotLab::AFigureStub *figure, std::string prefix)
Definition: plot_roadannotations.h:47
std::string prefix_
Definition: plot_roadannotations.h:43
adore::env::AFactory::TSpeedLimitFeed * speedlimit_feed_
Definition: plot_roadannotations.h:39
~PlotRoadAnnotations()
Definition: plot_roadannotations.h:55
adore::env::VehicleMotionState9d vehicle_state_
Definition: plot_roadannotations.h:42
DLR_TS::PlotLab::AFigureStub * figure_
Definition: plot_roadannotations.h:41
adore::mad::AReader< adore::env::VehicleMotionState9d > * motion_state_reader_
Definition: plot_roadannotations.h:38
virtual TSpeedLimitFeed * getSpeedLimitFeed()=0
virtual TVehicleMotionStateReader * getVehicleMotionStateReader()=0
static adore::env::AFactory * get()
Definition: afactory.h:236
Definition: com_patterns.h:29
virtual void getNext(T &value)=0
virtual bool hasNext() const =0
virtual void getData(T &value)=0
virtual bool hasUpdate() const =0
void plotLine(std::string hashtag, double x0, double y0, double x1, double y1, double z, std::string options, DLR_TS::PlotLab::AFigureStub *figure)
Definition: plot_shape.h:209
std::vector< SpeedLimit > TSpeedLimitBundle
Definition: speedlimit.h:49
Definition: areaofeffectconverter.h:20
Definition: speedlimit.h:31
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
double getTime() const
Get the time.
Definition: vehiclemotionstate9d.h:48