Commit 4e0fb284 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added a reference to all the segments to which the node belongs.

Added functions to create a copy of the node and update its references.
Added a function to clean any invalid reference.
parent 2e8b5c3d
......@@ -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();
};
......
......@@ -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();
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment