ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
plainxmlexporter.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 API and implementation
13  ********************************************************************************/
14 #pragma once
15 #include "if_plotlab/plot_border.h"
19 #include <adore/mad/centerline.h>
20 #include <string>
21 #include <vector>
22 #include <list>
23 #include <unordered_map>
24 #include <unordered_set>
25 #include <iostream>
26 #include <fstream>
27 
28 namespace adore
29 {
30 namespace apps
31 {
32 
39 {
40  private:
45  double min_length_start_;//minimum length of a start edge. a start edge is an edge which has no predecessor
46  double maxDistMeter_;
47  double maxDistRel_;
48  double min_length_;//minimum length for an edge
49  double max_length_;//maximum length for an edge before splitting into subcomponents
50  bool lateral_aggregate_;//determine whether all borders adjacent to the left are aggregated into one edge
53  bool use_lane_shape_;//if true, write shape (=geometry) for each lane
54 
55  private:
58 
59  struct ConnectionStep;
60  struct Node
61  {
62  int id_;
63  std::unordered_set<Border*> incoming_edge_; //baseline borders ending at node
64  std::unordered_set<Border*> outgoing_edge_; //baseline borders starting at node
65  std::unordered_set<ConnectionStep*> start_connections_; //connection elements, which start at node
66  std::unordered_set<ConnectionStep*> end_connections_; //connection elements, which end at node
67  adoreMatrix<double,3,1> position_; //average of all start/end positions
68  };
69  struct Edge
70  {
71  std::vector<Border*> represents_;//borders represented by baseline, represents_[0] is the baseline border
72  int number_of_lanes_;//number of lanes
73  double lane_width_;//average width of lanes
74  std::vector<int> ids_;//for splitting of edges, ids of first to last sub-edge are stored in this list
75  std::vector<int> split_node_ids_;//for splitting of edges, ids of interim nodes
76  std::vector<double> s_;//s values of baseline for intervals, given in percentage of border length
77  };
78  struct Connection
79  {
80  Border* from_;//baseline border
81  Border* to_;//baseline border
82  std::vector<std::pair<int,int>> from_lane_to_lane_; //lane number pairs
83  };
84 
85  struct ConnectionStep;
87  {
88  Border* connection_border_; //the border used by the step
89  Border* edge_border0_; //if connection step starts at junction fringe => edge_border0_!=0
90  Border* edge_border1_; //if connection step ends at junction fringe => edge_border1_!=0
91  ConnectionStep* predecessor_;//if connection step does not start at junction fringe => predecessor!=0
92  int depth_;
93  };
94 
95  std::unordered_set<Node*> nodes_;
96  std::unordered_map<Border*,Edge> edges_;//all edges: baseline + a number of dependent borders
97  std::unordered_map<Border*,Node*> leads_to_;//baseline border leads to node id
98  std::unordered_map<Border*,Node*> originates_at_;//baseline border originates at node id
99  std::unordered_map<Border*,Border*> represented_by_;//a border belongs to a baseline border
100  std::list<ConnectionStep*> open_connections_;//the open set for connection search
101  std::vector<ConnectionStep*> closed_connections_;//the closed set for connection search
102  std::unordered_map<std::string,std::set<Node*>> merge_nodes_;//the nodes which shall be merged into one intersection, according to xodr connecting road information and junction id
103  std::unordered_map<Node*,int> node_clusters_;//maps from node to its cluster id
104  int cluster_count = 0;//id of the next cluster
105  std::unordered_set<Edge*> deleted_edges_;//edges, which were filtered out and deleted due to some sort of deficiency
106 
107  private:
109  public:
111  {
112  figure_factory_ = 0;
113  figure_ = 0;
114  enable_plotting_ = false;
115  min_length_start_ = 1.0;
116  min_length_ = 0.1;
117  max_length_ = 5.0;
118  maxDistMeter_ = 1.0;
119  maxDistRel_ = 0.2;
120  lateral_aggregate_ = true;
121  use_constant_width_ = true;
122  constant_width_ = 3.0;
123  use_lane_shape_ = true;
124  }
125  void run(int argc,char **argv)
126  {
127  std::cout<<"PlainXMLExporter------------------------------------------------------------------"<<std::endl;
128  std::cout<<"usage: PlainXMLExporter [plot] infile1[,transform] [infile2[,transform]] ... outfile"<<std::endl;
129  std::cout<<"* will create files outfile.nod.xml, outfile.edg.xml and outfile.con.xml"<<std::endl;
130  std::cout<<"* will plot with plotlab, if first argument [plot] is given."<<std::endl;
131  std::cout<<"Create net.xml for SUMO as follows:"<<std::endl;
132  std::cout<<"./netconvert --node-files=[outfile].nod.xml --edge-files=[outfile].edg.xml --connection-files=[outfile].con.xml --output-file=[outfile].net.xml -R --geometry.remove.width-tolerance 1 --junctions.join --junctions.join-dist 10 -l [outfile].txt "<<std::endl;
133  std::cout<<"----------------------------------------------------------------------------------"<<std::endl;
134 
135  std::string output_prefix(argv[argc-1]);
136 
137  for(int i=1;i<argc-1;i++)
138  {
139  std::string trackConfig(argv[i]);
140 
141  if(std::strcmp(trackConfig.c_str(),"plot")==0)
142  {
143  enable_plotting_ = true;
144  continue;
145  }
146 
147  /* reading of single track configuration, comma separated */
148  bool transform = false;
149  std::string filename = "";
150  {
151  std::string token = "";
152  std::stringstream trackConfigStream(trackConfig);
153  while(std::getline(trackConfigStream,token,','))
154  {
155  if(token.compare("transform")==0)
156  {
157  transform = true;
158  }
159  else
160  {
161  filename = token;
162  }
163  }
164  }
165 
166  std::cout<<"loading track: "<<filename.c_str()<<", "<<(transform?"with":"without")<<" transformation"<<std::endl;
168  if(filename.substr(filename.length()-4,std::string::npos).compare("xodr")==0)
169  {
170  std::cout<<"OpenDrive loader..."<<std::endl;
171  try
172  {
174  converter.convert(filename.c_str(),&partialSet,transform,&idTranslation_);
175  }
176  catch(const std::exception& e)
177  {
178  std::cout<<"Could not parse file"<<std::endl;
179  std::cout << e.what() << '\n';
180  return;
181  }
182  }
183  else if(filename.substr(filename.length()-3,std::string::npos).compare("r2s")==0)
184  {
185  std::cout<<"Road2Simulation simple feature loader..."<<std::endl;
186  try
187  {
189  converter.convert(filename+"r",filename+"l", partialSet);
190  }
191  catch(const std::exception& e)
192  {
193  std::cout<<"Could not parse file"<<std::endl;
194  std::cout << e.what() << '\n';
195  return;
196  }
197 
198  }
199  else
200  {
201  std::cout<<"unknown file ending: "<<filename<<std::endl;
202  }
203  /* add partial map to global map */
204  auto its = partialSet.getAllBorders();
205  for(;its.first!=its.second;its.first++)
206  {
207  globalSet_.insert_border(its.first->second);
208  }
209  partialSet.setIsOwner(false);
210 
211 
212 
213  }
214 
215  std::cout<<"...successfully loaded all files"<<std::endl;
216  if(enable_plotting_)
217  {
220  figure_->show();
222  }
223 
224  std::cout<<"converting to node/edge format..."<<std::endl;
225 
226  std::unordered_set<BorderType::TYPE> valid_types;
227  valid_types.emplace(BorderType::DRIVING);
228 
229  //create edges: find all baseline borders in globalSet
230  std::cout<<"creating edges"<<std::endl;
231  for(auto it=globalSet_.getAllBorders();it.first!=it.second;it.first++)
232  {
233  Border* border = it.first->second;
236 
237  //reasons for exclusion of border
238  if(represented_by_.find(border)!=represented_by_.end())continue;//already represented
239  if(valid_types.find(border->m_type)==valid_types.end())continue;//not a valid type
240  if(lateral_aggregate_ && right!=0 && right->m_type==border->m_type)continue;//not a baseline border
241  if(left==0)continue;//this border is a wierd singleton
242  if(idTranslation_.isInJunction(border->m_id))continue;//inside junction: use as connection
243  if(border->getLength()<min_length_)continue;//border is too short
244 
245  //this is a valid baseline border: create edge
246  Edge e;
247  left = border;
248  if(lateral_aggregate_)
249  {
250  do
251  {
252  Border* nextleft = globalSet_.getLeftNeighbor(left);
253  if(nextleft!=0 && left->m_type==border->m_type)
254  {
255  e.represents_.push_back(left);
256  represented_by_.emplace(left,border);
257  left = nextleft;
258  }
259  else
260  {
261  left = 0;
262  }
263  }while(left!=0);
264  }
265  else
266  {
267  //border only represents itself
268  e.represents_.push_back(border);
269  represented_by_.emplace(border,border);
270  }
271 
272  e.number_of_lanes_ = e.represents_.size();
273  double dx = e.represents_[0]->m_id.m_first.m_X - e.represents_[e.number_of_lanes_-1]->m_left->m_first.m_X;
274  double dy = e.represents_[0]->m_id.m_first.m_Y - e.represents_[e.number_of_lanes_-1]->m_left->m_first.m_Y;
275  e.lane_width_ = std::sqrt(dx*dx+dy*dy)/(double)e.number_of_lanes_;
276  edges_.emplace(border,e);
277  }
278 
279  //create atomic from-to-links
280  std::cout<<"creating atomic nodes"<<std::endl;
281  for(auto it=globalSet_.getAllBorders();it.first!=it.second;it.first++)
282  {
283  Border* border = it.first->second;
284 
285  //reasons for exclusion of border
286  if(represented_by_.find(border)==represented_by_.end())continue;//previously not selected
287 
288  for(auto successors = globalSet_.getSuccessors(border);
289  successors.current()!=successors.end();
290  successors.current()++)
291  {
292  Border* next = successors.current()->second;
294  {
295  //resons for exclusion of next
296  if(represented_by_.find(next)==represented_by_.end()
297  && edges_.find(next)==edges_.end())continue;//previously not selected
298 
299  //there is a link here
300  Node* n = new Node();
301  n->incoming_edge_.emplace(border);
302  n->outgoing_edge_.emplace(next);
303  leads_to_.emplace(border,n);
304  originates_at_.emplace(next,n);
305  nodes_.emplace(n);
306  }
307  }
308  }
309 
310 
311  //merge nodes, which share a baseline link
312  std::cout<<"aggregating nodes"<<std::endl;
313  bool changed = true;
314  std::unordered_set<Node*> node_done_;
315  while(changed)
316  {
317  changed = false;
318  std::unordered_set<Node*> equivalent_nodes;
319  for(auto it = nodes_.begin();it!=nodes_.end();it++)
320  {
321  Node* n = *it;
322  if(node_done_.find(n)==node_done_.end())
323  {
324  node_done_.emplace(n);
325  //incoming nodes
326  for(auto it2 = n->incoming_edge_.begin();it2!=n->incoming_edge_.end();it2++)
327  {
328  Border* border = *it2;
329  Border* baseline = represented_by_[border];
330  auto it3 = leads_to_.find(baseline);
331  if(it3!=leads_to_.end() && it3->second!=n)
332  {
333  equivalent_nodes.emplace(it3->second);
334  }
335  }
336  //outgoing nodes
337  for(auto it2 = n->outgoing_edge_.begin();it2!=n->outgoing_edge_.end();it2++)
338  {
339  Border* border = *it2;
340  Border* baseline = represented_by_[border];
341  auto it3 = originates_at_.find(baseline);
342  if(it3!=originates_at_.end() && it3->second!=n)
343  {
344  equivalent_nodes.emplace(it3->second);
345  }
346  }
347  //if there are equivalent nodes, process those
348  for(auto it2 = equivalent_nodes.begin();it2!=equivalent_nodes.end();it2++)
349  {
350  Node* m = *it2;
351  //copy incoming and outgoing borders
352  for(auto it3 = m->incoming_edge_.begin();it3!=m->incoming_edge_.end();it3++)
353  {
354  n->incoming_edge_.emplace(*it3);
355  leads_to_[*it3] = n;//replace m with n
356  }
357  for(auto it3 = m->outgoing_edge_.begin();it3!=m->outgoing_edge_.end();it3++)
358  {
359  n->outgoing_edge_.emplace(*it3);
360  originates_at_[*it3] = n;//replace m with n
361  }
362  delete m;
363  }
364  if(equivalent_nodes.size()>0)
365  {
366  changed = true;
367  break;
368  }
369  }
370  }
371  for(auto it=equivalent_nodes.begin();it!=equivalent_nodes.end();it++)
372  {
373  nodes_.erase(*it);//remove the equivalent nodes from the nodes_ set
374  }
375  }
376 
377  //create dead-end nodes for edges, which have no start/end
378  for(auto it = edges_.begin();it!=edges_.end();it++)
379  {
380  Border* baseline = it->first;
381  Edge* e = &it->second;
382  if(originates_at_.find(baseline)==originates_at_.end())
383  {
384  Node* n = new Node();
385  nodes_.emplace(n);
386  for(auto it2 = e->represents_.begin();it2!=e->represents_.end();it2++)
387  {
388  n->outgoing_edge_.emplace(*it2);
389  originates_at_.emplace(*it2,n);
390  }
391  }
392  if(leads_to_.find(baseline)==leads_to_.end())
393  {
394  Node* n = new Node();
395  nodes_.emplace(n);
396  for(auto it2 = e->represents_.begin();it2!=e->represents_.end();it2++)
397  {
398  n->incoming_edge_.emplace(*it2);
399  leads_to_.emplace(*it2,n);
400  }
401  }
402  }
403 
404  //find connections
405  for(auto it=globalSet_.getAllBorders();it.first!=it.second;it.first++)
406  {
407  Border* border = it.first->second;
409 
410  //reasons for exclusion of border
411  if(represented_by_.find(border)!=represented_by_.end())continue;//already represented
412  if(valid_types.find(border->m_type)==valid_types.end())continue;//not a valid type
413  if(left==0)continue;//this border is a wierd singleton
414  if(!idTranslation_.isInJunction(border->m_id))continue;//inside junction: use as connection
415  std::string junctionID = idTranslation_.getJunctionID(border->m_id);
416 
417  //walk through predecessors and see whether the junction border can be connected to a regular border
418  for(auto it2 = globalSet_.getPredecessors(border);it2.current()!=it2.end();it2.current()++)
419  {
420  Border* pre = it2.current()->second;
421  if(represented_by_.find(pre)==represented_by_.end())continue;
422  if(!pre->isContinuousPredecessorOf(border,maxDistMeter_,maxDistRel_))continue;
423  //junction border has a known edge as predecessor
424  ConnectionStep* c = new ConnectionStep();
425  c->connection_border_ = border;
426  c->edge_border0_ = pre;
427  c->edge_border1_ = 0;
428  c->predecessor_ = 0;
429  c->depth_ = 0;
430  open_connections_.push_back(c);
431 
432  //remember junctionid for node merging
433  Node* n = leads_to_[pre];
434  if(merge_nodes_.find(junctionID)==merge_nodes_.end())
435  {
436  std::set<Node*> empty_set;
437  merge_nodes_.emplace(junctionID,empty_set);
438  }
439  merge_nodes_[junctionID].emplace(n);
440  }
441  }
442 
443  //breadth-first search for connections
444  int maxDepth = 15;
445  while(open_connections_.size()>0)
446  {
447  ConnectionStep* c = open_connections_.front();
448  open_connections_.pop_front();
449  closed_connections_.push_back(c);
450  std::vector<ConnectionStep*> children;
451  //std::cout<<"open list size: "<<open_connections_.size()<<", depth="<<c->depth_<<std::endl;
452 
453  //search for connected edges
454  for(auto it = globalSet_.getSuccessors(c->connection_border_);it.current()!=it.end();it.current()++)
455  {
456  Border* next = it.current()->second;
458  {
459  if(represented_by_.find(next)!=represented_by_.end())
460  {
461  //found a goal
462  c->edge_border1_ = next;
463  //std::cout<<"found a goal"<<std::endl;
464 
465  //connect start and end node, if they are not already in the same cluster
466  Node* n1 = originates_at_[represented_by_[next]];
467  ConnectionStep* c0 = c;
468  while(c0->predecessor_!=0)c0=c0->predecessor_;
470  n0->start_connections_.emplace(c0);
471  n1->end_connections_.emplace(c);
472 
473  auto it0 = node_clusters_.find(n0);
474  auto it1 = node_clusters_.find(n1);
475  if(it0==node_clusters_.end() && it1==node_clusters_.end())
476  {
477  node_clusters_.emplace(n0,cluster_count);
478  node_clusters_.emplace(n1,cluster_count);
479  cluster_count ++;
480  }
481  else if(it0==node_clusters_.end())
482  {
483  node_clusters_.emplace(n0,it1->second);
484  }
485  else if(it1==node_clusters_.end())
486  {
487  node_clusters_.emplace(n1,it0->second);
488  }
489  else
490  {
491  int replace = it1->second;
492  int by = it0->second;
493  for(auto it2=node_clusters_.begin();it2!=node_clusters_.end();it2++)
494  {
495  if(it2->second==replace)node_clusters_[it2->first]=by;
496  }
497  }
498  break;
499  }
500  else if(c->depth_<maxDepth)
501  {
502  //found a child
503  ConnectionStep* cc = new ConnectionStep();
504  cc->connection_border_ = next;
505  cc->edge_border0_ = 0;
506  cc->edge_border1_ = 0;
507  cc->predecessor_ = c;
508  cc->depth_ = c->depth_ + 1;
509  children.push_back(cc);
510  }
511  }
512  }
513 
514  if(c->edge_border1_==0)
515  {
516  for(auto cc:children)
517  {
518  open_connections_.push_back(cc);
519  }
520  }
521  else
522  {
523  for(auto cc:children)
524  {
525  delete cc;
526  }
527  }
528  }
529 
530  //cut edges which are too long
531  int edgeid = 0;//start counting edge ids here
532  int nodeid = 0;//start counting node ids here: intermediate nodes in edges
533  for(auto it = edges_.begin();it!=edges_.end();it++)
534  {
535  Edge* e = &it->second;
536  Border* b = it->first;
537  e->ids_.push_back(edgeid++);
538  e->s_.push_back(0.0);
539  for(double s = max_length_;s + max_length_<=b->getLength();s+=max_length_)
540  {
541  e->ids_.push_back(edgeid++);
542  e->s_.push_back(s/b->getLength());
543  e->split_node_ids_.push_back(nodeid++);
544  }
545  e->s_.push_back(1.0);
546  }
547 
548 
549  //write nodes file
550  std::cout<<"Writing to file "<<output_prefix<<".nod.xml ..."<<std::endl;
551  try
552  {
553  std::ofstream nodfile;
554  nodfile.open (output_prefix + ".nod.xml");
555  nodfile.setf(std::ios_base::fixed, std::ios_base::floatfield);
556  nodfile.precision(2);
557  nodfile << "<nodes>"<<std::endl;
558  for(auto it = nodes_.begin();it!=nodes_.end();it++)
559  {
560  Node* n = *it;
561  if(node_clusters_.find(n)==node_clusters_.end())//=>independet node
562  {
563  n->id_ = nodeid++;
564  double x = 0.0;
565  double y = 0.0;
566  int count = 0;
567  //compute center of incoming/outgoing
568  for(auto it2 = n->incoming_edge_.begin();it2!=n->incoming_edge_.end();it2++)
569  {
570  Border* b = *it2;
571  x+=b->m_id.m_last.m_X;
572  y+=b->m_id.m_last.m_Y;
573  count++;
574  }
575  for(auto it2 = n->outgoing_edge_.begin();it2!=n->outgoing_edge_.end();it2++)
576  {
577  Border* b = *it2;
578  x+=b->m_id.m_first.m_X;
579  y+=b->m_id.m_first.m_Y;
580  count++;
581  }
582  nodfile << "\t<node id=\""<<n->id_<<"\" "
583  << "x=\""<<x/(double)count<<"\" "
584  << "y=\""<<y/(double)count<<"\" "
585  << "type=\"priority\" />"
586  << std::endl;
587  }
588  }
589  nodfile << std::endl;
590 
591  //write the node clusters
592  std::cout<<"writing "<<cluster_count<<" joined nodes"<<std::endl;
593  for(int i=0;i<cluster_count;i++)
594  {
595  int nodes_in_cluster = 0;
596  double x = 0.0;
597  double y = 0.0;
598  int connectors_in_cluster = 0;
599  nodeid++;
600 
601  for(auto it = node_clusters_.begin();it!=node_clusters_.end();it++)
602  {
603  if(it->second==i)
604  {
605  Node* n = it->first;
606  nodes_in_cluster ++;
607  n->id_ = nodeid;
608 
609  //compute center of incoming/outgoing
610  for(auto it2 = n->incoming_edge_.begin();it2!=n->incoming_edge_.end();it2++)
611  {
612  Border* b = *it2;
613  x+=b->m_id.m_last.m_X;
614  y+=b->m_id.m_last.m_Y;
615  connectors_in_cluster++;
616  }
617 
618  for(auto it2 = n->outgoing_edge_.begin();it2!=n->outgoing_edge_.end();it2++)
619  {
620  Border* b = *it2;
621  x+=b->m_id.m_first.m_X;
622  y+=b->m_id.m_first.m_Y;
623  connectors_in_cluster++;
624  }
625  }
626  }
627  if(nodes_in_cluster>0)
628  {
629  nodfile << "\t<node id=\""<<nodeid<<"\" "
630  << "x=\""<<x/(double)connectors_in_cluster<<"\" "
631  << "y=\""<<y/(double)connectors_in_cluster<<"\" "
632  << "type=\"priority\" />"
633  << std::endl;
634  }
635  }
636 
637  //write the subnodes of all edges
638  for(auto it=edges_.begin();it!=edges_.end();it++)
639  {
640  Edge* e = &it->second;
641  Border* baseline = it->first;
642  for(int i=0;i<e->split_node_ids_.size();i++)
643  {
644  double seval = e->s_[i+1]*baseline->getLength()+baseline->m_path->limitLo();
645  // double seval = adore::mad::bound(baseline->m_path->limitLo(),e->s_[i+1]+baseline->m_path->limitLo(),baseline->m_path->limitHi());
646  auto p = baseline->m_path->f(seval);
647  int id = e->split_node_ids_[i];
648  nodfile << "\t<node id=\""<<id<<"\" "
649  << "x=\""<< (p(0)) <<"\" "
650  << "y=\""<< (p(1)) <<"\" "
651  << "type=\"priority\" />"
652  << std::endl;
653  }
654  }
655 
656  nodfile << "</nodes>"<<std::endl;
657  nodfile.close();
658  std::cout<<"done"<<std::endl;
659  }
660  catch(const std::exception& e)
661  {
662  std::cout << e.what() << '\n';
663  std::cout<<"Failed to write to file."<<std::endl;
664  }
665 
666  // write the .edg.xml file
667  std::cout<<"Writing to file "<<output_prefix<<".edg.xml ..."<<std::endl;
668  try
669  {
671  std::ofstream edgfile;
672  edgfile.open (output_prefix + ".edg.xml");
673  edgfile.setf(std::ios_base::fixed, std::ios_base::floatfield);
674  edgfile.precision(2);
675  edgfile << "<edges>"<<std::endl;
676 
677  for(auto it = edges_.begin();it!=edges_.end();it++)
678  {
679  Border* baseline = it->first;
680  Edge* e = &it->second;
681 
682  //spreadType=right => represent the left of the border array
683  Border* leftest = globalSet_.getLeftNeighbor(e->represents_.back());
684  Border* rightest = e->represents_.front();
685  if(leftest==0)
686  {
687  std::cout<<"Edge with id "<<e->ids_.front()<<" has no left hand border - skipping"<<std::endl;
688  deleted_edges_.emplace(e);
689  continue;
690  }
691  if(leftest->m_path==0)
692  {
693  std::cout<<"Edge with id "<<e->ids_.front()<<" has no left hand path - skipping"<<std::endl;
694  deleted_edges_.emplace(e);
695  continue;
696  }
697  if(baseline==0)
698  {
699  std::cout<<"Edge with id "<<e->ids_.front()<<" has no baseline - skipping"<<std::endl;
700  deleted_edges_.emplace(e);
701  continue;
702  }
703  if(originates_at_.find(baseline)==originates_at_.end())
704  {
705  std::cout<<"Edge with id "<<e->ids_.front()<<" has no baseline link to from-node"<<std::endl;
706  deleted_edges_.emplace(e);
707  continue;
708  }
709  if(leads_to_.find(baseline)==leads_to_.end())
710  {
711  std::cout<<"Edge with id "<<e->ids_.front()<<" has no baseline link to to-node"<<std::endl;
712  deleted_edges_.emplace(e);
713  continue;
714  }
715  Node* n0 = originates_at_[baseline];
716  Node* n1 = leads_to_[baseline];
717  if( n0->incoming_edge_.size()==0 && n0->end_connections_.size()==0
718  && n1->outgoing_edge_.size()==0 && n1->start_connections_.size()==0
719  && node_clusters_.find(n0)==node_clusters_.end()
720  && node_clusters_.find(n1)==node_clusters_.end() )
721  {
722  std::cout<<"Edge with id "<<e->ids_.front()<<" starts and ends at unconnected nodes."<<std::endl;
723  deleted_edges_.emplace(e);
724  continue;
725  }
726  if( baseline->getLength()<min_length_start_
727  && n0->incoming_edge_.size()==0 && n0->end_connections_.size()==0
728  && node_clusters_.find(n0)==node_clusters_.end() )
729  {
730  std::cout<<"Edge with id "<<e->ids_.front()<<" is a start edge and very short."<<std::endl;
731  deleted_edges_.emplace(e);
732  continue;
733  }
734 
735  for(int i=0;i<e->ids_.size();i++)
736  {
737 
738  int eid = e->ids_[i];
739  int fromid = (i==0?originates_at_[baseline]->id_:e->split_node_ids_[i-1]);
740  int toid = (i+1==e->ids_.size()?leads_to_[baseline]->id_:e->split_node_ids_[i]);
741 
742  if(fromid==toid)
743  {
744  std::cout<<"Edge with id "<<eid<<" has fromid="<<fromid<<" and toid="<<toid<<". "<<"It's index is "<<i<<", with a total of "<<e->ids_.size()<<"."<<std::endl;
745  deleted_edges_.emplace(e);
746  continue;
747  }
748 
749  if(use_lane_shape_)
750  {
751  edgfile << "\t<edge id=\""<< eid <<"\" "
752  << "numLanes=\""<<e->represents_.size()<<"\" "
753  << "from=\""<< fromid <<"\" "
754  << "to=\""<< toid << "\" >\n";
755 
756  for(int k=0;k<e->represents_.size();k++)
757  {
758  Border* right = e->represents_[k];
760  int point_count = adore::mad::computeCenterline(*left->m_path,*right->m_path,lane_buffer);
761  lane_buffer.getData()(0,0) = 0.0;//start value
762  for(int l=1;l<point_count;l++)
763  {
764  double d = 0.0;
765  double dd = 0.0;
766  for(int m=1;m<=3;m++)
767  {
768  d = lane_buffer.getData()(m,l)-lane_buffer.getData()(m,l-1);
769  dd+=d*d;
770  }
771  lane_buffer.getData()(0,l) = (std::sqrt)(dd) + lane_buffer.getData()(0,l-1);
772  }
773  double lane_length = lane_buffer.getData()(0,point_count-1);
774 
775 
776  std::stringstream laneid;
777  laneid<<eid<<"_"<<k;
778 
779  edgfile << "\t\t<lane index=\""
780  // << laneid.str() //id?
781  << k //or index?
782  << "\" shape=\"";
783 
784  //write the shape
785  int dim = 2;//output number of dimensions
786  int n=0;static const int N=500;
787  double X[N];
788  double Y[N];
789  for(double s=e->s_[i]*lane_length;
790  s<e->s_[i+1]*lane_length;
791  s+=1.0)
792  {
793  // double seval = adore::mad::bound(leftest->m_path->limitLo(),s+leftest->m_path->limitLo(),leftest->m_path->limitHi());
794  auto p = lane_buffer.f(s);
795  if(n<N)
796  {
797  X[n] = p(0);
798  Y[n] = p(1);
799  n++;
800  }
801  for(int j=0;j<dim;j++)
802  {
803  edgfile<<p(j)<<(j<dim-1?",":" ");
804  }
805  }
806  //last point
807  auto p = lane_buffer.f(e->s_[i+1]*lane_length);
808  if(n<N)
809  {
810  X[n] = p(0);
811  Y[n] = p(1);
812  n++;
813  }
814  //put shape in edge file
815  for(int j=0;j<dim;j++)
816  {
817  edgfile<<p(j)<<(j<dim-1?",":"");
818  }
819  //plot the shape
820  if(enable_plotting_)
821  {
822  figure_->plot("lane_shape_"+laneid.str(),X,Y,2.0,n,"LineWidth=1;LineColor=0,0,1");
823  X[1]=X[n-1];
824  Y[1]=Y[n-1];
825  figure_->plot("lane_shape_"+laneid.str()+"/marker",X,Y,2.0,2,"LineStyle=none;MarkerSize=9;LineColor=0,0,1");
826  }
827  //compute width at beginning of subsegment
828  double seval = e->s_[i]*left->getLength()+left->m_path->limitLo();
829  auto pleft = left->m_path->f(seval);
830  seval = e->s_[i]*right->getLength()+right->m_path->limitLo();
831  auto pright = right->m_path->f(seval);
832  double dx = pleft(0)-pright(0);
833  double dy = pleft(1)-pright(1);
834  double width = (std::sqrt)(dx*dx+dy*dy);
835 
836  edgfile <<"\" width=\""
837  <<width
838  << "\"/>\n";
839  }
840 
841  edgfile << "\t</edge>\n";
842  }
843  else
844  {
845 
846  edgfile << "\t<edge id=\""<< eid <<"\" "
847  << "from=\""<< fromid <<"\" "
848  << "to=\""<< toid << "\" "
849  << "spreadType=\"right\" "
850  << "shape=\"";
851 
852  //write the shape
853  int dim = 2;//output number of dimensions
854  for(double s=e->s_[i]*leftest->getLength()+leftest->m_path->limitLo();
855  s<e->s_[i+1]*leftest->getLength()+leftest->m_path->limitLo();
856  s+=1.0)
857  {
858  // double seval = adore::mad::bound(leftest->m_path->limitLo(),s+leftest->m_path->limitLo(),leftest->m_path->limitHi());
859  auto p = leftest->m_path->f(s);
860  for(int j=0;j<dim;j++)
861  {
862  edgfile<<p(j)<<(j<dim-1?",":" ");
863  }
864  }
865  double seval = e->s_[i+1]*leftest->getLength()+leftest->m_path->limitLo();
866  // double seval = adore::mad::bound(leftest->m_path->limitLo(),e->s_[i+1]+leftest->m_path->limitLo(),leftest->m_path->limitHi());
867  auto p = leftest->m_path->f(seval);
868  for(int j=0;j<dim;j++)
869  {
870  edgfile<<p(j)<<(j<dim-1?",":" ");
871  }
872  //compute width at beginning of subsegment
873  seval = e->s_[i]*leftest->getLength()+leftest->m_path->limitLo();
874  // seval = adore::mad::bound(leftest->m_path->limitLo(),e->s_[i]+leftest->m_path->limitLo(),leftest->m_path->limitHi());
875  auto pleft = leftest->m_path->f(seval);
876  seval = e->s_[i]*rightest->getLength()+rightest->m_path->limitLo();
877  // seval = adore::mad::bound(rightest->m_path->limitLo(),e->s_[i]+rightest->m_path->limitLo(),rightest->m_path->limitHi());
878  auto pright = rightest->m_path->f(seval);
879  double dx = pleft(0)-pright(0);
880  double dy = pleft(1)-pright(1);
881  double avrgwidth = std::sqrt(dx*dx+dy*dy)/(double)e->number_of_lanes_;
882 
883  if(use_constant_width_)avrgwidth = constant_width_;
884 
885  edgfile <<"\" numLanes=\""
886  <<e->number_of_lanes_
887  <<"\" width=\""
888  <<avrgwidth
889  <<"\"/>"<<std::endl;
890  }
891  }
892  }
893 
894  edgfile << "</edges>"<<std::endl;
895  edgfile.close();
896  std::cout<<"done"<<std::endl;
897  }
898  catch(const std::exception& e)
899  {
900  std::cout << e.what() << '\n';
901  std::cout<<"Failed to write to file."<<std::endl;
902  }
903 
904  // write the .con.xml file
905  std::cout<<"Writing to file "<<output_prefix<<".con.xml ..."<<std::endl;
906  try
907  {
908  std::ofstream confile;
909  confile.open (output_prefix + ".con.xml");
910  confile.setf(std::ios_base::fixed, std::ios_base::floatfield);
911  confile.precision(2);
912 
913  confile << "<connections>"<<std::endl;
914  int connection_count = 0;
915  for(auto it = closed_connections_.begin();it!=closed_connections_.end();it++)
916  {
917  ConnectionStep* c = *it;
918  if(c->edge_border1_!=0)//goal edge
919  {
920  connection_count ++;
921  Border* b1 = c->edge_border1_;
922  Border* baseline1 = represented_by_[b1];
923  Edge* e1 = &edges_[baseline1];
924  int l1 = 0;
925  for(int i=0;i<e1->represents_.size();i++)
926  {
927  if(e1->represents_[i]==b1)
928  {
929  //l1 = e1->represents_.size()-i-1;
930  l1 = i;
931  break;
932  }
933  }
934 
935  Edge* e0 = 0;
936  Border* b0 = 0;
937  Border* baseline0;
938  int l0;
939 
940  std::vector<adoreMatrix<double,3,1>> shape;
941  while(c!=0)
942  {
946  int point_count = adore::mad::computeCenterline(*left->m_path,*right->m_path,path);
947  auto p = path.getData();
948  for(int i=point_count-1;i>=0;i--)
949  {
950  shape.push_back(dlib::subm(p,dlib::range(1,3),dlib::range(i,i)));
951  }
952  if(c->edge_border0_!=0)
953  {
954  b0 = c->edge_border0_;
955  baseline0 = represented_by_[b0];
956  e0 = &edges_[baseline0];
957  for(int i=0;i<e0->represents_.size();i++)
958  {
959  if(e0->represents_[i]==b0)
960  {
961  // l0 = e0->represents_.size()-i-1;
962  l0 = i;
963  break;
964  }
965  }
966  c = 0;
967 
968  }
969  else
970  {
971  c = c->predecessor_;
972  }
973 
974  }
975  if( deleted_edges_.find(e0)!=deleted_edges_.end()
976  || deleted_edges_.find(e1)!=deleted_edges_.end())
977  {
978  std::cout<<"Connection from "<<e0->ids_.back()<<" to "<<e1->ids_.front()<<" not written, as one of the edges was deleted."<<std::endl;
979  continue;
980  }
981  double front_dx = shape.back()(0)-(b0->m_id.m_last.m_X+b0->m_left->m_last.m_X)*0.5;
982  double front_dy = shape.back()(1)-(b0->m_id.m_last.m_Y+b0->m_left->m_last.m_Y)*0.5;
983  double back_dx = (b1->m_id.m_first.m_X+b1->m_left->m_first.m_X)*0.5 - shape.front()(0);
984  double back_dy = (b1->m_id.m_first.m_Y+b1->m_left->m_first.m_Y)*0.5 - shape.front()(1);
985 
986  if(front_dx*front_dx+front_dy*front_dy>25.0)
987  {
988  int asdf=0;
989  }
990  if(back_dx*back_dx+back_dy*back_dy>25.0)
991  {
992  int asdf=0;
993  }
994 
995  confile<<"\t <connection "
996  <<"from=\""<<e0->ids_.back()<<"\" "
997  <<"to=\""<<e1->ids_.front()<<"\" "
998  <<"fromLane=\""<<l0<<"\" "
999  <<"toLane=\""<<l1<<"\" "
1000  <<"shape=\"";
1001  for(auto it2=shape.rbegin();it2!=shape.rend();it2++)
1002  {
1003  confile<<(*it2)(0)<<","<<(*it2)(1)<<" ";
1004  }
1005  confile<<"\"/>"<<std::endl;
1006 
1007  if(enable_plotting_)
1008  {
1009  int k=0;static const int N=200;
1010  double X[N];
1011  double Y[N];
1012  for(int i=0;i<shape.size();i++)
1013  {
1014  X[k]=shape[i](0);
1015  Y[k]=shape[i](1);
1016  k++;
1017  if(k==N||i+1==shape.size())
1018  {
1019  std::stringstream ss;
1020  ss<<"connection/"<<connection_count<<"/"<<i;
1021  figure_->plot(ss.str(),X,Y,2.0,k,"LineWidth=1;LineColor=1,0,0");
1022  X[0]=X[k-1];
1023  Y[0]=Y[k-1];
1024  k=1;
1025  }
1026  }
1027  std::stringstream ss;
1028  ss<<"connection/"<<connection_count<<"/markers";
1029  X[0] = shape.front()(0);
1030  Y[0] = shape.front()(1);
1031  X[1] = shape.back()(0);
1032  Y[1] = shape.back()(1);
1033  figure_->plot(ss.str(),X,Y,2.0,2,"LineStyle=none;MarkerSize=9;LineColor=1,0,0");
1034 
1035 
1036  //plot connection to b0
1037  X[0] = (b0->m_id.m_last.m_X+b0->m_left->m_last.m_X)*0.5;
1038  Y[0] = (b0->m_id.m_last.m_Y+b0->m_left->m_last.m_Y)*0.5;
1039  X[1] = shape.back()(0);
1040  Y[1] = shape.back()(1);
1041  figure_->plot(ss.str()+"frontend",X,Y,2.0,2,"LineWidth=5;LineColor=0,0.7,0;MarkerSize=9");
1042 
1043 
1044  //plot connection to b1
1045  X[0] = shape.front()(0);
1046  Y[0] = shape.front()(1);
1047  X[1] = (b1->m_id.m_first.m_X+b1->m_left->m_first.m_X)*0.5;
1048  Y[1] = (b1->m_id.m_first.m_Y+b1->m_left->m_first.m_Y)*0.5;
1049  figure_->plot(ss.str()+"backend",X,Y,2.0,2,"LineWidth=5;LineColor=0.7,0,0.7;MarkerSize=9");
1050 
1051  }
1052 
1053  }
1054  }
1055  confile << "</connections>"<<std::endl;
1056  }
1057  catch(const std::exception& e)
1058  {
1059  std::cout << e.what() << '\n';
1060  std::cout<<"Failed to write to file."<<std::endl;
1061  }
1062 
1063 
1064 
1065  std::cin.get();
1066  }
1067 };
1068 
1069 }
1070 }
Definition: figurestubfactory.h:25
AFigureStub * createFigureStub(int windowID)
Definition: figurestubfactory.h:45
Definition: figurestubzmq.h:30
virtual void show() override
Definition: figurestubzmq.h:266
virtual void plot(std::string hashtag, double *X, double *Y, double *Z, int size, std::string options) override
Definition: figurestubzmq.h:174
Definition: plainxmlexporter.h:39
int cluster_count
Definition: plainxmlexporter.h:104
DLR_TS::PlotLab::FigureStubFactory * figure_factory_
Definition: plainxmlexporter.h:41
double min_length_
Definition: plainxmlexporter.h:48
adore::env::BorderBased::Border Border
Definition: plainxmlexporter.h:56
std::unordered_set< Node * > nodes_
Definition: plainxmlexporter.h:95
PlainXMLExporter()
Definition: plainxmlexporter.h:110
std::vector< ConnectionStep * > closed_connections_
Definition: plainxmlexporter.h:101
bool use_constant_width_
Definition: plainxmlexporter.h:51
bool enable_plotting_
Definition: plainxmlexporter.h:108
double maxDistMeter_
Definition: plainxmlexporter.h:46
adore::env::BorderBased::BorderSet globalSet_
Definition: plainxmlexporter.h:43
std::unordered_map< Node *, int > node_clusters_
Definition: plainxmlexporter.h:103
double maxDistRel_
Definition: plainxmlexporter.h:47
double constant_width_
Definition: plainxmlexporter.h:52
adore::env::BorderBased::BorderID BorderID
Definition: plainxmlexporter.h:57
std::unordered_map< Border *, Node * > originates_at_
Definition: plainxmlexporter.h:98
double max_length_
Definition: plainxmlexporter.h:49
DLR_TS::PlotLab::FigureStubZMQ * figure_
Definition: plainxmlexporter.h:42
adore::if_xodr::BorderIDTranslation idTranslation_
Definition: plainxmlexporter.h:44
std::unordered_map< Border *, Edge > edges_
Definition: plainxmlexporter.h:96
std::unordered_map< Border *, Border * > represented_by_
Definition: plainxmlexporter.h:99
double min_length_start_
Definition: plainxmlexporter.h:45
void run(int argc, char **argv)
Definition: plainxmlexporter.h:125
std::unordered_map< std::string, std::set< Node * > > merge_nodes_
Definition: plainxmlexporter.h:102
std::unordered_set< Edge * > deleted_edges_
Definition: plainxmlexporter.h:105
bool lateral_aggregate_
Definition: plainxmlexporter.h:50
std::unordered_map< Border *, Node * > leads_to_
Definition: plainxmlexporter.h:97
bool use_lane_shape_
Definition: plainxmlexporter.h:53
std::list< ConnectionStep * > open_connections_
Definition: plainxmlexporter.h:100
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
itCoordinate2Border getSuccessors(Border *b)
get an interator pair for all borders which follow after b
Definition: borderset.h:996
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 * getLeftNeighbor(Border *b)
Get left neighbor of a border.
Definition: borderset.h:1252
void setIsOwner(bool isOwner)
set whether this owns objects in pointers
Definition: borderset.h:214
itCoordinate2Border getPredecessors(Border *b)
get an interator pair for all borders which lead to b
Definition: borderset.h:1017
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
BorderIDTranslation is a set of translation tables, which keeps track of road ids,...
Definition: idtranslation.h:32
TID getJunctionID(const BorderID &borderID)
Definition: idtranslation.h:109
bool isInJunction(TID roadID)
Definition: idtranslation.h:121
OpenDRIVE converter from file to object sets.
Definition: xodr2borderbased.h:43
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 CT f(DT x) const override
Definition: llinearpiecewisefunction.h:251
virtual DT limitLo() const override
Definition: llinearpiecewisefunction.h:264
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
void plotBorderSet(adore::env::BorderBased::BorderSet &borderSet, DLR_TS::PlotLab::AFigureStub *figure)
Definition: plot_border.h:194
@ DRIVING
Definition: border.h:39
@ right
Definition: indicator_hint.h:36
@ left
Definition: indicator_hint.h:35
int computeCenterline(const adoreMatrix< double, 0, 0 > &left, const adoreMatrix< double, 0, 0 > &right, adoreMatrix< double, 0, 0 > &center)
Definition: centerline.h:29
x
Definition: adore_set_goal.py:30
y
Definition: adore_set_goal.py:31
Definition: areaofeffectconverter.h:20
Definition: plainxmlexporter.h:87
Border * edge_border0_
Definition: plainxmlexporter.h:89
ConnectionStep * predecessor_
Definition: plainxmlexporter.h:91
Border * edge_border1_
Definition: plainxmlexporter.h:90
int depth_
Definition: plainxmlexporter.h:92
Border * connection_border_
Definition: plainxmlexporter.h:88
Definition: plainxmlexporter.h:79
Border * to_
Definition: plainxmlexporter.h:81
std::vector< std::pair< int, int > > from_lane_to_lane_
Definition: plainxmlexporter.h:82
Border * from_
Definition: plainxmlexporter.h:80
Definition: plainxmlexporter.h:70
std::vector< int > split_node_ids_
Definition: plainxmlexporter.h:75
std::vector< double > s_
Definition: plainxmlexporter.h:76
int number_of_lanes_
Definition: plainxmlexporter.h:72
std::vector< Border * > represents_
Definition: plainxmlexporter.h:71
std::vector< int > ids_
Definition: plainxmlexporter.h:74
double lane_width_
Definition: plainxmlexporter.h:73
Definition: plainxmlexporter.h:61
int id_
Definition: plainxmlexporter.h:62
std::unordered_set< ConnectionStep * > start_connections_
Definition: plainxmlexporter.h:65
std::unordered_set< Border * > outgoing_edge_
Definition: plainxmlexporter.h:64
std::unordered_set< Border * > incoming_edge_
Definition: plainxmlexporter.h:63
std::unordered_set< ConnectionStep * > end_connections_
Definition: plainxmlexporter.h:66
adoreMatrix< double, 3, 1 > position_
Definition: plainxmlexporter.h:67
This struct identifies a Border by the coordinates of the starting and the end point.
Definition: borderid.h:31
Coordinate m_last
Definition: borderid.h:32
Coordinate m_first
Definition: borderid.h:32
The border struct contains data of the smallest.
Definition: border.h:62
Tborderpath * m_path
Definition: border.h:70
bool isContinuousPredecessorOf(Border *other)
Check whether the border is a continuous predecessor of another border.
Definition: border.h:363
double getLength()
Get the length of the border.
Definition: border.h:703
BorderType::TYPE m_type
Definition: border.h:71
BorderID m_id
Definition: border.h:68
BorderID * m_left
Definition: border.h:69
double m_Y
Definition: coordinate.h:35
double m_X
Definition: coordinate.h:35
T1 & current()
Definition: borderset.h:50