From 9f14bb4ef2f9184b983a8b1a8a3b69f6e9a225b6 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 11 Dec 2020 17:58:27 +0100
Subject: [PATCH] Added the functions to link nodes and lanes: not working yet.
TO DO: add the functions to link road segments.
---
include/opendrive_lane.h | 6 +
include/opendrive_link.h | 1 +
include/opendrive_object.h | 2 +-
include/opendrive_road_node.h | 2 +-
include/opendrive_road_segment.h | 4 +
include/opendrive_signal.h | 2 +-
src/opendrive_lane.cpp | 155 +++++++++++++++++++++--
src/opendrive_link.cpp | 5 +
src/opendrive_object.cpp | 38 +++---
src/opendrive_road_node.cpp | 24 +++-
src/opendrive_road_segment.cpp | 210 ++++++++++++++++++++++++-------
src/opendrive_signal.cpp | 20 +--
12 files changed, 375 insertions(+), 94 deletions(-)
diff --git a/include/opendrive_lane.h b/include/opendrive_lane.h
index 48b7381..33e2120 100644
--- a/include/opendrive_lane.h
+++ b/include/opendrive_lane.h
@@ -15,6 +15,10 @@ class COpendriveLane
friend class COpendriveRoadSegment;
private:
std::vector<COpendriveRoadNode *> nodes;
+ COpendriveLane *left_lane;
+ opendrive_mark_t left_mark;
+ COpendriveLane *right_lane;
+ opendrive_mark_t right_mark;
COpendriveRoadSegment *segment;
double scale_factor;
double resolution;
@@ -23,6 +27,8 @@ class COpendriveLane
double offset;
int id;
protected:
+ void link_left_lane(COpendriveLane *lane);
+ void link_right_lane(COpendriveLane *lane);
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 bd4d7ca..5f5478b 100644
--- a/include/opendrive_link.h
+++ b/include/opendrive_link.h
@@ -25,6 +25,7 @@ class COpendriveLink
void set_road_mark(opendrive_mark_t mark);
void set_resolution(double res);
void set_scale_factor(double scale);
+ void generate(void);
public:
COpendriveLink();
COpendriveLink(const COpendriveLink &object);
diff --git a/include/opendrive_object.h b/include/opendrive_object.h
index 7db50f2..cb8bb16 100644
--- a/include/opendrive_object.h
+++ b/include/opendrive_object.h
@@ -50,7 +50,7 @@ class COpendriveObject
public:
COpendriveObject();
COpendriveObject(const COpendriveObject& object);
- void load(std::unique_ptr<objects::object_type> &object_info);
+ void load(objects::object_type &object_info);
TOpendriveTrackPoint get_pose(void) const;
std::string get_type(void) const;
std::string get_name(void) const;
diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index e7d704d..0e87653 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -19,7 +19,7 @@ class COpendriveRoadNode
COpendriveLane *lane;
COpendriveGeometry *geometry;
protected:
- unsigned int add_link(COpendriveRoadNode *node);
+ void add_link(COpendriveRoadNode *node,opendrive_mark_t mark);
void set_resolution(double res);
void set_scale_factor(double scale);
public:
diff --git a/include/opendrive_road_segment.h b/include/opendrive_road_segment.h
index 3824abd..b428549 100644
--- a/include/opendrive_road_segment.h
+++ b/include/opendrive_road_segment.h
@@ -28,10 +28,14 @@ class COpendriveRoadSegment
std::string name;
unsigned int id;
protected:
+ void free(void);
void set_resolution(double res);
void set_scale_factor(double scale);
void set_min_road_length(double length);
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);
public:
COpendriveRoadSegment();
COpendriveRoadSegment(const COpendriveRoadSegment &object);
diff --git a/include/opendrive_signal.h b/include/opendrive_signal.h
index 2048dbe..3f16293 100644
--- a/include/opendrive_signal.h
+++ b/include/opendrive_signal.h
@@ -20,7 +20,7 @@ class COpendriveSignal
COpendriveSignal();
COpendriveSignal(const COpendriveSignal &object);
COpendriveSignal(int id, double s, double t, double heading, std::string type, std::string sub_type, int value, std::string text, bool reverse = false);
- void load(std::unique_ptr<signals::signal_type> &signal_info);
+ void load(signals::signal_type &signal_info);
int get_id(void) const;
TOpendriveTrackPoint get_pose(void) const;
void get_type(std::string &type, std::string &sub_type) const;
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index 735204d..6720c1e 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -4,6 +4,10 @@
COpendriveLane::COpendriveLane()
{
this->nodes.clear();
+ this->left_lane=NULL;
+ this->left_mark=OD_MARK_NONE;
+ this->right_lane=NULL;
+ this->right_mark=OD_MARK_NONE;
this->segment=NULL;
this->resolution=DEFAULT_RESOLUTION;
this->scale_factor=DEFAULT_SCALE_FACTOR;
@@ -23,6 +27,10 @@ COpendriveLane::COpendriveLane(const COpendriveLane &object)
node=new COpendriveRoadNode(*object.nodes[i]);
this->nodes.push_back(node);
}
+ this->left_lane=object.left_lane;
+ this->left_mark=object.left_mark;
+ this->right_lane=object.right_lane;
+ this->right_mark=object.right_mark;
this->segment=object.segment;
this->resolution=object.resolution;
this->scale_factor=object.scale_factor;
@@ -32,6 +40,40 @@ COpendriveLane::COpendriveLane(const COpendriveLane &object)
this->id=object.id;
}
+void COpendriveLane::link_left_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++)
+ {
+ 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;
+ }
+ else
+ this->left_lane=NULL;
+}
+
+void COpendriveLane::link_right_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++)
+ {
+ 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;
+ }
+ else
+ this->right_lane=NULL;
+}
+
void COpendriveLane::add_node(COpendriveRoadNode *node)
{
for(unsigned int i=0;i<this->nodes.size();i++)
@@ -39,6 +81,9 @@ void COpendriveLane::add_node(COpendriveRoadNode *node)
return;
// add the new node
this->nodes.push_back(node);
+ // link the new node with the previous one in the current lane, if any
+ if(this->nodes.size()>1)
+ this->nodes[this->nodes.size()-1]->add_link(node,OD_MARK_NONE);
}
void COpendriveLane::set_resolution(double res)
@@ -70,6 +115,7 @@ void COpendriveLane::update_references(std::map<COpendriveRoadSegment *,COpendri
void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegment *segment)
{
std::stringstream error;
+ opendrive_mark_t mark;
for(unsigned int i=0;i<this->nodes.size();i++)
{
@@ -77,31 +123,56 @@ void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegmen
this->nodes[i]=NULL;
}
this->nodes.clear();
+ this->left_lane=NULL;
+ this->left_mark=OD_MARK_NONE;
+ this->right_lane=NULL;
+ this->right_mark=OD_MARK_NONE;
this->id=lane_info.id().get();
// import lane width
- if(lane_info.width().size()<1)
+ if(lane_info.width().size()!=1)
{
- error << "No width record present for lane " << this->id;
- throw CException(_HERE_,error.str());
- }
- else if(lane_info.width().size()>1)
- {
- error << "More than one width record present for lane " << this->id;
+ error << "Only one width record supported for lane " << this->id;
throw CException(_HERE_,error.str());
}
this->width=lane_info.width().begin()->a().get();
// import lane speed
- if(lane_info.speed().size()<1)
+ if(lane_info.speed().size()!=1)
{
- error << "No speed record present for lane " << this->id;
+ error << "Only one speed record supported for lane " << this->id;
throw CException(_HERE_,error.str());
}
- else if(lane_info.speed().size()>1)
+ this->speed=lane_info.speed().begin()->max().get();
+ //lane mark
+ if(lane_info.roadMark().size()!=1)
{
- error << "More than one speed record present for lane " << this->id;
+ error << "Only one road mark supported for lane " << this->id;
throw CException(_HERE_,error.str());
}
- this->speed=lane_info.speed().begin()->max().get();
+ if(lane_info.roadMark().begin()->type().present())
+ {
+ 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;
+ else
+ mark=OD_MARK_NONE;
+ }
+ else
+ mark=OD_MARK_NONE;
+ if(this->id<0)//right lanes
+ this->right_mark=mark;
+ else
+ this->left_mark=mark;
this->segment=segment;
}
@@ -172,6 +243,62 @@ std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
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;
+ else
+ out << " Left lane " << lane.get_id() << " (" << std::endl;
+ switch(lane.left_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;
+ }
+ if(lane.right_lane!=NULL)
+ out << " No right lane (" << std::endl;
+ else
+ out << " Right lane " << lane.get_id() << " (" << std::endl;
+ switch(lane.right_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;
+ }
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;
@@ -187,6 +314,10 @@ COpendriveLane::~COpendriveLane()
this->nodes[i]=NULL;
}
this->nodes.clear();
+ this->left_lane=NULL;
+ this->left_mark=OD_MARK_NONE;
+ this->right_lane=NULL;
+ this->right_mark=OD_MARK_NONE;
this->segment=NULL;
this->resolution=DEFAULT_RESOLUTION;
this->scale_factor=DEFAULT_SCALE_FACTOR;
diff --git a/src/opendrive_link.cpp b/src/opendrive_link.cpp
index 724da05..05c4e89 100644
--- a/src/opendrive_link.cpp
+++ b/src/opendrive_link.cpp
@@ -35,6 +35,11 @@ void COpendriveLink::set_scale_factor(double scale)
}
+void COpendriveLink::generate(void)
+{
+
+}
+
const COpendriveRoadNode &COpendriveLink::get_prev(void) const
{
diff --git a/src/opendrive_object.cpp b/src/opendrive_object.cpp
index 2cff044..9b6f71f 100644
--- a/src/opendrive_object.cpp
+++ b/src/opendrive_object.cpp
@@ -43,41 +43,41 @@ COpendriveObject::COpendriveObject(const COpendriveObject& object)
}
}
-void COpendriveObject::load(std::unique_ptr<objects::object_type> &object_info)
+void COpendriveObject::load(objects::object_type &object_info)
{
double u,v;
unsigned int i;
- std::stringstream ss(object_info->id().get());
+ std::stringstream ss(object_info.id().get());
outline::cornerRoad_iterator corner_road_it;
outline::cornerLocal_iterator corner_local_it;
ss >> this->id;
- this->pose.s = (object_info->s().present() ? object_info->s().get() : 0.0);
- this->pose.t = (object_info->t().present() ? object_info->t().get() : 0.0);
- this->pose.heading = (object_info->hdg().present() ? object_info->hdg().get() : 0.0);
- this->type = (object_info->type().present() ? object_info->type().get() : "");
- this->name = (object_info->name().present() ? object_info->name().get() : "");
- if(object_info->length().present() && object_info->width().present() && object_info->height().present())
+ this->pose.s = (object_info.s().present() ? object_info.s().get() : 0.0);
+ this->pose.t = (object_info.t().present() ? object_info.t().get() : 0.0);
+ this->pose.heading = (object_info.hdg().present() ? object_info.hdg().get() : 0.0);
+ this->type = (object_info.type().present() ? object_info.type().get() : "");
+ this->name = (object_info.name().present() ? object_info.name().get() : "");
+ if(object_info.length().present() && object_info.width().present() && object_info.height().present())
{
this->object_type=OD_BOX;
- this->object.box.length=object_info->length().get();
- this->object.box.width=object_info->width().get();
- this->object.box.height=object_info->height().get();
+ this->object.box.length=object_info.length().get();
+ this->object.box.width=object_info.width().get();
+ this->object.box.height=object_info.height().get();
}
- else if(object_info->height().present() && object_info->radius().present())
+ else if(object_info.height().present() && object_info.radius().present())
{
this->object_type=OD_CYLINDER;
- this->object.cylinder.radius=object_info->radius().get();
- this->object.cylinder.height=object_info->height().get();
+ this->object.cylinder.radius=object_info.radius().get();
+ this->object.cylinder.height=object_info.height().get();
}
- else if(object_info->outline().present())
+ else if(object_info.outline().present())
{
this->object_type=OD_POLYGON;
this->object.polygon.num_vertices=0;
// absolute track coordinates
- if(object_info->outline().get().cornerRoad().size()>0)// absolute track coordinates
+ if(object_info.outline().get().cornerRoad().size()>0)// absolute track coordinates
{
- for(i=0,corner_road_it=object_info->outline().get().cornerRoad().begin();i<OD_MAX_VERTICES && corner_road_it != object_info->outline().get().cornerRoad().end(); ++corner_road_it,++i)
+ for(i=0,corner_road_it=object_info.outline().get().cornerRoad().begin();i<OD_MAX_VERTICES && corner_road_it != object_info.outline().get().cornerRoad().end(); ++corner_road_it,++i)
{
this->object.polygon.vertices[i].s=(corner_road_it->s().present() ? corner_road_it->s().get() : 0.0);
this->object.polygon.vertices[i].t=(corner_road_it->t().present() ? corner_road_it->t().get() : 0.0);
@@ -85,9 +85,9 @@ void COpendriveObject::load(std::unique_ptr<objects::object_type> &object_info)
this->object.polygon.num_vertices++;
}
}
- else if(object_info->outline().get().cornerLocal().size()>0)// local coordinates with respect to the object anchor point
+ else if(object_info.outline().get().cornerLocal().size()>0)// local coordinates with respect to the object anchor point
{
- for(i=0,corner_local_it=object_info->outline().get().cornerLocal().begin();i<OD_MAX_VERTICES && corner_local_it != object_info->outline().get().cornerLocal().end(); ++corner_local_it,++i)
+ for(i=0,corner_local_it=object_info.outline().get().cornerLocal().begin();i<OD_MAX_VERTICES && corner_local_it != object_info.outline().get().cornerLocal().end(); ++corner_local_it,++i)
{
u=(corner_local_it->u().present() ? corner_local_it->u().get() : 0.0);
v=(corner_local_it->v().present() ? corner_local_it->v().get() : 0.0);
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index 8a17bfc..61c05b4 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -31,9 +31,29 @@ COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
}
}
-unsigned int COpendriveRoadNode::add_link(COpendriveRoadNode *parent)
+void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark)
{
- return -1;
+ COpendriveLink *new_link;
+
+ for(unsigned int i=0;i<this->links.size();i++)
+ {
+ if(this->links[i]->prev==this && this->links[i]->next==node)
+ throw CException(_HERE_,"Link already present in the source node");
+ }
+ for(unsigned int i=0;i<node->links.size();i++)
+ {
+ if(node->links[i]->prev==this && node->links[i]->next==node)
+ throw CException(_HERE_,"Link already present in the target node");
+ }
+ new_link=new COpendriveLink();
+ new_link->set_prev(this);
+ new_link->set_next(node);
+ new_link->set_resolution(this->resolution);
+ new_link->set_scale_factor(this->scale_factor);
+ new_link->set_road_mark(mark);
+ new_link->generate();
+ this->links.push_back(new_link);
+ node->links.push_back(new_link);
}
void COpendriveRoadNode::set_resolution(double res)
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index d1ecdcf..1b77e8e 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -55,6 +55,32 @@ COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object
this->min_road_length=object.min_road_length;
}
+void COpendriveRoadSegment::free(void)
+{
+ for(unsigned int i=-this->num_right_lanes;i<=this->num_left_lanes && i!=0;i++)
+ {
+ delete this->lanes[i];
+ this->lanes[i]=NULL;
+ }
+ this->lanes.clear();
+ this->num_left_lanes=0;
+ this->num_right_lanes=0;
+ for(unsigned int i=0;i<this->signals.size();i++)
+ {
+ delete this->signals[i];
+ this->signals[i]=NULL;
+ }
+ this->signals.clear();
+ for(unsigned int i=0;i<this->objects.size();i++)
+ {
+ delete this->objects[i];
+ this->objects[i]=NULL;
+ }
+ this->objects.clear();
+ this->next.clear();
+ this->prev.clear();
+}
+
void COpendriveRoadSegment::set_resolution(double res)
{
std::map<int,COpendriveLane *>::const_iterator lane_it;
@@ -91,32 +117,16 @@ void COpendriveRoadSegment::update_references(std::map<COpendriveRoadSegment *,C
this->lanes[i]->update_references(refs);
}
-void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
+void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
{
- std::map<int,COpendriveLane *>::const_iterator lane_it;
- lanes::laneSection_iterator lane_section;
right::lane_iterator r_lane_it;
left::lane_iterator l_lane_it;
- planView::geometry_iterator geom_it;
COpendriveLane *new_lane;
- COpendriveRoadNode *new_node;
- double lane_offset;
-
- this->name=road_info.name().get();
- this->id=std::stoi(road_info.id().get());
- // only one lane section is supported
- if(road_info.lanes().laneSection().size()<1)
- throw CException(_HERE_,"No lane sections defined for road "+road_info.id().get());
- else if(road_info.lanes().laneSection().size()>1)
- throw CException(_HERE_,"Road "+road_info.id().get()+" has more than one lane section");
- else
- lane_section=road_info.lanes().laneSection().begin();
// right lanes
- this->num_right_lanes=0;
- if(lane_section->right().present())
+ if(lane_section.right().present())
{
- for(r_lane_it=lane_section->right()->lane().begin();r_lane_it!=lane_section->right()->lane().end();r_lane_it++)
+ for(r_lane_it=lane_section.right()->lane().begin();r_lane_it!=lane_section.right()->lane().end();r_lane_it++)
{
try{
new_lane=new COpendriveLane();
@@ -131,10 +141,9 @@ void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
}
}
// left lanes
- this->num_left_lanes=0;
- if(lane_section->left().present())
+ if(lane_section.left().present())
{
- for(l_lane_it=lane_section->left()->lane().begin();l_lane_it!=lane_section->left()->lane().end();l_lane_it++)
+ for(l_lane_it=lane_section.left()->lane().begin();l_lane_it!=lane_section.left()->lane().end();l_lane_it++)
{
try{
new_lane=new COpendriveLane();
@@ -148,7 +157,15 @@ void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
}
}
}
- // add road nodes
+}
+
+void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
+{
+ std::map<int,COpendriveLane *>::const_iterator lane_it;
+ planView::geometry_iterator geom_it;
+ COpendriveRoadNode *new_node;
+ double lane_offset;
+
try{
for(geom_it=road_info.planView().geometry().begin(); geom_it!=road_info.planView().geometry().end(); ++geom_it)
{
@@ -166,6 +183,10 @@ void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
lane_offset-=this->lanes[i]->get_width();
}
}
+ }
+ for(geom_it=road_info.planView().geometry().end(); geom_it!=road_info.planView().geometry().begin();)
+ {
+ geom_it--;
lane_offset=0.0;
for(unsigned int i=1;i<=this->num_left_lanes;i++)
{
@@ -189,6 +210,122 @@ void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
}
}
+void COpendriveRoadSegment::link_lanes(lanes::laneSection_type &lane_section)
+{
+ opendrive_mark_t center_mark;
+ center::lane_type center_lane;
+
+ if(lane_section.center().lane().present())
+ {
+ center_lane=lane_section.center().lane().get();
+ if(center_lane.roadMark().begin()->type().present())
+ {
+ 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;
+ else
+ center_mark=OD_MARK_NONE;
+ }
+ else
+ center_mark=OD_MARK_NONE;
+ }
+ for(unsigned int i=-this->num_right_lanes;i<0;i++)
+ {
+ if(i==-this->num_right_lanes)// right most lane
+ {
+ if((i+1)==0)//center lane
+ {
+ 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;
+ }
+ }
+ }
+ for(unsigned int i=this->num_left_lanes;i>0;i--)
+ {
+ if(i==this->num_left_lanes)// right most lane
+ {
+ 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(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;
+ }
+ }
+ }
+}
+
+void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
+{
+ std::map<int,COpendriveLane *>::const_iterator lane_it;
+ signals::signal_iterator signal_it;
+ objects::object_iterator object_it;
+ lanes::laneSection_iterator lane_section;
+ COpendriveSignal *new_signal;
+ COpendriveObject *new_object;
+
+ this->free();
+ this->name=road_info.name().get();
+ this->id=std::stoi(road_info.id().get());
+ // only one lane section is supported
+ if(road_info.lanes().laneSection().size()<1)
+ throw CException(_HERE_,"No lane sections defined for road "+road_info.id().get());
+ else if(road_info.lanes().laneSection().size()>1)
+ throw CException(_HERE_,"Road "+road_info.id().get()+" has more than one lane section");
+ else
+ lane_section=road_info.lanes().laneSection().begin();
+
+ // add lanes
+ this->add_lanes(*lane_section);
+ // add road nodes
+ this->add_nodes(road_info);
+ // link lanes
+ this->link_lanes(*lane_section);
+ // add road signals
+ if(road_info.signals().present())
+ {
+ for(signal_it=road_info.signals().get().signal().begin();signal_it!=road_info.signals().get().signal().end();++signal_it)
+ {
+ new_signal=new COpendriveSignal();
+ new_signal->load(*signal_it);
+ this->signals.push_back(new_signal);
+ }
+ }
+ // add road objects
+ if(road_info.objects().present())
+ {
+ for(object_it=road_info.objects().get().object().begin();object_it!=road_info.objects().get().object().end();++object_it)
+ {
+ new_object=new COpendriveObject();
+ new_object->load(*object_it);
+ this->objects.push_back(new_object);
+ }
+ }
+}
+
std::string COpendriveRoadSegment::get_name(void) const
{
return this->name;
@@ -280,21 +417,19 @@ void COpendriveRoadSegment::operator=(const COpendriveRoadSegment &object)
COpendriveObject *new_object;
std::map<int,COpendriveLane *>::const_iterator lane_it;
+ this->free();
this->name=object.name;
this->id=object.id;
- this->lanes.clear();
for(lane_it=object.lanes.begin();lane_it!=object.lanes.end();++lane_it)
{
new_lane=new COpendriveLane(*lane_it->second);
this->lanes[lane_it->first]=new_lane;
}
- this->signals.clear();
for(unsigned int i=0;i<object.signals.size();i++)
{
new_signal=new COpendriveSignal(*object.signals[i]);
this->signals.push_back(new_signal);
}
- this->objects.clear();
for(unsigned int i=0;i<object.objects.size();i++)
{
new_object=new COpendriveObject(*object.objects[i]);
@@ -338,30 +473,9 @@ std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment)
COpendriveRoadSegment::~COpendriveRoadSegment()
{
+ this->free();
this->name="";
this->id=-1;
- for(unsigned int i=-this->num_right_lanes;i<=this->num_left_lanes && i!=0;i++)
- {
- delete this->lanes[i];
- this->lanes[i]=NULL;
- }
- this->lanes.clear();
- this->num_left_lanes=0;
- this->num_right_lanes=0;
- for(unsigned int i=0;i<this->signals.size();i++)
- {
- delete this->signals[i];
- this->signals[i]=NULL;
- }
- this->signals.clear();
- for(unsigned int i=0;i<this->objects.size();i++)
- {
- delete this->objects[i];
- this->objects[i]=NULL;
- }
- this->objects.clear();
- this->next.clear();
- this->prev.clear();
this->resolution=DEFAULT_RESOLUTION;
this->scale_factor=DEFAULT_SCALE_FACTOR;
this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
diff --git a/src/opendrive_signal.cpp b/src/opendrive_signal.cpp
index 7e7d744..2083166 100644
--- a/src/opendrive_signal.cpp
+++ b/src/opendrive_signal.cpp
@@ -37,19 +37,19 @@ COpendriveSignal::COpendriveSignal(int id, double s, double t, double heading, s
this->text=text;
}
-void COpendriveSignal::load(std::unique_ptr<signals::signal_type> &signal_info)
+void COpendriveSignal::load(signals::signal_type &signal_info)
{
- std::stringstream ss(signal_info->id().get());
+ std::stringstream ss(signal_info.id().get());
ss >> this->id;
- this->pose.s = (signal_info->s().present() ? signal_info->s().get() : 0.0);
- this->pose.t = (signal_info->t().present() ? signal_info->t().get() : 0.0);
- this->pose.heading = (signal_info->hOffset().present() ? signal_info->hOffset().get() : 0.0);
- this->type = (signal_info->type().present() ? signal_info->type().get() : "");
- this->sub_type = (signal_info->subtype().present() ? signal_info->subtype().get() : "");
- this->value = (signal_info->value().present() ? signal_info->value().get() : 0.0);
- this->text = (signal_info->text().present() ? signal_info->text().get() : "");
+ this->pose.s = (signal_info.s().present() ? signal_info.s().get() : 0.0);
+ this->pose.t = (signal_info.t().present() ? signal_info.t().get() : 0.0);
+ this->pose.heading = (signal_info.hOffset().present() ? signal_info.hOffset().get() : 0.0);
+ this->type = (signal_info.type().present() ? signal_info.type().get() : "");
+ this->sub_type = (signal_info.subtype().present() ? signal_info.subtype().get() : "");
+ this->value = (signal_info.value().present() ? signal_info.value().get() : 0.0);
+ this->text = (signal_info.text().present() ? signal_info.text().get() : "");
bool reverse = false;
- if (signal_info->orientation().present() && signal_info->orientation().get() == orientation::cxx_1)
+ if (signal_info.orientation().present() && signal_info.orientation().get() == orientation::cxx_1)
reverse = true;
this->pose.heading = normalize_angle(this->pose.heading + (reverse ? M_PI : 0.0));
}
--
GitLab