From f8b52ca54a95bceb2920acd4f4e95876575ca821 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Sun, 10 Jan 2021 18:50:45 +0100
Subject: [PATCH] Solved a bug when computing the closest point: a valid pose
was returned even if it was not found.
---
src/opendrive_road_node.cpp | 8 +++++---
1 file changed, 5 insertions(+), 3 deletions(-)
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index edfb351..a3cfb58 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -596,6 +596,7 @@ double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendrive
double dist,min_dist=std::numeric_limits<double>::max();
double angle;
double length,closest_length=std::numeric_limits<double>::max();
+ TOpendriveWorldPose tmp_pose;
closest_pose.x=std::numeric_limits<double>::max();
closest_pose.y=std::numeric_limits<double>::max();
@@ -604,15 +605,16 @@ double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendrive
{
if(&this->links[i]->get_prev()==this)// links starting at this node
{
- length=this->links[i]->find_closest_world_pose(pose,closest_pose);
- angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
+ length=this->links[i]->find_closest_world_pose(pose,tmp_pose);
+ angle=diff_angle(normalize_angle(pose.heading),tmp_pose.heading);
if(fabs(angle)<angle_tol)
{
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+ dist=sqrt(pow(tmp_pose.x-pose.x,2.0)+pow(tmp_pose.y-pose.y,2.0));
if(dist<min_dist)
{
min_dist=dist;
closest_length=length;
+ closest_pose=tmp_pose;
}
}
}
--
GitLab