ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
precedence.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 and API
13  ********************************************************************************/
14 
15 
16 #pragma once
17 #include <adore/mad/adoremath.h>
18 #include <adore/mad/com_patterns.h>
19 #include <boost/geometry.hpp>
20 #include <boost/geometry/geometries/point.hpp>
21 #include <boost/geometry/geometries/box.hpp>
22 #include <string>
23 #include <iostream>
24 #include <fstream>
25 
26 namespace adore
27 {
28  namespace env
29  {
35  {
36  public:
37  typedef boost::geometry::model::point<double,3,boost::geometry::cs::cartesian> boost_point;
38  typedef boost::geometry::model::box<boost_point> boost_box;
39  adoreMatrix<double,3,1> from_;
40  adoreMatrix<double,3,1> to_;
46  {
47  set(0.0,0.0,0.0,0.0,0.0,0.0);
48  coordinates_in_UTM_ = true;
49  }
53  void set(double f1,double f2,double f3,double t1,double t2,double t3)
54  {
55  from_(0)=f1;
56  from_(1)=f2;
57  from_(2)=f3;
58  to_(0)=t1;
59  to_(1)=t2;
60  to_(2)=t3;
61  }
66  void set(const std::string& s, bool coordinates_in_UTM=true)
67  {
68  std::stringstream is;
69  double f1,f2,f3,t1,t2,t3;
70  char sep;
71  is<<s;
72  is>> f1 >> sep >> f2 >> sep >> f3 >> sep >> t1 >> sep >> t2 >> sep >> t3;
73  set(f1,f2,f3,t1,t2,t3);
74  coordinates_in_UTM_ = coordinates_in_UTM;
75  std::cout<<s<<std::endl;
76  }
81  {
82  const double min0 = (std::min)(from_(0),to_(0));
83  const double min1 = (std::min)(from_(1),to_(1));
84  const double min2 = (std::min)(from_(2),to_(2));
85  const double max0 = (std::max)(from_(0),to_(0));
86  const double max1 = (std::max)(from_(1),to_(1));
87  const double max2 = (std::max)(from_(2),to_(2));
88  return boost_box(boost_point(min0,min1,min2),boost_point(max0,max1,max2));
89  }
93  bool equals(const PriorityRoute& other)const
94  {
95  return this->from_(0)==other.from_(0)
96  && this->from_(1)==other.from_(1)
97  && this->from_(2)==other.from_(2)
98  && this->to_(0)==other.to_(0)
99  && this->to_(1)==other.to_(1)
100  && this->to_(2)==other.to_(2);
101  }
102  };
108  {
109  public:
112  bool unary_;
115 
122  void set(const std::string& s,bool coordinates_in_UTM=true)
123  {
124  std::size_t pos = s.find(">");
125  if(pos!=std::string::npos)
126  {
127  high_.set(s.substr(0,pos),coordinates_in_UTM);
128  low_.set(s.substr(pos+1,s.length()-pos-1),coordinates_in_UTM);
129  unary_=false;
130  }
131  else
132  {
133  pos = s.find("<");
134  if(pos!=std::string::npos)
135  {
136  low_.set(s.substr(0,pos),coordinates_in_UTM);
137  high_.set(s.substr(pos+1,s.length()-pos-1),coordinates_in_UTM);
138  unary_=false;
139  }
140  else
141  {
142  high_.set(s,coordinates_in_UTM);
143  unary_=true;
144  }
145 
146  }
147  }
148  bool equals(const PrecedenceRule& other)const
149  {
150  if(this->unary_!=other.unary_)return false;
151  if(!this->high_.equals(other.high_))return false;
152  if(this->unary_ && !this->low_.equals(other.low_))return false;
153  return true;
154  }
155 
157  {
158  if(unary_)return high_.getBoostBox();
159  auto l = low_.getBoostBox();
160  auto h = high_.getBoostBox();
161  l.min_corner().get<0>();
162  const double min0 = (std::min)(l.min_corner().get<0>(),h.min_corner().get<0>());
163  const double min1 = (std::min)(l.min_corner().get<1>(),h.min_corner().get<1>());
164  const double min2 = (std::min)(l.min_corner().get<2>(),h.min_corner().get<2>());
165  const double max0 = (std::max)(l.max_corner().get<0>(),h.max_corner().get<0>());
166  const double max1 = (std::max)(l.max_corner().get<1>(),h.max_corner().get<1>());
167  const double max2 = (std::max)(l.max_corner().get<2>(),h.max_corner().get<2>());
169  PriorityRoute::boost_point(min0,min1,min2),
170  PriorityRoute::boost_point(max0,max1,max2)
171  );
172  }
173  };
174 
179  {
180  private:
182 
183  public:
186  {
187  ruleFeed_ = ruleFeed;
188  }
189  void update(double radius, double x, double y)
190  {
191  while(ruleFeed_->hasNext())
192  {
194  ruleFeed_->getNext(r);
195  insertRule(r);
196  }
197  refocus(x-radius,y-radius,x+radius,y+radius);
198  }
205  template<typename value_type,typename Tfirst>
206  struct my_equal
207  {
208  typedef bool result_type;
209  result_type operator() (value_type const& v1, value_type const& v2) const
210  {
211  return boost::geometry::equals<Tfirst,Tfirst>(v1.first, v2.first) && v1.second == v2.second;
212  }
213  };
214  typedef std::pair<PriorityRoute::boost_box,PrecedenceRule*> idxRegion2Precedence;
215  typedef boost::geometry::index::rtree< idxRegion2Precedence,
216  boost::geometry::index::quadratic<16>,
217  boost::geometry::index::indexable<idxRegion2Precedence>,
220  template<typename T1, typename T2>
221  struct itpair
222  {
223  T1 first;
224  T2 second;
226  T1& current(){return first;}
227  T2& end(){return second;}
228  };
230 
231  private:
233  public:
234 
238  bool readFile(const std::string& filename)
239  {
240  std::string line;
241  std::ifstream file(filename);
242  if(!file.is_open())
243  {
244  std::cout<<"failed to open file\n";
245  return false;
246  }
247  while(std::getline(file,line))
248  {
249  if (line.empty() || line.at(0) == '#')
250  {
251  continue;
252  }
253  auto rule = parseRule(line);//@TODO: parse coordinate format from file
254  std::cout<<precedenceRT_.size()<<": ";
255  std::cout<<
256  rule->high_.from_(0)<<","<<
257  rule->high_.from_(1)<<","<<
258  rule->high_.from_(2)<<";"<<
259  rule->high_.to_(0)<<","<<
260  rule->high_.to_(1)<<","<<
261  rule->high_.to_(2);
262  if(!rule->unary_)
263  {
264  std::cout<<
265  " > "<<
266  rule->low_.from_(0)<<","<<
267  rule->low_.from_(1)<<","<<
268  rule->low_.from_(2)<<";"<<
269  rule->low_.to_(0)<<","<<
270  rule->low_.to_(1)<<","<<
271  rule->low_.to_(2);
272  }
273  std::cout<<"\n";
274  }
275  return true;
276  }
277 
281  PrecedenceRule* parseRule(const std::string& s,bool use_UTM_coordinates = true)
282  {
283  PrecedenceRule* myrule = new PrecedenceRule();
284  myrule->set(s,use_UTM_coordinates);
285  precedenceRT_.insert(std::make_pair(myrule->getBoostBox(),myrule));
286  return myrule;
287  }
291  void insertRule(const PrecedenceRule& rule)
292  {
293  PrecedenceRule* myrule = new PrecedenceRule();
294  *myrule=rule;
295  precedenceRT_.insert(std::make_pair(myrule->getBoostBox(),myrule));
296  }
300  void init(PrecedenceSet* other)
301  {
302  std::cout<<"copying...";
303  for(auto const& pair : other->precedenceRT_)
304  {
305  std::cout<<"+";
306  auto rule = pair.second;
307  insertRule(*rule);
308  }
309  }
313  bool contains(const PrecedenceRule& rule)
314  {
315  for(auto it = precedenceRT_.qbegin(boost::geometry::index::intersects(
318  rule.high_.from_(0),
319  rule.high_.from_(1),
320  rule.high_.from_(2)
321  ),
323  rule.high_.from_(0),
324  rule.high_.from_(1),
325  rule.high_.from_(2)
326  ))));
327  it!=precedenceRT_.qend();
328  it++)
329  {
330  if(it->second->equals(rule))return true;
331  }
332  return false;
333  }
338  {
339  precedenceRT_.remove(std::make_pair(rule->getBoostBox(),rule));
340  delete rule;
341  }
345  itRegion2PrecedenceRT getRulesInRegion(double x0,double y0,double x1,double y1) const
346  {
347  static const double guard = 1.0e99;
348  auto it = precedenceRT_.qbegin(boost::geometry::index::intersects(
352  )
353  ));
354  return itRegion2PrecedenceRT(it,precedenceRT_.qend());
355  }
360  {
361  static const double guard = 1.0e99;
362  auto it = precedenceRT_.qbegin(boost::geometry::index::intersects(
364  PriorityRoute::boost_point(-guard,-guard,-guard),
365  PriorityRoute::boost_point(+guard,+guard,+guard)
366  )
367  ));
368  return itRegion2PrecedenceRT(it,precedenceRT_.qend());
369  }
370 
375  {
376  return &precedenceRT_;
377  }
381  itRegion2PrecedenceRT getRulesOutsideRegion(double x0,double y0,double x1,double y1)
382  {
383  static const double guard = 1.0e99;
384  auto it = precedenceRT_.qbegin(boost::geometry::index::disjoint(
388  )
389  ));
390  return itRegion2PrecedenceRT(it,precedenceRT_.qend());
391  }
395  void refocus(double x0,double y0,double x1,double y1)
396  {
397  std::vector<PrecedenceRule*> removeset;
398  for(auto it = getRulesOutsideRegion(x0,y0,x1,y1);it.current()!=it.end();it.current()++)
399  {
400  removeset.push_back(it.current()->second);
401  }
402  for(auto it: removeset)eraseRule(it);
403  }
404 
405 
406  };
407  }
408 }
PrecedenceSet contains PrecedenceRules, indexed by the area they affect.
Definition: precedence.h:179
std::pair< PriorityRoute::boost_box, PrecedenceRule * > idxRegion2Precedence
Definition: precedence.h:214
Region2PrecedenceRT * getAllRules()
returns all rules
Definition: precedence.h:374
void eraseRule(PrecedenceRule *rule)
removes a rule from the container
Definition: precedence.h:337
adore::mad::AFeed< PrecedenceRule > * ruleFeed_
Definition: precedence.h:181
PrecedenceSet()
Definition: precedence.h:184
void update(double radius, double x, double y)
Definition: precedence.h:189
itRegion2PrecedenceRT getRulesOutsideRegion(double x0, double y0, double x1, double y1)
returns a subset of rules outside of a region
Definition: precedence.h:381
bool contains(const PrecedenceRule &rule)
check whether a rule is contained
Definition: precedence.h:313
void refocus(double x0, double y0, double x1, double y1)
removes all rules outside of a region
Definition: precedence.h:395
itRegion2PrecedenceRT getRulesInRegion(double x0, double y0, double x1, double y1) const
returns a subset of rules in a region
Definition: precedence.h:345
itRegion2PrecedenceRT getAllRulesIt() const
returns all rules
Definition: precedence.h:359
void insertRule(const PrecedenceRule &rule)
inserts a copy of the given rule into container
Definition: precedence.h:291
Region2PrecedenceRT precedenceRT_
Definition: precedence.h:232
itpair< Region2PrecedenceRT::const_query_iterator, Region2PrecedenceRT::const_query_iterator > itRegion2PrecedenceRT
Definition: precedence.h:229
void init(PrecedenceSet *other)
initialize by copying entries
Definition: precedence.h:300
bool readFile(const std::string &filename)
reads a set of precedence rules from a file
Definition: precedence.h:238
boost::geometry::index::rtree< idxRegion2Precedence, boost::geometry::index::quadratic< 16 >, boost::geometry::index::indexable< idxRegion2Precedence >, my_equal< idxRegion2Precedence, PriorityRoute::boost_box > > Region2PrecedenceRT
Definition: precedence.h:219
PrecedenceRule * parseRule(const std::string &s, bool use_UTM_coordinates=true)
creates a precedence rule from string and adds it to container.
Definition: precedence.h:281
PrecedenceSet(adore::mad::AFeed< PrecedenceRule > *ruleFeed)
Definition: precedence.h:185
Definition: com_patterns.h:29
T min(T a, T b, T c, T d)
Definition: adoremath.h:663
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
x0
Definition: adore_set_goal.py:25
x
Definition: adore_set_goal.py:30
y0
Definition: adore_set_goal.py:26
y
Definition: adore_set_goal.py:31
y1
Definition: adore_set_pose.py:29
x1
Definition: adore_set_pose.py:28
r
Definition: adore_suppress_lanechanges.py:209
string file
Definition: adore_suppress_lanechanges.py:181
Definition: areaofeffectconverter.h:20
The PrecedenceRule defines a precedence relationship between two routes. Vehicles on the low_ priorit...
Definition: precedence.h:108
PrecedenceRule()
Definition: precedence.h:114
bool equals(const PrecedenceRule &other) const
Definition: precedence.h:148
bool unary_
Definition: precedence.h:112
void set(const std::string &s, bool coordinates_in_UTM=true)
reads PrecedenceRule's two PriorityRoutes from the input string exemplary valid string "0....
Definition: precedence.h:122
PriorityRoute low_
Definition: precedence.h:110
PriorityRoute high_
Definition: precedence.h:111
PriorityRoute::boost_box getBoostBox()
Definition: precedence.h:156
Definition: precedence.h:222
itpair(T1 first, T2 second)
Definition: precedence.h:225
T1 first
Definition: precedence.h:223
T2 second
Definition: precedence.h:224
T1 & current()
Definition: precedence.h:226
T2 & end()
Definition: precedence.h:227
custom equal test for iterators
Definition: precedence.h:207
result_type operator()(value_type const &v1, value_type const &v2) const
Definition: precedence.h:209
bool result_type
Definition: precedence.h:208
PriorityRoute implicitly references a route between two coordinates. The coordinates should be chosen...
Definition: precedence.h:35
bool equals(const PriorityRoute &other) const
returns true if all values are equal with other PriorityRoute
Definition: precedence.h:93
adoreMatrix< double, 3, 1 > to_
Definition: precedence.h:40
boost::geometry::model::point< double, 3, boost::geometry::cs::cartesian > boost_point
Definition: precedence.h:37
adoreMatrix< double, 3, 1 > from_
Definition: precedence.h:39
bool coordinates_in_UTM_
Definition: precedence.h:41
void set(double f1, double f2, double f3, double t1, double t2, double t3)
set the two coordinates
Definition: precedence.h:53
void set(const std::string &s, bool coordinates_in_UTM=true)
reads PriorityRoute's two coordinates from a string exemplary valid string "0.0,3....
Definition: precedence.h:66
PriorityRoute()
empty constructor sets coordinates to 0.0^3
Definition: precedence.h:45
boost_box getBoostBox()
returns a box in boost format, which encompasses start/end points
Definition: precedence.h:80
boost::geometry::model::box< boost_point > boost_box
Definition: precedence.h:38