ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
navigation.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  * Robert Markowski - initial implementation
13  ********************************************************************************/
14 
15 #pragma once
16 #include <adore/params/afactory.h>
18 #include <adore/env/afactory.h>
21 #include <adore/mad/csvlog.h>
22 
23 #include "if_plotlab/plot_shape.h"
24 #include "if_plotlab/plot_border.h"
25 // #include <iostream>
26 
27 namespace adore
28 {
29  namespace apps
30  {
31  class Navigation
32  {
33  public:
34  struct Config
35  {
38  {
39  trans_x_ = 0;
40  trans_y_ = 0;
41  trans_z_ = 0;
42  rot_x_ = 0;
43  rot_y_ = 0;
44  rot_psi_ = 0;
45  }
46  };
48  private:
52 
53  /* read: vehicle position */
55 
56  /* read: navigation goal*/
58 
59  /* write: border*/
61 
64 
66  {
67  figure_->show();
68  double maxcost = nav_management_.getMaxCost();
69  auto global_map = nav_management_.getGlobalMap();
70  auto itpair = global_map->getAllBorders();
71  for(auto it = itpair.first; it!= itpair.second; it++)
72  {
73  auto b = it->second;
74  double normedCost = nav_management_.getBorderCost(b->m_id)/maxcost;
75  adore::env::BorderBased::Border * l = nullptr;
76  if(b->m_left != nullptr)
77  {
78  l = global_map->getBorder(*(b->m_left));
79  }
80  PLOT::plotBorderNavigation(b,l,normedCost,figure_);
81 
82  }
83  }
84  void parseTrackConfigs(std::string trackConfigs, env::BorderBased::BorderSet& targetSet)
85  {
86  std::string trackConfig = "";
87  std::stringstream trackstream(trackConfigs);
88  while(std::getline(trackstream,trackConfig,';'))
89  {
90  /* reading of single track configuration, comma separated */
91  std::stringstream trackConfigStream(trackConfig);
92  bool transform = false;
93  std::string xodrFilename = "";
94  std::string r2sReflineFilename = "";
95  std::string r2sLaneFilename = "";
96  std::string token = "";
97  while(std::getline(trackConfigStream,token,','))
98  {
99  if(token.size()<=5)
100  {
101  LOG_W("Unrecognizable token: %s", token.c_str());
102  continue;
103  }
104  if(token.compare("transform")==0)
105  {
106  transform = true;
107  }
108  else if(token.substr(token.size()-5,5).compare(".xodr")==0)
109  {
110  xodrFilename = token;
111  }
112  else
113  {
114  if(token.substr(token.size()-5,5).compare(".r2sr")==0)
115  {
116  r2sReflineFilename = token;
117  }
118  else if(token.substr(token.size()-5,5).compare(".r2sl")==0)
119  {
120  r2sLaneFilename = token;
121  }
122  else
123  {
124  LOG_W("Unrecognizable token: %s", token.c_str());
125  continue;
126  }
127  }
128 
129  }
130  /* process current file */
132  if(!xodrFilename.empty())
133  {
136  try
137  {
138  LOG_I("Processing file %s ...", xodrFilename.c_str());
139  converter.convert(xodrFilename.c_str(),&partialSet,transform);
140  LOG_I("Done.");
141  }
142  catch(...)
143  {
144  LOG_E("Could not parse file %s", xodrFilename.c_str());
145  }
146  /* add partial map to global map */
147  auto its = partialSet.getAllBorders();
148  for(;its.first!=its.second;its.first++)
149  {
150  targetSet.insert_border(its.first->second);
151  }
152  /* global map has responsibility for object/pointers */
153  partialSet.setIsOwner(false);
154  }
155  else if(!(r2sReflineFilename.empty() || r2sLaneFilename.empty()))
156  {
157  try
158  {
159  LOG_I("Processing files %s and %s ...", r2sReflineFilename.c_str(), r2sLaneFilename.c_str());
161  converter.convert(r2sReflineFilename,r2sLaneFilename, partialSet);
162  LOG_I("Done.");
163  }
164  catch(...)
165  {
166  LOG_E("Could not parse R2S files %s and %s", r2sReflineFilename.c_str(), r2sLaneFilename.c_str());
167  }
168 
169  /* add partial map to global map */
170  auto its = partialSet.getAllBorders();
171  for(;its.first!=its.second;its.first++)
172  {
173  targetSet.insert_border(its.first->second);
174  }
175  partialSet.setIsOwner(false);
176  }
177  else
178  {
179  LOG_E("Could not parse configuration: %s", trackConfig.c_str());
180  }
181  }
182  }
183 
184  public:
185  Navigation(env::AFactory* env_factory,adore::params::AFactory* params_factory, std::string trackConfigs, Config config)
186  {
187  config_ = config;
189  map_params_ = params_factory->getMapProvider();
190  nav_params_ = params_factory->getNavigation();
191  parseTrackConfigs(trackConfigs, set);
196 
197  nav_goal_reader_ = env_factory->getNavigationGoalReader(); // add to nav_management_?
199  nav_management_.addFeed(env_factory->getBorderFeed());
201  nav_writer_ = env_factory->getNavigationDataWriter();
202  }
203  virtual void run()
204  {
207  std::vector<env::BorderBased::BorderID> outdated_data;
209 
210  motion_state_reader_->getData(motion_state);
211  nav_goal_reader_->getData(goal);
212 
213  // update goal
216 
217  // determine which borders require updates
218  nav_management_.run(motion_state.getX(),motion_state.getY(),map_params_->getVisibiltyRadius(),data,outdated_data,500);
219 
220  //plotting
222  {
224  }
225 
226  // send navigation updates
227  for(auto it = data.begin(); it!=data.end(); it++)
228  {
229  auto bId = (*it)->m_id;
230  double cost = nav_management_.getBorderCost(bId);
231  nav_writer_->write(std::make_pair(bId,cost));
232  }
233  }
234 
235  };
236  }
237 }
Definition: figurestubfactory.h:25
AFigureStub * createFigureStub(int windowID)
Definition: figurestubfactory.h:45
Definition: figurestubzmq.h:30
virtual void show() override
Definition: figurestubzmq.h:266
Definition: navigation.h:32
Navigation(env::AFactory *env_factory, adore::params::AFactory *params_factory, std::string trackConfigs, Config config)
Definition: navigation.h:185
DLR_TS::PlotLab::FigureStubZMQ * figure_
Definition: navigation.h:63
Config config_
Definition: navigation.h:47
adore::mad::AReader< adore::env::VehicleMotionState9d > * motion_state_reader_
Definition: navigation.h:54
void plotGlobalNavigation()
Definition: navigation.h:65
adore::env::NavigationManagement nav_management_
Definition: navigation.h:51
params::APMapProvider * map_params_
Definition: navigation.h:49
virtual void run()
Definition: navigation.h:203
adore::mad::AReader< adore::fun::NavigationGoal > * nav_goal_reader_
Definition: navigation.h:57
void parseTrackConfigs(std::string trackConfigs, env::BorderBased::BorderSet &targetSet)
Definition: navigation.h:84
params::APNavigation * nav_params_
Definition: navigation.h:50
DLR_TS::PlotLab::FigureStubFactory * figure_factory_
Definition: navigation.h:62
adore::mad::AWriter< std::pair< adore::env::BorderBased::BorderID, double > > * nav_writer_
Definition: navigation.h:60
abstract factory for adore::env communication
Definition: afactory.h:41
virtual TNavigationGoalReader * getNavigationGoalReader()=0
virtual TVehicleMotionStateReader * getVehicleMotionStateReader()=0
virtual TNavgationDataWriter * getNavigationDataWriter()=0
virtual TBorderFeed * getBorderFeed()=0
efficiently store borders in boost R-tree
Definition: borderset.h:99
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
void setIsOwner(bool isOwner)
set whether this owns objects in pointers
Definition: borderset.h:214
Definition: navigation_management.h:30
void addFeed(adore::mad::AFeed< adore::env::BorderBased::Border > *feed)
Definition: navigation_management.h:137
void update(BorderBased::Coordinate newDestination, bool destinationValid)
Definition: navigation_management.h:163
void init(BorderBased::BorderSet *set)
Definition: navigation_management.h:152
bool goalChanged()
Definition: navigation_management.h:186
adore::env::BorderBased::BorderSet * getGlobalMap()
Definition: navigation_management.h:261
void run(double x, double y, double r, std::vector< adore::env::BorderBased::Border * > &newBorders, std::vector< adore::env::BorderBased::BorderID > &outdatedBorders, int MAX_SEND_NUMBER=40)
Definition: navigation_management.h:239
void setLaneChangePenalty(double value)
Definition: navigation_management.h:132
double getMaxCost()
Definition: navigation_management.h:142
double getBorderCost(BorderBased::BorderID id)
Definition: navigation_management.h:244
input: 2 files, reference lines and borders output: border file
Definition: r2s2borderbased.h:31
void convert(std::string referenceLineFile, std::string laneBorderFile, env::BorderBased::BorderSet &targetset)
convert to borders
Definition: r2s2borderbased.cpp:182
OpenDRIVE converter from file to object sets.
Definition: xodr2borderbased.h:43
struct adore::if_xodr::XODR2BorderBasedConverter::@1 sampling
sampling configuration object
double numberOfPointsPerBorder
Definition: xodr2borderbased.h:75
void convert(const char *filename, adore::env::BorderBased::BorderSet *target_set, adore::env::TCDSet *tcdSet, adore::env::BorderBased::LanePositionedObjectSet *stoplineSet, adore::env::BorderBased::ParkingSpotSet *parkingSpotSet, BorderIDTranslation *idTranslation, double *x0, double *y0, bool transform=false)
full conversion of OpenDRIVE map to object representations
Definition: xodr2borderbased.cpp:26
virtual void getData(T &value)=0
Definition: com_patterns.h:97
virtual void write(const T &value)=0
abstract factory for adore::params classes
Definition: afactory.h:54
virtual APNavigation * getNavigation() const =0
virtual APMapProvider * getMapProvider() const =0
abstract class containing parameters to configure aspects of the map provider
Definition: ap_map_provider.h:25
virtual int getXODRLoaderPointsPerBorder() const =0
virtual double getVisibiltyRadius() const =0
abstract class containing parameters which configure navigation behaviour
Definition: ap_navigation.h:25
virtual double getLaneChangePenalty() const =0
virtual bool getActivePlottingGlobal()=0
#define LOG_W(...)
log on warning level
Definition: csvlog.h:46
#define LOG_I(...)
log on info level
Definition: csvlog.h:37
#define LOG_E(...)
log on error level
Definition: csvlog.h:55
void plotBorderNavigation(adore::env::BorderBased::Border *right, adore::env::BorderBased::Border *left, double normedCost, DLR_TS::PlotLab::AFigureStub *figure)
Definition: plot_border.h:157
std::vector< Border * > BorderSubSet
Definition: borderset.h:92
void set(T *data, T value, int size)
Definition: adoremath.h:39
Definition: areaofeffectconverter.h:20
Definition: navigation.h:35
double trans_y_
Definition: navigation.h:36
double rot_y_
Definition: navigation.h:36
double rot_psi_
Definition: navigation.h:36
double trans_z_
Definition: navigation.h:36
double rot_z_
Definition: navigation.h:36
double rot_x_
Definition: navigation.h:36
Config()
Definition: navigation.h:37
double trans_x_
Definition: navigation.h:36
The border struct contains data of the smallest.
Definition: border.h:62
This struct represents 3-dimensional coordines.
Definition: coordinate.h:34
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
Definition: navigationgoal.h:26
adore::mad::GlobalPosition target_
Definition: navigationgoal.h:28
double z_
Definition: globalposition.h:23
double x_
Definition: globalposition.h:23
double y_
Definition: globalposition.h:23