Moved the opendrive geometry from the lane to the segment. It describes the center lane.

The links keep the splines to generate the trajectory.
Solved a bug with the computation of the curvature for lanes other than the central one.

include/opendrive_arc.h

@@ -6,12 +6,13 @@
 class COpendriveArc : public COpendriveGeometry
 {
   friend class COpendriveRoadNode;
+  friend class COpendriveRoadSegment;
   private:
     double curvature;
   protected:
     COpendriveArc();
     COpendriveArc(const COpendriveArc &object);
-    virtual bool transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
+    virtual bool transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
     virtual void print(std::ostream &out);
     virtual void load_params(const planView::geometry_type &geometry_info);
     virtual std::string get_name(void);

include/opendrive_geometry.h

@@ -11,6 +11,7 @@
 class COpendriveGeometry
 {
   friend class COpendriveRoadNode;
+  friend class COpendriveRoadSegment;
   private:
   protected:
     COpendriveGeometry();
@@ -20,7 +21,7 @@ class COpendriveGeometry
     double min_s; ///< Starting track coordenate "s" for the geometry.
     double max_s; ///< Ending track coordenate "s" for the geometry.
     TOpendriveWorldPose pose;
-    virtual bool transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const = 0;
+    virtual bool transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const = 0;
     virtual void print(std::ostream &out);
     virtual void load_params(const planView::geometry_type &geometry_info) = 0;
     virtual std::string get_name(void)=0;
@@ -30,14 +31,15 @@ class COpendriveGeometry
     void set_min_s(double s);
   public:
     virtual COpendriveGeometry *clone(void) = 0;
-    bool get_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
-    bool get_world_pose(TOpendriveTrackPose &track,TOpendriveWorldPose &world) const;
+    bool get_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
+    bool get_world_pose(const TOpendriveTrackPose &track,TOpendriveWorldPose &world) const;
     bool in_range(double s) const;
     double get_length(void) const;
     virtual void get_curvature(double &start,double &end)=0;
     double get_max_s(void) const;
     double get_min_s(void) const;
     TOpendriveWorldPose get_start_pose(void) const;
+    TOpendriveWorldPose get_end_pose(void) const;
     void operator=(const COpendriveGeometry &object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveGeometry &geom);
     virtual ~COpendriveGeometry();

include/opendrive_lane.h

@@ -71,13 +71,14 @@ class COpendriveLane
     double get_width(void) const;
     double get_speed(void) const;
     double get_offset(void) const;
+    double get_center_offset(void) const;
     unsigned int get_index(void) const;
     int get_id(void) const;
     TOpendriveWorldPose get_start_pose(void) const;
     TOpendriveWorldPose get_end_pose(void) const;
     double get_length(void) const;
-    TOpendriveLocalPose transform_to_local_pose(TOpendriveTrackPose &track);
-    TOpendriveWorldPose transform_to_world_pose(TOpendriveTrackPose &track);
+    TOpendriveLocalPose transform_to_local_pose(TOpendriveTrackPose &track) const;
+    TOpendriveWorldPose transform_to_world_pose(TOpendriveTrackPose &track) const;
     unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double angle_tol=0.1);
     friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
     ~COpendriveLane();

include/opendrive_line.h

@@ -6,11 +6,12 @@
 class COpendriveLine : public COpendriveGeometry
 {
   friend class COpendriveRoadNode;
+  friend class COpendriveRoadSegment;
   private:
   protected:
     COpendriveLine();
     COpendriveLine(const COpendriveGeometry &object);
-    virtual bool transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
+    virtual bool transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
     virtual void print(std::ostream &out);
     virtual void load_params(const planView::geometry_type &geometry_info);
     virtual std::string get_name(void);

include/opendrive_link.h

@@ -36,8 +36,8 @@ class COpendriveLink
     void generate(double start_curvature,double end_curvature,double length,bool lane);
     void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lanei_refs,node_up_ref_t &node_refs);
     bool clean_references(node_up_ref_t &refs);
-    void update_start_pose(TOpendriveWorldPose *start=NULL);
-    void update_end_pose(TOpendriveWorldPose *end=NULL);
+    TPoint update_start_pose(TOpendriveWorldPose *start=NULL);
+    TPoint update_end_pose(TOpendriveWorldPose *end=NULL);
   public:
     const COpendriveRoadNode &get_prev(void) const;
     const COpendriveRoadNode &get_next(void) const;

include/opendrive_param_poly3.h

@@ -14,6 +14,7 @@ typedef struct
 class COpendriveParamPoly3 : public COpendriveGeometry
 {
   friend class COpendriveRoadNode;
+  friend class COpendriveRoadSegment;
   private:
     TOpendrivePoly3Params u;
     TOpendrivePoly3Params v;
@@ -21,7 +22,7 @@ class COpendriveParamPoly3 : public COpendriveGeometry
   protected:
     COpendriveParamPoly3();
     COpendriveParamPoly3(const COpendriveParamPoly3 &object);
-    virtual bool transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
+    virtual bool transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
     virtual void print(std::ostream &out);
     virtual void load_params(const planView::geometry_type &geometry_info);
     virtual std::string get_name(void);

include/opendrive_road_node.h

@@ -10,8 +10,10 @@
 typedef struct
 {
   COpendriveLane * lane;
-  COpendriveGeometry *geometry;
   COpendriveRoadSegment *segment;
+  double start_curvature;
+  double end_curvature;
+  double length;
 }TOpendriveRoadNodeParent;
 
 class COpendriveRoadNode
@@ -31,7 +33,6 @@ class COpendriveRoadNode
     COpendriveRoadNode();
     COpendriveRoadNode(const COpendriveRoadNode &object);
     void free(void);
-    void load(const planView::geometry_type &geom_info,COpendriveLane *lane,COpendriveRoadSegment *segment);
     bool add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane);
     void add_link(COpendriveLink *link);
     void remove_link(COpendriveLink *link);
@@ -42,8 +43,7 @@ class COpendriveRoadNode
     void set_scale_factor(double scale);
     void set_index(unsigned int index);
     void set_pose(TOpendriveWorldPose &start);
-    void add_parent(const planView::geometry_type &geom_info,COpendriveLane *lane,COpendriveRoadSegment *segment);
-    void add_parent(COpendriveGeometry *geometry,COpendriveLane *lane,COpendriveRoadSegment *segment);
+    void add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double length,double start_curvature,double end_curvature);
     TOpendriveRoadNodeParent *get_parent_with_lane(const COpendriveLane *lane);
     TOpendriveRoadNodeParent *get_parent_with_segment(const COpendriveRoadSegment *segment);
     bool updated(node_up_ref_t &refs);
@@ -59,7 +59,6 @@ class COpendriveRoadNode
     unsigned int get_num_parents(void) const;
     const COpendriveRoadSegment& get_parent_segment(unsigned int index) const;
     const COpendriveLane &get_parent_lane(unsigned int index) const;
-    const COpendriveGeometry &get_geometry(unsigned int index) const;
     TOpendriveWorldPose get_pose(void) const;
     unsigned int get_num_links(void) const;
     const COpendriveLink &get_link(unsigned int index) const;

include/opendrive_road_segment.h

@@ -11,6 +11,12 @@
 #include "opendrive_object.h"
 #include "opendrive_road.h"
 #include "opendrive_road_node.h"
+#include "opendrive_geometry.h"
+
+typedef struct{
+  COpendriveGeometry *opendrive;
+  CG2Spline *spline;
+}TOpendriveRoadSegmentGeometry;
 
 class COpendriveRoadSegment
 {
@@ -20,6 +26,7 @@ class COpendriveRoadSegment
   private:
     std::map<int,COpendriveLane *> lanes;
     std::vector<COpendriveRoadNode *> nodes;
+    std::vector<TOpendriveRoadSegmentGeometry> geometries;
     COpendriveRoad *parent_road;
     double resolution;
     double scale_factor;
@@ -51,8 +58,9 @@ class COpendriveRoadSegment
     void add_lanes(lanes::laneSection_type &lane_section);
     void add_lane(const ::lane &lane_info);
     void remove_lane(COpendriveLane *lane);
-    void add_nodes(OpenDRIVE::road_type &road_info);
-    void add_node(const planView::geometry_type &geom_info,COpendriveLane *lane);
+    void add_geometries(OpenDRIVE::road_type &road_info);
+    void add_nodes(void);
+    void add_node(TOpendriveRoadSegmentGeometry &geometry,COpendriveLane *lane);
     void add_empty_node(TOpendriveWorldPose &pose,COpendriveLane *lane);
     void remove_node(COpendriveRoadNode *node);
     bool has_node(COpendriveRoadNode *node);
@@ -80,6 +88,7 @@ class COpendriveRoadSegment
     const COpendriveObject &get_object(unsigned int index) const;
     unsigned int get_num_connecting(void) const;
     const COpendriveRoadSegment &get_connecting(unsigned int index) const;
+    double get_length(void);
     TOpendriveLocalPose transform_to_local_pose(const TOpendriveTrackPose &track);
     TOpendriveWorldPose transform_to_world_pose(const TOpendriveTrackPose &track);
     friend std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment);

include/opendrive_spiral.h

@@ -6,13 +6,14 @@
 class COpendriveSpiral : public COpendriveGeometry
 {
   friend class COpendriveRoadNode;
+  friend class COpendriveRoadSegment;
   private:
     double start_curvature;
     double end_curvature;
   protected:
     COpendriveSpiral();
     COpendriveSpiral(const COpendriveSpiral &object);
-    virtual bool transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
+    virtual bool transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
     virtual void print(std::ostream &out);
     virtual void load_params(const planView::geometry_type &geometry_info);
     virtual std::string get_name(void);

src/opendrive_arc.cpp

@@ -11,7 +11,7 @@ COpendriveArc::COpendriveArc(const COpendriveArc &object) : COpendriveGeometry(o
   this->curvature=object.curvature;
 }
 
-bool COpendriveArc::transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
+bool COpendriveArc::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
 {
   double alpha;
 

src/opendrive_geometry.cpp

@@ -62,12 +62,12 @@ void COpendriveGeometry::set_min_s(double s)
   this->min_s=s*this->scale_factor;
 }
 
-bool COpendriveGeometry::get_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
+bool COpendriveGeometry::get_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
 {
   return this->transform_local_pose(track,local);
 }
 
-bool COpendriveGeometry::get_world_pose(TOpendriveTrackPose &track,TOpendriveWorldPose &world) const
+bool COpendriveGeometry::get_world_pose(const TOpendriveTrackPose &track,TOpendriveWorldPose &world) const
 {
   TOpendriveLocalPose local;
 
@@ -116,6 +116,19 @@ TOpendriveWorldPose COpendriveGeometry::get_start_pose(void) const
   return tmp_pose;
 }
 
+TOpendriveWorldPose COpendriveGeometry::get_end_pose(void) const
+{
+  TOpendriveTrackPose track_pose;
+  TOpendriveWorldPose world_pose;
+ 
+  track_pose.s=this->get_length();
+  track_pose.t=0.0;
+  track_pose.heading=0.0;
+  this->get_world_pose(track_pose,world_pose);
+
+  return world_pose;
+}
+
 void COpendriveGeometry::operator=(const COpendriveGeometry &object)
 {
   this->min_s = object.min_s;

src/opendrive_lane.cpp

@@ -628,6 +628,14 @@ double COpendriveLane::get_offset(void) const
   return this->offset/this->scale_factor;
 }
 
+double COpendriveLane::get_center_offset(void) const
+{
+  if(this->id<0)
+    return (this->offset-this->width/2.0)/this->scale_factor;
+  else
+    return (this->offset+this->width/2.0)/this->scale_factor;
+}
+
 unsigned int COpendriveLane::get_index(void) const
 {
   return this->index;
@@ -642,40 +650,20 @@ TOpendriveWorldPose COpendriveLane::get_start_pose(void) const
 {
   TOpendriveTrackPose track_pose;
   TOpendriveWorldPose world_pose;
-  TOpendriveRoadNodeParent *parent;
   std::stringstream error;
 
-  track_pose.heading=0.0;
+  track_pose.t=0.0;
   if(this->id<0)// right lane
   {
-    track_pose.t=this->get_offset()-this->get_width()/2.0;
     track_pose.s=0.0;
-    parent=this->nodes[0]->get_parent_with_lane(this);
-    if(parent!=NULL)
-      parent->geometry->get_world_pose(track_pose,world_pose);
-    else
-    {
-      error << "Node " << this->nodes[0]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
-      throw CException(_HERE_,error.str());
-    }
+    track_pose.heading=0.0;
   }
   else
   {
-    track_pose.t=this->get_offset()+this->get_width()/2.0;
-    parent=this->nodes[0]->get_parent_with_lane(this);
-    if(parent!=NULL)
-    {
-      track_pose.s=parent->geometry->get_length();
-      parent->geometry->get_world_pose(track_pose,world_pose);
-    }
-    else
-    {
-      error << "Node " << this->nodes[0]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
-      throw CException(_HERE_,error.str());
-    }
+    track_pose.s=this->segment->get_length();
+    track_pose.heading=3.14159;
   }
-  if(this->id>0)
-    world_pose.heading=normalize_angle(world_pose.heading+3.14159);
+  world_pose=this->transform_to_world_pose(track_pose);
 
   return world_pose;
 }
@@ -684,141 +672,57 @@ TOpendriveWorldPose COpendriveLane::get_end_pose(void) const
 {
   TOpendriveTrackPose track_pose;
   TOpendriveWorldPose world_pose;
-  TOpendriveRoadNodeParent *parent;
   std::stringstream error;
   
-  track_pose.heading=0.0;
-  if(this->id>0)// left lane
-  { 
-    track_pose.t=this->get_offset()+this->get_width()/2.0;
-    track_pose.s=0.0;
-    parent=this->nodes[this->nodes.size()-1]->get_parent_with_lane(this);
-    if(parent!=NULL)
-      parent->geometry->get_world_pose(track_pose,world_pose);
-    else
-    {
-      error << "Node " << this->nodes[this->nodes.size()-1]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
-      throw CException(_HERE_,error.str());
-    }
+  track_pose.t=0.0;
+  if(this->id<0)// right_lane
+  {
+    track_pose.s=this->segment->get_length();
+    track_pose.heading=0.0;
   }
   else
-  { 
-    track_pose.t=this->get_offset()-this->get_width()/2.0;
-    parent=this->nodes[this->nodes.size()-1]->get_parent_with_lane(this);
-    if(parent!=NULL)
-    {
-      track_pose.s=parent->geometry->get_length();
-      parent->geometry->get_world_pose(track_pose,world_pose);
-    }
-    else
-    {
-      error << "Node " << this->nodes[this->nodes.size()-1]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
-      throw CException(_HERE_,error.str());
-    }
+  {
+    track_pose.s=0.0;
+    track_pose.heading=3.14159;
   }
-  if(this->id>0)
-    world_pose.heading=normalize_angle(world_pose.heading+3.14159);
+  world_pose=this->transform_to_world_pose(track_pose);
 
   return world_pose;
 }
 
 double COpendriveLane::get_length(void) const
 {
-  TOpendriveRoadNodeParent *parent;
   std::stringstream error;
   double length=0.0;
 
   for(unsigned int i=0;i<this->nodes.size();i++)
   {
-    parent=this->nodes[i]->get_parent_with_lane(this);
-    if(parent!=NULL)
-      length+=parent->geometry->get_length();
-    else
+    for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
     {
-      error << "Node " << this->nodes[i]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
-      throw CException(_HERE_,error.str());
+      if(&this->nodes[i]->get_link(j).get_parent_lane()==this)
+        length+=this->nodes[i]->links[j]->get_length();
     }
   }
 
   return length;
 }
 
-TOpendriveLocalPose COpendriveLane::transform_to_local_pose(TOpendriveTrackPose &track)
+TOpendriveLocalPose COpendriveLane::transform_to_local_pose(TOpendriveTrackPose &track) const
 {
-  TOpendriveLocalPose local;
-  TOpendriveRoadNodeParent *parent;
-  std::stringstream error;
+  TOpendriveTrackPose pose;
 
-  if(track.s<0.0)
-  {
-    error << "Invalid track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ")";
-    throw CException(_HERE_,error.str());
-  }
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  { 
-    parent=this->nodes[i]->get_parent_with_lane(this);
-    if(parent!=NULL)
-    {
-      if(track.s<=parent->geometry->get_length())
-      { 
-        if(!parent->geometry->get_local_pose(track,local))
-        {
-          error << "Impossible to transform to local coordinates pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") in lane " << parent->lane->get_id() << " of segment " << parent->segment->get_name();
-          throw CException(_HERE_,error.str());
-        }
-        return local;
-      }
-      else
-        track.s-=parent->geometry->get_length();
-    }
-    else
-    {
-      error << "Node " << this->nodes[i]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
-      throw CException(_HERE_,error.str());
-    }
-  }
-
-  error << "Track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") does not belong to lane  " << this->id << " of segment " << this->segment->get_name();
-  throw CException(_HERE_,error.str());
+  pose=track;
+  pose.t+=this->get_center_offset();
+  return this->segment->transform_to_local_pose(pose);
 }
 
-TOpendriveWorldPose COpendriveLane::transform_to_world_pose(TOpendriveTrackPose &track)
+TOpendriveWorldPose COpendriveLane::transform_to_world_pose(TOpendriveTrackPose &track) const
 { 
-  TOpendriveWorldPose world;
-  TOpendriveRoadNodeParent *parent;
-  std::stringstream error;
-
-  if(track.s<0.0)
-  {
-    error << "Invalid track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ")";
-    throw CException(_HERE_,error.str());
-  }
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  {
-    parent=this->nodes[i]->get_parent_with_lane(this);
-    if(parent!=NULL)
-    {
-      if(track.s<=parent->geometry->get_length())
-      {
-        if(!parent->geometry->get_world_pose(track,world))
-        {
-          error << "Impossible to transform to local coordinates pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") in lane " << parent->lane->get_id() << " of segment " << parent->segment->get_name();
-          throw CException(_HERE_,error.str());
-        }
-        return world;
-      }
-      else
-        track.s-=parent->geometry->get_length();
-    }
-    else
-    {
-      error << "Node " << this->nodes[i]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
-      throw CException(_HERE_,error.str());
-    }
-  }
+  TOpendriveTrackPose pose;
 
-  error << "Track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") does not belong to lane  " << this->id << " of segment " << this->segment->get_name();
-  throw CException(_HERE_,error.str());
+  pose=track;
+  pose.t+=this->get_center_offset();
+  return this->segment->transform_to_world_pose(pose);
 }
 
 unsigned int COpendriveLane::get_closest_node_index(TOpendriveWorldPose &pose,double angle_tol)

src/opendrive_line.cpp

@@ -10,7 +10,7 @@ COpendriveLine::COpendriveLine(const COpendriveGeometry &object) : COpendriveGeo
 
 }
 
-bool COpendriveLine::transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
+bool COpendriveLine::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
 {
   local.u=track.s;
   local.v=track.t;

src/opendrive_link.cpp

@@ -53,11 +53,34 @@ void COpendriveLink::set_parent_lane(COpendriveLane *lane)
 void COpendriveLink::set_resolution(double res)
 {
   this->resolution=res;
+
+  if(this->spline!=NULL)
+    this->spline->generate(res);
 }
 
 void COpendriveLink::set_scale_factor(double scale)
 {
+  TPoint spline_start,spline_end;
+
+  if(this->spline!=NULL)
+  {
+    this->spline->get_start_point(spline_start);
+    spline_start.x*=this->scale_factor/scale;
+    spline_start.y*=this->scale_factor/scale;
+    spline_start.curvature*=scale/this->scale_factor;
+    this->spline->set_start_point(spline_start);
+    this->spline->get_end_point(spline_end);
+    spline_end.x*=this->scale_factor/scale;
+    spline_end.y*=this->scale_factor/scale;
+    spline_end.curvature*=scale/this->scale_factor;
+    this->spline->set_end_point(spline_end);
+    this->spline->generate(this->resolution);
+  }
+
+
   this->scale_factor=scale;
+
+
 }
 
 void COpendriveLink::generate(double start_curvature,double end_curvature,double length,bool lane)
@@ -105,13 +128,13 @@ bool COpendriveLink::clean_references(node_up_ref_t &refs)
   return false;
 }
 
-void COpendriveLink::update_start_pose(TOpendriveWorldPose *start)
+TPoint COpendriveLink::update_start_pose(TOpendriveWorldPose *start)
 {
-  TPoint start_pose;
+  TPoint start_pose={0};
   double length;
 
   if(start==NULL)
-    return;
+    return start_pose;
   if(this->spline!=NULL)
   {
     length=this->spline->find_closest_point(start->x,start->y,start_pose);
@@ -119,21 +142,25 @@ void COpendriveLink::update_start_pose(TOpendriveWorldPose *start)
     this->spline->set_start_point(start_pose);
     this->spline->generate(this->resolution,length);
   }
+
+  return start_pose;
 }
 
-void COpendriveLink::update_end_pose(TOpendriveWorldPose *end)
+TPoint COpendriveLink::update_end_pose(TOpendriveWorldPose *end)
 {
-  TPoint end_pose;
+  TPoint end_pose={0};
   double length;
 
   if(end==NULL)
-    return;
+    return end_pose;
   if(this->spline!=NULL)
   {
     length=this->spline->find_closest_point(end->x,end->y,end_pose);
     this->spline->set_end_point(end_pose);
     this->spline->generate(this->resolution,length);
   }
+
+  return end_pose;
 }
 
 const COpendriveRoadNode &COpendriveLink::get_prev(void) const

src/opendrive_param_poly3.cpp

@@ -27,7 +27,7 @@ COpendriveParamPoly3::COpendriveParamPoly3(const COpendriveParamPoly3 &object) :
   this->normalized=object.normalized;
 }
 
-bool COpendriveParamPoly3::transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
+bool COpendriveParamPoly3::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
 {
   double p = (this->normalized ? track.s/((this->max_s - this->min_s)/this->scale_factor):track.s);
   double p2 = p*p;

src/opendrive_road.cpp

@@ -741,9 +741,9 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
   // add the last segment
   node=this->nodes[path_nodes[path_nodes.size()-1]];
   link_index=node->get_closest_link(end_pose,3.14159);
-  link=node->links[link_index];
-  if(link==NULL)
+  if(link_index==(unsigned int)-1)
     throw CException(_HERE_,"The provided path has unconnected nodes");
+  link=node->links[link_index];
   segment=link->segment;
   if(new_segment_ref.find(segment)==new_segment_ref.end())
   {
@@ -786,7 +786,26 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>