From 17b30b1938fb08dca12afb2cde2247a49bfa80c2 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 19 Jan 2021 10:48:33 +0100
Subject: [PATCH] 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.
---
include/opendrive_lane.h | 16 ++--
src/opendrive_lane.cpp | 162 +++++++++++++++++++++++++++++++--------
2 files changed, 142 insertions(+), 36 deletions(-)
diff --git a/include/opendrive_lane.h b/include/opendrive_lane.h
index 4208f4c..10b9977 100644
--- a/include/opendrive_lane.h
+++ b/include/opendrive_lane.h
@@ -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();
};
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index 57f9d9e..8e2f245 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -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;
--
GitLab