diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index e006cb5cf12c8dd44b5a7df820b75a6011b3ecfc..f660aeb58d4b02f58842c01a44dfec150cec1c45 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -2,18 +2,17 @@
#define _OPENDRIVE_ROAD_NODE_H
#include <vector>
+#include "opendrive_common.h"
#include "opendrive_link.h"
#include "opendrive_lane.h"
#include "opendrive_road_segment.h"
-class COpendriveLane;
-class COpendriveRoadSegment;
-
class COpendriveRoadNode
{
friend class COpendriveLane;
friend class COpendriveRoadSegment;
friend class COpendriveRoad;
+ friend class COpendriveLink;
private:
std::vector<COpendriveLink *> links;
double resolution;
@@ -21,26 +20,33 @@ class COpendriveRoadNode
TOpendriveWorldPoint start_point;
std::vector<COpendriveLane *> lanes;
std::vector<COpendriveGeometry *> geometries;
- COpendriveRoadSegment *parent_segment;
+ std::vector<COpendriveRoadSegment *>parents;
unsigned int index;
protected:
COpendriveRoadNode();
COpendriveRoadNode(const COpendriveRoadNode &object);
void load(const planView::geometry_type &geom_info,COpendriveLane *lane);
- void add_link(COpendriveRoadNode *node,opendrive_mark_t mark);
+ void add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane);
+ void add_link(COpendriveLink *link);
+ void remove_link(COpendriveLink *link);
void set_resolution(double res);
void set_scale_factor(double scale);
void set_index(unsigned int index);
- void set_parent_segment(COpendriveRoadSegment *segment);
+ void set_start_point(TOpendriveWorldPoint &start);
+ void add_parent_segment(COpendriveRoadSegment *segment);
void add_lane(const planView::geometry_type &geom_info,COpendriveLane *lane);
- void update_references(std::map<COpendriveRoadNode *,COpendriveRoadNode *> refs);
- void update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs);
- void update_references(std::map<COpendriveLane *,COpendriveLane *> refs);
+ bool updated(node_up_ref_t &refs);
+ void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
+ void clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
+ void update_start_pose(COpendriveLane *lane,TOpendriveWorldPoint *start=NULL);
+ COpendriveRoadNode *update_end_pose(COpendriveLane *lane,TOpendriveWorldPoint *end=NULL);
+ COpendriveLink *get_link_with(COpendriveRoadNode *end);
public:
double get_resolution(void) const;
double get_scale_factor(void) const;
unsigned int get_index(void) const;
- const COpendriveRoadSegment& get_parent_segment(void) const;
+ unsigned int get_num_parent_segments(void) const;
+ const COpendriveRoadSegment& get_parent_segment(unsigned int index) const;
TOpendriveWorldPoint get_start_pose(void) const;
unsigned int get_num_links(void) const;
const COpendriveLink &get_link(unsigned int index) const;
@@ -49,9 +55,9 @@ class COpendriveRoadNode
const COpendriveLane &get_lane(unsigned int index) const;
unsigned int get_num_geometries(void) const;
const COpendriveGeometry &get_geometry(unsigned int index) const;
- unsigned int get_closest_link(double x, double y,double heading,double angle_tol=0.1) const;
- double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1) const;
- void get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1) const;
+ unsigned int get_closest_link(TOpendriveWorldPoint &point,double angle_tol=0.1) const;
+ double get_closest_point(TOpendriveWorldPoint &point,TOpendriveWorldPoint &closest_point,double angle_tol=0.1) const;
+ void get_closest_points(TOpendriveWorldPoint &point,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1) const;
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
~COpendriveRoadNode();
};
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index 31c212895ce180ce9fd812740914fed8a7717398..edbf921b0cf7d7ceae650e7aa2e84774bc4c6a94 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -13,7 +13,7 @@ COpendriveRoadNode::COpendriveRoadNode()
this->lanes.clear();
this->geometries.clear();
this->index=-1;
- this->parent_segment=NULL;
+ this->parents.clear();
}
COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
@@ -25,9 +25,7 @@ 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->lanes.resize(object.lanes.size());
- for(unsigned int i=0;i<object.lanes.size();i++)
- this->lanes[i]=object.lanes[i];
+ this->lanes=object.lanes;
this->geometries.resize(object.geometries.size());
for(unsigned int i=0;i<object.geometries.size();i++)
this->geometries[i]=object.geometries[i]->clone();
@@ -38,10 +36,10 @@ COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
this->links.push_back(new_link);
}
this->index=object.index;
- this->parent_segment=object.parent_segment;
+ this->parents=object.parents;
}
-void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark)
+void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane)
{
TOpendriveWorldPoint lane_end,node_start;
double start_curvature,end_curvature,length;
@@ -57,6 +55,8 @@ void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
new_link->set_resolution(this->resolution);
new_link->set_scale_factor(this->scale_factor);
new_link->set_road_mark(mark);
+ new_link->set_parent_segment(segment);
+ new_link->set_parent_lane(lane);
// get the curvature
node_start=node->get_start_pose();
for(unsigned int i=0;i<this->lanes.size();i++)
@@ -89,6 +89,32 @@ void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
node->links.push_back(new_link);
}
+void COpendriveRoadNode::add_link(COpendriveLink *link)
+{
+ for(unsigned int i=0;i<this->links.size();i++)
+ if(this->links[i]==link)
+ return;
+
+ this->links.push_back(link);
+}
+
+void COpendriveRoadNode::remove_link(COpendriveLink *link)
+{
+ std::vector<COpendriveLink *>::iterator it;
+
+ for(it=this->links.begin();it!=this->links.end();)
+ {
+ if((*it)==link)
+ {
+ delete (*it);
+ it=this->links.erase(it);
+ break;
+ }
+ else
+ it++;
+ }
+}
+
void COpendriveRoadNode::set_resolution(double res)
{
this->resolution=res;
@@ -110,9 +136,18 @@ void COpendriveRoadNode::set_index(unsigned int index)
this->index=index;
}
-void COpendriveRoadNode::set_parent_segment(COpendriveRoadSegment *segment)
+void COpendriveRoadNode::set_start_point(TOpendriveWorldPoint &start)
+{
+ this->start_point=start;
+}
+
+void COpendriveRoadNode::add_parent_segment(COpendriveRoadSegment *segment)
{
- this->parent_segment=segment;
+ for(unsigned int i=0;i<this->parents.size();i++)
+ if(this->parents[i]==segment)
+ return;
+
+ this->parents.push_back(segment);
}
void COpendriveRoadNode::add_lane(const planView::geometry_type &geom_info,COpendriveLane *lane)
@@ -194,21 +229,198 @@ void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriv
this->lanes.push_back(lane);
}
-void COpendriveRoadNode::update_references(std::map<COpendriveRoadNode *,COpendriveRoadNode *> refs)
+bool COpendriveRoadNode::updated(node_up_ref_t &refs)
{
- for(unsigned int i=0;i<this->links.size();i++)
- this->links[i]->update_references(refs);
+ node_up_ref_t::iterator updated_it;
+
+ for(updated_it=refs.begin();updated_it!=refs.end();updated_it++)
+ if(updated_it->second==this)
+ return true;
+
+ return false;
}
-void COpendriveRoadNode::update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs)
+void COpendriveRoadNode::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
{
- this->parent_segment=refs[this->parent_segment];
+ std::vector<COpendriveRoadSegment *>::iterator seg_it;
+ std::vector<COpendriveLane *>::iterator lane_it;
+ std::vector<COpendriveLink *>::iterator link_it;
+
+ if(this->updated(node_refs))
+ {
+ for(link_it=this->links.begin();link_it!=this->links.end();link_it++)
+ (*link_it)->update_references(segment_refs,lane_refs,node_refs);
+ for(seg_it=this->parents.begin();seg_it!=this->parents.end();seg_it++)
+ if(segment_refs.find(*seg_it)!=segment_refs.end())
+ (*seg_it)=segment_refs[*seg_it];
+ for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
+ if(lane_refs.find(*lane_it)!=lane_refs.end())
+ (*lane_it)=lane_refs[*lane_it];
+ }
}
-void COpendriveRoadNode::update_references(std::map<COpendriveLane *,COpendriveLane *> refs)
+void COpendriveRoadNode::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
{
- for(unsigned int i=0;i<this->lanes.size();i++)
- this->lanes[i]=refs[this->lanes[i]];
+ std::vector<COpendriveRoadSegment *>::iterator seg_it;
+ std::vector<COpendriveLane *>::iterator lane_it;
+ std::vector<COpendriveLink *>::iterator link_it;
+ unsigned int index=0;
+
+ if(this->updated(node_refs))
+ {
+ for(link_it=this->links.begin();link_it!=this->links.end();)
+ {
+ if((*link_it)->clean_references(node_refs))
+ link_it=this->links.erase(link_it);
+ else
+ link_it++;
+ }
+ for(seg_it=this->parents.begin();seg_it!=this->parents.end();)
+ {
+ if(segment_refs.find(*seg_it)!=segment_refs.end())
+ {
+ (*seg_it)=segment_refs[*seg_it];
+ seg_it++;
+ }
+ else if(!(*seg_it)->updated(segment_refs))
+ seg_it=this->parents.erase(seg_it);
+ else
+ seg_it++;
+ }
+ for(index=0,lane_it=this->lanes.begin();lane_it!=this->lanes.end();index++)
+ {
+ if(lane_refs.find(*lane_it)!=lane_refs.end())
+ {
+ (*lane_it)=lane_refs[*lane_it];
+ lane_it++;
+ }
+ else if(!(*lane_it)->updated(lane_refs))
+ {
+ lane_it=this->lanes.erase(lane_it);
+ this->geometries.erase(this->geometries.begin()+index);
+ }
+ else
+ lane_it++;
+ }
+ }
+}
+
+void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldPoint *start)
+{
+ std::vector<COpendriveLane *>::iterator lane_it;
+ std::vector<COpendriveLink *>::iterator link_it;
+ unsigned int i;
+ double length;
+
+ if(start==NULL)
+ return;
+ // remove the references to all lanes and segments except for lane
+ for(i=0,lane_it=this->lanes.begin();lane_it!=this->lanes.end();i++)
+ {
+ if(*lane_it!=lane)
+ {
+ lane_it=this->lanes.erase(lane_it);
+ this->geometries.erase(this->geometries.begin()+i);
+ this->parents.erase(this->parents.begin()+i);
+ }
+ else
+ lane_it++;
+ }
+ length=this->get_closest_point(*start,this->start_point,3.14159);
+ // update geometry
+ for(unsigned int i=0;i<this->geometries.size();i++)
+ {
+ if(lane->get_id()<0)
+ {
+ this->geometries[i]->pose.x=this->start_point.x*this->scale_factor;
+ this->geometries[i]->pose.y=this->start_point.y*this->scale_factor;
+ this->geometries[i]->pose.heading=this->start_point.heading;
+ }
+ this->geometries[i]->max_s-=length*this->scale_factor;
+ }
+ // update the links
+ for(link_it=this->links.begin();link_it!=this->links.end();)
+ {
+ if((*link_it)->next==this)
+ {
+ delete *link_it;
+ link_it=this->links.erase(link_it);
+ }
+ else
+ {
+ (*link_it)->update_start_pose(start);
+ link_it++;
+ }
+ }
+}
+
+COpendriveRoadNode *COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPoint *end)
+{
+ std::vector<COpendriveLane *>::iterator lane_it;
+ std::vector<COpendriveLink *>::iterator link_it;
+ COpendriveRoadNode *new_node=NULL;
+ TOpendriveWorldPoint end_point;
+ unsigned int i;
+ double length;
+
+ if(end==NULL)
+ return NULL;
+ // remove the references to all lanes and segments except for lane
+ for(i=0,lane_it=this->lanes.begin();lane_it!=this->lanes.end();i++)
+ {
+ if(*lane_it!=lane)
+ {
+ lane_it=this->lanes.erase(lane_it);
+ this->geometries.erase(this->geometries.begin()+i);
+ this->parents.erase(this->parents.begin()+i);
+ }
+ else
+ lane_it++;
+ }
+ length=this->get_closest_point(*end,end_point,3.14159);
+ // update geometry
+ for(unsigned int i=0;i<this->geometries.size();i++)
+ this->geometries[i]->max_s=length*this->scale_factor;
+ // update the links
+ for(i=0,link_it=this->links.begin();link_it!=this->links.end();i++)
+ {
+ if((*link_it)->prev==this)
+ {
+ if((*link_it)->lane==lane)
+ {
+ // create a dummy node
+ new_node=new COpendriveRoadNode();
+ new_node->set_resolution(this->resolution);
+ new_node->set_scale_factor(this->scale_factor);
+ new_node->set_start_point(end_point);
+ (*link_it)->set_next(new_node);
+ new_node->add_link(*link_it);
+ // update the end position of the link
+ (*link_it)->update_end_pose(end);
+ link_it++;
+ }
+ else
+ {
+ delete *link_it;
+ link_it=this->links.erase(link_it);
+ }
+ }
+ else
+ link_it++;
+ }
+
+ return new_node;
+}
+
+COpendriveLink *COpendriveRoadNode::get_link_with(COpendriveRoadNode *end)
+{
+ for(unsigned int i=0;i<this->links.size();i++)
+ {
+ if(this->links[i]->prev==end || this->links[i]->next==end)
+ return this->links[i];
+ }
+
+ return NULL;
}
double COpendriveRoadNode::get_resolution(void) const
@@ -226,9 +438,17 @@ unsigned int COpendriveRoadNode::get_index(void) const
return this->index;
}
-const COpendriveRoadSegment &COpendriveRoadNode::get_parent_segment(void) const
+unsigned int COpendriveRoadNode::get_num_parent_segments(void) const
+{
+ return this->parents.size();
+}
+
+const COpendriveRoadSegment &COpendriveRoadNode::get_parent_segment(unsigned int index) const
{
- return *this->parent_segment;
+ if(index>=0 && index<this->parents.size())
+ return *this->parents[index];
+ else
+ throw CException(_HERE_,"Invalid parent index");
}
TOpendriveWorldPoint COpendriveRoadNode::get_start_pose(void) const
@@ -286,26 +506,22 @@ const COpendriveGeometry &COpendriveRoadNode::get_geometry(unsigned int index) c
throw CException(_HERE_,"Invalid geometry index");
}
-unsigned int COpendriveRoadNode::get_closest_link(double x, double y,double heading,double angle_tol)const
+unsigned int COpendriveRoadNode::get_closest_link(TOpendriveWorldPoint &point,double angle_tol)const
{
double dist,min_dist=std::numeric_limits<double>::max();
- TOpendriveWorldPoint target;
- unsigned int closest_index;
+ unsigned int closest_index=-1;
double angle;
- TPoint point;
+ TPoint spline_point;
- target.x=x;
- target.y=y;
- target.heading=heading;
for(unsigned int i=0;i<this->links.size();i++)
{
if(&this->links[i]->get_prev()==this)// links starting at this node
{
- this->links[i]->find_closest_world_point(target,point);
- angle=diff_angle(normalize_angle(heading),point.heading);
+ this->links[i]->find_closest_world_point(point,spline_point);
+ angle=diff_angle(normalize_angle(point.heading),spline_point.heading);
if(fabs(angle)<angle_tol)
{
- dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
+ dist=sqrt(pow(spline_point.x-point.x,2.0)+pow(spline_point.y-point.y,2.0));
if(dist<min_dist)
{
min_dist=dist;
@@ -318,32 +534,31 @@ unsigned int COpendriveRoadNode::get_closest_link(double x, double y,double head
return closest_index;
}
-double COpendriveRoadNode::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol) const
+double COpendriveRoadNode::get_closest_point(TOpendriveWorldPoint &point,TOpendriveWorldPoint &closest_point,double angle_tol) const
{
double dist,min_dist=std::numeric_limits<double>::max();
double angle;
- double length,closest_length;
- TOpendriveWorldPoint target;
- TPoint point;
+ double length,closest_length=std::numeric_limits<double>::max();
+ TPoint spline_point;
- target.x=x;
- target.y=y;
- target.heading=heading;
+ closest_point.x=std::numeric_limits<double>::max();
+ closest_point.y=std::numeric_limits<double>::max();
+ closest_point.heading=std::numeric_limits<double>::max();
for(unsigned int i=0;i<this->links.size();i++)
{
if(&this->links[i]->get_prev()==this)// links starting at this node
{
- length=this->links[i]->find_closest_world_point(target,point);
- angle=diff_angle(normalize_angle(heading),point.heading);
+ length=this->links[i]->find_closest_world_point(point,spline_point);
+ angle=diff_angle(normalize_angle(point.heading),spline_point.heading);
if(fabs(angle)<angle_tol)
{
- dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
+ dist=sqrt(pow(spline_point.x-point.x,2.0)+pow(spline_point.y-point.y,2.0));
if(dist<min_dist)
{
min_dist=dist;
- closest_point.x=point.x;
- closest_point.y=point.y;
- closest_point.heading=point.heading;
+ closest_point.x=spline_point.x;
+ closest_point.y=spline_point.y;
+ closest_point.heading=spline_point.heading;
closest_length=length;
}
}
@@ -353,33 +568,30 @@ double COpendriveRoadNode::get_closest_point(double x, double y,double heading,T
return closest_length;
}
-void COpendriveRoadNode::get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol) const
+void COpendriveRoadNode::get_closest_points(TOpendriveWorldPoint &point,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol) const
{
double dist;
double angle;
double length;
- TOpendriveWorldPoint target,new_point;
- TPoint point;
+ TOpendriveWorldPoint new_point;
+ TPoint spline_point;
- target.x=x;
- target.y=y;
- target.heading=heading;
closest_points.clear();
closest_lengths.clear();
for(unsigned int i=0;i<this->links.size();i++)
{
if(&this->links[i]->get_prev()==this)// links starting at this node
{
- length=this->links[i]->find_closest_world_point(target,point);
- angle=diff_angle(normalize_angle(heading),point.heading);
+ length=this->links[i]->find_closest_world_point(point,spline_point);
+ angle=diff_angle(normalize_angle(point.heading),spline_point.heading);
if(fabs(angle)<angle_tol)
{
- dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
+ dist=sqrt(pow(spline_point.x-point.x,2.0)+pow(spline_point.y-point.y,2.0));
if(dist<=dist_tol)
{
- new_point.x=point.x;
- new_point.y=point.y;
- new_point.heading=point.heading;
+ new_point.x=spline_point.x;
+ new_point.y=spline_point.y;
+ new_point.heading=spline_point.heading;
closest_points.push_back(new_point);
closest_lengths.push_back(length);
}
@@ -392,6 +604,9 @@ std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node)
{
out << " Node: " << node.get_index() << std::endl;
out << " start point: x: " << node.start_point.x << " y: " << node.start_point.y << " heading: " << node.start_point.heading << std::endl;
+ out << " Number of parent segments: " << node.get_num_parent_segments() << std::endl;
+ for(unsigned int i=0;i<node.get_num_parent_segments();i++)
+ out << " " << node.get_parent_segment(i).get_name() << std::endl;
out << " Number of parent lanes: " << node.lanes.size() << std::endl;
for(unsigned int i=0;i<node.lanes.size();i++)
{
@@ -430,6 +645,7 @@ COpendriveRoadNode::~COpendriveRoadNode()
this->links[i]=NULL;
}
}
+ this->parents.clear();
this->links.clear();
}