From d2f67dbeadb21b7bc102ffde9edce331f14fcbab Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 19 Jan 2021 16:31:30 +0100
Subject: [PATCH] Modified the gradient class to return false when the gradient
goes outside the limits. The spline class returns maximum length when the is
no closest point belonging to the spline. All other classes check the
returned length to check for the existance of the closest point.
---
include/opendrive_road_node.h | 4 +-
src/g2_spline.cpp | 8 +--
src/gradient.cpp | 5 +-
src/opendrive_lane.cpp | 37 ++++++++------
src/opendrive_road.cpp | 83 ++++++++++++++++++++-----------
src/opendrive_road_node.cpp | 90 +++++++++++++++++++++-------------
src/opendrive_road_segment.cpp | 74 +++++++++++++++++-----------
7 files changed, 186 insertions(+), 115 deletions(-)
diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index c5625b0..1436a11 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -36,7 +36,6 @@ class COpendriveRoadNode
void add_link(COpendriveLink *link);
void remove_link(COpendriveLink *link);
bool has_link(COpendriveLink *link);
- bool has_link_with(COpendriveRoadNode *node);
COpendriveLink *get_link_with(COpendriveRoadNode *node);
void set_resolution(double res);
void set_scale_factor(double scale);
@@ -62,10 +61,11 @@ class COpendriveRoadNode
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;
+ bool has_link_with(const COpendriveRoadNode &node) const;
unsigned int get_closest_link(TOpendriveWorldPose &pose,double angle_tol=0.1) const;
double get_closest_distance(TOpendriveWorldPose &pose,bool only_lanes=false,double angle_tol=0.1) const;
double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,bool only_lanes=false,double angle_tol=0.1) const;
- void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,bool only_lanes,double angle_tol=0.1) const;
+ void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,bool only_lanes=false,double angle_tol=0.1) const;
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
~COpendriveRoadNode();
};
diff --git a/src/g2_spline.cpp b/src/g2_spline.cpp
index acb107c..5709b60 100644
--- a/src/g2_spline.cpp
+++ b/src/g2_spline.cpp
@@ -801,7 +801,7 @@ double CG2Spline::get_max_curvature(double max_error,unsigned int max_iter)
{
start_point=this->get_max_sample_point(boost::bind(&CG2Spline::curvature_pow2,this,_1));
if(!this->curvature_grad.compute(max_error,max_iter,start_point,true))
- return std::numeric_limits<double>::infinity();
+ return std::numeric_limits<double>::max();
else
{
this->curvature_computed=true;
@@ -821,7 +821,7 @@ double CG2Spline::get_max_curvature_der(double max_error,unsigned int max_iter)
{
start_point=this->get_max_sample_point(boost::bind(&CG2Spline::curvature_der_pow2,this,_1));
if(!this->curvature_der_grad.compute(max_error,max_iter,start_point,true))
- return std::numeric_limits<double>::infinity();
+ return std::numeric_limits<double>::max();
else
{
this->curvature_der_computed=true;
@@ -839,7 +839,7 @@ double CG2Spline::find_closest_point(double x, double y, TPoint &point,double ma
this->dist_y=y;
start_point=this->get_min_sample_point(boost::bind(&CG2Spline::distance_pow2,this,_1));
if(!this->distance_grad.compute(max_error,max_iter,start_point,false))
- return std::numeric_limits<double>::infinity();
+ return std::numeric_limits<double>::max();
else
{
point=this->evaluate_parameter(this->distance_grad.get_coordinate());
@@ -858,7 +858,7 @@ double CG2Spline::find_closest_point(double x, double y,double max_error,unsigne
this->dist_y=y;
start_point=this->get_min_sample_point(boost::bind(&CG2Spline::distance_pow2,this,_1));
if(!this->distance_grad.compute(max_error,max_iter,start_point,false))
- return std::numeric_limits<double>::infinity();
+ return std::numeric_limits<double>::max();
else
{
if(this->num_points)
diff --git a/src/gradient.cpp b/src/gradient.cpp
index 92c876f..3301bb4 100644
--- a/src/gradient.cpp
+++ b/src/gradient.cpp
@@ -52,6 +52,7 @@ bool CGradient::compute(double max_func_error,unsigned int max_iter,double start
unsigned int iteration=0;
double gamma=1.0,func_value,func_der_value,prev_func_value,prev_func_der_value;
double coord,prev_coord,coord_inc;
+ bool beyond_limits=false;
coord=start_point;
func_value=this->p_function(coord);
@@ -72,6 +73,7 @@ bool CGradient::compute(double max_func_error,unsigned int max_iter,double start
coord=this->max_coord;
this->coordinate=coord;
this->value=this->p_function(coord);
+ beyond_limits=true;
break;
}
else if(coord<this->min_coord)
@@ -79,6 +81,7 @@ bool CGradient::compute(double max_func_error,unsigned int max_iter,double start
coord=this->min_coord;
this->coordinate=coord;
this->value=this->p_function(coord);
+ beyond_limits=true;
break;
}
prev_func_value=func_value;
@@ -90,7 +93,7 @@ bool CGradient::compute(double max_func_error,unsigned int max_iter,double start
}while((fabs(func_value-prev_func_value)>max_func_error || fabs(coord-prev_coord)>0.001) && iteration<max_iter);
this->coordinate=coord;
this->value=func_value;
- if(iteration==max_iter)
+ if(iteration==max_iter || beyond_limits)
return false;
else
return true;
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index 8e2f245..035100b 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -779,22 +779,25 @@ TOpendriveWorldPose COpendriveLane::transform_to_world_pose(TOpendriveTrackPose
unsigned int COpendriveLane::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
- double dist,min_dist=std::numeric_limits<double>::max();
+ double dist,min_dist=std::numeric_limits<double>::max(),length;
TOpendriveWorldPose found_pose;
unsigned int closest_index=-1;
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)
+ length=this->nodes[i]->get_closest_pose(pose,found_pose,true,angle_tol);
+ if(length<std::numeric_limits<double>::max())
{
- min_dist=dist;
- closest_index=i;
+ 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;
+ distance=length;
+ }
}
}
- distance=min_dist;
return closest_index;
}
@@ -810,7 +813,7 @@ const COpendriveRoadNode &COpendriveLane::get_closest_node(TOpendriveWorldPose &
double COpendriveLane::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
{
- double dist,min_dist=std::numeric_limits<double>::max(),distance;
+ double dist,min_dist=std::numeric_limits<double>::max(),distance,length;
TOpendriveWorldPose found_pose;
unsigned int closest_index=-1;
COpendriveLink *link;
@@ -820,17 +823,20 @@ double COpendriveLane::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorl
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)
+ length=this->nodes[i]->get_closest_pose(pose,found_pose,true,angle_tol);
+ if(length<std::numeric_limits<double>::max())
{
- min_dist=dist;
- closest_index=i;
- closest_pose=found_pose;
+ 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=length;
+ }
}
}
- distance=0.0;
for(unsigned int i=0;i<this->nodes.size();i++)
{
if(i<closest_index)
@@ -842,7 +848,6 @@ double COpendriveLane::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorl
else
break;
}
- distance+=min_dist;
return distance;
}
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index f26267a..42a9a77 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -671,18 +671,22 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
unsigned int COpendriveRoad::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
- double dist,min_dist=std::numeric_limits<double>::max();
+ double dist,min_dist=std::numeric_limits<double>::max(),length;
TOpendriveWorldPose closest_pose;
unsigned int closest_index=-1;
for(unsigned int i=0;i<this->nodes.size();i++)
{
- this->nodes[i]->get_closest_pose(pose,closest_pose,false,angle_tol);
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<min_dist)
+ length=this->nodes[i]->get_closest_pose(pose,closest_pose,false,angle_tol);
+ if(length<std::numeric_limits<double>::max())
{
- min_dist=dist;
- closest_index=i;
+ dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_index=i;
+ distance=length;
+ }
}
}
@@ -701,18 +705,22 @@ const COpendriveRoadNode &COpendriveRoad::get_closest_node(TOpendriveWorldPose &
unsigned int COpendriveRoad::get_closest_lane_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
- double dist,min_dist=std::numeric_limits<double>::max();
+ double dist,min_dist=std::numeric_limits<double>::max(),length;
TOpendriveWorldPose closest_pose;
unsigned int closest_index=-1;
for(unsigned int i=0;i<this->lanes.size();i++)
{
- this->lanes[i]->get_closest_pose(pose,closest_pose,angle_tol);
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<min_dist)
- {
- min_dist=dist;
- closest_index=i;
+ length=this->lanes[i]->get_closest_pose(pose,closest_pose,angle_tol);
+ if(length<std::numeric_limits<double>::max())
+ {
+ dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_index=i;
+ distance=length;
+ }
}
}
@@ -731,18 +739,22 @@ const COpendriveLane &COpendriveRoad::get_closest_lane(TOpendriveWorldPose &pose
unsigned int COpendriveRoad::get_closest_segment_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
- double dist,min_dist=std::numeric_limits<double>::max();
+ double dist,min_dist=std::numeric_limits<double>::max(),length;
TOpendriveWorldPose closest_pose;
unsigned int closest_index=-1;
for(unsigned int i=0;i<this->segments.size();i++)
{
- this->segments[i]->get_closest_pose(pose,closest_pose,angle_tol);
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<min_dist)
- {
- min_dist=dist;
- closest_index=i;
+ length=this->segments[i]->get_closest_pose(pose,closest_pose,angle_tol);
+ if(length<std::numeric_limits<double>::max())
+ {
+ dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_index=i;
+ distance=length;
+ }
}
}
@@ -768,12 +780,15 @@ double COpendriveRoad::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorl
for(unsigned int i=0;i<this->nodes.size();i++)
{
length=this->nodes[i]->get_closest_pose(pose,pose_found,false,angle_tol);
- dist=sqrt(pow(pose_found.x-pose.x,2.0)+pow(pose_found.y-pose.y,2.0));
- if(dist<min_dist)
+ if(length<std::numeric_limits<double>::max())
{
- min_dist=dist;
- closest_pose=pose_found;
- closest_length=length;
+ dist=sqrt(pow(pose_found.x-pose.x,2.0)+pow(pose_found.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_pose=pose_found;
+ closest_length=length;
+ }
}
}
@@ -784,16 +799,28 @@ void COpendriveRoad::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOp
{
std::vector<TOpendriveWorldPose> poses;
std::vector<double> lengths;
+ bool already_added;
closest_poses.clear();
closest_lengths.clear();
for(unsigned int i=0;i<this->nodes.size();i++)
{
this->nodes[i]->get_closest_poses(pose,poses,lengths,dist_tol,false,angle_tol);
- for(unsigned int j=0;j<poses.size();i++)
+ for(unsigned int j=0;j<poses.size();j++)
{
- closest_poses.push_back(poses[i]);
- closest_lengths.push_back(lengths[i]);
+ already_added=false;
+ for(unsigned int k=0;k<closest_poses.size();k++)
+ if(fabs(closest_poses[k].x-poses[j].x)<this->resolution &&
+ fabs(closest_poses[k].y-poses[j].y)<this->resolution)
+ {
+ already_added=true;
+ break;
+ }
+ if(!already_added)
+ {
+ closest_poses.push_back(poses[j]);
+ closest_lengths.push_back(lengths[j]);
+ }
}
}
}
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index ee7d6df..bff1134 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -47,7 +47,7 @@ bool COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
double start_curvature,end_curvature;
COpendriveLink *new_link;
- if(this->has_link_with(node) || node->has_link_with(this))
+ if(this->has_link_with(*node) || node->has_link_with(*this))
return false;
new_link=new COpendriveLink();
new_link->set_prev(this);
@@ -118,10 +118,10 @@ bool COpendriveRoadNode::has_link(COpendriveLink *link)
return false;
}
-bool COpendriveRoadNode::has_link_with(COpendriveRoadNode *node)
+bool COpendriveRoadNode::has_link_with(const COpendriveRoadNode &node) const
{
for(unsigned int i=0;i<this->links.size();i++)
- if(this->links[i]->prev==this && this->links[i]->next==node)
+ if(this->links[i]->prev==this && this->links[i]->next==&node)
return true;
return false;
@@ -325,7 +325,6 @@ void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPos
{
std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
std::vector<COpendriveLink *>::iterator link_it;
- TOpendriveRoadNodeParent *parent;
// remove the references to all lanes and segments except for lane
for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
@@ -333,10 +332,7 @@ void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPos
if(parent_it->lane!=lane)
parent_it=this->parents.erase(parent_it);
else
- {
- parent=&(*parent_it);
parent_it++;
- }
}
// update the links
for(link_it=this->links.begin();link_it!=this->links.end();)
@@ -438,22 +434,25 @@ unsigned int COpendriveRoadNode::get_closest_link(TOpendriveWorldPose &pose,doub
{
double dist,min_dist=std::numeric_limits<double>::max();
unsigned int closest_index=-1;
- double angle;
+ double angle,length;
TOpendriveWorldPose closest_pose;
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_pose(pose,closest_pose);
- angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
- if(fabs(angle)<angle_tol)
+ length=this->links[i]->find_closest_world_pose(pose,closest_pose);
+ if(length<std::numeric_limits<double>::max())
{
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<min_dist)
+ angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
+ if(fabs(angle)<angle_tol)
{
- min_dist=dist;
- closest_index=i;
+ dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_index=i;
+ }
}
}
}
@@ -465,7 +464,7 @@ unsigned int COpendriveRoadNode::get_closest_link(TOpendriveWorldPose &pose,doub
double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,bool only_lanes,double angle_tol)const
{
double dist,min_dist=std::numeric_limits<double>::max();
- double angle;
+ double angle,length;
TOpendriveWorldPose closest_pose;
for(unsigned int i=0;i<this->links.size();i++)
@@ -474,13 +473,16 @@ double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,bool o
{
if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
{
- this->links[i]->find_closest_world_pose(pose,closest_pose);
- angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
- if(fabs(angle)<angle_tol)
+ length=this->links[i]->find_closest_world_pose(pose,closest_pose);
+ if(length<std::numeric_limits<double>::max())
{
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<min_dist)
- min_dist=dist;
+ angle=diff_angle(normalize_angle(pose.heading),closest_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));
+ if(dist<min_dist)
+ min_dist=dist;
+ }
}
}
}
@@ -506,15 +508,18 @@ double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendrive
if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
{
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)
+ if(length<std::numeric_limits<double>::max())
{
- dist=sqrt(pow(tmp_pose.x-pose.x,2.0)+pow(tmp_pose.y-pose.y,2.0));
- if(dist<min_dist)
+ angle=diff_angle(normalize_angle(pose.heading),tmp_pose.heading);
+ if(fabs(angle)<angle_tol)
{
- min_dist=dist;
- closest_length=length;
- closest_pose=tmp_pose;
+ 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;
+ }
}
}
}
@@ -530,6 +535,7 @@ void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector
double angle;
double length;
TOpendriveWorldPose closest_pose;
+ bool already_added;
closest_poses.clear();
closest_lengths.clear();
@@ -540,14 +546,28 @@ void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector
if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
{
length=this->links[i]->find_closest_world_pose(pose,closest_pose);
- angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
- if(fabs(angle)<angle_tol)
+ if(length<std::numeric_limits<double>::max())
{
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<=dist_tol)
+ angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
+ if(fabs(angle)<angle_tol)
{
- closest_poses.push_back(closest_pose);
- closest_lengths.push_back(length);
+ dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+ if(dist<=dist_tol)
+ {
+ already_added=false;
+ for(unsigned int j=0;j<closest_poses.size();j++)
+ if(fabs(closest_poses[j].x-closest_pose.x)<this->resolution &&
+ fabs(closest_poses[j].y-closest_pose.y)<this->resolution)
+ {
+ already_added=true;
+ break;
+ }
+ if(!already_added)
+ {
+ closest_poses.push_back(closest_pose);
+ closest_lengths.push_back(length);
+ }
+ }
}
}
}
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index 6018639..c48d476 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -732,9 +732,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
for(geom_it=new_segment->geometries.begin();geom_it!=new_segment->geometries.end();)
{
length=geom_it->spline->find_closest_point(start_pose->x,start_pose->y,new_point);
- if(fabs(length-geom_it->spline->get_length())<this->resolution)
- geom_it=new_segment->geometries.erase(geom_it);
- else
+ if(length<std::numeric_limits<double>::max())
{
geom_it->spline->set_start_point(new_point);
geom_it->spline->generate(this->resolution);
@@ -742,6 +740,8 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
geom_it->opendrive->set_max_s(geom_it->opendrive->get_max_s()-length);
break;
}
+ else
+ geom_it=new_segment->geometries.erase(geom_it);
}
for(unsigned int i=1;i<new_segment->geometries.size();i++)
{
@@ -754,7 +754,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
for(geom_it=new_segment->geometries.begin();geom_it!=new_segment->geometries.end();)
{
length=geom_it->spline->find_closest_point(end_pose->x,end_pose->y,new_point);
- if(length<geom_it->spline->get_length())
+ if(length<std::numeric_limits<double>::max())
{
geom_it->spline->set_end_point(new_point);
geom_it->spline->generate(this->resolution);
@@ -1150,18 +1150,22 @@ TOpendriveWorldPose COpendriveRoadSegment::transform_to_world_pose(const TOpendr
unsigned int COpendriveRoadSegment::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
- double dist,min_dist=std::numeric_limits<double>::max();
+ double dist,min_dist=std::numeric_limits<double>::max(),length;
TOpendriveWorldPose closest_pose;
unsigned int closest_index=-1;
for(unsigned int i=0;i<this->nodes.size();i++)
{
- this->nodes[i]->get_closest_pose(pose,closest_pose,false,angle_tol);
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<min_dist)
+ length=this->nodes[i]->get_closest_pose(pose,closest_pose,false,angle_tol);
+ if(length<std::numeric_limits<double>::max())
{
- min_dist=dist;
- closest_index=i;
+ dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_index=i;
+ distance=length;
+ }
}
}
@@ -1180,19 +1184,23 @@ const COpendriveRoadNode &COpendriveRoadSegment::get_closest_node(TOpendriveWorl
int COpendriveRoadSegment::get_closest_lane_id(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
- double dist,min_dist=std::numeric_limits<double>::max();
+ double dist,min_dist=std::numeric_limits<double>::max(),length;
TOpendriveWorldPose closest_pose;
int closest_id=0;
std::map<int,COpendriveLane *>::const_iterator lane_it;
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
{
- lane_it->second->get_closest_pose(pose,closest_pose,angle_tol);
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<min_dist)
+ length=lane_it->second->get_closest_pose(pose,closest_pose,angle_tol);
+ if(length<std::numeric_limits<double>::max())
{
- min_dist=dist;
- closest_id=lane_it->first;
+ dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_id=lane_it->first;
+ distance=length;
+ }
}
}
@@ -1209,7 +1217,7 @@ const COpendriveLane &COpendriveRoadSegment::get_closest_lane(TOpendriveWorldPos
double COpendriveRoadSegment::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
{
- double dist,min_dist=std::numeric_limits<double>::max(),distance;
+ double dist,min_dist=std::numeric_limits<double>::max(),distance,length;
unsigned int closest_index=-1;
TPoint closest_spline_point;
@@ -1218,22 +1226,30 @@ double COpendriveRoadSegment::get_closest_pose(TOpendriveWorldPose &pose,TOpendr
closest_pose.heading=std::numeric_limits<double>::max();
for(unsigned int i=0;i<this->geometries.size();i++)
{
- this->geometries[i].spline->find_closest_point(pose.x,pose.y,closest_spline_point);
- dist=sqrt(pow(closest_spline_point.x-pose.x,2.0)+pow(closest_spline_point.y-pose.y,2.0));
- if(dist<min_dist)
+ length=this->geometries[i].spline->find_closest_point(pose.x,pose.y,closest_spline_point);
+ if(length<std::numeric_limits<double>::max())
{
- min_dist=dist;
- closest_index=i;
- closest_pose.x=closest_spline_point.x;
- closest_pose.y=closest_spline_point.y;
- closest_pose.heading=normalize_angle(closest_spline_point.heading);
+ dist=sqrt(pow(closest_spline_point.x-pose.x,2.0)+pow(closest_spline_point.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_index=i;
+ closest_pose.x=closest_spline_point.x;
+ closest_pose.y=closest_spline_point.y;
+ closest_pose.heading=normalize_angle(closest_spline_point.heading);
+ distance=length;
+ }
}
}
- distance=0.0;
- for(unsigned int i=0;i<closest_index;i++)
- distance+=this->geometries[i].spline->get_length();
- distance+=min_dist;
+ if(closest_index!=(unsigned int)-1)
+ {
+ for(unsigned int i=0;i<closest_index;i++)
+ distance+=this->geometries[i].spline->get_length();
+ distance+=min_dist;
+ }
+ else
+ distance=std::numeric_limits<double>::max();
return distance;
}
--
GitLab