From e8bdb4df082175013eacea2bb700b0f1213376b1 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 22 Dec 2020 12:09:55 +0100
Subject: [PATCH] Added functions to get the start and end position of a lane.
Added a function to return the length of a lane. Solved a bug with the names
of the road marks. Solved a bug when kinking two nodes in the same lane.
---
include/opendrive_lane.h | 3 ++
src/opendrive_lane.cpp | 94 ++++++++++++++++++++++++++++++++++------
2 files changed, 84 insertions(+), 13 deletions(-)
diff --git a/include/opendrive_lane.h b/include/opendrive_lane.h
index eeb4c40..f64730e 100644
--- a/include/opendrive_lane.h
+++ b/include/opendrive_lane.h
@@ -45,6 +45,9 @@ class COpendriveLane
double get_speed(void) const;
double get_offset(void) const;
int get_id(void) const;
+ TOpendriveWorldPoint get_start_point(void);
+ TOpendriveWorldPoint get_end_point(void);
+ double get_length(void);
void operator=(const COpendriveLane &object);
friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
~COpendriveLane();
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index 229dbac..501628b 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -94,15 +94,10 @@ void COpendriveLane::add_node(COpendriveRoadNode *node)
if(this->nodes[i]==node)
return;
// add the new node
- for(unsigned int i=0;i<node->get_num_lanes();i++)
- {
- if(&node->get_lane(i)==this)
- node->set_index(i,this->nodes.size());
- }
this->nodes.push_back(node);
// link the new node with the previous one in the current lane, if any
if(this->nodes.size()>1)
- this->nodes[this->nodes.size()-1]->add_link(node,OD_MARK_NONE);
+ this->nodes[this->nodes.size()-2]->add_link(node,OD_MARK_NONE);
}
void COpendriveLane::set_resolution(double res)
@@ -179,13 +174,13 @@ void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegmen
mark=OD_MARK_SOLID;
else if(lane_info.roadMark().begin()->type1().get()=="broken")
mark=OD_MARK_BROKEN;
- else if(lane_info.roadMark().begin()->type1().get()=="solid_solid")
+ else if(lane_info.roadMark().begin()->type1().get()=="solid solid")
mark=OD_MARK_SOLID_SOLID;
- else if(lane_info.roadMark().begin()->type1().get()=="solid_broken")
+ else if(lane_info.roadMark().begin()->type1().get()=="solid broken")
mark=OD_MARK_SOLID_BROKEN;
- else if(lane_info.roadMark().begin()->type1().get()=="broken_solid")
+ else if(lane_info.roadMark().begin()->type1().get()=="broken solid")
mark=OD_MARK_BROKEN_SOLID;
- else if(lane_info.roadMark().begin()->type1().get()=="broken_broken")
+ else if(lane_info.roadMark().begin()->type1().get()=="broken broken")
mark=OD_MARK_BROKEN_BROKEN;
else
mark=OD_MARK_NONE;
@@ -239,6 +234,82 @@ int COpendriveLane::get_id(void) const
return this->id;
}
+TOpendriveWorldPoint COpendriveLane::get_start_point(void)
+{
+ TOpendriveTrackPoint track_point;
+ TOpendriveWorldPoint world_point;
+
+ track_point.heading=0.0;
+ if(this->id<0)// right lane
+ {
+ track_point.t=this->get_offset()-this->get_width()/2.0;
+ track_point.s=0.0;
+ for(unsigned int i=0;i<this->nodes[0]->get_num_lanes();i++)
+ if(this->nodes[0]->get_lane(i).get_id()==this->id)
+ this->nodes[0]->get_geometry(i).get_world_pose(track_point,world_point);
+ }
+ else
+ {
+ track_point.t=this->get_offset()+this->get_width()/2.0;
+ for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
+ if(this->nodes[this->nodes.size()-1]->get_lane(i).get_id()==this->id)
+ {
+ track_point.s=this->nodes[this->nodes.size()-1]->get_geometry(i).get_length();
+ this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);
+ }
+ }
+ if(this->get_id()>0)
+ world_point.heading=normalize_angle(world_point.heading+3.14159);
+
+ return world_point;
+}
+
+TOpendriveWorldPoint COpendriveLane::get_end_point(void)
+{
+ TOpendriveTrackPoint track_point;
+ TOpendriveWorldPoint world_point;
+
+ track_point.heading=0.0;
+ if(this->id>0)// left lane
+ {
+ track_point.t=this->get_offset()-this->get_width()/2.0;
+ track_point.s=0.0;
+ for(unsigned int i=0;i<this->nodes[0]->get_num_lanes();i++)
+ if(this->nodes[0]->get_lane(i).get_id()==this->id)
+ this->nodes[0]->get_geometry(i).get_world_pose(track_point,world_point);
+ }
+ else
+ {
+ track_point.t=this->get_offset()+this->get_width()/2.0;
+ for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
+ if(this->nodes[this->nodes.size()-1]->get_lane(i).get_id()==this->id)
+ {
+ track_point.s=this->nodes[this->nodes.size()-1]->get_geometry(i).get_length();
+ this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);
+ }
+ }
+ if(this->get_id()>0)
+ world_point.heading=normalize_angle(world_point.heading+3.14159);
+
+ return world_point;
+}
+
+double COpendriveLane::get_length(void)
+{
+ double length=0.0;
+
+ for(unsigned int i=0;i<this->nodes.size();i++)
+ {
+ for(unsigned int j=0;j<this->nodes[i]->get_num_lanes();j++)
+ {
+ if(this->nodes[i]->get_lane(j).get_id()==this->id)
+ length+=this->nodes[i]->get_geometry(j).get_length();
+ }
+ }
+
+ return length;
+}
+
void COpendriveLane::operator=(const COpendriveLane &object)
{
COpendriveRoadNode *node;
@@ -325,10 +396,7 @@ std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
}
out << " Number of nodes: " << lane.nodes.size() << std::endl;
for(unsigned int i=0;i<lane.nodes.size();i++)
- {
- out << " Node " << i << ":" << std::endl;
out << *lane.nodes[i];
- }
return out;
}
--
GitLab