Initial commit of the library to import Opendrive roads.

include/opendrive_arc.h

+#ifndef _OPENDRIVE_ARC_H
+#define _OPENDRIVE_ARC_H
+
+#include "opendrive_geometry.h"
+
+class COpendriveArc : public COpendriveGeometry
+{
+  private:
+    double curvature;
+  protected:
+    virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
+    virtual void print(std::ostream &out);
+    virtual void load_params(const planView::geometry_type &geometry_info);
+  public:
+    COpendriveArc();
+    COpendriveArc(double min_s, double max_s, double x, double y, double heading, double curvature);
+    COpendriveArc(const COpendriveArc &object);
+    virtual COpendriveGeometry *clone(void);
+    double get_curvature(void);
+    void operator=(const COpendriveArc &object);
+    ~COpendriveArc();
+};
+
+#endif

include/opendrive_common.h

+#ifndef _OPENDRIVE_COMMON_H
+#define _OPENDRIVE_COMMON_H
+
+#define DEFAULT_RESOLUTION        0.1
+#define DEFAULT_SCALE_FACTOR      1.0
+#define DEFAULT_MIN_ROAD_LENGTH   0.1
+
+typedef struct
+{
+  double s;
+  double t;
+  double heading;
+}TOpendriveTrackPoint;
+
+typedef struct
+{
+  double u;
+  double v;
+  double heading;
+}TOpendriveLocalPoint;
+
+typedef struct
+{
+  double x;
+  double y;
+  double heading;
+}TOpendriveWorldPoint;
+
+typedef enum{OD_MARK_NONE,
+             OD_MARK_SOLID,
+             OD_MARK_BROKEN,
+             OD_MARK_SOLID_SOLID,
+             OD_MARK_SOLID_BROKEN,
+             OD_MARK_BROKEN_SOLID,
+             OD_MARK_BROKEN_BROKEN} opendrive_mark_t;
+
+double normalize_angle(double angle);
+double diff_angle(double angle1,double angle2);
+
+#endif

include/opendrive_geometry.h

+#ifndef _OPENDRIVE_GEOMETRY_H
+#define _OPENDRIVE_GEOMETRY_H
+
+#include "opendrive_common.h"
+#ifdef _HAVE_XSD
+#include "../src/xml/OpenDRIVE_1.4H.hxx"
+#endif
+
+#include <iostream>
+
+class COpendriveGeometry
+{
+  private:
+  protected:
+    double min_s; ///< Starting track coordenate "s" for the geometry.
+    double max_s; ///< Ending track coordenate "s" for the geometry.
+    TOpendriveWorldPoint pose;
+    virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) = 0;
+    virtual void print(std::ostream &out);
+    virtual void load_params(const planView::geometry_type &geometry_info) = 0;
+  public:
+    COpendriveGeometry();
+    COpendriveGeometry(double min_s, double max_s, double x, double y, double heading);
+    COpendriveGeometry(const COpendriveGeometry &object);
+    virtual COpendriveGeometry *clone(void) = 0;
+    void load(const planView::geometry_type &geometry_info);
+    bool get_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
+    bool get_world_pose(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world);
+    bool in_range(double s);
+    double get_length(void);
+    void operator=(const COpendriveGeometry &object);
+    friend std::ostream& operator<<(std::ostream& out, COpendriveGeometry &geom);
+    virtual ~COpendriveGeometry();
+};
+
+#endif

include/opendrive_lane.h

+#ifndef _OPENDRIVE_LANE_H
+#define _OPENDRIVE_LANE_H
+
+#ifdef _HAVE_XSD
+#include "../src/xml/OpenDRIVE_1.4H.hxx"
+#endif
+
+#include "opendrive_road_node.h"
+
+class COpendriveLane
+{
+  friend class COpendriveRoadSegment;
+  private:
+    std::vector<COpendriveRoadNode *> nodes;
+    COpendriveRoadSegment *segment;
+    double scale_factor;
+    double width;
+    double speed;
+    double offset;
+  protected:
+    void add_node(COpendriveRoadNode *node);
+    void set_resolution(double res);
+    void set_scale_factor(double scale);
+    void set_offset(double offset);
+    void update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs)
+  public:
+    COpendriveLane();
+    COpendriveLane(const COpendriveLane &object);
+    void load(const right::lane_type &lane_info,COpendriveRoadSegment *segment);
+    unsigned int get_num_nodes(void);
+    const COpendriveRoadNode &get_node(unsigned int index);
+    const COpendriveRoadSegment &get_segment(void);
+    double get_width(void);
+    double get_speed(void);
+    double get_offset(void);
+    void operator=(const COpendriveLane &object);
+    friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
+    ~COpendriveLane();
+};
+
+#endif

include/opendrive_line.h

+#ifndef _OPENDRIVE_LINE_H
+#define _OPENDRIVE_LINE_H
+
+#include "opendrive_geometry.h"
+
+class COpendriveLine : public COpendriveGeometry
+{
+  private:
+  protected:
+    virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
+    virtual void print(std::ostream &out);
+    virtual void load_params(const planView::geometry_type &geometry_info);
+  public:
+    COpendriveLine();
+    COpendriveLine(double min_s, double max_s, double x, double y, double heading);
+    COpendriveLine(const COpendriveGeometry &object);
+    virtual COpendriveGeometry *clone(void);
+    void operator=(const COpendriveLine &object);
+    ~COpendriveLine();
+};
+
+#endif

include/opendrive_link.h

+#ifndef _OPNEDRIVE_LINK_H
+#define _OPENDRIVE_LINK_H
+
+#include "opendrive_road_node.h"
+#include "g2_spline.h"
+#include "opendrive_line.h"
+#include "opendrive_spiral.h"
+#include "opendrive_arc.h"
+#include "opendrive_param_poly3.h"
+
+class COpendriveLink
+{
+  private:
+    COpendriveRoadNode *prev;
+    COpendriveRoadNode *next;
+    CG2Spline *spline;
+    opendrive_mark_t mark;
+    double resolution;
+    double scale_factor;    
+  protected:
+    void set_prev(COpendriveRoadNode *node);
+    void set_next(COpendriveRoadNode *node);
+    void set_road_mark(opendrive_mark_t mark);
+    void set_resolution(double res);
+    void set_scale_factor(double scale);
+  public:
+    COpendriveLink();
+    COpendriveLink(const COpendriveLink &object);
+    const COpendriveRoadNode &get_prev(void);
+    const COpendriveRoadNode &get_next(void);
+    opendrive_mark_t get_road_mark(void);
+    double get_resolution(void);
+    double get_scale_factor(void);
+    double find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point);
+    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);
+    double get_length(void);
+    void operator=(const COpendriveLink &object);
+    friend std::ostream& operator<<(std::ostream& out, COpendriveLink &link);
+    ~COpendriveLink();
+};
+
+#endif

include/opendrive_object.h

+#ifndef _OPENDRIVE_OBJECT_H
+#define _OPENDRIVE_OBJECT_H
+
+#include "opendrive_common.h"
+#ifdef _HAVE_XSD
+#include "../src/xml/OpenDRIVE_1.4H.hxx"
+#endif
+
+#define OD_MAX_VERTICES     32
+
+typedef enum {OD_NONE,OD_BOX,OD_CYLINDER,OD_POLYGON} object_type_t;
+
+typedef struct
+{
+  double length;
+  double width;
+  double height;
+}TOpendriveBox;
+
+typedef struct
+{
+  double radius;
+  double height;
+}TOpendriveCylinder;
+
+typedef struct
+{  
+  TOpendriveTrackPoint vertices[OD_MAX_VERTICES];
+  double height[OD_MAX_VERTICES];
+  unsigned int num_vertices;
+}TOpendrivePolygon;
+
+typedef union
+{
+  TOpendriveBox box;
+  TOpendriveCylinder cylinder;
+  TOpendrivePolygon polygon;
+}TOpendriveObject;
+
+class COpendriveObject
+{
+  private:
+    int id; ///< Object's id.
+    TOpendriveTrackPoint pose;
+    TOpendriveObject object;
+    std::string type; ///< Object's OpenDrive type.
+    std::string name; ///< Object's name.
+    object_type_t object_type;
+  protected:
+  public:
+    COpendriveObject();
+    COpendriveObject(const COpendriveObject& object);
+    void load(std::unique_ptr<objects::object_type> &object_info);
+    TOpendriveTrackPoint get_pose(void);
+    std::string get_type(void);
+    std::string get_name(void);
+    bool is_box(void);
+    bool is_cylinder(void);
+    bool is_polygon(void);
+    TOpendriveBox get_box_data(void);
+    TOpendriveCylinder get_cylinder_data(void);
+    TOpendrivePolygon get_polygon_data(void);
+    void operator=(const COpendriveObject &object);
+    friend std::ostream& operator<<(std::ostream& out, COpendriveObject &object);
+    ~COpendriveObject();
+};
+
+#endif

include/opendrive_param_poly3.h

+#ifndef _OPENDRIVE_PARAMPOLY3_H
+#define _OPENDRIVE_PARAMPOLY3_H
+
+#include "opendrive_geometry.h"
+
+typedef struct
+{
+  double a;
+  double b;
+  double c;
+  double d;
+}TOpendrivePoly3Params;
+
+class COpendriveParamPoly3 : public COpendriveGeometry
+{
+  private:
+    TOpendrivePoly3Params u;
+    TOpendrivePoly3Params v;
+    bool normalized;
+  protected:
+    virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
+    virtual void print(std::ostream &out);
+    virtual void load_params(const planView::geometry_type &geometry_info);
+  public:
+    COpendriveParamPoly3();
+    COpendriveParamPoly3(double min_s, double max_s, double x, double y, double heading,TOpendrivePoly3Params &u,TOpendrivePoly3Params &v,bool normalized);
+    COpendriveParamPoly3(const COpendriveParamPoly3 &object);
+    virtual COpendriveGeometry *clone(void);
+    TOpendrivePoly3Params get_u_params(void);
+    TOpendrivePoly3Params get_v_params(void);
+    bool is_normalized(void);
+    void operator=(const COpendriveParamPoly3 &object);
+    ~COpendriveParamPoly3();
+};
+
+#endif

include/opendrive_road.h

+#ifndef _OPENDRIVE_ROAD_H
+#define _OPENDRIVE_ROAD_H
+
+#ifdef _HAVE_XSD
+#include "../src/xml/OpenDRIVE_1.4H.hxx"
+#endif
+
+#include "opendrive_road_segment.h"
+
+class COpendriveRoad
+{
+  private:
+    std::vector<COpendriveRoadSegment *> segments;
+    double resolution;
+    double scale_factor;
+    double min_road_length;
+  protected:
+    void add_road_nodes(std::unique_ptr<OpenDRIVE::road_type> &road_info);
+    void set_resolution(double res);
+    void set_scale_factor(double scale);
+    void set_min_road_length(double length);
+  public:
+    COpendriveRoad();
+    COpendriveRoad(const COpendriveRoad& object);
+    void load(const std::string &filename);
+    double get_resolution(void);
+    double get_scale_factor(void);
+    double get_min_road_length(void);
+    unsigned int get_num_segments(void);
+    const COpendriveRoadSegment& get_segment(unsigned int index);
+    const COpendriveRoadSegment& operator[](std::size_t index);
+    void operator=(const COpendriveRoad& object);
+    friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
+    ~COpendriveRoad();
+};
+
+#endif

include/opendrive_road_node.h

+#ifndef _OPENDRIVE_ROAD_NODE_H
+#define _OPENDRIVE_ROAD_NODE_H
+
+#include <vector>
+#include "opendrive_link.h"
+#include "opendrive_lane.h"
+
+class COpendriveRoadNode
+{
+  private:
+    std::vector<COpendriveLink *> links;
+    double resolution;
+    double scale_factor;
+    TOpendriveWorldPoint start_point;
+    COpendriveLane *lane;
+    COpendriveGeometry *geometry;
+  protected:
+    unsigned int add_link(COpendriveRoadNode *node);
+    void set_resolution(double res);
+    void set_scale_factor(double scale);
+  public:
+    COpendriveRoadNode();
+    COpendriveRoadNode(const COpendriveRoadNode &object);
+    void load(const planView::geometry_type &node_info,COpendriveLane *lane);
+    double get_resolution(void);
+    double get_scale_factor(void);
+    TOpendriveWorldPoint get_start_pose(void);
+    unsigned int get_num_links(void);
+    const COpendriveLink &get_link(unsigned int index);
+    const COpendriveLane &get_lane(void);
+    void operator=(const COpendriveRoadNode &object);
+    friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
+    ~COpendriveRoadNode();
+};
+
+#endif

include/opendrive_road_segment.h

+#ifndef _OPENDRIVE_ROAD_SEGMENT_H
+#define _OPENDRIVE_ROAD_SEGMENT_H
+
+#ifdef _HAVE_XSD
+#include "../src/xml/OpenDRIVE_1.4H.hxx"
+#endif
+
+#include "opendrive_lane.h"
+#include "opendrive_signal.h"
+#include "opendrive_object.h"
+
+class COpendriveRoadSegment
+{
+  friend class COpendriveRoad;
+  private:
+    std::map<int,COpendriveLane *> lanes;
+    unsigned int num_right_lanes;
+    unsigned int num_left_lanes;
+    std::vector<COpendriveSignal *> signals;
+    std::vector<COpendriveObject *> objects;
+    std::vector<COpendriveRoadSegment *> next;
+    std::vector<COpendriveRoadSegment *> prev;
+    std::string name;
+    unsigned int id;
+  protected:
+    void set_resolution(double res);
+    void set_scale_factor(double scale);
+    void update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs);
+  public:
+    COpendriveRoadSegment();
+    COpendriveRoadSegment(const COpendriveRoadSegment &object);
+    void load_road(OpenDRIVE::road_type &road_info);
+    std::string get_name(void);
+    unsigned int get_id(void);
+    unsigned int get_num_right_lanes(void);
+    unsigned int get_num_left_lanes(void);
+    const COpendriveLane &get_lane(int index);
+    unsigned int get_num_signals(void);
+    const COpendriveSignal &get_signal(unsigned int index);
+    unsigned int get_num_obstacles(void);
+    const COpendriveObject &get_object(unsigned int index);
+    unsigned int get_num_previous(void);
+    const COpendriveRoadSegment &get_previous(unsigned int index);
+    unsigned int get_num_next(void);
+    const COpendriveRoadSegment &get_next(unsigned int index);
+    void operator=(const COpendriveRoadSegment &object);
+    friend std::ostream& operator<<(std::ostream& out, COpendriveRoadsegment &segment);
+    ~COpendriveRoadSegment();
+};
+
+#endif

include/opendrive_signal.h

+#ifndef _OPENDRIVE_SIGNAL_H
+#define _OPENDRIVE_SIGNAL_H
+
+#include "opendrive_common.h"
+#ifdef _HAVE_XSD
+#include "../src/xml/OpenDRIVE_1.4H.hxx"
+#endif
+
+class COpendriveSignal
+{
+  private:
+    int id; ///< Signal's id.
+    TOpendriveTrackPoint pose;
+    std::string type; ///< Signal's OpenDrive type.
+    std::string sub_type; ///< Signal's OpenDrive subtype.
+    int value; ///< Signal's value.
+    std::string text; ///< Signal's text.    
+  protected:
+  public:
+    COpendriveSignal();
+    COpendriveSignal(const COpendriveSignal &object);
+    COpendriveSignal(int id, double s, double t, double heading, std::string type, std::string sub_type, int value, std::string text, bool reverse = false);
+    void load(std::unique_ptr<signals::signal_type> &signal_info);
+    int get_id(void);
+    TOpendriveTrackPoint get_pose(void);
+    void get_type(std::string &type, std::string &sub_type);
+    int get_value(void);
+    std::string get_text(void);
+    void operator=(const COpendriveSignal &object);
+    friend std::ostream& operator<<(std::ostream& out, COpendriveSignal &signal);
+    ~COpendriveSignal();
+};
+
+#endif

include/opendrive_spiral.h

+#ifndef _OPENDRIVE_SPIRAL_H
+#define _OPENDRIVE_SPIRAL_H
+
+#include "opendrive_geometry.h"
+
+class COpendriveSpiral : public COpendriveGeometry
+{
+  private:
+    double start_curvature;
+    double end_curvature;
+  protected:
+    virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
+    virtual void print(std::ostream &out);
+    virtual void load_params(const planView::geometry_type &geometry_info);
+  public:
+    COpendriveSpiral();
+    COpendriveSpiral(double min_s, double max_s, double x, double y, double heading,double start_curv,double end_curv);
+    COpendriveSpiral(const COpendriveSpiral &object);
+    virtual COpendriveGeometry *clone(void);
+    double get_start_curvature(void);
+    double get_end_curvature(void);
+    void operator=(const COpendriveSpiral &object);
+    ~COpendriveSpiral();
+};
+
+#endif

src/CMakeLists.txt

 # driver source files
-SET(sources gradient.cpp g2_spline.cpp dijkstra.cpp)
+SET(sources gradient.cpp g2_spline.cpp dijkstra.cpp opendrive_geometry.cpp opendrive_common.cpp opendrive_line.cpp opendrive_spiral.cpp opendrive_arc.cpp opendrive_param_poly3.cpp opendrive_signal.cpp opendrive_object.cpp opendrive_road.cpp opendrive_road_segment.cpp opendrive_lane.cpp opendrive_road_node.cpp opendrive_link.cpp)
 
 # application header files
-SET(headers ../include/gradient.h ../include/g2_spline.h ../include/dijkstra.h)
+SET(headers ../include/gradient.h ../include/g2_spline.h ../include/dijkstra.h ../include/opendrive_common.h ../include/opendrive_geometry.h ../include/opendrive_line.h ../include/opendrive_spiral.h ../include/opendrive_arc.h ../include/opendrive_param_poly3.h ../include/opendrive_signal.h ../include/opendrive_object.h ../include/opendrive_road.h ../include/opendrive_road_segment.h ../include/opendrive_lane.h ../include/opendrive_road_node.h ../include/opendrive_link.h)
 
 # locate the necessary dependencies
 find_package(Eigen3 REQUIRED)
+find_package(iriutils REQUIRED)
+
+ADD_SUBDIRECTORY(xml)
 
 # add the necessary include directories
 INCLUDE_DIRECTORIES(../include)
 INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
 
 # create the shared library
-ADD_LIBRARY(${PROJECT_NAME} SHARED ${sources})
+ADD_LIBRARY(${PROJECT_NAME} SHARED ${sources} ${XSD_SOURCES})
+
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${XSD_LIBRARY})
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${iriutils_LIBRARY})
+
+ADD_DEPENDENCIES(${PROJECT_NAME} xsd_files_gen)
+
+SET_SOURCE_FILES_PROPERTIES(${XSD_SOURCES} PROPERTIES GENERATED 1)
 
 # link necessary libraries
 INSTALL(TARGETS ${PROJECT_NAME}

src/examples/CMakeLists.txt

@@ -2,3 +2,8 @@
 ADD_EXECUTABLE(dijkstra_test dijkstra_test.cpp)
 # link necessary libraries
 TARGET_LINK_LIBRARIES(dijkstra_test ${PROJECT_NAME})
+
+# create an example application
+ADD_EXECUTABLE(opendrive_test opendrive_test.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(opendrive_test ${PROJECT_NAME})

src/examples/opendrive_test.cpp

+#include "opendrive_road.h"
+#include "exceptions.h"
+#include <iostream>
+#include <vector>
+#include <limits>
+
+int main(int argc, char *argv[])
+{
+  COpendriveRoad road;
+  std::string xml_file="/home/shernand/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/adc_road.xodr";
+  
+  try
+  {
+    road.set_resolution(0.01);
+    road.set_scale_factor(10.0);
+    road.set_min_road_length(0.01);
+    road.load(xml_file);
+    std::cout << road << std::endl;
+  }
+  catch (CException &e)
+  {
+    std::cout << "[Exception caught] : " << e.what() << std::endl;
+  }
+  return 0;
+}

src/opendrive_arc.cpp

+#include "opendrive_arc.h"
+#include <cmath>
+
+COpendriveArc::COpendriveArc()
+{
+  this->curvature=0.0;
+}
+
+COpendriveArc::COpendriveArc(double min_s, double max_s, double x, double y, double heading, double curvature) : COpendriveGeometry(min_s,max_s,x,y,heading)
+{
+  this->curvature=curvature;
+}
+
+COpendriveArc::COpendriveArc(const COpendriveArc &object) : COpendriveGeometry(object)
+{
+  this->curvature=object.curvature;
+}
+
+bool COpendriveArc::transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local)
+{
+  double alpha;
+  bool pos_arc;
+
+  alpha = std::fabs((track.s-this->min_s)*this->curvature);
+  pos_arc = (this->curvature < 0.0 ? false : true);
+  local.u = std::sin(alpha)/this->curvature - track.t*std::sin(alpha)*(pos_arc ? 1 : -1);
+  local.v = (1 - std::cos(alpha))*(pos_arc ? 1 : -1)/this->curvature + track.t*std::cos(alpha);
+  local.heading = normalize_angle(track.heading + alpha*(pos_arc ? 1 : -1));
+
+  return true;
+}
+
+void COpendriveArc::print(std::ostream &out)
+{
+  out << "  curvature = " << this->curvature << std::endl;
+}
+
+void COpendriveArc::load_params(const planView::geometry_type &geometry_info)
+{
+  this->curvature = (geometry_info.arc().get().curvature().present() ? geometry_info.arc().get().curvature().get() : 0.0);
+}
+
+COpendriveGeometry *COpendriveArc::clone(void)
+{
+  COpendriveArc *new_arc=new COpendriveArc(*this);
+
+  return new_arc;
+}
+
+double COpendriveArc::get_curvature(void)
+{
+  return this->curvature;
+}
+
+void COpendriveArc::operator=(const COpendriveArc &object)
+{
+  COpendriveGeometry::operator=(object);
+  this->curvature=object.curvature;
+}
+
+COpendriveArc::~COpendriveArc()
+{
+  this->curvature=0.0;
+}
+

src/opendrive_common.cpp

+#include "opendrive_common.h"
+#include <cmath>
+
+double normalize_angle(double angle)
+{
+  double norm_angle;
+
+  if(angle>=2*3.14159)
+    norm_angle=angle-2*3.14159;
+  else if(angle<0.0)
+    norm_angle=angle+2*3.14159;
+  else
+    norm_angle=angle;
+
+  return norm_angle;
+}
+
+double diff_angle(double angle1,double angle2)
+{
+  double diff1,diff2;
+
+  diff1=std::fabs(angle1-angle2);
+  if(angle1<angle2)
+    diff2=std::fabs(angle1+2*3.14159-angle2);
+  else
+    diff2=std::fabs(angle1-angle2-2*3.14159);
+
+  return std::fmin(diff1,diff2);
+}

src/opendrive_geometry.cpp

+#include "opendrive_geometry.h"
+#include <cmath>
+
+COpendriveGeometry::COpendriveGeometry()
+{
+  this->min_s = 0.0;
+  this->max_s = 0.0;
+  this->pose.x = 0.0;
+  this->pose.y = 0.0;
+  this->pose.heading = 0.0;
+}
+
+COpendriveGeometry::COpendriveGeometry(double min_s, double max_s, double x, double y, double heading)
+{
+  this->min_s = min_s;
+  this->max_s = max_s;
+  this->pose.x = x;
+  this->pose.y = y;
+  this->pose.heading = heading;
+}
+
+COpendriveGeometry::COpendriveGeometry(const COpendriveGeometry &object)
+{
+  this->min_s = object.min_s;
+  this->max_s = object.max_s;
+  this->pose.x = object.pose.x;
+  this->pose.y = object.pose.y;
+  this->pose.heading = object.pose.heading;
+}
+
+void COpendriveGeometry::print(std::ostream& out)
+{
+  out << "  Geometry from " << this->min_s << " to " << this->max_s << std::endl;
+  out << "    pose: x = " << this->pose.x << ", y = " << this->pose.y << ", heading = " << this->pose.heading << std::endl;
+}
+
+void COpendriveGeometry::load(const planView::geometry_type &geometry_info)
+{
+  this->min_s = (geometry_info.s().present() ? geometry_info.s().get() : 0.0);
+  this->max_s = min_s + (geometry_info.length().present() ? geometry_info.length().get() : 0.0);
+  this->pose.x = (geometry_info.x().present() ? geometry_info.x().get() : 0.0);
+  this->pose.y = (geometry_info.y().present() ? geometry_info.y().get() : 0.0);
+  this->pose.heading = (geometry_info.hdg().present() ? geometry_info.hdg().get() : 0.0);
+  this->load_params(geometry_info);
+}
+
+bool COpendriveGeometry::get_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local)
+{
+  return this->transform_local_pose(track,local);
+}
+
+bool COpendriveGeometry::get_world_pose(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world)
+{
+  TOpendriveLocalPoint local;
+
+  if(this->transform_local_pose(track,local))
+  {
+    world.heading=normalize_angle(this->pose.heading+local.heading);
+    world.x=local.u*std::cos(this->pose.heading)-local.v*std::sin(this->pose.heading)+this->pose.x;
+    world.y=local.u*std::sin(this->pose.heading)+local.v*std::cos(this->pose.heading)+this->pose.y;
+    return true;
+  }
+  else
+    return false;
+}
+
+bool COpendriveGeometry::in_range(double s)
+{
+  if((s<this->max_s) && (s>=this->min_s))
+    return true;
+  else 
+    return false;
+}
+
+double COpendriveGeometry::get_length(void)
+{
+  return this->max_s-this->min_s;
+}
+
+void COpendriveGeometry::operator=(const COpendriveGeometry &object)
+{
+  this->min_s = object.min_s;
+  this->max_s = object.max_s;
+  this->pose.x = object.pose.x;
+  this->pose.y = object.pose.y;
+  this->pose.heading = object.pose.heading;
+}
+
+std::ostream& operator<<(std::ostream& out, COpendriveGeometry &geom)
+{
+  geom.print(out);
+  return out;
+}
+
+COpendriveGeometry::~COpendriveGeometry()
+{
+  this->min_s = 0.0;
+  this->max_s = 0.0;
+  this->pose.x = 0.0;
+  this->pose.y = 0.0;
+  this->pose.heading = 0.0;
+}
+

src/opendrive_lane.cpp

+#include "opendrive_lane.h"
+
+COpendriveLane::COpendriveLane()
+{
+  this->nodes.clear();
+  this->segment=NULL;
+  this->scale_factor=DEFAULT_SCALE_FACTOR;
+  this->width=0.0;
+  this->speed=0.0;
+  this->offset=0.0;
+}
+
+COpendriveLane::COpendriveLane(const COpendriveLane &object)
+{
+  COpendriveRoadNode *node;
+
+  this->nodes.clear();
+  for(unsigned int i=0;i<object.nodes.size();i++)
+  {
+    node=new COpendriveRoadNode(*object.nodes[i]);
+    this->nodes.push_back(node);
+  }
+  this->width=object.width;
+  this->speed=object.speed;
+  this->offset=object.offset;
+}
+
+void COpendriveLane::add_node(COpendriveRoadNode *node)
+{
+
+}
+
+void COpendriveLane::set_resolution(double res)
+{ 
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    this->nodes[i]->set_resolution(res);
+}
+
+void COpendriveLane::set_scale_factor(double scale)
+{ 
+  this->scale_factor=scale;
+
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    this->nodes[i]->set_scale_factor(scale);
+}
+
+void COpendriveLane::set_offset(double offset)
+{
+  this->offset=offset;
+}
+
+void COpendriveLane::update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs)
+{
+  this->segment=refs[this->segment];
+}
+
+void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegment *segment)
+{
+  std::stringstream error;
+
+  for(unsigned int i=0;i<this->nodes.size();i++)
+  {
+    delete this->nodes[i];
+    this->nodes[i]=NULL;
+  }
+  this->nodes.clear();
+  // import lane width
+  if(lane_info.width().size()<1)
+  {
+    error << "No width record present for lane " << lane_info.id().get() << " for road " << road_name;
+    throw CException(_HERE_,error.str());
+  }
+  else if(lane_info.width().size()>1)
+  {
+    error << "More than one width record present for lane " << lane_info.id().get() << " for road " << road_name;
+    throw CException(_HERE_,error.str());
+  }
+  this->width=lane_info.width().begin()->a().get()/this->scale_factor;
+  // import lane speed
+  if(lane_info.speed().size()<1)
+  {
+    error << "No speed record present for lane " << lane_info.id().get() << " for road " << road_name;
+    throw CException(_HERE_,error.str());
+  }
+  else if(lane_info.speed().size()>1)
+  {
+    error << "More than one speed record present for lane " << lane_info.id().get() << " for road " << road_name;
+    throw CException(_HERE_,error.str());
+  }
+  this->speed=lane_info.speed().begin()->max().get();
+
+  this->segment=segment;
+}
+
+unsigned int COpendriveLane::get_num_nodes(void)
+{
+  return this->nodes.size();
+}
+
+const COpendriveRoadNode &COpendriveLane::get_node(unsigned int index)
+{
+
+}
+
+const COpendriveRoadSegment &COpendriveLane::get_segment(void)
+{
+
+}
+
+double COpendriveLane::get_width(void)
+{
+  return this->width;
+}
+
+double COpendriveLane::get_speed(void)
+{
+  return this->speed;
+}
+
+double COpendriveLane::get_offset(void)
+{
+  return this->offset;
+}
+
+void COpendriveLane::operator=(const COpendriveLane &object)
+{
+
+}
+
+friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
+{
+
+}
+
+COpendriveLane::~COpendriveLane()
+{
+
+}