From cc472a2ab140a4f9c8b90a3117f5d531edc6e4f3 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Thu, 31 Dec 2020 17:55:28 +0100
Subject: [PATCH] Added a function to get the closest point to a given one. Got
the curvature information to generate the spline for each link.
---
include/opendrive_road_node.h | 1 +
src/opendrive_road_node.cpp | 66 ++++++++++++++++++++++++++++++++++-
2 files changed, 66 insertions(+), 1 deletion(-)
diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index f42ec44..65d5fdb 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -48,6 +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);
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 91b3e74..eb91077 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -1,6 +1,7 @@
#include "opendrive_road_node.h"
#include "opendrive_road.h"
#include "exceptions.h"
+#include <math.h>
COpendriveRoadNode::COpendriveRoadNode()
{
@@ -42,7 +43,10 @@ COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark)
{
+ TOpendriveWorldPoint lane_end,node_start;
+ double start_curvature,end_curvature,length;
COpendriveLink *new_link;
+ bool lane_found=false;
for(unsigned int i=0;i<this->links.size();i++)
if(this->links[i]->prev==this && this->links[i]->next==node)
@@ -53,7 +57,31 @@ void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
new_link->set_resolution(this->resolution);
new_link->set_scale_factor(this->scale_factor);
new_link->set_road_mark(mark);
- new_link->generate();
+ // get the curvature
+ node_start=node->get_start_pose();
+ for(unsigned int i=0;i<this->lanes.size();i++)
+ {
+ lane_end=this->lanes[i]->get_end_point();
+ if(fabs(lane_end.x-node_start.x)<this->resolution && fabs(lane_end.y-node_start.y)<this->resolution)
+ {
+ this->geometries[i]->get_curvature(start_curvature,end_curvature);
+ length=this->geometries[i]->get_length();
+ if(this->lanes[i]->get_id()>0)
+ {
+ start_curvature*=-1.0;
+ end_curvature*=-1.0;
+ }
+ lane_found=true;
+ break;
+ }
+ }
+ if(!lane_found)
+ {
+ start_curvature=0.0;
+ end_curvature=0.0;
+ length=sqrt(pow(this->start_point.x-node_start.x,2.0)+pow(this->start_point.y-node_start.y,2.0));
+ }
+ new_link->generate(start_curvature,end_curvature,length,lane_found);
this->links.push_back(new_link);
node->links.push_back(new_link);
}
@@ -244,6 +272,42 @@ 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 dist,min_dist=std::numeric_limits<double>::max();
+ double angle,min_angle=std::numeric_limits<double>::max();
+ double length,closest_length;
+ TOpendriveWorldPoint target;
+ 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
+ {
+ length=this->links[i]->find_closest_world_point(target,point);
+ angle=diff_angle(normalize_angle(heading),point.heading);
+ if(angle<min_angle)
+ {
+ 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;
+ closest_point.heading=point.heading;
+ closest_length=length;
+ }
+ }
+ }
+ }
+
+ return closest_length;
+}
+
std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node)
{
out << " Node: " << node.get_index() << std::endl;
--
GitLab