From c628306af5f735abfd4200aaff5d1a8d8ee2d621 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Thu, 31 Dec 2020 17:49:42 +0100
Subject: [PATCH] Implemented the function to generate the spline. Added
functions to get the spline trajectory (with and without heading).
---
include/opendrive_link.h | 4 ++-
src/opendrive_link.cpp | 58 +++++++++++++++++++++++++++++++++++++---
2 files changed, 57 insertions(+), 5 deletions(-)
diff --git a/include/opendrive_link.h b/include/opendrive_link.h
index fa6946e..f8ad897 100644
--- a/include/opendrive_link.h
+++ b/include/opendrive_link.h
@@ -27,7 +27,7 @@ class COpendriveLink
void set_road_mark(opendrive_mark_t mark);
void set_resolution(double res);
void set_scale_factor(double scale);
- void generate(void);
+ void generate(double start_curvature,double end_curvature,double length,bool lane);
void update_references(std::map<COpendriveRoadNode *,COpendriveRoadNode *> refs);
public:
const COpendriveRoadNode &get_prev(void) const;
@@ -39,6 +39,8 @@ class COpendriveLink
double find_closest_local_point(TOpendriveLocalPoint &local,TPoint &point);
double get_track_point_world(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world);
double get_track_point_local(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
+ void get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length=0.0, double end_length=-1.0) const;
+ void get_trajectory(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double start_length=0.0, double end_length=-1.0) const;
double get_length(void) const;
friend std::ostream& operator<<(std::ostream& out, COpendriveLink &link);
~COpendriveLink();
diff --git a/src/opendrive_link.cpp b/src/opendrive_link.cpp
index 0c17e55..65b3771 100644
--- a/src/opendrive_link.cpp
+++ b/src/opendrive_link.cpp
@@ -46,9 +46,24 @@ void COpendriveLink::set_scale_factor(double scale)
this->scale_factor=scale;
}
-void COpendriveLink::generate(void)
-{
-
+void COpendriveLink::generate(double start_curvature,double end_curvature,double length,bool lane)
+{
+ TOpendriveWorldPoint node_start,node_end;
+ TPoint start,end;
+
+ node_start=this->prev->get_start_pose();
+ start.x=node_start.x;
+ start.y=node_start.y;
+ start.heading=node_start.heading;
+ start.curvature=start_curvature;
+ node_end=this->next->get_start_pose();
+ end.x=node_end.x;
+ end.y=node_end.y;
+ end.heading=node_end.heading;
+ end.curvature=end_curvature;
+// std::cout << start.x << "," << start.y << "," << start.heading << "," << start_curvature << "," << end.x << "," << end.y << "," << end.heading << "," << end_curvature << std::endl;
+ this->spline=new CG2Spline(start,end);
+ this->spline->generate(this->resolution,length);
}
void COpendriveLink::update_references(std::map<COpendriveRoadNode *,COpendriveRoadNode *> refs)
@@ -102,9 +117,44 @@ double COpendriveLink::get_track_point_local(TOpendriveTrackPoint &track,TOpendr
}
-double COpendriveLink::get_length(void) const
+void COpendriveLink::get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length, double end_length) const
+{
+ std::vector<double> curvature,heading;
+ CG2Spline *partial_spline=NULL;
+
+ if(start_length!=0.0 || end_length!=-1.0)// get partial spline
+ {
+ partial_spline=new CG2Spline;
+ this->spline->get_part(partial_spline,start_length,end_length);
+ partial_spline->evaluate_all(x,y,curvature,heading);
+ delete partial_spline;
+ }
+ else
+ this->spline->evaluate_all(x,y,curvature,heading);
+}
+
+void COpendriveLink::get_trajectory(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double start_length, double end_length) const
{
+ std::vector<double> curvature;
+ CG2Spline *partial_spline=NULL;
+ if(start_length!=0.0 || end_length!=-1.0)// get partial spline
+ {
+ partial_spline=new CG2Spline;
+ this->spline->get_part(partial_spline,start_length,end_length);
+ partial_spline->evaluate_all(x,y,curvature,yaw);
+ delete partial_spline;
+ }
+ else
+ this->spline->evaluate_all(x,y,curvature,yaw);
+}
+
+double COpendriveLink::get_length(void) const
+{
+ if(this->spline!=NULL)
+ return this->spline->get_length();
+ else
+ return 0.0;
}
std::ostream& operator<<(std::ostream& out, COpendriveLink &link)
--
GitLab