23 #include <unordered_map>
24 #include <unordered_set>
76 std::vector<double>
s_;
96 std::unordered_map<Border*,Edge>
edges_;
125 void run(
int argc,
char **argv)
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;
135 std::string output_prefix(argv[argc-1]);
137 for(
int i=1;i<argc-1;i++)
139 std::string trackConfig(argv[i]);
141 if(std::strcmp(trackConfig.c_str(),
"plot")==0)
148 bool transform =
false;
149 std::string filename =
"";
151 std::string token =
"";
152 std::stringstream trackConfigStream(trackConfig);
153 while(std::getline(trackConfigStream,token,
','))
155 if(token.compare(
"transform")==0)
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)
170 std::cout<<
"OpenDrive loader..."<<std::endl;
176 catch(
const std::exception& e)
178 std::cout<<
"Could not parse file"<<std::endl;
179 std::cout << e.what() <<
'\n';
183 else if(filename.substr(filename.length()-3,std::string::npos).compare(
"r2s")==0)
185 std::cout<<
"Road2Simulation simple feature loader..."<<std::endl;
189 converter.
convert(filename+
"r",filename+
"l", partialSet);
191 catch(
const std::exception& e)
193 std::cout<<
"Could not parse file"<<std::endl;
194 std::cout << e.what() <<
'\n';
201 std::cout<<
"unknown file ending: "<<filename<<std::endl;
205 for(;its.first!=its.second;its.first++)
215 std::cout<<
"...successfully loaded all files"<<std::endl;
224 std::cout<<
"converting to node/edge format..."<<std::endl;
226 std::unordered_set<BorderType::TYPE> valid_types;
230 std::cout<<
"creating edges"<<std::endl;
233 Border* border = it.first->second;
239 if(valid_types.find(border->
m_type)==valid_types.end())
continue;
253 if(nextleft!=0 &&
left->m_type==border->
m_type)
280 std::cout<<
"creating atomic nodes"<<std::endl;
283 Border* border = it.first->second;
289 successors.
current()!=successors.end();
290 successors.current()++)
292 Border* next = successors.current()->second;
312 std::cout<<
"aggregating nodes"<<std::endl;
314 std::unordered_set<Node*> node_done_;
318 std::unordered_set<Node*> equivalent_nodes;
322 if(node_done_.find(n)==node_done_.end())
324 node_done_.emplace(n);
331 if(it3!=
leads_to_.end() && it3->second!=n)
333 equivalent_nodes.emplace(it3->second);
344 equivalent_nodes.emplace(it3->second);
348 for(
auto it2 = equivalent_nodes.begin();it2!=equivalent_nodes.end();it2++)
364 if(equivalent_nodes.size()>0)
371 for(
auto it=equivalent_nodes.begin();it!=equivalent_nodes.end();it++)
380 Border* baseline = it->first;
381 Edge* e = &it->second;
407 Border* border = it.first->second;
412 if(valid_types.find(border->
m_type)==valid_types.end())
continue;
420 Border* pre = it2.current()->second;
436 std::set<Node*> empty_set;
450 std::vector<ConnectionStep*> children;
456 Border* next = it.current()->second;
491 int replace = it1->second;
492 int by = it0->second;
500 else if(c->
depth_<maxDepth)
509 children.push_back(cc);
516 for(
auto cc:children)
523 for(
auto cc:children)
535 Edge* e = &it->second;
537 e->
ids_.push_back(edgeid++);
538 e->
s_.push_back(0.0);
541 e->
ids_.push_back(edgeid++);
545 e->
s_.push_back(1.0);
550 std::cout<<
"Writing to file "<<output_prefix<<
".nod.xml ..."<<std::endl;
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;
582 nodfile <<
"\t<node id=\""<<n->
id_<<
"\" "
583 <<
"x=\""<<
x/(double)count<<
"\" "
584 <<
"y=\""<<
y/(
double)count<<
"\" "
585 <<
"type=\"priority\" />"
589 nodfile << std::endl;
592 std::cout<<
"writing "<<
cluster_count<<
" joined nodes"<<std::endl;
595 int nodes_in_cluster = 0;
598 int connectors_in_cluster = 0;
615 connectors_in_cluster++;
623 connectors_in_cluster++;
627 if(nodes_in_cluster>0)
629 nodfile <<
"\t<node id=\""<<nodeid<<
"\" "
630 <<
"x=\""<<
x/(double)connectors_in_cluster<<
"\" "
631 <<
"y=\""<<
y/(
double)connectors_in_cluster<<
"\" "
632 <<
"type=\"priority\" />"
640 Edge* e = &it->second;
641 Border* baseline = it->first;
646 auto p = baseline->
m_path->
f(seval);
648 nodfile <<
"\t<node id=\""<<
id<<
"\" "
649 <<
"x=\""<< (p(0)) <<
"\" "
650 <<
"y=\""<< (p(1)) <<
"\" "
651 <<
"type=\"priority\" />"
656 nodfile <<
"</nodes>"<<std::endl;
658 std::cout<<
"done"<<std::endl;
660 catch(
const std::exception& e)
662 std::cout << e.what() <<
'\n';
663 std::cout<<
"Failed to write to file."<<std::endl;
667 std::cout<<
"Writing to file "<<output_prefix<<
".edg.xml ..."<<std::endl;
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;
679 Border* baseline = it->first;
680 Edge* e = &it->second;
687 std::cout<<
"Edge with id "<<e->
ids_.front()<<
" has no left hand border - skipping"<<std::endl;
693 std::cout<<
"Edge with id "<<e->
ids_.front()<<
" has no left hand path - skipping"<<std::endl;
699 std::cout<<
"Edge with id "<<e->
ids_.front()<<
" has no baseline - skipping"<<std::endl;
705 std::cout<<
"Edge with id "<<e->
ids_.front()<<
" has no baseline link to from-node"<<std::endl;
711 std::cout<<
"Edge with id "<<e->
ids_.front()<<
" has no baseline link to to-node"<<std::endl;
722 std::cout<<
"Edge with id "<<e->
ids_.front()<<
" starts and ends at unconnected nodes."<<std::endl;
730 std::cout<<
"Edge with id "<<e->
ids_.front()<<
" is a start edge and very short."<<std::endl;
735 for(
int i=0;i<e->
ids_.size();i++)
738 int eid = e->
ids_[i];
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;
751 edgfile <<
"\t<edge id=\""<< eid <<
"\" "
753 <<
"from=\""<< fromid <<
"\" "
754 <<
"to=\""<< toid <<
"\" >\n";
761 lane_buffer.
getData()(0,0) = 0.0;
762 for(
int l=1;l<point_count;l++)
766 for(
int m=1;m<=3;m++)
771 lane_buffer.
getData()(0,l) = (std::sqrt)(dd) + lane_buffer.
getData()(0,l-1);
773 double lane_length = lane_buffer.
getData()(0,point_count-1);
776 std::stringstream laneid;
779 edgfile <<
"\t\t<lane index=\""
786 int n=0;
static const int N=500;
789 for(
double s=e->
s_[i]*lane_length;
790 s<e->s_[i+1]*lane_length;
794 auto p = lane_buffer.
f(s);
801 for(
int j=0;j<dim;j++)
803 edgfile<<p(j)<<(j<dim-1?
",":
" ");
807 auto p = lane_buffer.
f(e->
s_[i+1]*lane_length);
815 for(
int j=0;j<dim;j++)
817 edgfile<<p(j)<<(j<dim-1?
",":
"");
822 figure_->
plot(
"lane_shape_"+laneid.str(),X,Y,2.0,n,
"LineWidth=1;LineColor=0,0,1");
825 figure_->
plot(
"lane_shape_"+laneid.str()+
"/marker",X,Y,2.0,2,
"LineStyle=none;MarkerSize=9;LineColor=0,0,1");
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);
836 edgfile <<
"\" width=\""
841 edgfile <<
"\t</edge>\n";
846 edgfile <<
"\t<edge id=\""<< eid <<
"\" "
847 <<
"from=\""<< fromid <<
"\" "
848 <<
"to=\""<< toid <<
"\" "
849 <<
"spreadType=\"right\" "
859 auto p = leftest->
m_path->
f(s);
860 for(
int j=0;j<dim;j++)
862 edgfile<<p(j)<<(j<dim-1?
",":
" ");
867 auto p = leftest->
m_path->
f(seval);
868 for(
int j=0;j<dim;j++)
870 edgfile<<p(j)<<(j<dim-1?
",":
" ");
875 auto pleft = leftest->
m_path->
f(seval);
878 auto pright = rightest->
m_path->
f(seval);
879 double dx = pleft(0)-pright(0);
880 double dy = pleft(1)-pright(1);
885 edgfile <<
"\" numLanes=\""
894 edgfile <<
"</edges>"<<std::endl;
896 std::cout<<
"done"<<std::endl;
898 catch(
const std::exception& e)
900 std::cout << e.what() <<
'\n';
901 std::cout<<
"Failed to write to file."<<std::endl;
905 std::cout<<
"Writing to file "<<output_prefix<<
".con.xml ..."<<std::endl;
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);
913 confile <<
"<connections>"<<std::endl;
914 int connection_count = 0;
940 std::vector<adoreMatrix<double,3,1>> shape;
948 for(
int i=point_count-1;i>=0;i--)
950 shape.push_back(dlib::subm(p,dlib::range(1,3),dlib::range(i,i)));
978 std::cout<<
"Connection from "<<e0->
ids_.back()<<
" to "<<e1->
ids_.front()<<
" not written, as one of the edges was deleted."<<std::endl;
986 if(front_dx*front_dx+front_dy*front_dy>25.0)
990 if(back_dx*back_dx+back_dy*back_dy>25.0)
995 confile<<
"\t <connection "
996 <<
"from=\""<<e0->
ids_.back()<<
"\" "
997 <<
"to=\""<<e1->
ids_.front()<<
"\" "
998 <<
"fromLane=\""<<l0<<
"\" "
999 <<
"toLane=\""<<l1<<
"\" "
1001 for(
auto it2=shape.rbegin();it2!=shape.rend();it2++)
1003 confile<<(*it2)(0)<<
","<<(*it2)(1)<<
" ";
1005 confile<<
"\"/>"<<std::endl;
1009 int k=0;
static const int N=200;
1012 for(
int i=0;i<shape.size();i++)
1017 if(k==N||i+1==shape.size())
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");
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");
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");
1045 X[0] = shape.front()(0);
1046 Y[0] = shape.front()(1);
1049 figure_->
plot(ss.str()+
"backend",X,Y,2.0,2,
"LineWidth=5;LineColor=0.7,0,0.7;MarkerSize=9");
1055 confile <<
"</connections>"<<std::endl;
1057 catch(
const std::exception& e)
1059 std::cout << e.what() <<
'\n';
1060 std::cout<<
"Failed to write to file."<<std::endl;
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 > ¢er)
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