From 19d30ab3ec7d85eae16a24a11b5efb6812aa41e5 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Sat, 2 Jan 2021 12:56:59 +0100
Subject: [PATCH] Added several functions: * A function to get the link
ending at a given node. * A function to get the closest link to a point.
* A function to get the set to closest points to a given point.
---
include/opendrive_road_node.h | 5 ++-
src/opendrive_road_node.cpp | 81 ++++++++++++++++++++++++++++++++++-
2 files changed, 83 insertions(+), 3 deletions(-)
diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index 0e083de..e006cb5 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -44,11 +44,14 @@ class COpendriveRoadNode
TOpendriveWorldPoint get_start_pose(void) const;
unsigned int get_num_links(void) const;
const COpendriveLink &get_link(unsigned int index) const;
+ const COpendriveLink &get_link_ending_at(unsigned int node_index) const;
unsigned int get_num_lanes(void) const;
const COpendriveLane &get_lane(unsigned int index) const;
unsigned int get_num_geometries(void) const;
const COpendriveGeometry &get_geometry(unsigned int index) const;
- double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1);
+ unsigned int get_closest_link(double x, double y,double heading,double angle_tol=0.1) const;
+ double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1) const;
+ void get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1) const;
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
~COpendriveRoadNode();
};
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index 3c0dfea..31c2128 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -249,6 +249,17 @@ const COpendriveLink &COpendriveRoadNode::get_link(unsigned int index) const
throw CException(_HERE_,"Invalid link index");
}
+const COpendriveLink &COpendriveRoadNode::get_link_ending_at(unsigned int node_index) const
+{
+ for(unsigned int i=0;i<this->links.size();i++)
+ {
+ if(this->links[i]->get_next().get_index()==node_index)
+ return *this->links[i];
+ }
+
+ throw CException(_HERE_,"No link in this node ends at the desired node");
+}
+
unsigned int COpendriveRoadNode::get_num_lanes(void) const
{
return this->lanes.size();
@@ -275,7 +286,39 @@ const COpendriveGeometry &COpendriveRoadNode::get_geometry(unsigned int index) c
throw CException(_HERE_,"Invalid geometry index");
}
-double COpendriveRoadNode::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol)
+unsigned int COpendriveRoadNode::get_closest_link(double x, double y,double heading,double angle_tol)const
+{
+ double dist,min_dist=std::numeric_limits<double>::max();
+ TOpendriveWorldPoint target;
+ unsigned int closest_index;
+ double angle;
+ TPoint point;
+
+ target.x=x;
+ target.y=y;
+ target.heading=heading;
+ for(unsigned int i=0;i<this->links.size();i++)
+ {
+ if(&this->links[i]->get_prev()==this)// links starting at this node
+ {
+ this->links[i]->find_closest_world_point(target,point);
+ angle=diff_angle(normalize_angle(heading),point.heading);
+ if(fabs(angle)<angle_tol)
+ {
+ dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_index=i;
+ }
+ }
+ }
+ }
+
+ return closest_index;
+}
+
+double COpendriveRoadNode::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol) const
{
double dist,min_dist=std::numeric_limits<double>::max();
double angle;
@@ -291,7 +334,6 @@ double COpendriveRoadNode::get_closest_point(double x, double y,double heading,T
if(&this->links[i]->get_prev()==this)// links starting at this node
{
length=this->links[i]->find_closest_world_point(target,point);
- std::cout << " " << point.x << "," << point.y << "," << point.heading << "," << length << std::endl;
angle=diff_angle(normalize_angle(heading),point.heading);
if(fabs(angle)<angle_tol)
{
@@ -311,6 +353,41 @@ double COpendriveRoadNode::get_closest_point(double x, double y,double heading,T
return closest_length;
}
+void COpendriveRoadNode::get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol) const
+{
+ double dist;
+ double angle;
+ double length;
+ TOpendriveWorldPoint target,new_point;
+ TPoint point;
+
+ target.x=x;
+ target.y=y;
+ target.heading=heading;
+ closest_points.clear();
+ closest_lengths.clear();
+ for(unsigned int i=0;i<this->links.size();i++)
+ {
+ if(&this->links[i]->get_prev()==this)// links starting at this node
+ {
+ length=this->links[i]->find_closest_world_point(target,point);
+ angle=diff_angle(normalize_angle(heading),point.heading);
+ if(fabs(angle)<angle_tol)
+ {
+ dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
+ if(dist<=dist_tol)
+ {
+ new_point.x=point.x;
+ new_point.y=point.y;
+ new_point.heading=point.heading;
+ closest_points.push_back(new_point);
+ closest_lengths.push_back(length);
+ }
+ }
+ }
+ }
+}
+
std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node)
{
out << " Node: " << node.get_index() << std::endl;
--
GitLab