Commit 17b30b19 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added some functions:

* to get the pose at a given distance in the link.
* to get the curvature at a given distance in the link.
* to get the closest node, node index and pose to a given point.
Added the link reference update to the clone(),get_sub_lane(), update_start_node() and update_end_node() functions.
parent 4246dc71
......@@ -36,7 +36,7 @@ class COpendriveLane
COpendriveLane(const COpendriveLane &object);
void load(const ::lane &lane_info,COpendriveRoadSegment *segment);
void link_neightbour_lane(COpendriveLane *lane);
void link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start,bool to_start);
void link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start,bool to_start,bool belongs_to_lane);
void add_next_lane(COpendriveLane *lane);
void add_prev_lane(COpendriveLane *lane);
void remove_lane(COpendriveLane *lane);
......@@ -56,10 +56,10 @@ class COpendriveLane
bool updated(lane_up_ref_t &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_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPose *start=NULL);
void update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPose *end=NULL);
COpendriveLane *get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL);
COpendriveLane *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref);
void update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start=NULL);
void update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *end=NULL);
COpendriveLane *get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL);
COpendriveLane *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref);
public:
unsigned int get_num_nodes(void) const;
const COpendriveRoadNode &get_node(unsigned int index) const;
......@@ -77,9 +77,13 @@ class COpendriveLane
TOpendriveWorldPose get_start_pose(void) const;
TOpendriveWorldPose get_end_pose(void) const;
double get_length(void) const;
TOpendriveWorldPose get_pose_at(double length);
double get_curvature_at(double length);
TOpendriveLocalPose transform_to_local_pose(TOpendriveTrackPose &track) const;
TOpendriveWorldPose transform_to_world_pose(TOpendriveTrackPose &track) const;
unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double angle_tol=0.1);
const COpendriveRoadNode &get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
~COpendriveLane();
};
......
......@@ -52,12 +52,9 @@ void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
min_num_nodes=lane->get_num_nodes();
for(unsigned int i=0;i<min_num_nodes-1;i++)
{
this->nodes[i]->add_link(lane->nodes[i+1],this->left_mark,this->segment,this);
lane->nodes[i]->add_link(this->nodes[i+1],this->left_mark,this->segment,lane);
if(this->id>0)// left lanes
this->right_mark=lane->left_mark;
else// right lanes
this->left_mark=lane->right_mark;
this->nodes[i]->add_link(lane->nodes[i+1],lane->right_mark,this->segment,NULL);
lane->nodes[i]->add_link(this->nodes[i+1],lane->right_mark,this->segment,NULL);
this->left_mark=lane->right_mark;
}
if(min_num_nodes>0)
{
......@@ -67,7 +64,7 @@ void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
}
}
void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start, bool to_start)
void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start, bool to_start,bool belongs_to_lane)
{
COpendriveRoadNode *start,*end;
std::stringstream error;
......@@ -118,7 +115,10 @@ void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool f
end=lane->nodes[0];
}
}
start->add_link(end,mark,this->segment,this);
if(belongs_to_lane)
start->add_link(end,mark,this->segment,this);
else
start->add_link(end,mark,this->segment,NULL);
// link lane
this->add_next_lane(lane);
lane->add_prev_lane(this);
......@@ -350,22 +350,26 @@ void COpendriveLane::clean_references(segment_up_ref_t &segment_refs,lane_up_ref
}
}
void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPose *start)
void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start)
{
std::vector<COpendriveRoadNode *>::iterator it;
segment_up_ref_t new_segment_ref;
unsigned int start_node_index,i;
TOpendriveWorldPose start_pose;
COpendriveRoadNode *new_node;
std::stringstream error;
double distance;
if(start==NULL)
return;
start_node_index=this->get_closest_node_index(*start,3.14159);
start_node_index=this->get_closest_node_index(*start,distance,3.14159);
if(start_node_index==(unsigned int)-1)
{
error << "No node close to (x=" << start->x << ",y=" << start->y << ",heading=" << start->heading << ") in lane " << this->id << " of segment " << this->segment->get_name();
throw CException(_HERE_,error.str());
}
// get the actual start position on the lane
this->get_closest_pose(*start,start_pose,3.14159);
// eliminate all the node before the start one
for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
{
......@@ -382,7 +386,7 @@ void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t
{
if(!(*it)->updated(new_node_ref))
{
new_node=new COpendriveRoadNode(**it);
new_node=(*it)->clone(new_link_ref);
new_node_ref[*it]=new_node;
}
it++;
......@@ -390,25 +394,28 @@ void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t
}
this->prev.clear();// it is no longer connected to any previous lane
this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
this->nodes[0]->update_start_pose(this,start);
this->nodes[0]->update_start_pose(this,start_pose,distance);
}
void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPose *end)
void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *end)
{
std::vector<COpendriveRoadNode *>::iterator it;
segment_up_ref_t new_segment_ref;
unsigned int end_node_index,i;
TOpendriveWorldPose end_pose;
COpendriveRoadNode *new_node;
std::stringstream error;
double distance;
if(end==NULL)
return;
end_node_index=this->get_closest_node_index(*end,3.14159);
end_node_index=this->get_closest_node_index(*end,distance,3.14159);
if(end_node_index==(unsigned int)-1)
{
error << "No node close to (x=" << end->x << ",y=" << end->y << ",heading=" << end->heading << ") in lane " << this->id << " of segment " << this->segment->get_name();
throw CException(_HERE_,error.str());
}
this->get_closest_pose(*end,end_pose,3.14159);
// eliminate all the node before the start one
for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
{
......@@ -425,7 +432,7 @@ void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &
{
if(!(*it)->updated(new_node_ref))
{
new_node=new COpendriveRoadNode(**it);
new_node=(*it)->clone(new_link_ref);
new_node_ref[*it]=new_node;
}
it++;
......@@ -433,32 +440,32 @@ void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &
}
this->next.clear();// it is no longer connected to any next lane
this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
this->nodes[this->nodes.size()-1]->update_end_pose(this,end);
this->nodes[this->nodes.size()-1]->update_end_pose(this,end_pose,distance);
}
COpendriveLane *COpendriveLane::get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPose *start,TOpendriveWorldPose *end)
COpendriveLane *COpendriveLane::get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start,TOpendriveWorldPose *end)
{
COpendriveLane *new_lane;
if(start==NULL && end==NULL)
return this->clone(new_node_ref,new_lane_ref);
return this->clone(new_node_ref,new_lane_ref,new_link_ref);
new_lane=new COpendriveLane(*this);
new_lane_ref[this]=new_lane;
if(this->id<0)
{
new_lane->update_start_node(new_node_ref,new_lane_ref,start);
new_lane->update_end_node(new_node_ref,new_lane_ref,end);
new_lane->update_start_node(new_node_ref,new_lane_ref,new_link_ref,start);
new_lane->update_end_node(new_node_ref,new_lane_ref,new_link_ref,end);
}
else
{
new_lane->update_start_node(new_node_ref,new_lane_ref,end);
new_lane->update_end_node(new_node_ref,new_lane_ref,start);
new_lane->update_start_node(new_node_ref,new_lane_ref,new_link_ref,end);
new_lane->update_end_node(new_node_ref,new_lane_ref,new_link_ref,start);
}
return new_lane;
}
COpendriveLane *COpendriveLane::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref)
COpendriveLane *COpendriveLane::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref)
{
COpendriveLane *new_lane;
std::vector<COpendriveRoadNode *>::iterator it;
......@@ -469,7 +476,7 @@ COpendriveLane *COpendriveLane::clone(node_up_ref_t &new_node_ref,lane_up_ref_t
new_lane_ref[this]=new_lane;
for(it=new_lane->nodes.begin();it!=new_lane->nodes.end();it++)
{
new_node=new COpendriveRoadNode(**it);
new_node=(*it)->clone(new_link_ref);
new_node_ref[*it]=new_node;
}
this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
......@@ -546,10 +553,7 @@ void COpendriveLane::load(const ::lane &lane_info,COpendriveRoadSegment *segment
else
mark=OD_MARK_NONE;
}
if(this->id<0)//right lanes
this->right_mark=mark;
else
this->left_mark=mark;
this->right_mark=mark;
this->set_parent_segment(segment);
}
......@@ -707,6 +711,54 @@ double COpendriveLane::get_length(void) const
return length;
}
TOpendriveWorldPose COpendriveLane::get_pose_at(double length)
{
TOpendriveWorldPose world_pose={0};
for(unsigned int i=0;i<this->nodes.size();i++)
{
for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
{
if(&this->nodes[i]->get_link(j).get_parent_lane()==this)
{
if((length-this->nodes[i]->links[j]->get_length())<0.0)
{
world_pose=this->nodes[i]->links[j]->get_pose_at(length);
return world_pose;
}
else
length-=this->nodes[i]->links[j]->get_length();
}
}
}
return world_pose;
}
double COpendriveLane::get_curvature_at(double length)
{
double curvature;
for(unsigned int i=0;i<this->nodes.size();i++)
{
for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
{
if(&this->nodes[i]->get_link(j).get_parent_lane()==this)
{
if((length-this->nodes[i]->links[j]->get_length())<0.0)
{
curvature=this->nodes[i]->links[j]->get_curvature_at(length);
return curvature;
}
else
length-=this->nodes[i]->links[j]->get_length();
}
}
}
return 0.0;
}
TOpendriveLocalPose COpendriveLane::transform_to_local_pose(TOpendriveTrackPose &track) const
{
TOpendriveTrackPose pose;
......@@ -725,7 +777,7 @@ TOpendriveWorldPose COpendriveLane::transform_to_world_pose(TOpendriveTrackPose
return this->segment->transform_to_world_pose(pose);
}
unsigned int COpendriveLane::get_closest_node_index(TOpendriveWorldPose &pose,double angle_tol)
unsigned int COpendriveLane::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
double dist,min_dist=std::numeric_limits<double>::max();
TOpendriveWorldPose found_pose;
......@@ -733,7 +785,7 @@ unsigned int COpendriveLane::get_closest_node_index(TOpendriveWorldPose &pose,do
for(unsigned int i=0;i<this->nodes.size();i++)
{
this->nodes[i]->get_closest_pose(pose,found_pose,angle_tol);
this->nodes[i]->get_closest_pose(pose,found_pose,true,angle_tol);
dist=sqrt(pow(found_pose.x-pose.x,2.0)+pow(found_pose.y-pose.y,2.0));
if(dist<min_dist)
{
......@@ -742,9 +794,59 @@ unsigned int COpendriveLane::get_closest_node_index(TOpendriveWorldPose &pose,do
}
}
distance=min_dist;
return closest_index;
}
const COpendriveRoadNode &COpendriveLane::get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
unsigned int closest_index;
closest_index=this->get_closest_node_index(pose,distance,angle_tol);
if(closest_index==(unsigned int)-1)
throw CException(_HERE_,"Impossible to find the closest node");
return *this->nodes[closest_index];
}
double COpendriveLane::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
{
double dist,min_dist=std::numeric_limits<double>::max(),distance;
TOpendriveWorldPose found_pose;
unsigned int closest_index=-1;
COpendriveLink *link;
closest_pose.x=std::numeric_limits<double>::max();
closest_pose.y=std::numeric_limits<double>::max();
closest_pose.heading=std::numeric_limits<double>::max();
for(unsigned int i=0;i<this->nodes.size();i++)
{
this->nodes[i]->get_closest_pose(pose,found_pose,true,angle_tol);
dist=sqrt(pow(found_pose.x-pose.x,2.0)+pow(found_pose.y-pose.y,2.0));
if(dist<min_dist)
{
min_dist=dist;
closest_index=i;
closest_pose=found_pose;
}
}
distance=0.0;
for(unsigned int i=0;i<this->nodes.size();i++)
{
if(i<closest_index)
{
link=this->nodes[i]->get_link_with(this->nodes[i+1]);
if(link!=NULL)
distance+=link->get_length();
}
else
break;
}
distance+=min_dist;
return distance;
}
std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
{
out << " Lane: " << lane.id << " (offset: " << lane.offset << ")" << std::endl;
......
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