From e8d22de9b337feafebe946f5badfab33a9d4bc7c Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Sat, 9 Jan 2021 12:41:05 +0100
Subject: [PATCH] Solved a bug when the lanes have more than one node.
---
src/opendrive_lane.cpp | 20 +++++++++++++++++++-
src/opendrive_road.cpp | 3 +--
2 files changed, 20 insertions(+), 3 deletions(-)
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index 802b9df..d01bd87 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -45,7 +45,7 @@ void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
if(lane!=NULL)
{
if(this->get_num_nodes()!=lane->get_num_nodes())
- throw CException(_HERE_,"Impossible to liink lanes because they have different number of nodes");
+ throw CException(_HERE_,"Impossible to link lanes because they have different number of nodes");
if(this->get_num_nodes()>0 && lane->get_num_nodes()>0)
{
if(this->id*lane->get_id()<0) // oposite directions
@@ -349,7 +349,16 @@ void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t
for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
{
if(i<start_node_index)
+ {
+ for(exist_it=new_node_ref.begin();exist_it!=new_node_ref.end();exist_it++)
+ if((*it)==exist_it->second)
+ {
+ delete *it;
+ new_node_ref.erase(exist_it);
+ break;
+ }
it=this->nodes.erase(it);
+ }
else
{
exists=false;
@@ -389,7 +398,16 @@ void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &
for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
{
if(i>end_node_index)
+ {
+ for(exist_it=new_node_ref.begin();exist_it!=new_node_ref.end();exist_it++)
+ if((*it)==exist_it->second)
+ {
+ delete *it;
+ new_node_ref.erase(exist_it);
+ break;
+ }
it=this->nodes.erase(it);
+ }
else
{
exists=false;
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index 7fcf98c..c1318f3 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -431,7 +431,6 @@ void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
(*lane_it)->segment->remove_lane(*lane_it);
for(unsigned int i=0;i<(*lane_it)->nodes.size();i++)
this->remove_node((*lane_it)->nodes[i]);
- delete *lane_it;
lane_it=this->lanes.erase(lane_it);
}
else
@@ -771,6 +770,7 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
}
}
new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);
+ new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref);
// add additional nodes not explicitly in the path
/*
for(unsigned int i=0;i<path_nodes.size();i++)
@@ -796,7 +796,6 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
*/
// 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;
--
GitLab