From a943f5efb1391c17ca15143b75f3ccb12b039053 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 16 Dec 2020 18:20:23 +0100
Subject: [PATCH] Added support to process the junctions. Added support to link
road segments.
---
include/opendrive_lane.h | 4 +-
include/opendrive_link.h | 2 +-
include/opendrive_road.h | 4 +-
include/opendrive_road_node.h | 3 +
include/opendrive_road_segment.h | 8 +-
src/opendrive_lane.cpp | 112 ++++++++++++++----------
src/opendrive_link.cpp | 83 ++++++++++++++----
src/opendrive_road.cpp | 133 +++++++++++++++++++++++++++-
src/opendrive_road_node.cpp | 22 ++++-
src/opendrive_road_segment.cpp | 145 ++++++++++++++++++++-----------
10 files changed, 391 insertions(+), 125 deletions(-)
diff --git a/include/opendrive_lane.h b/include/opendrive_lane.h
index 33e2120..f3b9a24 100644
--- a/include/opendrive_lane.h
+++ b/include/opendrive_lane.h
@@ -27,8 +27,8 @@ class COpendriveLane
double offset;
int id;
protected:
- void link_left_lane(COpendriveLane *lane);
- void link_right_lane(COpendriveLane *lane);
+ void link_neightbour_lane(COpendriveLane *lane);
+ void link_lane(COpendriveLane *lane,opendrive_mark_t mark);
void add_node(COpendriveRoadNode *node);
void set_resolution(double res);
void set_scale_factor(double scale);
diff --git a/include/opendrive_link.h b/include/opendrive_link.h
index 5f5478b..9dafa58 100644
--- a/include/opendrive_link.h
+++ b/include/opendrive_link.h
@@ -1,4 +1,4 @@
-#ifndef _OPNEDRIVE_LINK_H
+#ifndef _OPENDRIVE_LINK_H
#define _OPENDRIVE_LINK_H
#include "g2_spline.h"
diff --git a/include/opendrive_road.h b/include/opendrive_road.h
index 0d75497..23f002f 100644
--- a/include/opendrive_road.h
+++ b/include/opendrive_road.h
@@ -15,7 +15,9 @@ class COpendriveRoad
double scale_factor;
double min_road_length;
protected:
- void add_road_nodes(std::unique_ptr<OpenDRIVE::road_type> &road_info);
+ COpendriveRoadSegment &operator[](std::string &key);
+ void link_segments(OpenDRIVE &open_drive);
+ std::string get_junction_road_id(OpenDRIVE &open_drive,std::string &incoming,std::string &connecting);
public:
COpendriveRoad();
COpendriveRoad(const COpendriveRoad& object);
diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index 0e87653..2db3d6e 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -18,16 +18,19 @@ class COpendriveRoadNode
TOpendriveWorldPoint start_point;
COpendriveLane *lane;
COpendriveGeometry *geometry;
+ unsigned int index;
protected:
void add_link(COpendriveRoadNode *node,opendrive_mark_t mark);
void set_resolution(double res);
void set_scale_factor(double scale);
+ void set_index(unsigned int index);
public:
COpendriveRoadNode();
COpendriveRoadNode(const COpendriveRoadNode &object);
void load(const planView::geometry_type &node_info,COpendriveLane *lane);
double get_resolution(void) const;
double get_scale_factor(void) const;
+ unsigned int get_index(void) const;
TOpendriveWorldPoint get_start_pose(void) const;
unsigned int get_num_links(void) const;
const COpendriveLink &get_link(unsigned int index) const;
diff --git a/include/opendrive_road_segment.h b/include/opendrive_road_segment.h
index b428549..712f1ee 100644
--- a/include/opendrive_road_segment.h
+++ b/include/opendrive_road_segment.h
@@ -19,8 +19,8 @@ class COpendriveRoadSegment
double resolution;
double scale_factor;
double min_road_length;
- unsigned int num_right_lanes;
- unsigned int num_left_lanes;
+ int num_right_lanes;
+ int num_left_lanes;
std::vector<COpendriveSignal *> signals;
std::vector<COpendriveObject *> objects;
std::vector<COpendriveRoadSegment *> next;
@@ -35,7 +35,9 @@ class COpendriveRoadSegment
void update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs);
void add_lanes(lanes::laneSection_type &lane_section);
void add_nodes(OpenDRIVE::road_type &road_info);
- void link_lanes(lanes::laneSection_type &lane_section);
+ void link_neightbour_lanes(lanes::laneSection_type &lane_section);
+ void link_segment(COpendriveRoadSegment &segment);
+ void link_segment(COpendriveRoadSegment &segment,int from, int to);
public:
COpendriveRoadSegment();
COpendriveRoadSegment(const COpendriveRoadSegment &object);
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index 6720c1e..32c500c 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -40,38 +40,52 @@ COpendriveLane::COpendriveLane(const COpendriveLane &object)
this->id=object.id;
}
-void COpendriveLane::link_left_lane(COpendriveLane *lane)
+void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
{
if(lane!=NULL)
{
if(this->get_num_nodes()!=lane->get_num_nodes())
- throw CException(_HERE_,"Impossible to link lanes because they have different number of nodes");
- for(unsigned int i=1;i<this->get_num_nodes();i++)
+ throw CException(_HERE_,"Impossible to liink lanes because they have different number of nodes");
+ if(this->get_num_nodes()>0 && lane->get_num_nodes()>0)
{
- this->nodes[i]->add_link(lane->nodes[i-1],this->left_mark);
- lane->nodes[i]->add_link(this->nodes[i-1],this->left_mark);
- }
- this->left_lane=lane;
+ if(this->id*lane->get_id()<0) // oposite directions
+ {
+ for(unsigned int i=0;i<this->get_num_nodes();i++)
+ {
+ this->nodes[i]->add_link(lane->nodes[lane->get_num_nodes()-i-1],this->left_mark);
+ lane->nodes[lane->get_num_nodes()-i-1]->add_link(this->nodes[i],this->left_mark);
+ lane->right_mark=this->left_mark;
+ }
+ }
+ else
+ {
+ for(unsigned int i=0;i<this->get_num_nodes()-1;i++)
+ {
+ this->nodes[i]->add_link(lane->nodes[i+1],this->left_mark);
+ lane->nodes[i]->add_link(this->nodes[i+1],this->left_mark);
+ if(this->id>0)// left lanes
+ this->right_mark=lane->left_mark;
+ else// right lanes
+ this->left_mark=lane->right_mark;
+ }
+ }
+ this->left_lane=lane;
+ lane->right_lane=this;
+ }
+ else
+ throw CException(_HERE_,"One of the lanes to link has no nodes");
}
- else
- this->left_lane=NULL;
}
-void COpendriveLane::link_right_lane(COpendriveLane *lane)
+void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark)
{
if(lane!=NULL)
{
- if(this->get_num_nodes()!=lane->get_num_nodes())
- throw CException(_HERE_,"Impossible to link lanes because they have different number of nodes");
- for(unsigned int i=1;i<this->get_num_nodes();i++)
- {
- this->nodes[i]->add_link(lane->nodes[i-1],this->right_mark);
- lane->nodes[i]->add_link(this->nodes[i-1],this->right_mark);
- }
- this->right_lane=lane;
+ if(this->get_num_nodes()>0 && lane->get_num_nodes()>0)
+ this->nodes[this->nodes.size()-1]->add_link(lane->nodes[0],mark);
+ else
+ throw CException(_HERE_,"One of the lanes to link has no nodes");
}
- else
- this->right_lane=NULL;
}
void COpendriveLane::add_node(COpendriveRoadNode *node)
@@ -80,6 +94,7 @@ void COpendriveLane::add_node(COpendriveRoadNode *node)
if(this->nodes[i]==node)
return;
// add the new node
+ node->set_index(this->nodes.size());
this->nodes.push_back(node);
// link the new node with the previous one in the current lane, if any
if(this->nodes.size()>1)
@@ -143,32 +158,37 @@ void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegmen
}
this->speed=lane_info.speed().begin()->max().get();
//lane mark
- if(lane_info.roadMark().size()!=1)
+ if(lane_info.roadMark().size()==0)
+ mark=OD_MARK_NONE;
+ else if(lane_info.roadMark().size()>1)
{
error << "Only one road mark supported for lane " << this->id;
throw CException(_HERE_,error.str());
}
- if(lane_info.roadMark().begin()->type().present())
+ else if(lane_info.roadMark().size()==1)
{
- if(lane_info.roadMark().begin()->type().get()=="none")
- mark=OD_MARK_NONE;
- else if(lane_info.roadMark().begin()->type().get()=="solid")
- mark=OD_MARK_SOLID;
- else if(lane_info.roadMark().begin()->type().get()=="broken")
- mark=OD_MARK_BROKEN;
- else if(lane_info.roadMark().begin()->type().get()=="solid_solid")
- mark=OD_MARK_SOLID_SOLID;
- else if(lane_info.roadMark().begin()->type().get()=="solid_broken")
- mark=OD_MARK_SOLID_BROKEN;
- else if(lane_info.roadMark().begin()->type().get()=="broken_solid")
- mark=OD_MARK_BROKEN_SOLID;
- else if(lane_info.roadMark().begin()->type().get()=="broken_broken")
- mark=OD_MARK_BROKEN_BROKEN;
+ if(lane_info.roadMark().begin()->type1().present())
+ {
+ if(lane_info.roadMark().begin()->type1().get()=="none")
+ mark=OD_MARK_NONE;
+ else if(lane_info.roadMark().begin()->type1().get()=="solid")
+ mark=OD_MARK_SOLID;
+ else if(lane_info.roadMark().begin()->type1().get()=="broken")
+ mark=OD_MARK_BROKEN;
+ else if(lane_info.roadMark().begin()->type1().get()=="solid_solid")
+ mark=OD_MARK_SOLID_SOLID;
+ else if(lane_info.roadMark().begin()->type1().get()=="solid_broken")
+ mark=OD_MARK_SOLID_BROKEN;
+ else if(lane_info.roadMark().begin()->type1().get()=="broken_solid")
+ mark=OD_MARK_BROKEN_SOLID;
+ else if(lane_info.roadMark().begin()->type1().get()=="broken_broken")
+ mark=OD_MARK_BROKEN_BROKEN;
+ else
+ mark=OD_MARK_NONE;
+ }
else
mark=OD_MARK_NONE;
}
- else
- mark=OD_MARK_NONE;
if(this->id<0)//right lanes
this->right_mark=mark;
else
@@ -236,17 +256,17 @@ void COpendriveLane::operator=(const COpendriveLane &object)
std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
{
- out << " id: " << lane.id << " (offset: " << lane.offset << ")" << std::endl;
+ out << " Lane: " << lane.id << " (offset: " << lane.offset << ")" << std::endl;
out << " width: " << lane.get_width() << std::endl;
out << " speed: " << lane.get_speed() << std::endl;
if(lane.segment!=NULL)
out << " Parent road segment: " << lane.segment->get_name() << std::endl;
else
out << " No parent segment" << std::endl;
- if(lane.left_lane!=NULL)
- out << " No left lane (" << std::endl;
+ if(lane.left_lane==NULL)
+ out << " No left lane (";
else
- out << " Left lane " << lane.get_id() << " (" << std::endl;
+ out << " Left lane " << lane.left_lane->get_id() << " (";
switch(lane.left_mark)
{
case OD_MARK_NONE:
@@ -271,10 +291,10 @@ std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
out << "broken broken)" << std::endl;
break;
}
- if(lane.right_lane!=NULL)
- out << " No right lane (" << std::endl;
+ if(lane.right_lane==NULL)
+ out << " No right lane (";
else
- out << " Right lane " << lane.get_id() << " (" << std::endl;
+ out << " Right lane " << lane.right_lane->get_id() << " (";
switch(lane.right_mark)
{
case OD_MARK_NONE:
@@ -301,7 +321,7 @@ std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
}
out << " Number of nodes: " << lane.nodes.size() << std::endl;
for(unsigned int i=0;i<lane.nodes.size();i++)
- out << *lane.nodes[i] << std::endl;
+ out << *lane.nodes[i];
return out;
}
diff --git a/src/opendrive_link.cpp b/src/opendrive_link.cpp
index 05c4e89..4b68da2 100644
--- a/src/opendrive_link.cpp
+++ b/src/opendrive_link.cpp
@@ -1,38 +1,49 @@
#include "opendrive_link.h"
+#include "opendrive_road_node.h"
COpendriveLink::COpendriveLink()
{
-
+ this->prev=NULL;
+ this->next=NULL;
+ this->spline=NULL;
+ this->mark=OD_MARK_NONE;
+ this->resolution=DEFAULT_RESOLUTION;
+ this->scale_factor=DEFAULT_SCALE_FACTOR;
}
COpendriveLink::COpendriveLink(const COpendriveLink &object)
{
-
+ this->prev=object.prev;
+ this->next=object.next;
+ this->spline=new CG2Spline(*object.spline);
+ this->mark=object.mark;
+ this->resolution=object.resolution;
+ this->scale_factor=object.scale_factor;
}
void COpendriveLink::set_prev(COpendriveRoadNode *node)
{
-
+ this->prev=node;
}
void COpendriveLink::set_next(COpendriveRoadNode *node)
{
-
+ this->next=node;
}
void COpendriveLink::set_road_mark(opendrive_mark_t mark)
{
-
+ this->mark=mark;
}
void COpendriveLink::set_resolution(double res)
{
-
+ this->resolution=res;
}
void COpendriveLink::set_scale_factor(double scale)
{
-
+ this->scale_factor=scale;
}
void COpendriveLink::generate(void)
@@ -42,27 +53,27 @@ void COpendriveLink::generate(void)
const COpendriveRoadNode &COpendriveLink::get_prev(void) const
{
-
+ return *this->prev;
}
const COpendriveRoadNode &COpendriveLink::get_next(void) const
{
-
+ return *this->next;
}
opendrive_mark_t COpendriveLink::get_road_mark(void) const
{
-
+ return this->mark;
}
double COpendriveLink::get_resolution(void) const
{
-
+ return this->resolution;
}
double COpendriveLink::get_scale_factor(void) const
{
-
+ return this->scale_factor;
}
double COpendriveLink::find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point)
@@ -92,16 +103,58 @@ double COpendriveLink::get_length(void) const
void COpendriveLink::operator=(const COpendriveLink &object)
{
-
+ this->prev=object.prev;
+ this->next=object.next;
+ this->spline=new CG2Spline(*object.spline);
+ this->mark=object.mark;
+ this->resolution=object.resolution;
+ this->scale_factor=object.scale_factor;
}
std::ostream& operator<<(std::ostream& out, COpendriveLink &link)
{
-
+ out << " Previous node: " << link.get_prev().get_index() << " of lane " << link.get_prev().get_lane().get_id() << " of road " << link.get_prev().get_lane().get_segment().get_name() << std::endl;
+ out << " Next node: " << link.get_next().get_index() << " of lane " << link.get_next().get_lane().get_id() << " of road " << link.get_next().get_lane().get_segment().get_name() << std::endl;
+ out << " Road mark: ";
+ switch(link.mark)
+ {
+ case OD_MARK_NONE:
+ out << "no mark" << std::endl;
+ break;
+ case OD_MARK_SOLID:
+ out << "solid" << std::endl;
+ break;
+ case OD_MARK_BROKEN:
+ out << "broken" << std::endl;
+ break;
+ case OD_MARK_SOLID_SOLID:
+ out << "solid solid" << std::endl;
+ break;
+ case OD_MARK_SOLID_BROKEN:
+ out << "solid broken" << std::endl;
+ break;
+ case OD_MARK_BROKEN_SOLID:
+ out << "broken solid" << std::endl;
+ break;
+ case OD_MARK_BROKEN_BROKEN:
+ out << "broken broken" << std::endl;
+ break;
+ }
+
+ return out;
}
COpendriveLink::~COpendriveLink()
{
-
+ this->prev=NULL;
+ this->next=NULL;
+ if(this->spline!=NULL)
+ {
+ delete this->spline;
+ this->spline=NULL;
+ }
+ this->mark=OD_MARK_NONE;
+ this->resolution=DEFAULT_RESOLUTION;
+ this->scale_factor=DEFAULT_SCALE_FACTOR;
}
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index 5c33438..ae0ca2e 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -30,10 +30,88 @@ COpendriveRoad::COpendriveRoad(const COpendriveRoad& object)
this->segments[i]->update_references(new_references);
}
+COpendriveRoadSegment &COpendriveRoad::operator[](std::string &key)
+{
+ for(unsigned int i=0;i<this->segments.size();i++)
+ {
+ if(this->segments[i]->get_id()==(unsigned int)std::stoi(key))
+ return *this->segments[i];
+ }
+
+ throw CException(_HERE_,"No road segment with the given ID");
+}
+
+void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
+{
+ for(OpenDRIVE::road_iterator road_it(open_drive.road().begin());road_it!=open_drive.road().end();++road_it)
+ {
+ COpendriveRoadSegment &segment=(*this)[road_it->id().get()];
+ if(std::stoi(road_it->junction().get())==-1)// process only non junction road segments
+ {
+ // get predecessor
+ if(road_it->lane_link().present())
+ {
+ if(road_it->lane_link().get().predecessor().present())// predecessor present
+ {
+ if(road_it->lane_link().get().predecessor().get().elementType().get()=="road")
+ {
+ COpendriveRoadSegment &link_segment=(*this)[road_it->lane_link().get().predecessor().get().elementId().get()];
+ link_segment.link_segment(segment);
+ }
+ // else ignore juntions
+ }
+ if(road_it->lane_link().get().successor().present())// predecessor present
+ {
+ if(road_it->lane_link().get().successor().get().elementType().get()=="road")
+ {
+ COpendriveRoadSegment &link_segment=(*this)[road_it->lane_link().get().successor().get().elementId().get()];
+ segment.link_segment(link_segment);
+ }
+ // else ignore juntions
+ }
+ }
+ }
+ }
+}
+
+std::string COpendriveRoad::get_junction_road_id(OpenDRIVE &open_drive,std::string &incoming,std::string &connecting)
+{
+ bool predecessor_match,successor_match;
+ for (OpenDRIVE::road_iterator road_it(open_drive.road().begin()); road_it != open_drive.road().end(); ++road_it)
+ {
+ predecessor_match=false;
+ successor_match=false;
+ if(std::stoi(road_it->junction().get())!=-1)// process only junction road segments
+ {
+ // get predecessor
+ if(road_it->lane_link().present())
+ {
+ if(road_it->lane_link().get().predecessor().present())// predecessor present
+ {
+ if(road_it->lane_link().get().predecessor().get().elementType().get()=="road")
+ if(road_it->lane_link().get().predecessor().get().elementId().get()==incoming)
+ predecessor_match=true;
+ }
+ if(road_it->lane_link().get().successor().present())// predecessor present
+ {
+ if(road_it->lane_link().get().successor().get().elementType().get()=="road")
+ {
+ if(road_it->lane_link().get().successor().get().elementId().get()==connecting)
+ successor_match=true;
+ }
+ }
+ }
+ }
+ if(predecessor_match && successor_match)
+ return road_it->id().get();
+ }
+
+ return std::string("");
+}
+
void COpendriveRoad::load(const std::string &filename)
{
struct stat buffer;
- COpendriveRoadSegment *segment;
if(stat(filename.c_str(),&buffer)==0)
{
@@ -47,7 +125,7 @@ void COpendriveRoad::load(const std::string &filename)
for (OpenDRIVE::road_iterator road_it(open_drive->road().begin()); road_it != open_drive->road().end(); ++road_it)
{
try{
- segment=new COpendriveRoadSegment();
+ COpendriveRoadSegment *segment=new COpendriveRoadSegment();
segment->set_resolution(this->resolution);
segment->set_scale_factor(this->scale_factor);
segment->load(*road_it);
@@ -56,6 +134,55 @@ void COpendriveRoad::load(const std::string &filename)
std::cout << e.what() << std::endl;
}
}
+ // link segments
+ this->link_segments(*open_drive);
+ // process junctions
+/*
+ for(OpenDRIVE::junction_iterator junction_it(open_drive->junction().begin());junction_it!=open_drive->junction().end();++junction_it)
+ {
+ for(junction::connection_iterator connection_it(junction_it->connection().begin()); connection_it!=junction_it->connection().end();++connection_it)
+ {
+ std::string incoming_road_id;
+ std::string connecting_road_id;
+ std::string contact_point;
+ if(connection_it->incomingRoad().present())
+ incoming_road_id=connection_it->incomingRoad().get();
+ else
+ throw CException(_HERE_,"Connectivity information missing");
+ if(connection_it->connectingRoad().present())
+ connecting_road_id=connection_it->connectingRoad().get();
+ else
+ throw CException(_HERE_,"Connectivity information missing");
+ if(connection_it->contactPoint().present())
+ contact_point=connection_it->contactPoint().get();
+ else
+ throw CException(_HERE_,"Connectivity information missing");
+ for(connection::laneLink_iterator lane_link_it(connection_it->laneLink().begin()); lane_link_it!=connection_it->laneLink().end();++lane_link_it)
+ {
+ int from_lane_id;
+ int to_lane_id;
+ if(lane_link_it->from().present())
+ from_lane_id=lane_link_it->from().get();
+ else
+ throw CException(_HERE_,"Connectivity information missing");
+ if(lane_link_it->to().present())
+ to_lane_id=lane_link_it->to().get();
+ else
+ throw CException(_HERE_,"Connectivity information missing");
+ // search the road segment starting at incoming_road_id and ending at connecting_road_id
+ std::string road_id=this->get_junction_road_id(*open_drive,incoming_road_id,connecting_road_id);
+ if(!road_id.empty())
+ {
+ COpendriveRoadSegment &prev_road=(*this)[incoming_road_id];
+ COpendriveRoadSegment &road=(*this)[road_id];
+ COpendriveRoadSegment &next_road=(*this)[connecting_road_id];
+ prev_road.link_segment(road,from_lane_id,-1);
+ road.link_segment(next_road,-1,to_lane_id);
+ }
+ }
+ }
+ }
+ */
}catch (const xml_schema::exception& e){
std::ostringstream os;
os << e;
@@ -154,7 +281,7 @@ std::ostream& operator<<(std::ostream& out, COpendriveRoad &road)
out << "Scale factor: " << road.scale_factor << std::endl;
out << "Minimum road lenght: " << road.min_road_length << std::endl;
for(unsigned int i=0;i<road.segments.size();i++)
- std::cout << *road.segments[i] << std::endl;
+ std::cout << *road.segments[i];
return out;
}
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index 61c05b4..33a70c5 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -10,6 +10,7 @@ COpendriveRoadNode::COpendriveRoadNode()
this->start_point.heading=0.0;
this->lane=NULL;
this->geometry=NULL;
+ this->index=-1;
}
COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
@@ -29,6 +30,7 @@ COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
new_link=new COpendriveLink(*object.links[i]);
this->links.push_back(new_link);
}
+ this->index=object.index;
}
void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark)
@@ -72,6 +74,11 @@ void COpendriveRoadNode::set_scale_factor(double scale)
this->links[i]->set_scale_factor(scale);
}
+void COpendriveRoadNode::set_index(unsigned int index)
+{
+ this->index=index;
+}
+
void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriveLane *lane)
{
TOpendriveTrackPoint track_point;
@@ -123,6 +130,11 @@ double COpendriveRoadNode::get_scale_factor(void) const
return this->scale_factor;
}
+unsigned int COpendriveRoadNode::get_index(void) const
+{
+ return this->index;
+}
+
TOpendriveWorldPoint COpendriveRoadNode::get_start_pose(void) const
{
return this->start_point;
@@ -173,22 +185,28 @@ void COpendriveRoadNode::operator=(const COpendriveRoadNode &object)
new_link=new COpendriveLink(*object.links[i]);
this->links.push_back(new_link);
}
+ this->index=object.index;
}
std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node)
{
+ out << " Node " << node.index << ":" << std::endl;
+ out << " parent lane: " << node.lane->get_id() << std::endl;
out << " start point: x: " << node.start_point.x << " y: " << node.start_point.y << " heading: " << node.start_point.heading << std::endl;
if(node.lane!=NULL)
out << " lane id: " << node.lane->get_id() << std::endl;
else
out << " Does not belong to a lane." << std::endl;
if(node.geometry!=NULL)
- out << *node.geometry << std::endl;
+ out << *node.geometry;
else
out << " Does not have any associated geometry." << std::endl;
out << " Number of links: " << node.links.size() << std::endl;
for(unsigned int i=0;i<node.links.size();i++)
- out << *node.links[i] << std::endl;
+ {
+ out << " Link " << i << ":" << std::endl;
+ out << *node.links[i];
+ }
return out;
}
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index 1b77e8e..c021614 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -57,7 +57,7 @@ COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object
void COpendriveRoadSegment::free(void)
{
- for(unsigned int i=-this->num_right_lanes;i<=this->num_left_lanes && i!=0;i++)
+ for(int i=-this->num_right_lanes;i<=this->num_left_lanes && i!=0;i++)
{
delete this->lanes[i];
this->lanes[i]=NULL;
@@ -170,7 +170,7 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
for(geom_it=road_info.planView().geometry().begin(); geom_it!=road_info.planView().geometry().end(); ++geom_it)
{
lane_offset=0.0;
- for(unsigned int i=-1;i>=-this->num_right_lanes;i--)
+ for(int i=-1;i>=-this->num_right_lanes;i--)
{
new_node=new COpendriveRoadNode();
new_node->set_resolution(this->resolution);
@@ -188,7 +188,7 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
{
geom_it--;
lane_offset=0.0;
- for(unsigned int i=1;i<=this->num_left_lanes;i++)
+ for(int i=1;i<=this->num_left_lanes;i++)
{
new_node=new COpendriveRoadNode();
new_node->set_resolution(this->resolution);
@@ -210,7 +210,7 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
}
}
-void COpendriveRoadSegment::link_lanes(lanes::laneSection_type &lane_section)
+void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_section)
{
opendrive_mark_t center_mark;
center::lane_type center_lane;
@@ -218,66 +218,107 @@ void COpendriveRoadSegment::link_lanes(lanes::laneSection_type &lane_section)
if(lane_section.center().lane().present())
{
center_lane=lane_section.center().lane().get();
- if(center_lane.roadMark().begin()->type().present())
+ if(center_lane.roadMark().size()>1)
+ throw CException(_HERE_,"Only one road mark supported for lane");
+ else if(center_lane.roadMark().size()==0)
+ center_mark=OD_MARK_NONE;
+ else if(center_lane.roadMark().size()==1)
{
- if(center_lane.roadMark().begin()->type().get()=="none")
- center_mark=OD_MARK_NONE;
- else if(center_lane.roadMark().begin()->type().get()=="solid")
- center_mark=OD_MARK_SOLID;
- else if(center_lane.roadMark().begin()->type().get()=="broken")
- center_mark=OD_MARK_BROKEN;
- else if(center_lane.roadMark().begin()->type().get()=="solid_solid")
- center_mark=OD_MARK_SOLID_SOLID;
- else if(center_lane.roadMark().begin()->type().get()=="solid_broken")
- center_mark=OD_MARK_SOLID_BROKEN;
- else if(center_lane.roadMark().begin()->type().get()=="broken_solid")
- center_mark=OD_MARK_BROKEN_SOLID;
- else if(center_lane.roadMark().begin()->type().get()=="broken_broken")
- center_mark=OD_MARK_BROKEN_BROKEN;
+ if(center_lane.roadMark().begin()->type1().present())
+ {
+ if(center_lane.roadMark().begin()->type1().get()=="none")
+ center_mark=OD_MARK_NONE;
+ else if(center_lane.roadMark().begin()->type1().get()=="solid")
+ center_mark=OD_MARK_SOLID;
+ else if(center_lane.roadMark().begin()->type1().get()=="broken")
+ center_mark=OD_MARK_BROKEN;
+ else if(center_lane.roadMark().begin()->type1().get()=="solid_solid")
+ center_mark=OD_MARK_SOLID_SOLID;
+ else if(center_lane.roadMark().begin()->type1().get()=="solid_broken")
+ center_mark=OD_MARK_SOLID_BROKEN;
+ else if(center_lane.roadMark().begin()->type1().get()=="broken_solid")
+ center_mark=OD_MARK_BROKEN_SOLID;
+ else if(center_lane.roadMark().begin()->type1().get()=="broken_broken")
+ center_mark=OD_MARK_BROKEN_BROKEN;
+ else
+ center_mark=OD_MARK_NONE;
+ }
else
center_mark=OD_MARK_NONE;
}
+ }
+ for(int i=-this->num_right_lanes;i<0;i++)
+ {
+ if(this->lanes.find(i+1)!=this->lanes.end())// if the lane exists
+ this->lanes[i]->link_neightbour_lane(this->lanes[i+1]);
else
- center_mark=OD_MARK_NONE;
+ this->lanes[i]->left_mark=OD_MARK_NONE;
}
- for(unsigned int i=-this->num_right_lanes;i<0;i++)
+ for(int i=this->num_left_lanes;i>0;i--)
+ {
+ if(this->lanes.find(i-1)!=this->lanes.end())// if the lane exists
+ this->lanes[i]->link_neightbour_lane(this->lanes[i-1]);
+ else
+ this->lanes[i]->right_mark=OD_MARK_NONE;
+ }
+ if(this->lanes.find(1)!=this->lanes.end() && this->lanes.find(-1)!=this->lanes.end())
+ {
+ this->lanes[-1]->left_mark=center_mark;
+ this->lanes[1]->right_mark=center_mark;
+ this->lanes[-1]->link_neightbour_lane(this->lanes[1]);
+ }
+}
+
+void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment)
+{
+ for(unsigned int i=0;i<this->next.size();i++)
+ if(this->next[i]->get_id()==segment.get_id())// the segment is already included
+ return;
+ this->next.push_back(&segment);
+ // link lanes
+ for(int i=-this->num_right_lanes;i<0;i++)
{
- if(i==-this->num_right_lanes)// right most lane
+ for(int j=i-1;j<=i+1;j++)
{
- if((i+1)==0)//center lane
+ if(segment.lanes.find(j)!=segment.lanes.end())
{
- if(this->lanes.find(i+2)!=this->lanes.end())
- this->lanes[i]->link_left_lane(this->lanes[i+2]);
- this->lanes[i]->left_mark=center_mark;
- }
- else
- {
- if(this->lanes.find(i+1)!=this->lanes.end())
- this->lanes[i]->link_left_lane(this->lanes[i+1]);
- this->lanes[i]->left_mark=this->lanes[i+1]->right_mark;
+ if(j==i-1)
+ this->lanes[i]->link_lane(segment.lanes[j],this->lanes[i]->right_mark);
+ else if(j==i)
+ this->lanes[i]->link_lane(segment.lanes[j],OD_MARK_NONE);
+ else
+ this->lanes[i]->link_lane(segment.lanes[j],this->lanes[i]->left_mark);
}
}
}
- for(unsigned int i=this->num_left_lanes;i>0;i--)
+ for(int i=1;i<=segment.num_left_lanes;i++)
{
- if(i==this->num_left_lanes)// right most lane
+ for(int j=i-1;j<=i+1;j++)
{
- if((i-1)==0)//center lane
- {
- if(this->lanes.find(i-2)!=this->lanes.end())
- this->lanes[i]->link_right_lane(this->lanes[i-2]);
- this->lanes[i]->right_mark=center_mark;
- }
- else
+ if(this->lanes.find(j)!=this->lanes.end())
{
- if(this->lanes.find(i-1)!=this->lanes.end())
- this->lanes[i]->link_left_lane(this->lanes[i-1]);
- this->lanes[i]->right_mark=this->lanes[i+1]->left_mark;
+ if(j==i-1)
+ segment.lanes[i]->link_lane(this->lanes[j],segment.lanes[i]->right_mark);
+ else if(j==i)
+ segment.lanes[i]->link_lane(this->lanes[j],OD_MARK_NONE);
+ else
+ segment.lanes[i]->link_lane(this->lanes[j],segment.lanes[i]->left_mark);
}
}
}
}
+void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment,int from, int to)
+{
+ for(unsigned int i=0;i<this->next.size();i++)
+ if(this->next[i]->get_id()==segment.get_id())// the segment is already included
+ return;
+ this->next.push_back(&segment);
+ // link lanes
+ if(this->lanes.find(from)!=this->lanes.end() && segment.lanes.find(to)!=segment.lanes.end())
+ this->lanes[from]->link_lane(segment.lanes[to],OD_MARK_NONE);
+}
+
void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
{
std::map<int,COpendriveLane *>::const_iterator lane_it;
@@ -303,7 +344,7 @@ void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
// add road nodes
this->add_nodes(road_info);
// link lanes
- this->link_lanes(*lane_section);
+ this->link_neightbour_lanes(*lane_section);
// add road signals
if(road_info.signals().present())
{
@@ -448,7 +489,7 @@ void COpendriveRoadSegment::operator=(const COpendriveRoadSegment &object)
std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment)
{
- out << "Road " << segment.get_name() << std::endl;
+ out << "Road " << segment.get_name() << " (" << segment.get_id() << ")" << std::endl;
out << " Previous road segments: " << segment.prev.size() << std::endl;
for(unsigned int i=0;i<segment.prev.size();i++)
out << " " << segment.prev[i]->get_name() << std::endl;
@@ -456,17 +497,17 @@ std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment)
for(unsigned int i=0;i<segment.next.size();i++)
out << " " << segment.next[i]->get_name() << std::endl;
out << " Number of right lanes: " << segment.num_right_lanes << std::endl;
- for(unsigned int i=-1;i>=-segment.num_right_lanes;i--)
- out << *segment.lanes[i] << std::endl;
+ for(int i=-1;i>=-segment.num_right_lanes;i--)
+ out << *segment.lanes[i];
out << " Number of left lanes: " << segment.num_left_lanes << std::endl;
- for(unsigned int i=1;i<=segment.num_left_lanes;i++)
- out << *segment.lanes[i] << std::endl;
+ for(int i=1;i<=segment.num_left_lanes;i++)
+ out << *segment.lanes[i];
out << " Number of signals: " << segment.signals.size() << std::endl;
for(unsigned int i=0;i<segment.signals.size();i++)
- std::cout << *segment.signals[i] << std::endl;
+ std::cout << *segment.signals[i];
out << " Number of objects: " << segment.objects.size() << std::endl;
for(unsigned int i=0;i<segment.objects.size();i++)
- std::cout << *segment.objects[i] << std::endl;
+ std::cout << *segment.objects[i];
return out;
}
--
GitLab