From 3c98883997b8a6366b61911929997b496bdbf8d3 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 1 Jan 2021 20:01:27 +0100
Subject: [PATCH] Function to find the closest point in the road operational.
---
include/opendrive_road.h | 2 +-
include/opendrive_road_node.h | 2 +-
src/opendrive_road.cpp | 12 ++++++------
src/opendrive_road_node.cpp | 8 ++++----
4 files changed, 12 insertions(+), 12 deletions(-)
diff --git a/include/opendrive_road.h b/include/opendrive_road.h
index a4d7ae3..d3b1556 100644
--- a/include/opendrive_road.h
+++ b/include/opendrive_road.h
@@ -42,7 +42,7 @@ class COpendriveRoad
unsigned int get_num_nodes(void) const;
const COpendriveRoadNode& get_node(unsigned int index) const;
const COpendriveRoadSegment& operator[](std::size_t index);
- double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point);
+ double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1);
void operator=(const COpendriveRoad& object);
friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
~COpendriveRoad();
diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index 65d5fdb..0e083de 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -48,7 +48,7 @@ class COpendriveRoadNode
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 get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1);
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
~COpendriveRoadNode();
};
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index 2f3710d..44baef1 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -316,23 +316,23 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
throw CException(_HERE_,"Invalid segment index");
}
-double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point)
+double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol)
{
double dist,min_dist=std::numeric_limits<double>::max();
- double angle,min_angle=std::numeric_limits<double>::max();
+ double angle;
TOpendriveWorldPoint point;
double length,closest_length;
for(unsigned int i=0;i<this->nodes.size();i++)
{
- length=this->nodes[i]->get_closest_point(x,y,heading,point);
+ length=this->nodes[i]->get_closest_point(x,y,heading,point,angle_tol);
+ dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
+ std::cout << point.x << "," << point.y << "," << point.heading << "," << length << "," << dist << std::endl;
angle=diff_angle(normalize_angle(heading),point.heading);
- if(angle<min_angle)
+ if(fabs(angle)<angle_tol)
{
- dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
if(dist<min_dist)
{
- min_angle=angle;
min_dist=dist;
closest_point=point;
closest_length=length;
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index 7e21d0e..3c0dfea 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -275,10 +275,10 @@ 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 COpendriveRoadNode::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol)
{
double dist,min_dist=std::numeric_limits<double>::max();
- double angle,min_angle=std::numeric_limits<double>::max();
+ double angle;
double length,closest_length;
TOpendriveWorldPoint target;
TPoint point;
@@ -291,13 +291,13 @@ 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(angle<min_angle)
+ if(fabs(angle)<angle_tol)
{
dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
if(dist<min_dist)
{
- min_angle=angle;
min_dist=dist;
closest_point.x=point.x;
closest_point.y=point.y;
--
GitLab