ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
plot_lanes.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  * Thomas Lobig
14  ********************************************************************************/
15 
16 #pragma once
23 #include <adore/env/afactory.h>
25 #include <adore/params/afactory.h>
26 #include <plotlablib/afigurestub.h>
27 #include <plotlablib/plcommands.h>
28 #include <unordered_map>
29 #include <unordered_set>
30 #include <string>
31 #include <list>
32 #include <dlib/matrix.h>
33 #include <chrono>
34 #include <deque>
35 #include <fstream>
36 
37 
38 namespace adore
39 {
40  namespace apps
41  {
46  class PlotLanes
47  {
48  private:
53 
54  std::unordered_set<std::string> plot_tags_old_;
55  std::unordered_set<std::string> plot_tags_current_;
56 
60  std::string prefix_;
61 
62  double fov_width_;
63  double fov_height_;
64 
66 
67  struct info
68  {
69  std::string name_;
70  bool visible_;
71  info(bool visible,std::string name):name_(name),visible_(visible){}
72  };
73  std::map<adore::env::BorderBased::BorderID,info> plotMap_;
74 
76  {
77  auto result = plotMap_.find(id);
78  if(result!=plotMap_.end())
79  {
80  if(result->second.visible_)
81  {
82  figure_->erase(result->second.name_);
83  }
84  plotMap_.erase(id);
85  }
86  }
87 
89  {
90  auto result = plotMap_.find(id);
91  return result!=plotMap_.end() && result->second.visible_;
92  }
94  {
95  static long long counter = 0;
96  auto result = plotMap_.find(id);
97  if(result!=plotMap_.end())
98  {
99  result->second.visible_ = visible;
100  }
101  else
102  {
103  counter ++;
104  std::stringstream ss;
105  ss<<prefix_<<"/localmap/border"<<counter;
106  plotMap_.emplace(std::make_pair(id,info(visible,ss.str())));
107  }
108  }
110  {
111  auto result = plotMap_.find(id);
112  return result->second.name_;
113  }
114 
115  public:
116  PlotLanes(DLR_TS::PlotLab::AFigureStub* figure,std::string prefix,const adore::PLOT::LanePlotConfig & config)
117  {
122  figure_ = figure;
123  prefix_ = prefix;
124  fov_width_ = 640.0;
125  fov_height_ = 480.0;
126  config_ = config;
127  }
128 
130  {
131  }
132 
133  void run()
134  {
135 
137  plot_tags_current_.clear();
138 
139  // adore::env::BorderBased::BorderSet newBorderSet_;
140  while(borderfeed_->hasNext())
141  {
142 
143  bool to_be_plotted = false;
144  auto border = new adore::env::BorderBased::Border();
145  borderfeed_->getNext(*border);
146  borderSet_.insert_border(border,true);
147  }
148 
150  for(auto it=borderSet_.getAllBorders();it.first!=it.second;it.first++)
151  {
152  adore::env::BorderBased::Border* border = it.first->second;
153  adore::env::BorderBased::Border* leftNeighbor = 0;
154  bool do_plot = false;
155  switch(border->m_type)
156  {
158  do_plot = config_.plot_drive_lane;
159  break;
161  do_plot = config_.plot_emergency_lane;
162  break;
164  do_plot = config_.plot_other_lane;
165  break;
166  }
167 
168  if(do_plot){
169  if(border->m_left!=0)
170  {
171  leftNeighbor = borderSet_.getBorder(*(border->m_left));
172  if (leftNeighbor != 0 && (!isVisible(border->m_id) || !isVisible(leftNeighbor->m_id)) )
173  {
174  setVisible(border->m_id,true);
175  std::stringstream ss;
176  ss<<"border/"<<std::hex<<border;
177  // auto name = getName(border->m_id);
178  auto name = ss.str();
179  if (border->m_type == adore::env::BorderBased::BorderType::DRIVING)
180  {
181  auto highlightRightBorder = (!borderSet_.hasRightNeighbor(border)) || (borderSet_.getRightNeighbor(border)->m_type != adore::env::BorderBased::BorderType::DRIVING);
182  auto highlightLeftBorder = (!borderSet_.hasLeftNeighbor(leftNeighbor)) || (leftNeighbor->m_type != adore::env::BorderBased::BorderType::DRIVING);
183  // adore::PLOT::plotBorder_fancy(name,border,leftNeighbor,0.0,highlightLeftBorder,highlightRightBorder,config_,figure_);
184  adore::PLOT::plotBorder(name,border,leftNeighbor,0.0,"FillColor=0.4,0.4,0.4",figure_);
185  }
186  else
187  {
188  // adore::PLOT::plotBorder_fancy(name,border,leftNeighbor,0.0,false,false,config_,figure_);
189  adore::PLOT::plotBorder(name,border,leftNeighbor,0.0,"FillColor=1,0.8,0.8",figure_);
190  }
191  }
192  }
193  }
194  }
195 
197  {
199  }
200 
201  for(auto s:plot_tags_old_)
202  {
203  if(plot_tags_current_.find(s)==plot_tags_current_.end())
204  {
205  figure_->erase(s);
206  }
207  }
208  }
209  };
210  }
211 }
Definition: afigurestub.h:24
virtual void erase(std::string hashtag)=0
Definition: laneplot_config.h:24
bool plot_other_lane
Definition: laneplot_config.h:34
bool plot_emergency_lane
Definition: laneplot_config.h:33
bool plot_drive_lane
Definition: laneplot_config.h:35
a optimzed plotting application to plot map borders, vehicles and environment information and backgro...
Definition: plot_lanes.h:47
DLR_TS::PlotLab::AFigureStub * figure_
Definition: plot_lanes.h:57
void setVisible(adore::env::BorderBased::BorderID id, bool visible)
Definition: plot_lanes.h:93
std::string getName(adore::env::BorderBased::BorderID id)
Definition: plot_lanes.h:109
adore::params::APMapProvider * pmap_
Definition: plot_lanes.h:50
adore::params::APVehicle * pvehicle_
Definition: plot_lanes.h:49
double fov_height_
Definition: plot_lanes.h:63
adore::mad::AFeed< adore::env::BorderBased::Border > * borderfeed_
Definition: plot_lanes.h:52
adore::PLOT::LanePlotConfig config_
Definition: plot_lanes.h:65
adore::env::VehicleMotionState9d position_
Definition: plot_lanes.h:59
adore::env::BorderBased::BorderSet borderSet_
Definition: plot_lanes.h:58
std::unordered_set< std::string > plot_tags_current_
Definition: plot_lanes.h:55
~PlotLanes()
Definition: plot_lanes.h:129
void unfocus(adore::env::BorderBased::BorderID id)
Definition: plot_lanes.h:75
PlotLanes(DLR_TS::PlotLab::AFigureStub *figure, std::string prefix, const adore::PLOT::LanePlotConfig &config)
Definition: plot_lanes.h:116
adore::mad::AReader< adore::env::VehicleMotionState9d > * positionReader_
Definition: plot_lanes.h:51
bool isVisible(adore::env::BorderBased::BorderID id)
Definition: plot_lanes.h:88
double fov_width_
Definition: plot_lanes.h:62
std::string prefix_
Definition: plot_lanes.h:60
std::unordered_set< std::string > plot_tags_old_
Definition: plot_lanes.h:54
void run()
Definition: plot_lanes.h:133
std::map< adore::env::BorderBased::BorderID, info > plotMap_
Definition: plot_lanes.h:73
virtual TVehicleMotionStateReader * getVehicleMotionStateReader()=0
virtual TBorderFeed * getBorderFeed()=0
efficiently store borders in boost R-tree
Definition: borderset.h:99
Border * getRightNeighbor(Border *b)
get the right neighbor of a border
Definition: borderset.h:1279
bool hasRightNeighbor(Border *b)
checks whether right neighbor exists for a border
Definition: borderset.h:1292
void insert_border(Border *b, bool force_insert=false)
insert new border into this
Definition: borderset.h:225
BorderIteratorPair2 getAllBorders()
get all borders in this
Definition: borderset.h:356
Border * getBorder(const BorderID &id) const
retrieve a border by ID
Definition: borderset.h:628
bool hasLeftNeighbor(Border *b)
checks whether left border exists for a border
Definition: borderset.h:1267
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 hasUpdate() const =0
virtual APVehicle * getVehicle() const =0
virtual APMapProvider * getMapProvider() const =0
abstract class containing parameters to configure aspects of the map provider
Definition: ap_map_provider.h:25
abstract class for vehicle configuration related paremeters
Definition: ap_vehicle.h:29
static adore::params::AFactory * get()
Definition: afactory.h:103
void plotBorder(std::string name, adore::env::BorderBased::Border *right, adore::env::BorderBased::Border *left, double z, std::string options, DLR_TS::PlotLab::AFigureStub *figure, bool plotarrows=false)
Definition: plot_border.h:58
@ OTHER
Definition: border.h:38
@ EMERGENCY
Definition: border.h:41
@ DRIVING
Definition: border.h:39
Definition: areaofeffectconverter.h:20
Definition: plot_lanes.h:68
std::string name_
Definition: plot_lanes.h:69
bool visible_
Definition: plot_lanes.h:70
info(bool visible, std::string name)
Definition: plot_lanes.h:71
This struct identifies a Border by the coordinates of the starting and the end point.
Definition: borderid.h:31
The border struct contains data of the smallest.
Definition: border.h:62
BorderType::TYPE m_type
Definition: border.h:71
BorderID m_id
Definition: border.h:68
BorderID * m_left
Definition: border.h:69
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39