Implemented the function to generate the spline.

Added functions to get the spline trajectory (with and without heading).

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();

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)