From 1ef3ee18d8a2f2d35918e43cee39f44461b4121e Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 2 Aug 2023 08:18:30 +0200
Subject: [PATCH] The parent element are only erased if neither the segment not
the lane have been updated. Improved the way to update the pose of the node.
---
include/opendrive_road_node.h | 4 ++--
src/opendrive_road_node.cpp | 40 ++++++-----------------------------
2 files changed, 8 insertions(+), 36 deletions(-)
diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index 9ae44c6..a647f02 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -48,8 +48,8 @@ class COpendriveRoadNode
COpendriveRoadNode *get_original_node(node_up_ref_t &node_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,link_up_ref_t &new_link_ref);
- void update_start_pose(COpendriveLane *lane,TOpendriveWorldPose &start,double distance);
- void update_end_pose(COpendriveLane *lane,TOpendriveWorldPose &end,double distance);
+ void update_start_pose(TOpendriveWorldPose &start,node_up_ref_t &node_refs,double distance);
+ void update_end_pose(TOpendriveWorldPose &end,node_up_ref_t &node_refs);
public:
/**
* \brief Returns the resolution
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index 4631bab..40f7b06 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -297,7 +297,7 @@ void COpendriveRoadNode::clean_references(segment_up_ref_t &segment_refs,lane_up
}
for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
{
- if(!parent_it->segment->updated(segment_refs) || !parent_it->lane->updated(lane_refs))
+ if(!parent_it->segment->updated(segment_refs) && !parent_it->lane->updated(lane_refs))
parent_it=this->parents.erase(parent_it);
else
parent_it++;
@@ -305,63 +305,35 @@ void COpendriveRoadNode::clean_references(segment_up_ref_t &segment_refs,lane_up
// }
}
-void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldPose &start,double distance)
+void COpendriveRoadNode::update_start_pose(TOpendriveWorldPose &start,node_up_ref_t &node_refs,double distance)
{
- std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
std::vector<COpendriveLink *>::iterator link_it;
- TOpendriveRoadNodeParent *parent;
this->pose=start;
- // remove the references to all lanes and segments except for lane
- for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
- {
- if(parent_it->lane!=lane)
- parent_it=this->parents.erase(parent_it);
- else
- {
- parent=&(*parent_it);
- parent_it++;
- }
- }
// update the links
for(link_it=this->links.begin();link_it!=this->links.end();)
{
- if((*link_it)->next==this)
+ if((*link_it)->next==this->get_original_node(node_refs))
{
delete *link_it;
link_it=this->links.erase(link_it);
}
else
{
- if((*link_it)->lane==lane)
- {
- parent->start_curvature=(*link_it)->get_curvature_at(distance);
- (*link_it)->update_start_pose(start,parent->start_curvature);
- }
- else
- (*link_it)->update_start_pose(start,0.0);
+ (*link_it)->update_start_pose(start,(*link_it)->get_curvature_at(distance));
link_it++;
}
}
}
-void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPose &end,double distance)
+void COpendriveRoadNode::update_end_pose(TOpendriveWorldPose &end,node_up_ref_t &node_refs)
{
- std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
std::vector<COpendriveLink *>::iterator link_it;
- // remove the references to all lanes and segments except for lane
- for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
- {
- if(parent_it->lane!=lane)
- parent_it=this->parents.erase(parent_it);
- else
- parent_it++;
- }
// update the links
for(link_it=this->links.begin();link_it!=this->links.end();)
{
- if((*link_it)->prev==this)
+ if((*link_it)->prev==this->get_original_node(node_refs))
{
delete *link_it;
link_it=this->links.erase(link_it);
--
GitLab