Skip to content
Snippets Groups Projects
Commit 1ef3ee18 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

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.
parent d1577d40
No related branches found
No related tags found
1 merge request!2Solved a bug when creating a new geometry object: the sale factor is...
......@@ -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
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment