Commit 4246dc71 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added a clone() function instead of the copy constructor to be able to update the link references.

The get_closest*() functions have a new parameter to limit the search to the links belonging to a lane.
The parent information no longer includes the length of the link.
parent 5bff140b
......@@ -13,7 +13,6 @@ typedef struct
COpendriveRoadSegment *segment;
double start_curvature;
double end_curvature;
double length;
}TOpendriveRoadNodeParent;
class COpendriveRoadNode
......@@ -31,7 +30,7 @@ class COpendriveRoadNode
unsigned int index;
protected:
COpendriveRoadNode();
COpendriveRoadNode(const COpendriveRoadNode &object);
COpendriveRoadNode *clone(link_up_ref_t &new_link_ref);
void free(void);
bool add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane);
void add_link(COpendriveLink *link);
......@@ -43,15 +42,15 @@ class COpendriveRoadNode
void set_scale_factor(double scale);
void set_index(unsigned int index);
void set_pose(TOpendriveWorldPose &start);
void add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double length,double start_curvature,double end_curvature);
void add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double start_curvature,double end_curvature);
TOpendriveRoadNodeParent *get_parent_with_lane(const COpendriveLane *lane);
TOpendriveRoadNodeParent *get_parent_with_segment(const COpendriveRoadSegment *segment);
bool updated(node_up_ref_t &refs);
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);
void update_start_pose(COpendriveLane *lane,TOpendriveWorldPose *start=NULL);
void update_end_pose(COpendriveLane *lane,TOpendriveWorldPose *end=NULL);
void update_start_pose(COpendriveLane *lane,TOpendriveWorldPose &start,double distance);
void update_end_pose(COpendriveLane *lane,TOpendriveWorldPose &end,double distance);
public:
double get_resolution(void) const;
double get_scale_factor(void) const;
......@@ -64,9 +63,9 @@ class COpendriveRoadNode
const COpendriveLink &get_link(unsigned int index) const;
const COpendriveLink &get_link_ending_at(unsigned int node_index) const;
unsigned int get_closest_link(TOpendriveWorldPose &pose,double angle_tol=0.1) const;
double get_closest_distance(TOpendriveWorldPose &pose,double angle_tol=0.1) const;
double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1) const;
double get_closest_distance(TOpendriveWorldPose &pose,bool only_lanes=false,double angle_tol=0.1) const;
double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,bool only_lanes=false,double angle_tol=0.1) const;
void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,bool only_lanes,double angle_tol=0.1) const;
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
~COpendriveRoadNode();
};
......
......@@ -15,29 +15,37 @@ COpendriveRoadNode::COpendriveRoadNode()
this->index=-1;
}
COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
COpendriveRoadNode *COpendriveRoadNode::clone(link_up_ref_t &new_link_ref)
{
COpendriveLink *new_link;
COpendriveRoadNode *new_node;
this->resolution=object.resolution;
this->scale_factor=object.scale_factor;
this->pose=object.pose;
this->parents=object.parents;
this->links.clear();
for(unsigned int i=0;i<object.links.size();i++)
new_node=new COpendriveRoadNode();
new_node->resolution=this->resolution;
new_node->scale_factor=this->scale_factor;
new_node->pose=this->pose;
new_node->parents=this->parents;
new_node->links.resize(this->links.size());
for(unsigned int i=0;i<this->links.size();i++)
{
new_link=new COpendriveLink(*object.links[i]);
this->links.push_back(new_link);
if(new_link_ref.find(this->links[i])!=new_link_ref.end())
new_node->links[i]=new_link_ref[this->links[i]];
else
{
new_node->links[i]=new COpendriveLink(*this->links[i]);
new_link_ref[this->links[i]]=new_node->links[i];
}
}
this->index=object.index;
new_node->index=this->index;
return new_node;
}
bool COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane)
{
TOpendriveRoadNodeParent *parent;
TOpendriveWorldPose node_pose;
double start_curvature,end_curvature,length;
double start_curvature,end_curvature;
COpendriveLink *new_link;
bool lane_found=false;
if(this->has_link_with(node) || node->has_link_with(this))
return false;
......@@ -51,24 +59,26 @@ bool COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
new_link->set_parent_lane(lane);
// get the curvature
node_pose=node->get_pose();
for(unsigned int i=0;i<this->parents.size();i++)
parent=this->get_parent_with_lane(lane);
if(parent!=NULL)
{
if(this->parents[i].lane==lane && this->parents[i].segment==segment)
{
start_curvature=this->parents[i].start_curvature;
end_curvature=this->parents[i].end_curvature;
length=this->parents[i].length;
lane_found=true;
break;
}
start_curvature=parent->start_curvature;
end_curvature=parent->end_curvature;
}
if(!lane_found)
else
{
start_curvature=0.0;
end_curvature=0.0;
length=sqrt(pow(this->pose.x-node_pose.x,2.0)+pow(this->pose.y-node_pose.y,2.0));
if(this->get_num_parents()==1)
{
start_curvature=this->parents[0].start_curvature;
end_curvature=this->parents[0].end_curvature;
}
else
{
start_curvature=0.0;
end_curvature=0.0;
}
}
new_link->generate(start_curvature,end_curvature,length,lane_found);
new_link->generate(start_curvature,end_curvature);
this->add_link(new_link);
node->add_link(new_link);
......@@ -89,7 +99,8 @@ void COpendriveRoadNode::remove_link(COpendriveLink *link)
{
if((*it)==link)
{
delete (*it);
if((*it)->prev==this)
delete (*it);
it=this->links.erase(it);
break;
}
......@@ -146,7 +157,6 @@ void COpendriveRoadNode::set_scale_factor(double scale)
{
this->parents[i].start_curvature*=scale/this->scale_factor;
this->parents[i].end_curvature*=scale/this->scale_factor;
this->parents[i].length*=this->scale_factor/scale;
}
}
......@@ -160,7 +170,7 @@ void COpendriveRoadNode::set_pose(TOpendriveWorldPose &start)
this->pose=start;
}
void COpendriveRoadNode::add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double length,double start_curvature,double end_curvature)
void COpendriveRoadNode::add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double start_curvature,double end_curvature)
{
TOpendriveRoadNodeParent new_parent;
......@@ -171,7 +181,6 @@ void COpendriveRoadNode::add_parent(COpendriveLane *lane,COpendriveRoadSegment *
new_parent.segment=segment;
new_parent.start_curvature=start_curvature;
new_parent.end_curvature=end_curvature;
new_parent.length=length;
this->parents.push_back(new_parent);
}
......@@ -272,15 +281,24 @@ void COpendriveRoadNode::clean_references(segment_up_ref_t &segment_refs,lane_up
}
}
void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldPose *start)
void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldPose &start,double distance)
{
std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
std::vector<COpendriveLink *>::iterator link_it;
double new_length;
TPoint new_pose;
TOpendriveRoadNodeParent *parent;
if(start==NULL)
return;
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();)
{
......@@ -291,35 +309,35 @@ void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldP
}
else
{
new_pose=(*link_it)->update_start_pose(start);
new_length=(*link_it)->get_length();
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++;
}
}
}
void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPose &end,double distance)
{
std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
std::vector<COpendriveLink *>::iterator link_it;
TOpendriveRoadNodeParent *parent;
// 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++;
parent_it->length=new_length;
parent_it->start_curvature=new_pose.curvature;
}
}
}
void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPose *end)
{
std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
std::vector<COpendriveLink *>::iterator link_it;
double new_length;
TPoint new_pose;
if(end==NULL)
return;
// update the links
for(link_it=this->links.begin();link_it!=this->links.end();)
{
......@@ -329,23 +347,7 @@ void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPos
link_it=this->links.erase(link_it);
}
else
{
new_pose=(*link_it)->update_end_pose(end);
new_length=(*link_it)->get_length();
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++;
parent_it->length=new_length;
parent_it->end_curvature=new_pose.curvature;
}
}
}
......@@ -460,7 +462,7 @@ unsigned int COpendriveRoadNode::get_closest_link(TOpendriveWorldPose &pose,doub
return closest_index;
}
double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,double angle_tol)const
double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,bool only_lanes,double angle_tol)const
{
double dist,min_dist=std::numeric_limits<double>::max();
double angle;
......@@ -470,13 +472,16 @@ double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,double
{
if(&this->links[i]->get_prev()==this)// links starting at this node
{
this->links[i]->find_closest_world_pose(pose,closest_pose);
angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
if(fabs(angle)<angle_tol)
if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
{
dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
if(dist<min_dist)
min_dist=dist;
this->links[i]->find_closest_world_pose(pose,closest_pose);
angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
if(fabs(angle)<angle_tol)
{
dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
if(dist<min_dist)
min_dist=dist;
}
}
}
}
......@@ -484,7 +489,7 @@ double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,double
return min_dist;
}
double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,bool only_lanes,double angle_tol) const
{
double dist,min_dist=std::numeric_limits<double>::max();
double angle;
......@@ -498,16 +503,19 @@ double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendrive
{
if(&this->links[i]->get_prev()==this)// links starting at this node
{
length=this->links[i]->find_closest_world_pose(pose,tmp_pose);
angle=diff_angle(normalize_angle(pose.heading),tmp_pose.heading);
if(fabs(angle)<angle_tol)
if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
{
dist=sqrt(pow(tmp_pose.x-pose.x,2.0)+pow(tmp_pose.y-pose.y,2.0));
if(dist<min_dist)
length=this->links[i]->find_closest_world_pose(pose,tmp_pose);
angle=diff_angle(normalize_angle(pose.heading),tmp_pose.heading);
if(fabs(angle)<angle_tol)
{
min_dist=dist;
closest_length=length;
closest_pose=tmp_pose;
dist=sqrt(pow(tmp_pose.x-pose.x,2.0)+pow(tmp_pose.y-pose.y,2.0));
if(dist<min_dist)
{
min_dist=dist;
closest_length=length;
closest_pose=tmp_pose;
}
}
}
}
......@@ -516,7 +524,7 @@ double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendrive
return closest_length;
}
void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol) const
void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,bool only_lanes,double angle_tol) const
{
double dist;
double angle;
......@@ -529,15 +537,18 @@ void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector
{
if(&this->links[i]->get_prev()==this)// links starting at this node
{
length=this->links[i]->find_closest_world_pose(pose,closest_pose);
angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
if(fabs(angle)<angle_tol)
if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
{
dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
if(dist<=dist_tol)
length=this->links[i]->find_closest_world_pose(pose,closest_pose);
angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
if(fabs(angle)<angle_tol)
{
closest_poses.push_back(closest_pose);
closest_lengths.push_back(length);
dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
if(dist<=dist_tol)
{
closest_poses.push_back(closest_pose);
closest_lengths.push_back(length);
}
}
}
}
......
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