diff --git a/include/opendrive_geometry.h b/include/opendrive_geometry.h
index 1ca5939c82b7dace15748416e07f38a28fb72d3a..a49f6700825e70e620dc572e08f7337986fb0c60 100644
--- a/include/opendrive_geometry.h
+++ b/include/opendrive_geometry.h
@@ -13,6 +13,7 @@ class COpendriveGeometry
friend class COpendriveRoadNode;
private:
protected:
+ void load(const planView::geometry_type &geometry_info);
double scale_factor;
double min_s; ///< Starting track coordenate "s" for the geometry.
double max_s; ///< Ending track coordenate "s" for the geometry.
@@ -27,7 +28,6 @@ class COpendriveGeometry
COpendriveGeometry(double min_s, double max_s, double x, double y, double heading);
COpendriveGeometry(const COpendriveGeometry &object);
virtual COpendriveGeometry *clone(void) = 0;
- void load(const planView::geometry_type &geometry_info);
bool get_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const;
bool get_world_pose(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world) const;
bool in_range(double s) const;
diff --git a/include/opendrive_lane.h b/include/opendrive_lane.h
index f3b9a24246b5d943c4ed7f43878dd18e4ab460be..eeb4c409ecffdef6c809631dee92e9382b61005c 100644
--- a/include/opendrive_lane.h
+++ b/include/opendrive_lane.h
@@ -27,6 +27,7 @@ class COpendriveLane
double offset;
int id;
protected:
+ void load(const right::lane_type &lane_info,COpendriveRoadSegment *segment);
void link_neightbour_lane(COpendriveLane *lane);
void link_lane(COpendriveLane *lane,opendrive_mark_t mark);
void add_node(COpendriveRoadNode *node);
@@ -37,7 +38,6 @@ class COpendriveLane
public:
COpendriveLane();
COpendriveLane(const COpendriveLane &object);
- void load(const right::lane_type &lane_info,COpendriveRoadSegment *segment);
unsigned int get_num_nodes(void) const;
const COpendriveRoadNode &get_node(unsigned int index) const;
const COpendriveRoadSegment &get_segment(void) const;
diff --git a/include/opendrive_object.h b/include/opendrive_object.h
index cb8bb160b31bcc0c0c6d95d11e3f619bdc798030..00ae2447ea8c9b9b985e7e6f5a28e2c4ec6448f5 100644
--- a/include/opendrive_object.h
+++ b/include/opendrive_object.h
@@ -39,6 +39,7 @@ typedef union
class COpendriveObject
{
+ friend class COpendriveRoadSegment;
private:
int id; ///< Object's id.
TOpendriveTrackPoint pose;
@@ -47,10 +48,10 @@ class COpendriveObject
std::string name; ///< Object's name.
object_type_t object_type;
protected:
+ void load(objects::object_type &object_info);
public:
COpendriveObject();
COpendriveObject(const COpendriveObject& object);
- 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.h b/include/opendrive_road.h
index 23f002fa611d3667bdef7dea5c09b09e7a1fffe8..b8c9a28e090ff63d84fb231f994e4f71bcb0f1ac 100644
--- a/include/opendrive_road.h
+++ b/include/opendrive_road.h
@@ -9,14 +9,20 @@
class COpendriveRoad
{
+ friend class COpendriveRoadSegment;
+ friend class COpendriveRoadNode;
private:
std::vector<COpendriveRoadSegment *> segments;
+ std::vector<COpendriveRoadNode *> nodes;
double resolution;
double scale_factor;
double min_road_length;
protected:
COpendriveRoadSegment &operator[](std::string &key);
void link_segments(OpenDRIVE &open_drive);
+ void add_node(COpendriveRoadNode *node);
+ bool node_exists_at(const TOpendriveWorldPoint &pose);
+ COpendriveRoadNode* get_node_at(const TOpendriveWorldPoint &pose) const;
std::string get_junction_road_id(OpenDRIVE &open_drive,std::string &incoming,std::string &connecting);
public:
COpendriveRoad();
@@ -30,6 +36,8 @@ class COpendriveRoad
double get_min_road_length(void) const;
unsigned int get_num_segments(void) const;
const COpendriveRoadSegment& get_segment(unsigned int index) const;
+ unsigned int get_num_nodes(void) const;
+ const COpendriveRoadNode& get_node(unsigned int index) const;
const COpendriveRoadSegment& operator[](std::size_t index);
void operator=(const COpendriveRoad& object);
friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index 2db3d6e9f03431d187df958580a08aa38f16e990..49c920d7707284fbfbceebb5c6d561a7cac4330e 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -16,26 +16,29 @@ class COpendriveRoadNode
double resolution;
double scale_factor;
TOpendriveWorldPoint start_point;
- COpendriveLane *lane;
- COpendriveGeometry *geometry;
- unsigned int index;
+ std::vector<COpendriveLane *> lanes;
+ std::vector<COpendriveGeometry *> geometries;
+ std::vector<unsigned int> indexes;
protected:
+ void load(const planView::geometry_type &geom_info,COpendriveLane *lane);
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);
+ void set_index(unsigned int lane_index,unsigned int index);
+ void add_lane(const planView::geometry_type &geom_info,COpendriveLane *lane);
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;
+ unsigned int get_index(unsigned int lane_index) const;
TOpendriveWorldPoint get_start_pose(void) const;
unsigned int get_num_links(void) const;
const COpendriveLink &get_link(unsigned int index) const;
- const COpendriveLane &get_lane(void) const;
- const COpendriveGeometry &get_geometry(void) const;
+ unsigned int get_num_lanes(void) const;
+ const COpendriveLane &get_lane(unsigned int index) const;
+ unsigned int get_num_geometries(void) const;
+ const COpendriveGeometry &get_geometry(unsigned int index) const;
void operator=(const COpendriveRoadNode &object);
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
~COpendriveRoadNode();
diff --git a/include/opendrive_road_segment.h b/include/opendrive_road_segment.h
index fd83131f757d0ec48ebb547fc40ec8d1d7668f08..255b411e4d98a300123a8fcc54039dffa7b8f5bf 100644
--- a/include/opendrive_road_segment.h
+++ b/include/opendrive_road_segment.h
@@ -10,12 +10,14 @@
#include "opendrive_object.h"
class COpendriveLane;
+class COpendriveRoad;
class COpendriveRoadSegment
{
friend class COpendriveRoad;
private:
std::map<int,COpendriveLane *> lanes;
+ COpendriveRoad *parent_road;
double resolution;
double scale_factor;
double min_road_length;
@@ -27,10 +29,12 @@ class COpendriveRoadSegment
std::string name;
unsigned int id;
protected:
+ void load(OpenDRIVE::road_type &road_info);
void free(void);
void set_resolution(double res);
void set_scale_factor(double scale);
void set_min_road_length(double length);
+ void set_parent_road(COpendriveRoad *parent);
void update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs);
void add_lanes(lanes::laneSection_type &lane_section);
void add_nodes(OpenDRIVE::road_type &road_info);
@@ -40,12 +44,12 @@ class COpendriveRoadSegment
public:
COpendriveRoadSegment();
COpendriveRoadSegment(const COpendriveRoadSegment &object);
- void load(OpenDRIVE::road_type &road_info);
std::string get_name(void) const;
unsigned int get_id(void) const;
unsigned int get_num_right_lanes(void) const;
unsigned int get_num_left_lanes(void) const;
const COpendriveLane &get_lane(int index) const;
+ const COpendriveRoad &get_parent_road(void) const;
unsigned int get_num_signals(void) const;
const COpendriveSignal &get_signal(unsigned int index) const;
unsigned int get_num_obstacles(void) const;
diff --git a/include/opendrive_signal.h b/include/opendrive_signal.h
index 3f162933862ffa1450b6062a1e7e5deec9a6b367..f49a6f1923ddbee17087d857ac5c679ca92260a9 100644
--- a/include/opendrive_signal.h
+++ b/include/opendrive_signal.h
@@ -8,6 +8,7 @@
class COpendriveSignal
{
+ friend class COpendriveRoadSegment;
private:
int id; ///< Signal's id.
TOpendriveTrackPoint pose;
@@ -16,11 +17,11 @@ class COpendriveSignal
int value; ///< Signal's value.
std::string text; ///< Signal's text.
protected:
+ void load(signals::signal_type &signal_info);
public:
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(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 32c500c501cdc7e0e96eb1de2d8227b1f5316783..229dbac93bf2da3d6e646baeb46f2063f54e79d9 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -94,7 +94,11 @@ void COpendriveLane::add_node(COpendriveRoadNode *node)
if(this->nodes[i]==node)
return;
// add the new node
- node->set_index(this->nodes.size());
+ for(unsigned int i=0;i<node->get_num_lanes();i++)
+ {
+ if(&node->get_lane(i)==this)
+ node->set_index(i,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)
@@ -321,18 +325,16 @@ 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 << " Node " << i << ":" << std::endl;
out << *lane.nodes[i];
+ }
return out;
}
COpendriveLane::~COpendriveLane()
{
- for(unsigned int i=0;i<this->nodes.size();i++)
- {
- delete this->nodes[i];
- this->nodes[i]=NULL;
- }
this->nodes.clear();
this->left_lane=NULL;
this->left_mark=OD_MARK_NONE;
diff --git a/src/opendrive_link.cpp b/src/opendrive_link.cpp
index 4b68da248d93adb8e83554d4add15b8494925d6c..bb112cbd4d045711e89b34e9d77cb3738cb2e540 100644
--- a/src/opendrive_link.cpp
+++ b/src/opendrive_link.cpp
@@ -113,8 +113,32 @@ void COpendriveLink::operator=(const COpendriveLink &object)
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;
+ TOpendriveTrackPoint end_point_track;
+ TOpendriveWorldPoint end_point_world;
+
+ for(unsigned int i=0;i<link.get_prev().get_num_lanes();i++)
+ {
+ if(link.get_prev().lanes[i]->get_id()<0)
+ {
+ end_point_track.t=link.get_prev().lanes[i]->get_offset()-link.get_prev().lanes[i]->get_width()/2.0;
+ end_point_track.s=link.get_prev().geometries[i]->get_length();
+ }
+ else
+ {
+ end_point_track.t=link.get_prev().lanes[i]->get_offset()+link.get_prev().lanes[i]->get_width()/2.0;
+ end_point_track.s=0.0;
+ }
+ end_point_track.heading=0.0;
+ link.get_prev().geometries[i]->get_world_pose(end_point_track,end_point_world);
+ for(unsigned int j=0;j<link.get_next().get_num_lanes();j++)
+ {
+ if(link.get_prev().lanes[i]->get_segment().get_parent_road().get_node_at(end_point_world)==link.next)
+ {
+ out << " Previous node: " << link.get_prev().get_index(i) << " of lane " << link.get_prev().get_lane(i).get_id() << " of road " << link.get_prev().get_lane(i).get_segment().get_name() << std::endl;
+ out << " Next node: " << link.get_next().get_index(j) << " of lane " << link.get_next().get_lane(j).get_id() << " of road " << link.get_next().get_lane(j).get_segment().get_name() << std::endl;
+ }
+ }
+ }
out << " Road mark: ";
switch(link.mark)
{
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index b0c67e439912f492bd235f923816821d4ba93fb9..6699c2bfdd1e0e255a396ab0352abc8ebad2ab01 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -9,11 +9,13 @@ COpendriveRoad::COpendriveRoad()
this->scale_factor=DEFAULT_SCALE_FACTOR;
this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
this->segments.clear();
+ this->nodes.clear();
}
COpendriveRoad::COpendriveRoad(const COpendriveRoad& object)
{
COpendriveRoadSegment *segment;
+ COpendriveRoadNode *node;
std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> new_references;
this->resolution=object.resolution;
@@ -26,6 +28,12 @@ COpendriveRoad::COpendriveRoad(const COpendriveRoad& object)
this->segments.push_back(segment);
new_references[object.segments[i]]=segment;
}
+ this->nodes.clear();
+ for(unsigned int i=0;i<object.nodes.size();i++)
+ {
+ node=new COpendriveRoadNode(*object.nodes[i]);
+ this->nodes.push_back(node);
+ }
for(unsigned int i=0;i<this->segments.size();i++)
this->segments[i]->update_references(new_references);
}
@@ -74,6 +82,44 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
}
}
+void COpendriveRoad::add_node(COpendriveRoadNode *node)
+{
+ for(unsigned int i=0;i<this->nodes.size();i++)
+ {
+ if(this->nodes[i]==node)
+ throw CException(_HERE_,"Node already present");
+ }
+ this->nodes.push_back(node);
+}
+
+bool COpendriveRoad::node_exists_at(const TOpendriveWorldPoint &pose)
+{
+ TOpendriveWorldPoint node_pose;
+
+ for(unsigned int i=0;i<nodes.size();i++)
+ {
+ node_pose=this->nodes[i]->get_start_pose();
+ if(std::abs(pose.x-node_pose.x)<0.01 && std::abs(pose.y-node_pose.y)<0.01)
+ return true;
+ }
+
+ return false;
+}
+
+COpendriveRoadNode* COpendriveRoad::get_node_at(const TOpendriveWorldPoint &pose) const
+{
+ TOpendriveWorldPoint node_pose;
+
+ for(unsigned int i=0;i<nodes.size();i++)
+ {
+ node_pose=this->nodes[i]->get_start_pose();
+ if(std::abs(pose.x-node_pose.x)<0.01 && std::abs(pose.y-node_pose.y)<0.01)
+ return this->nodes[i];
+ }
+
+ return NULL;
+}
+
std::string COpendriveRoad::get_junction_road_id(OpenDRIVE &open_drive,std::string &incoming,std::string &connecting)
{
bool predecessor_match,successor_match;
@@ -128,6 +174,8 @@ void COpendriveRoad::load(const std::string &filename)
COpendriveRoadSegment *segment=new COpendriveRoadSegment();
segment->set_resolution(this->resolution);
segment->set_scale_factor(this->scale_factor);
+ segment->set_min_road_length(this->min_road_length);
+ segment->set_parent_road(this);
segment->load(*road_it);
this->segments.push_back(segment);
}catch(CException &e){
@@ -246,6 +294,19 @@ const COpendriveRoadSegment& COpendriveRoad::get_segment(unsigned int index) con
throw CException(_HERE_,"Invalid segment index");
}
+unsigned int COpendriveRoad::get_num_nodes(void) const
+{
+ return this->nodes.size();
+}
+
+const COpendriveRoadNode& COpendriveRoad::get_node(unsigned int index) const
+{
+ if(index>=0 && index<this->nodes.size())
+ return *this->nodes[index];
+ else
+ throw CException(_HERE_,"Invalid node index");
+}
+
const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
{
if(index>=0 && index<this->segments.size())
@@ -256,12 +317,18 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
void COpendriveRoad::operator=(const COpendriveRoad& object)
{
- COpendriveRoadSegment *segment;
+ COpendriveRoadSegment *segment;
+ COpendriveRoadNode *node;
std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> new_references;
this->resolution=object.resolution;
this->scale_factor=object.scale_factor;
this->min_road_length=object.min_road_length;
+ for(unsigned int i=0;i<this->segments.size();i++)
+ {
+ delete this->segments[i];
+ this->segments[i]=NULL;
+ }
this->segments.clear();
for(unsigned int i=0;i<object.segments.size();i++)
{
@@ -269,6 +336,17 @@ void COpendriveRoad::operator=(const COpendriveRoad& object)
this->segments.push_back(segment);
new_references[object.segments[i]]=segment;
}
+ for(unsigned int i=0;i<this->nodes.size();i++)
+ {
+ delete this->nodes[i];
+ this->nodes[i]=NULL;
+ }
+ this->nodes.clear();
+ for(unsigned int i=0;i<object.nodes.size();i++)
+ {
+ node=new COpendriveRoadNode(*object.nodes[i]);
+ this->nodes.push_back(node);
+ }
for(unsigned int i=0;i<this->segments.size();i++)
this->segments[i]->update_references(new_references);
}
@@ -278,6 +356,7 @@ std::ostream& operator<<(std::ostream& out, COpendriveRoad &road)
out << "Resolution: " << road.resolution << std::endl;
out << "Scale factor: " << road.scale_factor << std::endl;
out << "Minimum road lenght: " << road.min_road_length << std::endl;
+ out << "Total number of nodes: " << road.nodes.size() << std::endl;
for(unsigned int i=0;i<road.segments.size();i++)
std::cout << *road.segments[i];
@@ -287,7 +366,15 @@ std::ostream& operator<<(std::ostream& out, COpendriveRoad &road)
COpendriveRoad::~COpendriveRoad()
{
for(unsigned int i=0;i<this->segments.size();i++)
+ {
delete this->segments[i];
+ this->segments[i]=NULL;
+ }
+ for(unsigned int i=0;i<this->nodes.size();i++)
+ {
+ delete this->nodes[i];
+ this->nodes[i]=NULL;
+ }
this->segments.clear();
this->resolution=DEFAULT_RESOLUTION;
this->scale_factor=DEFAULT_SCALE_FACTOR;
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index 33a70c5d2626380ae9b543a04bacfe9553028e18..2c2b6f0b39b7cea100464e9929fd7be9aad3b294 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -1,4 +1,5 @@
#include "opendrive_road_node.h"
+#include "opendrive_road.h"
#include "exceptions.h"
COpendriveRoadNode::COpendriveRoadNode()
@@ -8,9 +9,9 @@ COpendriveRoadNode::COpendriveRoadNode()
this->start_point.x=0.0;
this->start_point.y=0.0;
this->start_point.heading=0.0;
- this->lane=NULL;
- this->geometry=NULL;
- this->index=-1;
+ this->lanes.clear();
+ this->geometries.clear();
+ this->indexes.clear();
}
COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
@@ -22,15 +23,19 @@ COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
this->start_point.x=object.start_point.x;
this->start_point.y=object.start_point.y;
this->start_point.heading=object.start_point.heading;
- this->lane=object.lane;
- this->geometry=object.geometry->clone();
+ this->lanes.resize(object.lanes.size());
+ for(unsigned int i=0;i<object.lanes.size();i++)
+ this->lanes[i]=object.lanes[i];
+ this->geometries.resize(object.geometries.size());
+ for(unsigned int i=0;i<object.geometries.size();i++)
+ this->geometries[i]=object.geometries[i]->clone();
this->links.clear();
for(unsigned int i=0;i<object.links.size();i++)
{
new_link=new COpendriveLink(*object.links[i]);
this->links.push_back(new_link);
}
- this->index=object.index;
+ this->indexes=object.indexes;
}
void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark)
@@ -38,15 +43,8 @@ void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
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");
- }
+ return;
new_link=new COpendriveLink();
new_link->set_prev(this);
new_link->set_next(node);
@@ -74,50 +72,93 @@ void COpendriveRoadNode::set_scale_factor(double scale)
this->links[i]->set_scale_factor(scale);
}
-void COpendriveRoadNode::set_index(unsigned int index)
+void COpendriveRoadNode::set_index(unsigned int lane_index,unsigned int index)
+{
+ if(lane_index>=0 && lane_index<this->lanes.size())
+ this->indexes[lane_index]=index;
+ else
+ throw CException(_HERE_,"Invalid lane index");
+}
+
+void COpendriveRoadNode::add_lane(const planView::geometry_type &geom_info,COpendriveLane *lane)
{
- this->index=index;
+ COpendriveGeometry *geometry;
+
+ // import geometry
+ if(geom_info.line().present())
+ geometry=new COpendriveLine();
+ else if(geom_info.spiral().present())
+ geometry=new COpendriveSpiral();
+ else if(geom_info.arc().present())
+ geometry=new COpendriveArc();
+ else if(geom_info.paramPoly3().present())
+ geometry=new COpendriveParamPoly3();
+ else
+ throw CException(_HERE_,"Unsupported or Missing geometry");
+ geometry->set_scale_factor(this->scale_factor);
+ geometry->load(geom_info);
+ this->geometries.push_back(geometry);
+ this->lanes.push_back(lane);
+ this->indexes.resize(this->lanes.size());
}
void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriveLane *lane)
{
TOpendriveTrackPoint track_point;
+ COpendriveGeometry *geometry;
- if(this->geometry!=NULL)
+ for(unsigned int i=0;i<this->geometries.size();i++)
{
- delete this->geometry;
- this->geometry=NULL;
+ delete this->geometries[i];
+ this->geometries[i]=NULL;
}
+ this->geometries.clear();
// import geometry
if(geom_info.line().present())
- this->geometry=new COpendriveLine();
+ geometry=new COpendriveLine();
else if(geom_info.spiral().present())
- this->geometry=new COpendriveSpiral();
+ geometry=new COpendriveSpiral();
else if(geom_info.arc().present())
- this->geometry=new COpendriveArc();
+ geometry=new COpendriveArc();
else if(geom_info.paramPoly3().present())
- this->geometry=new COpendriveParamPoly3();
+ geometry=new COpendriveParamPoly3();
else
throw CException(_HERE_,"Unsupported or Missing geometry");
- this->geometry->set_scale_factor(this->scale_factor);
- this->geometry->load(geom_info);
+ geometry->set_scale_factor(this->scale_factor);
+ geometry->load(geom_info);
// import start position
- track_point.s=0.0;
- track_point.t=lane->get_offset()+lane->get_width()/2.0;
+ if(lane->get_id()<0)
+ {
+ track_point.t=lane->get_offset()-lane->get_width()/2.0;
+ track_point.s=0.0;
+ }
+ else
+ {
+ track_point.t=lane->get_offset()+lane->get_width()/2.0;
+ track_point.s=geometry->get_length();
+ }
track_point.heading=0.0;
- if(!this->geometry->get_world_pose(track_point,this->start_point))
+ if(!geometry->get_world_pose(track_point,this->start_point))
{
- delete this->geometry;
- this->geometry=NULL;
+ delete geometry;
+ geometry=NULL;
throw CException(_HERE_,"Impossible to get world coordinates");
}
+ else
+ {
+ if(lane->get_id()>0)
+ this->start_point.heading=normalize_angle(this->start_point.heading+3.14159);
+ }
+ this->geometries.push_back(geometry);
for(unsigned int i=0;i<this->links.size();i++)
{
delete this->links[i];
this->links[i]=NULL;
}
this->links.clear();
- this->lane=lane;
+ this->lanes.clear();
+ this->lanes.push_back(lane);
+ this->indexes.resize(1);
}
double COpendriveRoadNode::get_resolution(void) const
@@ -130,9 +171,12 @@ double COpendriveRoadNode::get_scale_factor(void) const
return this->scale_factor;
}
-unsigned int COpendriveRoadNode::get_index(void) const
+unsigned int COpendriveRoadNode::get_index(unsigned int lane_index) const
{
- return this->index;
+ if(lane_index>=0 && lane_index<this->indexes.size())
+ return this->indexes[lane_index];
+ else
+ throw CException(_HERE_,"Invalid lane index");
}
TOpendriveWorldPoint COpendriveRoadNode::get_start_pose(void) const
@@ -153,14 +197,30 @@ const COpendriveLink &COpendriveRoadNode::get_link(unsigned int index) const
throw CException(_HERE_,"Invalid link index");
}
-const COpendriveLane &COpendriveRoadNode::get_lane(void) const
+unsigned int COpendriveRoadNode::get_num_lanes(void) const
{
- return *this->lane;
+ return this->lanes.size();
}
-const COpendriveGeometry &COpendriveRoadNode::get_geometry(void) const
+const COpendriveLane &COpendriveRoadNode::get_lane(unsigned int index) const
{
- return *this->geometry;
+ if(index>=0 && index<this->lanes.size())
+ return *this->lanes[index];
+ else
+ throw CException(_HERE_,"Invalid lane index");
+}
+
+unsigned int COpendriveRoadNode::get_num_geometries(void) const
+{
+ return this->geometries.size();
+}
+
+const COpendriveGeometry &COpendriveRoadNode::get_geometry(unsigned int index) const
+{
+ if(index>=0 && index<this->geometries.size())
+ return *this->geometries[index];
+ else
+ throw CException(_HERE_,"Invalid geometry index");
}
void COpendriveRoadNode::operator=(const COpendriveRoadNode &object)
@@ -172,8 +232,17 @@ void COpendriveRoadNode::operator=(const COpendriveRoadNode &object)
this->start_point.x=object.start_point.x;
this->start_point.y=object.start_point.y;
this->start_point.heading=object.start_point.heading;
- this->lane=object.lane;
- this->geometry=object.geometry->clone();
+ this->lanes.resize(object.lanes.size());
+ for(unsigned int i=0;i<object.lanes.size();i++)
+ this->lanes[i]=object.lanes[i];
+ for(unsigned int i=0;i<this->geometries.size();i++)
+ {
+ delete this->geometries[i];
+ this->geometries[i]=NULL;
+ }
+ this->geometries.resize(object.geometries.size());
+ for(unsigned int i=0;i<object.geometries.size();i++)
+ this->geometries[i]=object.geometries[i]->clone();
for(unsigned int i=0;i<this->links.size();i++)
{
delete this->links[i];
@@ -185,22 +254,20 @@ void COpendriveRoadNode::operator=(const COpendriveRoadNode &object)
new_link=new COpendriveLink(*object.links[i]);
this->links.push_back(new_link);
}
- this->index=object.index;
+ this->indexes=object.indexes;
}
std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node)
{
- out << " Node " << node.index << ":" << std::endl;
- out << " parent lane: " << node.lane->get_id() << 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;
- else
- out << " Does not have any associated geometry." << std::endl;
+ out << " Number of parent lanes: " << node.lanes.size() << std::endl;
+ for(unsigned int i=0;i<node.lanes.size();i++)
+ {
+ out << " Lane " << i << ":" << std::endl;
+ out << " lane id: " << node.lanes[i]->get_id() << std::endl;
+ out << *node.geometries[i];
+ }
out << " Number of links: " << node.links.size() << std::endl;
for(unsigned int i=0;i<node.links.size();i++)
{
@@ -217,12 +284,13 @@ COpendriveRoadNode::~COpendriveRoadNode()
this->start_point.x=0.0;
this->start_point.y=0.0;
this->start_point.heading=0.0;
- this->lane=NULL;
- if(this->geometry!=NULL)
+ this->lanes.clear();
+ for(unsigned int i=0;i<this->geometries.size();i++)
{
- delete this->geometry;
- this->geometry=NULL;
+ delete this->geometries[i];
+ this->geometries[i]=NULL;
}
+ this->geometries.clear();
for(unsigned int i=0;i<this->links.size();i++)
{
delete this->links[i];
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index 8f64af09ff945985d9ac0f1606f8aa67dc624463..911130c5114af79c8bb9ef872e8ff2a3b263b573 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -1,4 +1,5 @@
#include "opendrive_road_segment.h"
+#include "opendrive_road.h"
#include "exceptions.h"
COpendriveRoadSegment::COpendriveRoadSegment()
@@ -101,6 +102,11 @@ void COpendriveRoadSegment::set_min_road_length(double length)
this->min_road_length=length;
}
+void COpendriveRoadSegment::set_parent_road(COpendriveRoad *parent)
+{
+ this->parent_road=parent;
+}
+
void COpendriveRoadSegment::update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs)
{
for(unsigned int i=0;i<this->connecting.size();i++)
@@ -157,6 +163,7 @@ 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;
+ TOpendriveWorldPoint node_pose;
double lane_offset;
try{
@@ -170,8 +177,17 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
new_node->set_scale_factor(this->scale_factor);
this->lanes[i]->set_offset(lane_offset);
new_node->load(*geom_it,this->lanes[i]);
- if(new_node->get_geometry().get_length()>this->min_road_length)
+ if(new_node->get_geometry(0).get_length()>this->min_road_length)
{
+ node_pose=new_node->get_start_pose();
+ if(this->parent_road->node_exists_at(node_pose))
+ {
+ delete new_node;
+ new_node=this->parent_road->get_node_at(node_pose);
+ new_node->add_lane(*geom_it,this->lanes[i]);
+ }
+ else
+ this->parent_road->add_node(new_node);
this->lanes[i]->add_node(new_node);
lane_offset-=this->lanes[i]->get_width();
}
@@ -188,8 +204,17 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
new_node->set_scale_factor(this->scale_factor);
this->lanes[i]->set_offset(lane_offset);
new_node->load(*geom_it,this->lanes[i]);
- if(new_node->get_geometry().get_length()>this->min_road_length)
+ if(new_node->get_geometry(0).get_length()>this->min_road_length)
{
+ node_pose=new_node->get_start_pose();
+ if(this->parent_road->node_exists_at(node_pose))
+ {
+ delete new_node;
+ new_node=this->parent_road->get_node_at(node_pose);
+ new_node->add_lane(*geom_it,this->lanes[i]);
+ }
+ else
+ this->parent_road->add_node(new_node);
this->lanes[i]->add_node(new_node);
lane_offset+=this->lanes[i]->get_width();
}
@@ -400,6 +425,11 @@ const COpendriveLane &COpendriveRoadSegment::get_lane(int index) const
throw CException(_HERE_,"invalid lane index");
}
+const COpendriveRoad &COpendriveRoadSegment::get_parent_road(void) const
+{
+ return *this->parent_road;
+}
+
unsigned int COpendriveRoadSegment::get_num_signals(void) const
{
return this->signals.size();