diff --git a/include/opendrive_lane.h b/include/opendrive_lane.h
index 1a615d4c9b8a03327a848223ed1e431138210287..988dc2338c62e28bbdd180480eb5f38f78396ca4 100644
--- a/include/opendrive_lane.h
+++ b/include/opendrive_lane.h
@@ -54,8 +54,8 @@ class COpendriveLane
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_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *start=NULL);
- COpendriveRoadNode *update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *end=NULL);
- COpendriveLane *get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,COpendriveRoadNode **new_node,TOpendriveWorldPoint *start=NULL,TOpendriveWorldPoint *end=NULL);
+ void update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *end=NULL);
+ COpendriveLane *get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *start=NULL,TOpendriveWorldPoint *end=NULL);
COpendriveLane *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref);
public:
unsigned int get_num_nodes(void) const;
diff --git a/include/opendrive_road.h b/include/opendrive_road.h
index 43d76f2a817d9a9eb3215f4fbe319d1c384fba6e..a00e0a9398528984ca5de063302612355b9cba91 100644
--- a/include/opendrive_road.h
+++ b/include/opendrive_road.h
@@ -30,6 +30,7 @@ class COpendriveRoad
void remove_node(COpendriveRoadNode *node);
unsigned int add_lane(COpendriveLane *lane);
void remove_lane(COpendriveLane *lane);
+ void complete_open_lanes(void);
void add_segment(COpendriveRoadSegment *segment,std::vector<unsigned int> &old_path,std::vector<unsigned int> &new_path);
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);
@@ -54,7 +55,7 @@ class COpendriveRoad
unsigned int get_closest_node(TOpendriveWorldPoint &point,double angle_tol=0.1);
double get_closest_point(TOpendriveWorldPoint &point,TOpendriveWorldPoint &closest_point,double angle_tol=0.1);
void get_closest_points(TOpendriveWorldPoint &point,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1);
- void get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPoint &start_point,TOpendriveWorldPoint &end_point,COpendriveRoad &new_road);
+ std::vector<unsigned int> get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPoint &start_point,TOpendriveWorldPoint &end_point,COpendriveRoad &new_road);
void get_sub_road(TOpendriveWorldPoint &start_point,double length,COpendriveRoad &new_road);
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 f660aeb58d4b02f58842c01a44dfec150cec1c45..6e919e2b163f7a8c93def8a4477bd3c5bc3c37e8 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -39,7 +39,7 @@ class COpendriveRoadNode
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);
+ void update_end_pose(COpendriveLane *lane,TOpendriveWorldPoint *end=NULL);
COpendriveLink *get_link_with(COpendriveRoadNode *end);
public:
double get_resolution(void) const;
@@ -56,6 +56,7 @@ class COpendriveRoadNode
unsigned int get_num_geometries(void) const;
const COpendriveGeometry &get_geometry(unsigned int index) const;
unsigned int get_closest_link(TOpendriveWorldPoint &point,double angle_tol=0.1) const;
+ double get_closest_distance(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);
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index a523f0b26176c523d690065be6f4d6bd13ac0243..802b9df51321eb10b0ba6874354c32a47fc97d87 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -129,7 +129,6 @@ void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool f
}
start->add_link(end,mark,this->segment,this);
// link lane
- // link laness
this->add_next_lane(lane);
lane->add_prev_lane(this);
}
@@ -374,7 +373,7 @@ void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t
this->nodes[0]->update_start_pose(this,start);
}
-COpendriveRoadNode *COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *end)
+void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *end)
{
std::vector<COpendriveRoadNode *>::iterator it;
segment_up_ref_t new_segment_ref;
@@ -384,7 +383,7 @@ COpendriveRoadNode *COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,
bool exists;
if(end==NULL)
- return NULL;
+ return;
end_node_index=this->get_closest_node(*end,3.14159);
// eliminate all the node before the start one
for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
@@ -411,18 +410,10 @@ COpendriveRoadNode *COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,
}
this->next.clear();// it is no longer connected to any next lane
this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
- new_node=this->nodes[this->nodes.size()-1]->update_end_pose(this,end);
- if(new_node!=NULL)
- {
- this->add_node(new_node);
- new_node_ref[new_node]=new_node;
- this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
- }
-
- return new_node;
+ this->nodes[this->nodes.size()-1]->update_end_pose(this,end);
}
-COpendriveLane *COpendriveLane::get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,COpendriveRoadNode **new_node,TOpendriveWorldPoint *start,TOpendriveWorldPoint *end)
+COpendriveLane *COpendriveLane::get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *start,TOpendriveWorldPoint *end)
{
COpendriveLane *new_lane;
@@ -433,12 +424,12 @@ COpendriveLane *COpendriveLane::get_sub_lane(node_up_ref_t &new_node_ref,lane_up
if(this->id<0)
{
new_lane->update_start_node(new_node_ref,new_lane_ref,start);
- (*new_node)=new_lane->update_end_node(new_node_ref,new_lane_ref,end);
+ new_lane->update_end_node(new_node_ref,new_lane_ref,end);
}
else
{
new_lane->update_start_node(new_node_ref,new_lane_ref,end);
- (*new_node)=new_lane->update_end_node(new_node_ref,new_lane_ref,start);
+ new_lane->update_end_node(new_node_ref,new_lane_ref,start);
}
return new_lane;
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index 53ad80e6d35bf8435efe0b07e8e4ff3342fc32c8..7fcf98c6fa8aa893aedd37c884a1504423a8c0d8 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -240,6 +240,41 @@ void COpendriveRoad::remove_lane(COpendriveLane *lane)
}
}
+void COpendriveRoad::complete_open_lanes(void)
+{
+ std::vector<COpendriveLane *>::iterator lane_it;
+ COpendriveRoadNode *new_node;
+ TOpendriveWorldPoint end_point;
+ unsigned int new_node_index;
+
+ // remove all nodes and lanes not present in the path
+ for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
+ {
+ if((*lane_it)->next.empty())// lane is not connected
+ {
+ if((*lane_it)->get_id()<0)
+ end_point=(*lane_it)->get_end_point();
+ else
+ end_point=(*lane_it)->get_start_point();
+ if(!this->node_exists_at(end_point))// there is no node at the end point
+ {
+ 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);
+ new_node_index=this->add_node(new_node);
+ new_node->set_index(new_node_index);
+ (*lane_it)->add_node(new_node);
+ (*lane_it)->segment->add_node(new_node);
+ }
+ else
+ lane_it++;
+ }
+ else
+ lane_it++;
+ }
+}
+
void COpendriveRoad::add_segment(COpendriveRoadSegment *segment,std::vector<unsigned int> &old_path,std::vector<unsigned int> &new_path)
{
for(unsigned int i=0;i<this->segments.size();i++)
@@ -263,7 +298,7 @@ void COpendriveRoad::add_segment(COpendriveRoadSegment *segment,std::vector<unsi
{
for(unsigned int j=0;j<old_path.size();j++)
if(old_path[j]==segment->nodes[i]->index)
- new_path.push_back(this->nodes.size());
+ new_path[j]=this->nodes.size();
segment->nodes[i]->set_index(this->nodes.size());
this->nodes.push_back(segment->nodes[i]);
}
@@ -405,6 +440,16 @@ void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
else
lane_it++;
}
+ // re-index all nodes and the path
+ for(unsigned int i=0;i<this->nodes.size();i++)
+ {
+ for(unsigned int j=0;j<path_nodes.size();j++)
+ {
+ if(this->nodes[i]->get_index()==path_nodes[j])
+ path_nodes[j]=i;
+ }
+ this->nodes[i]->set_index(i);
+ }
// remove links to non-consecutive nodes
for(unsigned int i=0;i<this->nodes.size();i++)
@@ -627,17 +672,19 @@ void COpendriveRoad::get_closest_points(TOpendriveWorldPoint &point,std::vector<
}
}
-void COpendriveRoad::get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPoint &start_point,TOpendriveWorldPoint &end_point,COpendriveRoad &new_road)
+std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPoint &start_point,TOpendriveWorldPoint &end_point,COpendriveRoad &new_road)
{
segment_up_ref_t new_segment_ref;
lane_up_ref_t new_lane_ref;
node_up_ref_t new_node_ref;
- COpendriveRoadNode *node,*next_node;
- COpendriveRoadSegment *segment,*new_segment;
+ COpendriveRoadNode *node,*next_node,*repeat_node,*repeat_next_node;
+ COpendriveRoadSegment *segment,*new_segment,*repeat_segment;
std::vector<unsigned int> new_path_nodes;
unsigned int link_index;
+ bool added,repeated;
int node_side;
- bool added;
+
+ new_path_nodes.resize(path_nodes.size());
new_road.free();
new_road.set_resolution(this->resolution);
@@ -668,7 +715,30 @@ void COpendriveRoad::get_sub_road(std::vector<unsigned int> &path_nodes,TOpendri
if(i==0)
{
node_side=segment->get_node_side(node);
- new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,node_side,&start_point,NULL);
+ repeated=false;
+ for(unsigned int j=1;j<path_nodes.size();j++)
+ {
+ repeat_node=this->nodes[path_nodes[j]];
+ if(j==path_nodes.size()-1)
+ {
+ link_index=node->get_closest_link(end_point,3.14159);
+ repeat_segment=node->links[link_index]->segment;
+ }
+ else
+ {
+ repeat_next_node=this->nodes[path_nodes[j+1]];
+ repeat_segment=repeat_node->get_link_with(repeat_next_node)->segment;
+ }
+ if(segment==repeat_segment)
+ {
+ repeated=true;
+ break;
+ }
+ }
+ if(repeated)
+ new_segment=segment->clone(new_node_ref,new_lane_ref);
+ else
+ new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,node_side,&start_point,NULL);
}
else
new_segment=segment->clone(new_node_ref,new_lane_ref);
@@ -680,10 +750,25 @@ void COpendriveRoad::get_sub_road(std::vector<unsigned int> &path_nodes,TOpendri
node=this->nodes[path_nodes[path_nodes.size()-1]];
link_index=node->get_closest_link(end_point,3.14159);
segment=node->links[link_index]->segment;
- node_side=segment->get_node_side(node);
- new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,node_side,NULL,&end_point);
- new_road.add_segment(new_segment,path_nodes,new_path_nodes);
- new_segment_ref[segment]=new_segment;
+ repeated=false;
+ for(unsigned int j=0;j<path_nodes.size()-1;j++)
+ {
+ repeat_node=this->nodes[path_nodes[j]];
+ repeat_next_node=this->nodes[path_nodes[j+1]];
+ repeat_segment=repeat_node->get_link_with(repeat_next_node)->segment;
+ if(segment==repeat_segment)
+ {
+ repeated=true;
+ break;
+ }
+ }
+ if(!repeated)
+ {
+ node_side=segment->get_node_side(node);
+ new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,node_side,NULL,&end_point);
+ new_road.add_segment(new_segment,path_nodes,new_path_nodes);
+ new_segment_ref[segment]=new_segment;
+ }
}
new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);
// add additional nodes not explicitly in the path
@@ -712,6 +797,9 @@ void COpendriveRoad::get_sub_road(std::vector<unsigned int> &path_nodes,TOpendri
// remove unconnected elements
new_road.prune(new_path_nodes);
new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref);
+ new_road.complete_open_lanes();
+
+ return new_path_nodes;
}
void COpendriveRoad::get_sub_road(TOpendriveWorldPoint &start_point,double length,COpendriveRoad &road)
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index edbf921b0cf7d7ceae650e7aa2e84774bc4c6a94..f6a3e74db47bd57a85205dc05bde248c674b1e5c 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -354,17 +354,16 @@ void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldP
}
}
-COpendriveRoadNode *COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPoint *end)
+void 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;
+ 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++)
{
@@ -382,34 +381,19 @@ COpendriveRoadNode *COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOp
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++)
+ for(link_it=this->links.begin();link_it!=this->links.end();)
{
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);
- }
+ delete *link_it;
+ link_it=this->links.erase(link_it);
}
else
+ {
+ (*link_it)->update_end_pose(end);
link_it++;
+ }
}
-
- return new_node;
}
COpendriveLink *COpendriveRoadNode::get_link_with(COpendriveRoadNode *end)
@@ -534,6 +518,30 @@ unsigned int COpendriveRoadNode::get_closest_link(TOpendriveWorldPoint &point,do
return closest_index;
}
+double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPoint &point,double angle_tol)const
+{
+ double dist,min_dist=std::numeric_limits<double>::max();
+ double angle;
+ TPoint spline_point;
+
+ 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(point,spline_point);
+ angle=diff_angle(normalize_angle(point.heading),spline_point.heading);
+ if(fabs(angle)<angle_tol)
+ {
+ 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;
+ }
+ }
+ }
+
+ return min_dist;
+}
+
double COpendriveRoadNode::get_closest_point(TOpendriveWorldPoint &point,TOpendriveWorldPoint &closest_point,double angle_tol) const
{
double dist,min_dist=std::numeric_limits<double>::max();
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index 3d9e57297333924d16015f0910d6ed6211e7e920..b33f8bd2093658b1d75a32ba5c5095feb86ab49b 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -603,7 +603,6 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
COpendriveRoadSegment *new_segment;
std::vector<COpendriveRoadNode *>::iterator node_it;
node_up_ref_t::iterator node_ref_it;
- COpendriveRoadNode *new_node;
if(start==NULL && end==NULL)
return this->clone(new_node_ref,new_lane_ref);
@@ -613,12 +612,10 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
for(lane_it=new_segment->lanes.begin();lane_it!=new_segment->lanes.end();lane_it++)
{
if(node_side<0)
- new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,&new_node,start,end);
+ new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,start,end);
else
- new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,&new_node,end,start);
+ new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,end,start);
new_lane_ref[lane_it->second]=new_lane;
- if(new_node!=NULL)
- new_segment->add_node(new_node);
}
new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);
// update signals and objects