diff --git a/include/common.h b/include/common.h
new file mode 100644
index 0000000000000000000000000000000000000000..27552d26790b36d8366b4af746258c03c59ba83e
--- /dev/null
+++ b/include/common.h
@@ -0,0 +1,23 @@
+#ifndef _COMMON_H
+#define _COMMON_H
+
+#include <map>
+#include <vector>
+
+class COpendriveRoadSegment;
+class COSMRoadSegment;
+class CRoad;
+
+typedef std::pair<COpendriveRoadSegment *,std::vector<CRoad *> > opendrive_map_pair_t;
+typedef std::vector<opendrive_map_pair_t> opendrive_road_map_t;
+
+typedef std::pair<std::vector<CRoad *>,std::vector<COpendriveRoadSegment *> > opendrive_inverse_map_pair_t;
+typedef std::vector<opendrive_inverse_map_pair_t> opendrive_road_inverse_map_t;
+
+typedef std::pair<COSMRoadSegment *,std::vector<CRoad *> > osm_map_pair_t;
+typedef std::vector<osm_map_pair_t> osm_road_map_t;
+
+double normalize_angle(double angle);
+double diff_angle(double angle1,double angle2);
+
+#endif
diff --git a/include/g2_spline.h b/include/g2_spline.h
index f5e6951c527ea53ca737768e35aa514105a7cdcb..c2faa6c64eb5fc126a8b791f8ec4eb9c0d695a65 100644
--- a/include/g2_spline.h
+++ b/include/g2_spline.h
@@ -62,14 +62,15 @@ class CG2Spline
     void get_end_point(TPoint &point);
     void generate(double resolution,unsigned int iterations=10);
     void generate(double resolution,double length,unsigned int iterations=10);
-    TPoint evaluate(double length);
+    void generate(double &resolution,double n1,double n2, double n3, double n4);
+    bool evaluate(double length, TPoint& point);
     void evaluate_all(std::vector<double> &x, std::vector<double> &y,std::vector<double> &curvature,std::vector<double> &heading);
     double get_length(void);
     double get_max_curvature(double max_error=0.001,unsigned int max_iter=10);
     double get_max_curvature_der(double max_error=0.001,unsigned int max_iter=10);
     double find_closest_point(double x,double y,TPoint &point,double max_error=0.001,unsigned int max_iter=10);
     double find_closest_point(double x,double y,double max_error=0.001,unsigned int max_iter=10);
-    void get_part(CG2Spline *spline,double start_length, double end_length=-1.0);
+    bool get_part(CG2Spline *spline,double start_length, double end_length=-1.0);
     CG2Spline& operator=(const CG2Spline &obj);
     ~CG2Spline();
 };
diff --git a/include/junction.h b/include/junction.h
new file mode 100644
index 0000000000000000000000000000000000000000..58ff7efd2392b3d0e9b500a66e95906b1538788f
--- /dev/null
+++ b/include/junction.h
@@ -0,0 +1,93 @@
+#ifndef _JUNCTION_H
+#define _JUNCTION_H
+
+#include "road.h"
+#include "road_map.h"
+#include "road_segment.h"
+
+#include <Eigen/Dense>
+
+class CRoadMap;
+class CRoad;
+class CRoadSegment;
+
+class CJunction
+{
+  friend class CRoad;
+  friend class CRoadMap;
+  friend class CRoadSegment;
+  friend class COpendriveJunction;
+  private:
+    unsigned int id;
+    double resolution;
+    /* connectivity */
+    CRoadMap *parent_roadmap;
+    std::vector<CRoad *> incomming_roads;
+    std::vector<CRoad *> outgoing_roads;
+    Eigen::MatrixXi connectivity;
+    /* geometry */
+    std::vector<CRoadSegment *> segments;
+  protected:
+    CJunction(unsigned int id);
+    void set_id(unsigned int id);
+    static unsigned int get_index_by_id(const std::vector<CJunction *> &junctions,unsigned int id);
+    /* connectivity */
+    void set_parent_roadmap(CRoadMap *road);
+    CRoad *get_incomming_road_by_index(unsigned int index);
+    CRoad *get_outgoing_road_by_index(unsigned int index);
+    void remove_incomming_road_by_index(unsigned int index);
+    void remove_outgoing_road_by_index(unsigned int index);
+    void link_roads_by_index(unsigned int incomming_road,unsigned int outgoing_road);
+    void unlink_roads_by_index(unsigned int incomming_road,unsigned int outgoing_road);
+    bool are_roads_linked_by_index(unsigned int incomming_road,unsigned int outgoing_road);
+    CRoadSegment *link_road_to_point(unsigned int incomming_road,TPoint &outgoing_point,unsigned int outgoing_num_lanes);
+    CRoadSegment *link_point_to_road(TPoint &incomming_point,unsigned int incomming_num_lanes,unsigned int outgoing_road);
+    CRoadSegment *link_point_to_point(TPoint &incomming_point,unsigned int incomming_num_lanes,TPoint &outgoing_point,unsigned int outgoing_num_lanes);
+    void link_lanes_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out);
+    void unlink_lanes_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out);
+    bool are_lanes_linked_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out);
+    /* geometry */
+    CRoadSegment *get_segment_between_by_index(unsigned int incomming_road,unsigned int outgoing_road);
+    CRoadSegment *get_segment_by_index(unsigned int index);
+  public:
+    CJunction();
+    unsigned int get_id(void);
+    void set_resolution(double resolution);
+    double get_resolution(double resolution);
+    /* connectivity */
+    CRoadMap &get_parent_roadmap(void);
+    unsigned int get_num_incomming_roads(void);
+    CRoad &get_incomming_road_by_id(unsigned int id);
+    unsigned int get_incomming_road_index_by_id(unsigned int id);
+    bool has_incomming_road(unsigned int id);
+    unsigned int get_num_outgoing_roads(void);
+    CRoad &get_outgoing_road_by_id(unsigned int id);
+    unsigned int get_outgoing_road_index_by_id(unsigned int id);
+    bool has_outgoing_road(unsigned int id);
+    void add_incomming_road(CRoad *road);
+    void remove_incomming_road_by_id(unsigned int id);
+    void add_outgoing_road(CRoad *road);
+    void remove_outgoing_road_by_id(unsigned int id);
+    void link_roads_by_id(unsigned int incomming_road,unsigned int outgoing_road);
+    void unlink_roads_by_id(unsigned int incomming_road,unsigned int outgoing_road);
+    bool are_roads_linked_by_id(unsigned int incomming_road,unsigned int outgoing_road);
+    void link_lanes_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out);
+    void link_same_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road);
+    void link_full_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road);
+    void unlink_lanes_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out);
+    void unlink_same_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road);
+    void unlink_full_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road);
+    bool are_lanes_linked_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out);
+    void get_connectivity_matrix(Eigen::MatrixXi &connectivity,std::vector<unsigned int> &incomming_id_map,std::vector<unsigned int> &outgoing_id_map);
+    /* geometry */
+    unsigned int get_num_segments(void);
+    CRoadSegment &get_segment_by_id(unsigned int id);
+    CRoadSegment &get_segment_between_by_id(unsigned int incomming_road,unsigned int outgoing_road); 
+    bool get_closest_segment_id(TPoint &point,unsigned int &segment_id,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max());
+    bool get_closest_segment_ids(TPoint &point,std::vector<unsigned int >&segment_ids,std::vector<double >&distances,std::vector<double> &lateral_offsets,double angle_tol=std::numeric_limits<double>::max(),double offset_tol=std::numeric_limits<double>::max());
+    void get_segment_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw);
+    void get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw);
+    ~CJunction();
+};
+
+#endif
diff --git a/include/opendrive_arc.h b/include/opendrive/opendrive_arc.h
similarity index 77%
rename from include/opendrive_arc.h
rename to include/opendrive/opendrive_arc.h
index 683b321b5fe6dc35c67068c0f1bddb4679f26a0a..eecdba3675fe24cfdd95efc5781a0857ec67d422 100644
--- a/include/opendrive_arc.h
+++ b/include/opendrive/opendrive_arc.h
@@ -15,10 +15,13 @@ class COpendriveArc : public COpendriveGeometry
     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 void save_params(planView::geometry_type &geometry_info);
     virtual std::string get_name(void);
   public:
+    COpendriveArc(TOpendriveWorldPose &start_pose,double length,double curvature);
     virtual COpendriveGeometry *clone(void);
     virtual void get_curvature(double &start,double &end);
+    virtual void get_curvature_at(double length,double &curvature);
     void operator=(const COpendriveArc &object);
     ~COpendriveArc();
 };
diff --git a/include/opendrive/opendrive_common.h b/include/opendrive/opendrive_common.h
new file mode 100644
index 0000000000000000000000000000000000000000..34832ab337f0d6c9bd82a9e6edded1fc53c27518
--- /dev/null
+++ b/include/opendrive/opendrive_common.h
@@ -0,0 +1,33 @@
+#ifndef _OPENDRIVE_COMMON_H
+#define _OPENDRIVE_COMMON_H
+
+typedef struct
+{
+  double s;
+  double t;
+  double heading;
+}TOpendriveTrackPose;
+
+typedef struct
+{
+  double u;
+  double v;
+  double heading;
+}TOpendriveLocalPose;
+
+typedef struct
+{
+  double x;
+  double y;
+  double heading;
+}TOpendriveWorldPose;
+
+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;
+
+#endif
diff --git a/include/opendrive_geometry.h b/include/opendrive/opendrive_geometry.h
similarity index 81%
rename from include/opendrive_geometry.h
rename to include/opendrive/opendrive_geometry.h
index f9c7285f1624cd3bf2dff7c2dbeb160f62de9681..50baf13b881b196650c291b0368da3ea77752a6c 100644
--- a/include/opendrive_geometry.h
+++ b/include/opendrive/opendrive_geometry.h
@@ -1,7 +1,7 @@
 #ifndef _OPENDRIVE_GEOMETRY_H
 #define _OPENDRIVE_GEOMETRY_H
 
-#include "opendrive_common.h"
+#include "opendrive/opendrive_common.h"
 #ifdef _HAVE_XSD
 #include "xml/OpenDRIVE_1.4H.hxx"
 #endif
@@ -17,29 +17,32 @@ class COpendriveGeometry
     COpendriveGeometry();
     COpendriveGeometry(const COpendriveGeometry &object);
     void load(const planView::geometry_type &geometry_info);
-    double scale_factor;
+    void save(planView::geometry_type &geometry_info);
     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(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 void save_params(planView::geometry_type &geometry_info) = 0;
     virtual std::string get_name(void)=0;
-    void set_scale_factor(double scale);
     void set_start_pose(TOpendriveWorldPose &pose);
     void set_max_s(double s);
     void set_min_s(double s);
   public:
+    COpendriveGeometry(TOpendriveWorldPose &start_pose,double length);
     virtual COpendriveGeometry *clone(void) = 0;
     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;
+    virtual void get_curvature_at(double length,double &curvature)=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;
+    bool get_pose_at(double length,TOpendriveWorldPose &pose) const;
     void operator=(const COpendriveGeometry &object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveGeometry &geom);
     virtual ~COpendriveGeometry();
diff --git a/include/opendrive/opendrive_junction.h b/include/opendrive/opendrive_junction.h
new file mode 100644
index 0000000000000000000000000000000000000000..4a0a03db92888aeaf1c0486cadb510ada6a6d16f
--- /dev/null
+++ b/include/opendrive/opendrive_junction.h
@@ -0,0 +1,47 @@
+#ifndef _OPENDRIVE_JUNCTION_H
+#define _OPENDRIVE_JUNCTION_H
+
+#include "opendrive/opendrive_road_segment.h"
+
+typedef struct{
+  int from_lane_id;
+  int to_lane_id;
+}TJunctionLink;
+
+typedef struct{
+  unsigned int id;
+  unsigned int incomming_road_id;
+  unsigned int connecting_road_id;
+  bool end_contact_point;
+  std::vector<TJunctionLink> links;
+}TJunctionConnection;
+
+class COpendriveJunction
+{
+  friend class CRoadMap;
+  friend class CJunction;
+  friend class COpendriveRoadSegment;
+  friend class COpendriveLane;
+  private:
+    std::vector<TJunctionConnection> connections;
+    std::string name;
+    unsigned int id;
+  protected:
+    void set_name(const std::string &name);
+    void set_id(unsigned int id);
+    void load(OpenDRIVE::junction_type &junction_info);
+    void save(OpenDRIVE::junction_type &junction_info);
+    void convert(CJunction **junction,opendrive_road_map_t &road_map);
+    static bool get_road_data(unsigned int id,opendrive_road_inverse_map_t &road_map,COpendriveRoadSegment **road,bool incomming);
+    static void generate_road_segment(CRoadSegment *segment,unsigned int &current_id,unsigned int road_in,int lane_in,unsigned int road_out,int lane_out,COpendriveRoadSegment *new_segment);
+    static void convert(CJunction *junction_in,unsigned int &current_id,COpendriveJunction *junction_out,std::vector<COpendriveRoadSegment *> &segments,opendrive_road_inverse_map_t &road_map);
+  public:
+    COpendriveJunction();
+    std::string get_name(void) const;
+    unsigned int get_id(void) const;    
+    unsigned int get_num_connections(void) const;
+    const TJunctionConnection &get_connection(unsigned int index) const;
+    ~COpendriveJunction();
+};
+
+#endif
diff --git a/include/opendrive/opendrive_lane.h b/include/opendrive/opendrive_lane.h
new file mode 100644
index 0000000000000000000000000000000000000000..959667d6e858e1ba97d477f39047e10b96a80d3e
--- /dev/null
+++ b/include/opendrive/opendrive_lane.h
@@ -0,0 +1,66 @@
+#ifndef _OPENDRIVE_LANE_H
+#define _OPENDRIVE_LANE_H
+
+#ifdef _HAVE_XSD
+#include "xml/OpenDRIVE_1.4H.hxx"
+#endif
+
+#include "opendrive/opendrive_common.h"
+
+class COpendriveLane
+{
+  friend class COpendriveRoadSegment;
+  friend class COpendriveJunction;
+  private:
+    opendrive_mark_t right_mark;
+    double speed;
+    double width;
+    int id;
+  protected:
+    void load(const ::lane &lane_info);
+    void save(::lane &lane_info);
+    void set_right_mark(opendrive_mark_t mark);
+    void set_width(double width);
+    void set_speed(double speed);
+    void set_id(int id);
+  public:
+    COpendriveLane();
+    /**
+     * \brief return the road mark at the right side of the lane
+     *
+     * This function returns the road mark identifier at the right side of the lane, even
+     * if the right lane is not defined.
+     *
+     * \return the road mark identifier at the right side of the lane.
+     */
+    opendrive_mark_t get_right_road_mark(void) const;
+    /**
+     * \brief Returns the width of the lane
+     * 
+     * \returns the width in meters of the lane.
+     */
+    double get_width(void) const;
+    /**
+     * \brief Returns the speed limit of the lane
+     *
+     * \return the speed limit of the lane in km/h
+     */
+    double get_speed(void) const;
+    /**
+     * \brief Returns the opendrive identifier of the lane
+     *
+     * This function returns the opendrive identifier of the lane: negative integers
+     * values are used to identify right lanes, and positive values to identify left 
+     * lanes. These values increase (for left lanes) or decrease (for right lanes) the 
+     * further they are from the center lane of the segment.
+     *
+     * \return the opendrive identifier of the lane.
+     */
+    int get_id(void) const;
+    /**
+     * brief Destructor
+     */
+    ~COpendriveLane();
+};
+
+#endif
diff --git a/include/opendrive_line.h b/include/opendrive/opendrive_line.h
similarity index 79%
rename from include/opendrive_line.h
rename to include/opendrive/opendrive_line.h
index b544bceed0fd9f0eeb325fd0331a05d25bd97d0f..dce82a0579e7fe2b4a485610bded569a0e30ff85 100644
--- a/include/opendrive_line.h
+++ b/include/opendrive/opendrive_line.h
@@ -14,10 +14,13 @@ class COpendriveLine : public COpendriveGeometry
     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 void save_params(planView::geometry_type &geometry_info);
     virtual std::string get_name(void);
   public:
+    COpendriveLine(TOpendriveWorldPose &start_pose,double length);
     virtual COpendriveGeometry *clone(void);
     virtual void get_curvature(double &start,double &end);
+    virtual void get_curvature_at(double length,double &curvature);
     void operator=(const COpendriveLine &object);
     ~COpendriveLine();
 };
diff --git a/include/opendrive_object.h b/include/opendrive/opendrive_object.h
similarity index 78%
rename from include/opendrive_object.h
rename to include/opendrive/opendrive_object.h
index 3a0d30f85688623fc5409a17d3cc06dc2ce98587..3e42e1d9c59fbd6cc38e0b797a171ab45ea4ed5f 100644
--- a/include/opendrive_object.h
+++ b/include/opendrive/opendrive_object.h
@@ -1,11 +1,10 @@
 #ifndef _OPENDRIVE_OBJECT_H
 #define _OPENDRIVE_OBJECT_H
 
-#include "opendrive_common.h"
-#include "opendrive_road_segment.h"
 #ifdef _HAVE_XSD
 #include "xml/OpenDRIVE_1.4H.hxx"
 #endif
+#include "opendrive/opendrive_common.h"
 
 #define OD_MAX_VERTICES     32
 
@@ -38,30 +37,22 @@ typedef union
   TOpendrivePolygon polygon;
 }TOpendriveObject;
 
-class COpendriveRoadSegment;
-
 class COpendriveObject
 {
   friend class COpendriveRoadSegment;
   private:
-    COpendriveRoadSegment *segment;
     int id; ///< Object's id.
     TOpendriveTrackPose pose;
     TOpendriveObject object;
     std::string type; ///< Object's OpenDrive type.
     std::string name; ///< Object's name.
     object_type_t object_type;
-    double scale_factor;
   protected:
-    void load(objects::object_type &object_info,COpendriveRoadSegment *segment);
-    void update_references(segment_up_ref_t &segment_refs);
-    void set_scale_factor(double scale);
+    void load(objects::object_type &object_info);
   public:
     COpendriveObject();
     COpendriveObject(const COpendriveObject& object);
-    double get_scale_factor(void);
     TOpendriveTrackPose get_track_pose(void) const;
-    TOpendriveWorldPose get_world_pose(void) const;
     int get_id(void) const;
     std::string get_type(void) const;
     std::string get_name(void) const;
diff --git a/include/opendrive_param_poly3.h b/include/opendrive/opendrive_param_poly3.h
similarity index 75%
rename from include/opendrive_param_poly3.h
rename to include/opendrive/opendrive_param_poly3.h
index c40fdaee54ef4b0f3e4fcb2f0ef23bf99768329b..53fa3aa0525de9bf5ebaadc81d620991c360c703 100644
--- a/include/opendrive_param_poly3.h
+++ b/include/opendrive/opendrive_param_poly3.h
@@ -25,10 +25,14 @@ class COpendriveParamPoly3 : public COpendriveGeometry
     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 void save_params(planView::geometry_type &geometry_info);
     virtual std::string get_name(void);
   public:
+    COpendriveParamPoly3(TOpendriveWorldPose &start_pose,TOpendrivePoly3Params &u_params,TOpendrivePoly3Params &v_params);
+    COpendriveParamPoly3(TOpendriveWorldPose &start_pose,TOpendriveWorldPose &end_pose);
     virtual COpendriveGeometry *clone(void);
     virtual void get_curvature(double &start,double &end);
+    virtual void get_curvature_at(double length,double &curvature);
     TOpendrivePoly3Params get_u_params(void);
     TOpendrivePoly3Params get_v_params(void);
     bool is_normalized(void);
diff --git a/include/opendrive/opendrive_road_segment.h b/include/opendrive/opendrive_road_segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..f6127f19da9bf0da782a705d81bb192c9bbdd689
--- /dev/null
+++ b/include/opendrive/opendrive_road_segment.h
@@ -0,0 +1,203 @@
+#ifndef _OPENDRIVE_ROAD_SEGMENT_H
+#define _OPENDRIVE_ROAD_SEGMENT_H
+
+#ifdef _HAVE_XSD
+#include "xml/OpenDRIVE_1.4H.hxx"
+#endif
+
+#include "road.h"
+
+#include "opendrive/opendrive_common.h"
+#include "opendrive/opendrive_lane.h"
+#include "opendrive/opendrive_geometry.h"
+#include "opendrive/opendrive_object.h"
+#include "opendrive/opendrive_signal.h"
+
+#include "road_map.h"
+#include "road.h"
+
+typedef struct{
+  bool road;
+  unsigned int id;
+  bool end_contact;
+}TLinkElement;
+
+class COpendriveRoadSegment
+{
+  friend class CRoadMap;
+  friend class CRoad;
+  friend class COpendriveLane;
+  friend class COpendriveJunction;
+  private:
+    std::vector<COpendriveGeometry *> geometries;
+    std::vector<COpendriveLane *> right_lanes;
+    std::vector<COpendriveLane *> left_lanes;
+    std::vector<COpendriveSignal *> signals;
+    std::vector<COpendriveObject *> objects;
+    std::string name;
+    unsigned int id;
+    bool predecessor_exists;
+    TLinkElement predecessor;
+    bool successor_exists;
+    TLinkElement successor;
+    opendrive_mark_t center_mark;
+    unsigned int junction_id;
+  protected:
+    void free(void);
+    void load(OpenDRIVE::road_type &road_info);
+    void convert(CRoad **left_road,CRoad **right_road,double resolution);
+    static void convert(CRoad *left_road,CRoad *right_road,unsigned int &current_id,std::vector<COpendriveRoadSegment *> &segments);
+    void save(OpenDRIVE::road_type **road_info);
+    void set_name(const std::string &name);
+    void set_id(unsigned int id);
+    void set_center_mark(opendrive_mark_t mark);
+    void add_lanes(lanes::laneSection_type &lane_section);
+    void add_right_lane(const ::lane &lane_info);
+    void add_right_lane(COpendriveLane *lane);
+    void add_left_lane(const ::lane &lane_info);
+    void add_left_lane(COpendriveLane *lane);
+    bool add_geometries(OpenDRIVE::road_type &road_info);
+    void add_geometry(COpendriveGeometry *geometry,double resolution);
+    void set_junction_id(unsigned int id);
+  public:
+    COpendriveRoadSegment();
+    /**
+     * \brief Returns the Opendrive name
+     *
+     * \return The Opendrive name of the road segment 
+     */
+    std::string get_name(void) const;
+    /**
+     * \brief Returns the Opendirve numeric identifier
+     *
+     * \return The Opendrive numeric identifier of the road segment.
+     */
+    unsigned int get_id(void) const;
+    /**
+     * \brief Returns the number of right lanes 
+     *
+     * This function returns how many lanes there are on the right side of the road
+     * segment. The number of left and right lanes can be different.
+     *
+     * \return This function returns the number of right lanes. The range of 
+     *         valid indexes goes from -1 to the value returned by this function 
+     *         negated.
+     */
+    unsigned int get_num_right_lanes(void) const;
+    /**
+     * \brief Returns the number of left lanes 
+     *
+     * This function returns how many lanes there are on the left side of the road
+     * segment. The number of left and right lanes can be different.
+     *
+     * \return This function returns the number of left lanes. The range of 
+     *         valid indexes goes from 1 to the value returned by this function.
+     */
+    unsigned int get_num_left_lanes(void) const;
+    /**
+     * \brief Returns the number of road signals
+     *
+     * This function returns how many road signals are placed at this road segment.
+     *
+     * \return This function returns the number of road signals. The range of 
+     *         valid indexes goes from 0 to the value returned by this function -1.
+     */
+    unsigned int get_num_signals(void) const;
+    /**
+     * \brief Returns a reference to a road signal by local index
+     *
+     * This function returns a constant reference to the desired road signal 
+     * object, which can not be modified by the user. If the index is not valid,
+     * this function will throw an exception.
+     *
+     * \param index is the 0 based local index of the desired road signal. 
+     *
+     * \return constant reference to the desired road signal object.
+     */
+    const COpendriveSignal &get_signal(unsigned int index) const;
+    /**
+     * \brief Returns the number of road objects
+     *
+     * This function returns how many road objects are placed at this road segment.
+     *
+     * \return This function returns the number of road objects. The range of 
+     *         valid indexes goes from 0 to the value returned by this function -1.
+     */
+    unsigned int get_num_objects(void) const;
+    /**
+     * \brief Returns a reference to a road object by local index
+     *
+     * This function returns a constant reference to the desired road object 
+     * object, which can not be modified by the user. If the index is not valid,
+     * this function will throw an exception.
+     *
+     * \param index is the 0 based local index of the desired road object. 
+     *
+     * \return constant reference to the desired road object object.
+     */
+    const COpendriveObject &get_object(unsigned int index) const;
+    /**
+     * \brief Returns true if the road segment is part of a junction, false otherwise
+     *
+     * \return a boolean indicating if the road segment is part of a junction or not
+     */
+    bool is_junction(void) const;
+    /**
+     *
+     */
+    bool has_predecessor(void);
+    /**
+     *
+     */
+    bool is_predecessor_road(void);
+    /**
+     *
+     */
+    bool is_predecessor_junction(void);
+    /**
+     *
+     */
+    unsigned int get_predecessor_id(void);
+    /**
+     *
+     */
+    bool is_predecessor_contact_end(void);
+    /**
+     *
+     */
+    bool is_predecessor_contact_start(void);
+    /**
+     *
+     */
+    bool has_successor(void);
+    /**
+     *
+     */
+    bool is_successor_road(void);
+    /**
+     *
+     */
+    bool is_successor_junction(void);
+    /**
+     *
+     */
+    unsigned int get_successor_id(void);
+    /**
+     *
+     */
+    bool is_successor_contact_end(void);
+    /**
+     *
+     */
+    bool is_successor_contact_start(void);
+    /**
+     *
+     */
+    double get_length(void);
+    /**
+     * \brief Destructor 
+     */
+    ~COpendriveRoadSegment();
+};
+
+#endif
diff --git a/include/opendrive_signal.h b/include/opendrive/opendrive_signal.h
similarity index 67%
rename from include/opendrive_signal.h
rename to include/opendrive/opendrive_signal.h
index 5dfe3df686cc39b25424b1e5d2447145b7ab91f2..a57974c6ea3b97c918ef6f36c93f5dff62332bcf 100644
--- a/include/opendrive_signal.h
+++ b/include/opendrive/opendrive_signal.h
@@ -1,37 +1,29 @@
 #ifndef _OPENDRIVE_SIGNAL_H
 #define _OPENDRIVE_SIGNAL_H
 
-#include "opendrive_common.h"
-#include "opendrive_road_segment.h"
 #ifdef _HAVE_XSD
 #include "xml/OpenDRIVE_1.4H.hxx"
 #endif
 
-class COpendriveRoadSegment;
+#include "opendrive/opendrive_common.h"
 
 class COpendriveSignal
 {
   friend class COpendriveRoadSegment;
   private:
-    COpendriveRoadSegment *segment;
     int id; ///< Signal's id.
     TOpendriveTrackPose 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.    
-    double scale_factor;
   protected:
-    void load(signals::signal_type &signal_info,COpendriveRoadSegment *segment);
-    void update_references(segment_up_ref_t &segment_refs);
-    void set_scale_factor(double scale);
+    void load(signals::signal_type &signal_info);
   public:
     COpendriveSignal();
     COpendriveSignal(const COpendriveSignal &object);
-    double get_scale_factor(void);
     int get_id(void) const;
     TOpendriveTrackPose get_track_pose(void) const;
-    TOpendriveWorldPose get_world_pose(void) const;
     void get_type(std::string &type, std::string &sub_type) const;
     int get_value(void) const;
     std::string get_text(void) const;
diff --git a/include/opendrive_spiral.h b/include/opendrive/opendrive_spiral.h
similarity index 76%
rename from include/opendrive_spiral.h
rename to include/opendrive/opendrive_spiral.h
index 106597e8801b0e8f47efa480be43e80c88699eeb..97d4ea8303af5f57647d5d3bc3aaf978b636ab9f 100644
--- a/include/opendrive_spiral.h
+++ b/include/opendrive/opendrive_spiral.h
@@ -16,10 +16,13 @@ class COpendriveSpiral : public COpendriveGeometry
     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 void save_params(planView::geometry_type &geometry_info);
     virtual std::string get_name(void);
   public:
+    COpendriveSpiral(TOpendriveWorldPose &start_pose,double length,double start_curvature,double end_curvature);
     virtual COpendriveGeometry *clone(void);
     virtual void get_curvature(double &start,double &end);
+    virtual void get_curvature_at(double length,double &curvature);
     void operator=(const COpendriveSpiral &object);
     ~COpendriveSpiral();
 };
diff --git a/include/opendrive_common.h b/include/opendrive_common.h
deleted file mode 100644
index da8a28e8cfd42e62aef351fa9efd2cd4347aa73c..0000000000000000000000000000000000000000
--- a/include/opendrive_common.h
+++ /dev/null
@@ -1,55 +0,0 @@
-#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
-
-#include <map>
-
-class COpendriveLane;
-class COpendriveRoadNode;
-class COpendriveRoadSegment;
-class COpendriveSignal;
-class COpendriveObject;
-class COpendriveRoad;
-class COpendriveLink;
-
-typedef std::map<COpendriveRoadNode *,COpendriveRoadNode *> node_up_ref_t;
-typedef std::map<COpendriveLane *,COpendriveLane *> lane_up_ref_t;
-typedef std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> segment_up_ref_t;
-typedef std::map<COpendriveLink *,COpendriveLink *> link_up_ref_t;
-
-typedef struct
-{
-  double s;
-  double t;
-  double heading;
-}TOpendriveTrackPose;
-
-typedef struct
-{
-  double u;
-  double v;
-  double heading;
-}TOpendriveLocalPose;
-
-typedef struct
-{
-  double x;
-  double y;
-  double heading;
-}TOpendriveWorldPose;
-
-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
diff --git a/include/opendrive_lane.h b/include/opendrive_lane.h
deleted file mode 100644
index ce8482125e3274303156202ee939649d299e782b..0000000000000000000000000000000000000000
--- a/include/opendrive_lane.h
+++ /dev/null
@@ -1,382 +0,0 @@
-#ifndef _OPENDRIVE_LANE_H
-#define _OPENDRIVE_LANE_H
-
-#ifdef _HAVE_XSD
-#include "xml/OpenDRIVE_1.4H.hxx"
-#endif
-
-#include "opendrive_common.h"
-#include "opendrive_road_node.h"
-#include "opendrive_road_segment.h"
-#include "opendrive_lane.h"
-
-class COpendriveLane
-{
-  friend class COpendriveRoadSegment;
-  friend class COpendriveRoadNode;
-  friend class COpendriveRoad;
-  private:
-    std::vector<COpendriveRoadNode *> nodes;
-    std::vector<COpendriveLane *> next;
-    std::vector<COpendriveLane *> prev;
-    COpendriveLane *left_lane;
-    opendrive_mark_t left_mark;
-    COpendriveLane *right_lane;
-    opendrive_mark_t right_mark;
-    COpendriveRoadSegment *segment;
-    double scale_factor;
-    double resolution;
-    double width;
-    double speed;
-    double offset;
-    int id;
-    unsigned int index;
-  protected:
-    COpendriveLane();
-    COpendriveLane(const COpendriveLane &object);
-    void load(const ::lane &lane_info,COpendriveRoadSegment *segment);
-    void link_neightbour_lane(COpendriveLane *lane);
-    void link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start,bool to_start,bool belongs_to_lane);
-    void add_next_lane(COpendriveLane *lane);
-    void add_prev_lane(COpendriveLane *lane);
-    void remove_lane(COpendriveLane *lane);
-    void add_node(COpendriveRoadNode *node);
-    bool has_node(COpendriveRoadNode *node);
-    bool has_node_with_index(unsigned int index);
-    void set_parent_segment(COpendriveRoadSegment *segment);
-    void set_left_lane(COpendriveLane *lane,opendrive_mark_t mark);
-    void set_right_lane(COpendriveLane *lane,opendrive_mark_t mark);
-    void set_resolution(double res);
-    void set_scale_factor(double scale);
-    void set_width(double width);
-    void set_speed(double speed);
-    void set_offset(double offset);
-    void set_id(int id);
-    void set_index(unsigned int index);
-    bool updated(lane_up_ref_t &refs);
-    void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
-    void clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
-    void update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start=NULL);
-    void update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *end=NULL);
-    COpendriveLane *get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL);
-    COpendriveLane *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref);
-    TOpendriveLocalPose transform_to_local_pose(TOpendriveTrackPose &track) const;
-    TOpendriveWorldPose transform_to_world_pose(TOpendriveTrackPose &track) const;
-  public:
-    /**
-     * \brief Returns the number of road nodes in the lane
-     *
-     * This function returns how many road nodes belong to the lane.
-     *
-     * \return This function returns the number of road nodes. The range of 
-     *         valid indexes goes from 0 to the value returned by this function -1.
-     */
-    unsigned int get_num_nodes(void) const;
-    /**
-     * \brief Returns a reference to a road node by local index
-     *
-     * This function returns a constant reference to the desired road node 
-     * object, which can not be modified by the user. If the index is not valid,
-     * this function will throw an exception.
-     *
-     * \param index is the 0 based local index of the desired road node. It is not
-     *              the unique index assigned to the road nodes when they are created.
-     *
-     * \return constant reference to the desired road node object.
-     */
-    const COpendriveRoadNode &get_node(unsigned int index) const;
-    /**
-     * \brief Returns a reference to the parent road segment
-     * 
-     * This function returns a constant reference to the road segment where the lane
-     * belongs, which can not be modified by the user. If the parent segment is not
-     * defined, this function will throw an exception.
-     *
-     * \return constant reference to the parent road segment. 
-     */
-    const COpendriveRoadSegment &get_segment(void) const;
-    /**
-     * \brief Returns the number of next lanes 
-     *
-     * This function returns how many lanes are connected to this one in the direction
-     * of the traffic (the next lanes).
-     *
-     * \return This function returns the number of next lanes. The range of 
-     *         valid indexes goes from 0 to the value returned by this function -1.
-     */
-    unsigned int get_num_next_lanes(void) const;
-    /**
-     * \brief Returns a reference to a next lane
-     *
-     * This function returns a constant reference to a next lane connected to this one,
-     * which can not be modified by the user. If the index is not valid, this function 
-     * will throw an exception.
-     *
-     * \param index is the 0 based local index of the desired next lane. It is not
-     *              the unique index assigned to the lanes when they are created.
-     *
-     * \return constant reference to a next lane.
-     */
-    const COpendriveLane &get_next_lane(unsigned int index) const;
-    /**
-     * \brief Returns the number of previous lanes 
-     *
-     * This function returns how many lanes are connected to this one in the opposite 
-     * direction of the traffic (the previous lanes).
-     *
-     * \return This function returns the number of previous lanes. The range of 
-     *         valid indexes goes from 0 to the value returned by this function -1.
-     */
-    unsigned int get_num_prev_lanes(void) const;
-    /**
-     * \brief Returns a reference to a previous lane
-     *
-     * This function returns a constant reference to a previous lane connected to this one,
-     * which can not be modified by the user. If the index is not valid, this function 
-     * will throw an exception.
-     *
-     * \param index is the 0 based local index of the desired previous lane. It is not
-     *              the unique index assigned to the lanes when they are created.
-     *
-     * \return constant reference to a previous lane.
-     */
-    const COpendriveLane &get_prev_lane(unsigned int index) const;
-    /**
-     * \brief Checks whether there exist a left lane or not
-     *
-     * This function checks if the lane has an adjacent left lane. Adjacent lanes are 
-     * on the same side of the road. Lanes of opposite sides of the road are not 
-     * considered adjacent.
-     *
-     * \return true if the lane has an adjacent left lane or false otherwise.
-     */
-    bool exist_left_lane(void);
-    /**
-     * \brief Returns a reference to a left lane
-     *
-     * This function returns a constant reference to a left lane adjacent to this one,
-     * which can not be modified by the user. If there is no adjacent left lane, this 
-     * function will throw an exception.
-     *
-     * \return constant reference to the left lane.
-     */
-    const COpendriveLane &get_left_lane(void) const;
-    /**
-     * \brief Checks whether there exist a right lane or not
-     *
-     * This function checks if the lane has an adjacent right lane. Adjacent lanes are 
-     * on the same side of the road. Lanes of opposite sides of the road are not 
-     * considered adjacent.
-     *
-     * \return true if the lane has an adjacent right lane or false otherwise.
-     */
-    bool exist_right_lane(void);
-    /**
-     * \brief Returns a reference to a right lane
-     *
-     * This function returns a constant reference to a right lane adjacent to this one,
-     * which can not be modified by the user. If there is no adjacent right lane, this 
-     * function will throw an exception.
-     *
-     * \return constant reference to the right lane.
-     */
-    const COpendriveLane &get_right_lane(void) const;
-    /**
-     * \brief return the road mark at the left side of the lane
-     *
-     * This function returns the road mark identifier at the left side of the lane, even
-     * if the left lane is not defined.
-     *
-     * \return the road mark identifier at the left side of the lane.
-     */
-    opendrive_mark_t get_left_road_mark(void) const;
-    /**
-     * \brief return the road mark at the right side of the lane
-     *
-     * This function returns the road mark identifier at the right side of the lane, even
-     * if the right lane is not defined.
-     *
-     * \return the road mark identifier at the right side of the lane.
-     */
-    opendrive_mark_t get_right_road_mark(void) const;
-    /**
-     * \brief Returns the width of the lane
-     * 
-     * \returns the width in meters of the lane.
-     */
-    double get_width(void) const;
-    /**
-     * \brief Returns the speed limit of the lane
-     *
-     * \return the speed limit of the lane in km/h
-     */
-    double get_speed(void) const;
-    /**
-     * \brief Returns the offset from the road segment center.
-     *
-     * This function returns the offset of the center of the lane with respect of
-     * the center of the road segment where it belongs.
-     *
-     * \return the distance form the center of the segment in meters.
-     */
-    double get_center_offset(void) const;
-    /**
-     * \brief Returns the unique index of the lane
-     *
-     * \return the unique index assigned to the lane when it was created.
-     */
-    unsigned int get_index(void) const;
-    /**
-     * \brief Returns the opendrive identifier of the lane
-     *
-     * This function returns the opendrive identifier of the lane: negative integers
-     * values are used to identify right lanes, and positive values to identify left 
-     * lanes. These values increase (for left lanes) or decrease (for right lanes) the 
-     * further they are from the center lane of the segment.
-     *
-     * \return the opendrive identifier of the lane.
-     */
-    int get_id(void) const;
-    /**
-     * \brief Returns the start pose of the lane
-     *
-     * This function returns the start pose (x and y coordinates and heading) of the 
-     * lane in the world reference system. The lanes are defined in the direction of 
-     * the traffic. That is, right lanes have the same heading as the road segment, 
-     * but left lanes have opposite heading, starting at the end of the road segment 
-     * and ending at the start.
-     *
-     * \return the start pose of the lane in world coordinates.
-     */
-    TOpendriveWorldPose get_start_pose(void) const;
-    /**
-     * \brief Returns the end pose of the lane
-     *
-     * This function returns the end pose (x and y coordinates and heading) of the 
-     * lane in the world reference system. The lanes are defined in the direction of 
-     * the traffic. That is, right lanes have the same heading as the road segment, 
-     * but left lanes have opposite heading, starting at the end of the road segment 
-     * and ending at the start.
-     *
-     * \return the end pose of the lane in world coordinates.
-     */
-    TOpendriveWorldPose get_end_pose(void) const;
-    /** 
-     * \brief returns the length of the lane
-     *
-     * This function returns the length of the lane geometry. The lane geometry is 
-     * the same as the link geometries of the lane links connecting the road nodes
-     * belonging to the lane.
-     *
-     * \return length of the lane in meters.
-     */
-    double get_length(void) const;
-    /** 
-     * \brief returns the pose of the lane at a given point
-     *
-     * This function returns the pose (x and y coordinates and heading) of the lane geometry
-     * at a given distance from the origin point of the lane. The lane geometry is 
-     * the same as the link geometries of the lane links connecting the road nodes
-     * belonging to the lane.
-     *
-     * \param length desired distance from the origin point of the lane in meters. This 
-     *               distance must be smaller or equal than the actual lane length.
-     *
-     * \return the pose at the desired point in the world reference system.
-     */
-    TOpendriveWorldPose get_pose_at(double length) const;
-    /** 
-     * \brief returns the curvature of the lane at a given point
-     *
-     * This function returns the curvature of the lane geometry at a given distance from 
-     * the origin point of the lane. The lane geometry is the same as the link geometries 
-     * of the lane links connecting the road nodes belonging to the lane.
-     *
-     * \param length desired distance from the origin point of the lane in meters. This 
-     *               distance must be smaller or equal than the actual lane length.
-     *
-     * \return the curvature at the desired point.
-     */
-    double get_curvature_at(double length) const;    
-    /** 
-     * \brief returns the closest node to a given pose
-     *
-     * This functions finds the closest road node belonging to the lane to a given 
-     * pose, and returns it. This function only returns a valid reference when the 
-     * given pose coincides with the lane. Otherwise, it throws an exception.
-     *
-     * \param world given pose to find the closest road node.
-     * \param distance the node distance between the projection of the given pose to 
-     *                 the lane link geometry of the closest road node, and the start 
-     *                 position of the node, following the corresponding lane link geometry. 
-     *                 If the closest road node does not exist this argument is set to the 
-     *                 maximum double value (std::numeric_limits<double>::max()) 
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the lane link geometry of the closest road node
-     *  
-     * \return a constant reference to the closest node which can not be modified by the 
-     *         user.
-     */
-    const COpendriveRoadNode &get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
-    /** 
-     * \brief returns the index of the closest node to a given pose
-     *
-     * This functions finds the closest road node belonging to the lane to a given 
-     * pose, and returns its index. This function only returns a valid index 
-     * when the given pose coincides with the lane.
-     *
-     * \param world given pose to find the closest road node.
-     * \param distance the node distance between the projection of the given pose to 
-     *                 the lane link geometry of the closest road node, and the start 
-     *                 position of the node, following the corresponding lane link geometry. 
-     *                 If the closest road node does not exist this argument is set to the 
-     *                 maximum double value (std::numeric_limits<double>::max()) 
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the lane link geometry of the closest road node
-     *  
-     * \return the index of the closest node or -1 if there is no node close to the given 
-     *         pose.
-     */
-    unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
-    /**
-     * \brief returns the closest pose to a given pose
-     *
-     * This functions finds the closest pose (minimum distance) on the lane geometry 
-     * to a given pose. The closest pose only exist if the imaginary line between the 
-     * given pose and its projection into the lane is orthogonal to the lane geometry. 
-     * Otherwise, it is considered that the given pose does not coincide with the lane.
-     * 
-     * \param world given pose to find the closest pose.
-     * \param closest_pose the closest pose to the lane if the given pose coincides with 
-     *                     it. Otherwise the data on this parameters is not valid.
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the lane geometry.
-     *
-     * \return the lane distance between the projection of the given pose to the 
-     *         lane geometry, following its geometry. If the closest pose does not exist,
-     *         the maximum double value (std::numeric_limits<double>::max()) will be 
-     *         returned.
-     */
-    double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
-    /** 
-     * \brief overloaded stream operator
-     * 
-     * This functions streams human readable information about the lane. This information
-     * includes:
-     *  * The opendrive lane id.
-     *  * Its width, sped limit and offset from the center of the road segment.
-     *  * The parent road segment.
-     *  * For each previous and next lanes: the road segment and lane information.
-     *  * The left lane and the left road mark.
-     *  * The right lane and the right road mark.
-     *  * The road nodes information.
-     *
-     */
-    friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
-    /**
-     * brief Destructor
-     */
-    ~COpendriveLane();
-};
-
-#endif
diff --git a/include/opendrive_link.h b/include/opendrive_link.h
deleted file mode 100644
index bca711ab7cf501546230eba3f71e9c4a72921648..0000000000000000000000000000000000000000
--- a/include/opendrive_link.h
+++ /dev/null
@@ -1,223 +0,0 @@
-#ifndef _OPENDRIVE_LINK_H
-#define _OPENDRIVE_LINK_H
-
-#include "opendrive_common.h"
-#include "g2_spline.h"
-#include "opendrive_line.h"
-#include "opendrive_spiral.h"
-#include "opendrive_arc.h"
-#include "opendrive_param_poly3.h"
-#include "opendrive_road_node.h"
-
-class COpendriveLink
-{
-  friend class COpendriveRoadSegment;
-  friend class COpendriveRoadNode;
-  friend class COpendriveRoad;
-  private:
-    COpendriveRoadNode *prev;
-    COpendriveRoadNode *next;
-    COpendriveRoadSegment *segment;
-    COpendriveLane *lane;
-    CG2Spline *spline;
-    opendrive_mark_t mark;
-    double resolution;
-    double scale_factor;    
-  protected:
-    COpendriveLink();
-    COpendriveLink(const COpendriveLink &object);
-    void set_prev(COpendriveRoadNode *node);
-    void set_next(COpendriveRoadNode *node);
-    void set_road_mark(opendrive_mark_t mark);
-    void set_parent_segment(COpendriveRoadSegment *segment);
-    void set_parent_lane(COpendriveLane *lane);
-    void set_resolution(double res);
-    void set_scale_factor(double scale);
-    void generate(double start_curvature,double end_curvature);
-    void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
-    bool clean_references(node_up_ref_t &refs);
-    void update_start_pose(TOpendriveWorldPose &start,double curvature);
-    void update_end_pose(TOpendriveWorldPose &end,double curvature);
-  public:
-    /** 
-     * \brief Returns a reference to the previous road node
-     *
-     * This function returns a constant reference to the previous COpendriveRoadNode 
-     * object. If the previous road node is not defines, an exception will be thrown.
-     *
-     * \return a constant reference to the previous COpendriveRoadNode object.
-     */
-    const COpendriveRoadNode &get_prev(void) const;
-    /** 
-     * \brief Returns a reference to the next road node
-     *
-     * This function returns a constant reference to the next COpendriveRoadNode 
-     * object. If the next road node is not defines, an exception will be thrown.
-     *
-     * \return a constant reference to the next COpendriveRoadNode object.
-     */
-    const COpendriveRoadNode &get_next(void) const;
-    /** 
-     * \brief Returns the road mark identifier between the two road nodes
-     *
-     * This function returns the possible road mark between the two COpendriveRoadNode 
-     * objects linked by each instance of this class. For lane links this will always 
-     * be OD_MARK_NONE, but for non-lane links it will correspond to the actual road 
-     * mark.
-     *
-     * \return the road mark identifier.
-     */
-    opendrive_mark_t get_road_mark(void) const;
-    /** 
-     * \brief Returns the reference to the parent road segment
-     *
-     * This function returns a constant reference to the COpendriveRoadSegment object
-     * where the each instance of this class belongs. If the reference is not valid,
-     * an exception will be thrown.
-     *
-     * \return a constant reference to the parent COpendriveRoadSegment object.
-     */
-    const COpendriveRoadSegment &get_parent_segment(void) const;
-    /** 
-     * \brief Returns the reference to the parent lane
-     *
-     * This function returns a constant reference to the COpendriveLane object
-     * where the each instance of this class belongs. This function will fail for 
-     * non-lane links since they do not have a parent lane. In this case an 
-     * exception will be thrown.
-     * 
-     * Use the is_lane_link() function to check the type of link.
-     *
-     * \return a constant reference to the parent COpendriveRoadSegment object.
-     */
-    const COpendriveLane &get_parent_lane(void) const;
-    /** 
-     * \brief returns the type of link
-     *
-     * This function returns whether each instance of this class is a lane link 
-     * or not.
-     *
-     * \return true if the instance is a lane link and false otherwise.
-     */
-    bool is_lane_link(void) const;
-    /**
-     * \brief Returns the resolution
-     *
-     * \return This function returns the current resolution
-     */
-    double get_resolution(void) const;
-    /**
-     * \brief Returns the scale factor
-     *
-     * \return This function returns the current scale factor
-     */
-    double get_scale_factor(void) const;
-    /** 
-     * \brief returns the closest pose to a given one
-     *
-     * This functions finds the closest pose (minimum distance) on the link geometry 
-     * to a given position. The closest pose only exist if the imaginary line between the 
-     * given pose and its projection into the link is orthogonal to the link geometry.
-     * Otherwise, it is considered that the given pose does not coincide with the link
-     * object.
-     *
-     * \param world given pose to find the closest point. The heading information is
-     *              ignored.
-     * \param pose the closest pose on the link geometry if the given pose coincide 
-     *             with the link object. Otherwise the data on this parameters is not
-     *             valid.
-     *  
-     * \return the link distance between the projected pose and the link start position, 
-     *         following the link geometry, if given pose coincide with the link object. 
-     *         Otherwise the maximum double value (std::numeric_limits<double>::max()) 
-     *         will be returned.
-     */
-    double find_closest_world_pose(TOpendriveWorldPose &world,TOpendriveWorldPose &pose) const;
-    /** 
-     * \brief returns the link trajectory (only position)
-     * 
-     * This function returns the x and y coordinates of the link geometry evaluated at
-     * the internal resolution. By default the whole trajectory is returned, but is 
-     * possible to get only a sub-trajectory by specifying the start and end lengths.
-     *
-     * \param x a vector with the x coordinates of the link trajectory (sub-trajectory)
-     *          in the world reference system.
-     * \param y a vector with the y coordinates of the link trajectory (sub-trajectory)
-     *          in the world reference system.
-     * \param start_length initial distance from the origin position of the link in 
-     *                     meters. By default this value is 0.0.
-     * \param end_length final distance from the origin position of the link in meters. 
-     *                   By default this value is -1.0 to indicate the end of the link.
-     */
-    void get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length=0.0, double end_length=-1.0) const;
-    /** 
-     * \brief returns the link trajectory (position and heading)
-     *
-     * This function returns the x and y coordinates, as well as the heading of the 
-     * link geometry evaluated at the internal resolution. By default the whole 
-     * trajectory is returned, but is possible to get only a sub-trajectory by 
-     * specifying the start and end lengths.
-     *
-     * \param x a vector with the x coordinates of the link trajectory (sub-trajectory)
-     *          in the world reference system.
-     * \param y a vector with the y coordinates of the link trajectory (sub-trajectory)
-     *          in the world reference system.
-     * \param yaw a vector with the heading of the link trajectory (sub-trajectory)
-     *          in the world reference system.
-     * \param start_length initial distance from the origin position of the link in 
-     *                     meters. By default this value is 0.0.
-     * \param end_length final distance from the origin position of the link in meters. 
-     *                   By default this value is -1.0 to indicate the end of the link.
-     */
-    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;
-    /** 
-     * \brief returns the length of the link
-     *
-     * This function returns the length of the link geometry.
-     *
-     * \return length of the link geometry in meters.
-     */
-    double get_length(void) const;
-    /** 
-     * \brief returns the pose of the link at a given point
-     *
-     * This function returns the pose (x and y coordinates and heading) of the link geometry
-     * at a given distance from the origin point of the link.
-     *
-     * \param length desired distance from the origin point of the link in meters. This 
-     *               distance must be smaller or equal than the actual link length.
-     *
-     * \return the pose at the desired point in the world reference system.
-     */
-    TOpendriveWorldPose get_pose_at(double length);
-    /** 
-     * \brief returns the curvature of the link at a given point
-     *
-     * This function returns the curvature of the link geometry at a given distance from 
-     * the origin point of the link.
-     *
-     * \param length desired distance from the origin point of the link in meters. This 
-     *               distance must be smaller or equal than the actual link length.
-     *
-     * \return the curvature at the desired point.
-     */
-    double get_curvature_at(double length);
-    /** 
-     * \brief overloaded stream operator
-     * 
-     * This functions streams human readable information about the link. This information
-     * includes:
-     *  * Index of the previous and next COpendriveRoad node objects.
-     *  * Name of the parent COpendriveROadSegment object.
-     *  * Id of the parent lane for lane links.
-     *  * The road mark between the two road nodes.
-     *
-     */
-    friend std::ostream& operator<<(std::ostream& out, COpendriveLink &link);
-    /** 
-     * \brief Destructor
-     */
-    ~COpendriveLink();
-};
-
-#endif
diff --git a/include/opendrive_road.h b/include/opendrive_road.h
deleted file mode 100644
index 4d3966e90a859c8d64739ecd88f2a4a44a140957..0000000000000000000000000000000000000000
--- a/include/opendrive_road.h
+++ /dev/null
@@ -1,430 +0,0 @@
-#ifndef _OPENDRIVE_ROAD_H
-#define _OPENDRIVE_ROAD_H
-
-#ifdef _HAVE_XSD
-#include "xml/OpenDRIVE_1.4H.hxx"
-#endif
-
-#include "opendrive_common.h"
-#include "opendrive_road_segment.h"
-#include "opendrive_road_node.h"
-#include "opendrive_lane.h"
-
-class COpendriveRoad
-{
-  friend class COpendriveRoadSegment;
-  friend class COpendriveRoadNode;
-  friend class COpendriveLane;
-  private:
-    std::vector<COpendriveRoadSegment *> segments;
-    std::vector<COpendriveRoadNode *> nodes;
-    std::vector<COpendriveLane *> lanes;
-    double resolution;
-    double scale_factor;
-    double min_road_length;
-  protected:
-    void free(void);
-    void link_segments(OpenDRIVE &open_drive);
-    unsigned int add_node(COpendriveRoadNode *node);
-    bool remove_node(COpendriveRoadNode *node);
-    COpendriveRoadNode* get_node_at(TOpendriveWorldPose &pose);
-    bool node_exists_at(TOpendriveWorldPose &pose);
-    unsigned int add_lane(COpendriveLane *lane);
-    bool remove_lane(COpendriveLane *lane);
-    void complete_open_lanes(void);
-    void add_segment(COpendriveRoadSegment *segment);
-    bool has_segment(COpendriveRoadSegment *segment);
-    std::vector<unsigned int> update_path(node_up_ref_t &node_refs,std::vector<unsigned int> &path);
-    void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
-    void clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
-    void reindex(void);
-    void prune(std::vector<unsigned int> &path_nodes);
-  public:
-    /**
-     * \brief Default constructor
-     */ 
-    COpendriveRoad();
-    /**
-     * \brief Copy constructor
-     *
-     * \param object constant reference to the object to be copied
-     */ 
-    COpendriveRoad(const COpendriveRoad& object);
-    /**
-     * \brief Load an Opendrive road description
-     *
-     * This function loads the Opendrive description file initializes the internal 
-     * data structures. If the file referenced by the parameter does not exist or 
-     * the format is not valid, this function will throw an exception.
-     *
-     * \param filename the name of the opendrive file to load with the extension 
-     *                 and the full path.
-     */
-    void load(const std::string &filename);
-    /**
-     * \brief Sets the resolution
-     *
-     * This functions sets the default resolution used to generate all the internal 
-     * geometry. This functions automatically changes the resolution of all underlying
-     * objects.
-     *
-     * \param res desired value of the resolution in meters. The default resolution
-     *            is 0.1 m.
-     */
-    void set_resolution(double res);
-    /**
-     * \brief Sets the scale factor
-     *
-     * This function sets the scale factor to be applied to all the internal geometry.
-     * The scale factor makes it possible to change the size of the road without actually
-     * modifying the internal geometry data. This functions automatically changes 
-     * the scale factor of all underlying objects.
-     *
-     * \param scale desired scale factor. A value bigger than 1.0 will scale down the 
-     *              road and a value smaller than 1.0 will scale it up. The default 
-     *              value is 1.0.
-     */
-    void set_scale_factor(double scale);
-    /**
-     * \brief Sets the minim road length
-     *
-     * This function sets the length threshold to include any imported road segments into 
-     * the internal structure. This functions automatically changes the minimum road length
-     * of all underlying objects.
-     *
-     * \param length threshold value to include a road segment. The default value is 0.01.
-     */
-    void set_min_road_length(double length);
-    /**
-     * \brief Returns the resolution
-     *
-     * \return This function returns the current resolution
-     */
-    double get_resolution(void) const;
-    /**
-     * \brief Returns the scale factor
-     *
-     * \return This function returns the current scale factor
-     */
-    double get_scale_factor(void) const;
-    /**
-     * \brief Returns the minimum road length
-     *
-     * \return This function returns the current minimum road length
-     */
-    double get_min_road_length(void) const;
-    /**
-     * \brief Returns the number of road segment objects in the whole road
-     *
-     * \return This function returns the number of road segments present in 
-     *         whole road. The range of valid indexes goes from 0 to the 
-     *         value returned by this function -1.
-     */
-    unsigned int get_num_segments(void) const;
-    /**
-     * \brief Returns a road segment by index
-     *
-     * This function returns a constant reference to the desired road segment 
-     * object, which can not be modified by the user. If the index is not valid,
-     * this function will throw an exception.
-     *
-     * \param index is the 0 based index of the desired road segment
-     *
-     * \return constant reference to the desired road segment object.
-     */
-    const COpendriveRoadSegment& get_segment(unsigned int index) const;
-    /**
-     * \brief Returns a road segment by id
-     *
-     * This function returns a constant reference to the desired road segment 
-     * object, which can not be modified by the user. If the id is not valid,
-     * this function will throw an exception.
-     *
-     * \param id is the Opendrive numeric id of the road segment.
-     *
-     * \return constant reference to the desired road segment object.
-     */
-    const COpendriveRoadSegment &get_segment_by_id(unsigned int id) const;
-    /**
-     * \brief Returns a road segment by name
-     *
-     * This function returns a constant reference to the desired road segment 
-     * object, which can not be modified by the user. If the name is not valid,
-     * this function will throw an exception.
-     *
-     * \param name is the Opendrive name of the road segment.
-     *
-     * \return constant reference to the desired road segment object.
-     */
-    const COpendriveRoadSegment &get_segment_by_name(std::string &name) const;
-    /**
-     * \brief Returns the number of lane objects in the whole road
-     *
-     * \return This function returns the number of lanes present in whole
-     *         road. The range of valid indexes goes from 0 to the value 
-     *         returned by this function -1.
-     */
-    unsigned int get_num_lanes(void) const;
-    /**
-     * \brief Returns a lane by index
-     *
-     * This function returns a constant reference to the desired lane 
-     * object, which can not be modified by the user. If the index is not valid,
-     * this function will throw an exception.
-     *
-     * \param index is the 0 based index of the desired lane
-     *
-     * \return constant reference to the desired lane object.
-     */
-    const COpendriveLane &get_lane(unsigned int index) const;
-    /**
-     * \brief Returns the number of road node objects in the whole road
-     *
-     * \return This function returns the number of road nodes present in whole
-     *         road. The range of valid indexes goes from 0 to the value 
-     *         returned by this function -1.
-     */
-    unsigned int get_num_nodes(void) const;
-    /**
-     * \brief Returns a road node by index
-     *
-     * This function returns a constant reference to the desired road node 
-     * object, which can not be modified by the user. If the index is not valid,
-     * this function will throw an exception.
-     *
-     * \param index is the 0 based index of the desired road node
-     *
-     * \return constant reference to the desired road node object.
-     */
-    const COpendriveRoadNode& get_node(unsigned int index) const;
-    /**
-     * \brief overloaded index operator
-     *
-     * This operator is similar to the get_segment() function. It returns a 
-     * constant reference to the desired road segment object, which can not 
-     * be modified by the user. If the index is not valid,this function will 
-     * throw an exception.
-     *
-     * \param index is the 0 based index of the desired road segment
-     *
-     * \return constant reference to the desired road segment object.
-     */
-    const COpendriveRoadSegment &operator[](std::size_t index);
-    /** 
-     * \brief returns the closest node to a given pose
-     *
-     * This functions finds the closest road node belonging to the whole road to a
-     * given pose, and returns it.  
-     *
-     * \param world given pose to find the closest road node.
-     * \param distance the node distance between the projection of the given pose to 
-     *                 the lane link geometry of the closest road node, and the start 
-     *                 position of the node, following the corresponding lane link geometry. 
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the lane link geometry of the closest road node.
-     *  
-     * \return a constant reference to the closest node which can not be modified by the 
-     *         user.
-     */
-    const COpendriveRoadNode &get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
-    /** 
-     * \brief returns the index of the closest node to a given pose
-     *
-     * This functions finds the closest road node belonging to the whole road to a given 
-     * pose, and returns its index.  
-     *
-     * \param world given pose to find the closest road node.
-     * \param distance the node distance between the projection of the given pose to 
-     *                 the lane link geometry of the closest road node, and the start 
-     *                 position of the node, following the corresponding lane link geometry. 
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the lane link geometry of the closest road node.
-     *  
-     * \return the index of the closest road node.
-     */
-    unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
-    /** 
-     * \brief returns the closest lane to a given pose
-     *
-     * This functions finds the closest lane belonging to the whole road to a
-     * given pose, and returns it. 
-     *
-     * \param world given pose to find the closest lane.
-     * \param distance the lane distance between the projection of the given pose to 
-     *                 the lane geometry, and the start position of the lane, following 
-     *                 its geometry. 
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the lane geometry.
-     *  
-     * \return a constant reference to the closest lane which can not be modified by the 
-     *         user.
-     */
-    const COpendriveLane &get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
-    /** 
-     * \brief returns the index of the closest lane to a given pose
-     *
-     * This functions finds the closest lane belonging to the whole road to a given 
-     * pose, and returns its index. 
-     *
-     * \param world given pose to find the closest lane.
-     * \param distance the lane distance between the projection of the given pose to 
-     *                 the lane geometry, and the start position of the lane, following 
-     *                 its geometry. 
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the lane geometry.
-     *  
-     * \return the index of the closest lane.
-     */
-    unsigned int get_closest_lane_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
-    /** 
-     * \brief returns the closest road segment to a given pose
-     *
-     * This functions finds the closest road segment belonging to the whole road to a
-     * given pose, and returns it. The geometry used to find the closest road segment is the
-     * one that describes the center lane of the segment, not the lane/link geometries.
-     *
-     * \param world given pose to find the closest road segment.
-     * \param distance the segment distance between the projection of the given pose to 
-     *                 the road segment geometry, and the start position of the segment, following 
-     *                 its geometry. 
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the road segment geometry.
-     *  
-     * \return a constant reference to the closest road segment which can not be modified by the 
-     *         user.
-     */
-    const COpendriveRoadSegment &get_closest_segment(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
-    /** 
-     * \brief returns the index of the closest road segment to a given pose
-     *
-     * This functions finds the closest road segment belonging to the whole road to a given 
-     * pose, and returns its index. The geometry used to find the closest road segment is the
-     * one that describes the center lane of the segment, not the lane/link geometries.
-     *
-     * \param world given pose to find the closest road segment.
-     * \param distance the segment distance between the projection of the given pose to 
-     *                 the road segment geometry, and the start position of the segment, following 
-     *                 its geometry. 
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the road segment geometry.
-     *  
-     * \return the index of the closest road segment.
-     */
-    unsigned int get_closest_segment_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
-    /**
-     * \brief returns the closest pose 
-     *
-     * This functions finds the closest pose (minimum distance) to the whole road 
-     * to a given pose. 
-     * 
-     * \param world given pose to find the closest pose.
-     * \param closest_pose the closest pose to the closest to any road geometry.
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on any road geometry.
-     *
-     * \return the link distance between the projection of the given pose to the 
-     *         closest road geometry, following its geometry. 
-     */
-    double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
-    /**
-     * \brief Returns a set of poses on the whole road close to a given pose
-     *
-     * This function is similar to the get_closest_pose(), but in this case, the closest 
-     * pose of any road geometry which is closer to the given pose that a given distance will be 
-     * returned.
-     * 
-     * \param world given pose to find the closest poses.
-     * \param closest_poses the closest poses on any road geometry.
-     * \param distances the link distance between the projection of the given pose to each  
-     *                  of the closest road geometries, following the corresponding geometry.
-     * \param dist_tol maximum allowed distance from the given position to any road geometry. 
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on any road geometry.
-     */
-    void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &distances,double dist_tol,double angle_tol=0.1) const;
-    /**
-     * \brief creates a sub-road with only the road elements of a given path
-     *  
-     * This functions creates a new sub road with only the whole road segments that 
-     * include the road nodes of the given path. These new road segments will only have 
-     * the lanes and the road nodes on the same side as the given path. 
-     *
-     * The road elements of the new road keep the required original information and connectivity
-     * but are completely independent from the original ones. The only information that 
-     * changes in the new sub-road is:
-     *
-     * * the remaining road segments, lanes and road nodes are re-indexed, so that their 
-     *   unique index is coherent with the new data structure. 
-     * * for any open road segment (that is, a road segment that is not connected), new road 
-     *   nodes are placed at the end of the open road segment to keep its lane and link 
-     *   geometric information.
-     *
-     * If the given path has non-consecutive road nodes, this function will fail throwing an
-     * exception.
-     *
-     * \param path_nodes sequence of road nodes describing the path. The road nodes are 
-     *                   identified by their unique index.
-     * \param end_pose end pose of the path. When final road node has multiple road segment
-     *                 parents, the end pose indicates which one of them must be included 
-     *                 in the new road.
-     * \param new_road The new sub-road object.
-     *
-     * \return the sequence of road nodes describing the path in the new sub-road.
-     */
-    std::vector<unsigned int> get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPose &end_pose,COpendriveRoad &new_road);
-    /**
-     * \brief creates a sub-road with only the road elements within a distance to a given pose
-     *
-     * This functions creates a new sub road stating a given distance (margin) from the start 
-     * pose and reaching up to a given distance following the traffic direction of the start
-     * pose. The new road segments will keep all the original lanes and road nodes. 
-     *
-     * The distance is measured following the road segment geometries, not the euclidean
-     * distance. The new road may be shorter than expected if the original road is not long 
-     * enough.
-     *
-     * The road elements of the new road keep the required original information and connectivity
-     * but are completely independent from the original ones. The only information that 
-     * changes in the new sub-road is:
-     *
-     * * the remaining road segments, lanes and road nodes are re-indexed, so that their 
-     *   unique index is coherent with the new data structure. 
-     * * for any open road segment (that is, a road segment that is not connected), new road 
-     *   nodes are placed at the end of the open road segment to keep its lane and link 
-     *   geometric information.
-     *
-     * If the part of the road of interest has multiple paths, this function will fail throwing an
-     * exception.
-     *
-     * \param start_pose start pose of the new road. This pose implicitly defines the side
-     *                   of the road taken to generate the sub-road.
-     * \param length this is the desired length of the new road in meters. On success, this
-     *               parameter will be updated with the actual length of the new road.
-     * \param margin a distance to be added at the beginning and the end of the new road, if
-     *               possible. This makes the actual length of the new road equal to length+
-     *               2.0*margin.
-     * \param new_road The new sub-road object.
-     * 
-     */
-    void get_sub_road(TOpendriveWorldPose &start_pose,double &length,double margin,COpendriveRoad &new_road);
-    /**
-     * \brief creates a full copy of the road
-     */
-    void operator=(const COpendriveRoad& object);
-    /** 
-     * \brief overloaded stream operator
-     * 
-     * This functions streams human readable information about the whole road. This information
-     * includes:
-     *  * The resolution, scale_factor and minimum road length
-     *  * The road segments information.
-     *
-     */
-    friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
-    /**
-     * \brief Destructor
-     */
-    ~COpendriveRoad();
-};
-
-#endif
diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
deleted file mode 100644
index b5d0aae3fbdb753b79e62bafe3ae0b8979f13bb1..0000000000000000000000000000000000000000
--- a/include/opendrive_road_node.h
+++ /dev/null
@@ -1,305 +0,0 @@
-#ifndef _OPENDRIVE_ROAD_NODE_H
-#define _OPENDRIVE_ROAD_NODE_H
-
-#include <vector>
-#include "opendrive_common.h"
-#include "opendrive_link.h"
-#include "opendrive_lane.h"
-#include "opendrive_road_segment.h"
-
-typedef struct
-{
-  COpendriveLane * lane;
-  COpendriveRoadSegment *segment;
-  double start_curvature;
-  double end_curvature;
-}TOpendriveRoadNodeParent;
-
-class COpendriveRoadNode
-{
-  friend class COpendriveLane;
-  friend class COpendriveRoadSegment;
-  friend class COpendriveRoad;
-  friend class COpendriveLink;
-  private:
-    std::vector<COpendriveLink *> links;
-    double resolution;
-    double scale_factor;
-    TOpendriveWorldPose pose;
-    std::vector<TOpendriveRoadNodeParent> parents;
-    unsigned int index;
-  protected:
-    COpendriveRoadNode();
-    COpendriveRoadNode *clone(link_up_ref_t &new_link_ref);
-    void free(void);
-    bool add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane);
-    void add_link(COpendriveLink *link);
-    void remove_link(COpendriveLink *link);
-    bool has_link(COpendriveLink *link);
-    COpendriveLink *get_link_with(COpendriveRoadNode *node);
-    void set_resolution(double res);
-    void set_scale_factor(double scale);
-    void set_index(unsigned int index);
-    void set_pose(TOpendriveWorldPose &start);
-    void add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,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);
-    COpendriveRoadNode *get_original_node(node_up_ref_t &node_refs);
-    void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
-    void clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
-    void update_start_pose(COpendriveLane *lane,TOpendriveWorldPose &start,double distance);
-    void update_end_pose(COpendriveLane *lane,TOpendriveWorldPose &end,double distance);
-  public:
-    /**
-     * \brief Returns the resolution
-     *
-     * \return This function returns the current resolution
-     */
-    double get_resolution(void) const;
-    /**
-     * \brief Returns the scale factor
-     *
-     * \return This function returns the current scale factor
-     */
-    double get_scale_factor(void) const;
-    /**
-     * \brief returns the node index
-     *
-     * This function returns the unique node index which identified the road node object. 
-     * This index is assigned when the Opendrive file is loaded and may only change when 
-     * a new sub-road is generated, in which case the road nodes in the new road are re-
-     * indexed. 
-     *
-     * \return the index of the road node.
-     */
-    unsigned int get_index(void) const;
-    /**
-     * \brief Returns the number of parents
-     *
-     * This function returns how many parents the road node have. Each parent is identified
-     * by the road segment and also the lane where it belongs.
-     *
-     * \return This function returns the number of parent of the road node. The range of 
-     *         valid indexes goes from 0 to the value returned by this function -1.
-     */
-    unsigned int get_num_parents(void) const;
-    /**
-     * \brief Returns a reference to a road segment parent
-     *
-     * This function returns a constant reference to a COpendriveRoadSegment object which
-     * is a parent of the road node. This reference can not be modified by the user. 
-     * If the index is not valid, this function will throw an exception.
-     *
-     * \param index is the 0 based index of the desired parent
-     *
-     * \return constant reference to the desired road segment parent object.
-     */
-    const COpendriveRoadSegment& get_parent_segment(unsigned int index) const;
-    /**
-     * \brief Returns a reference to a lane parent
-     *
-     * This function returns a constant reference to a COpendriveLane object which
-     * is a parent of the road node. This reference can not be modified by the user.
-     * If the index is not valid, this function will throw an exception.
-     *
-     * \param index is the 0 based index of the desired parent
-     *
-     * \return constant reference to the desired lane parent object.
-     */
-    const COpendriveLane &get_parent_lane(unsigned int index) const;
-    /**
-     * \brief returns the position of the road node
-     *
-     * This function returns the pose of the road node in the world reference system. 
-     *
-     * \return world pose of the road node.
-     */
-    TOpendriveWorldPose get_pose(void) const;
-    /**
-     * \brief Returns the number of links
-     *
-     * This function returns how many links are connecting to this road node, either
-     * starting or ending at it.
-     *
-     * \return This function returns the number of links of the road node. The range of 
-     *         valid indexes goes from 0 to the value returned by this function -1.
-     */
-    unsigned int get_num_links(void) const;
-    /**
-     * \brief Returns a reference to a link
-     *
-     * This function returns a constant reference to a COpendriveLink object which
-     * connects this node to another one. In this case both links starting and ending 
-     * at the desired road node can be accessed. This reference can not be modified 
-     * by the user. If the index is not valid, this function will throw an exception.
-     *
-     * \param index is the 0 based index of the desired parent
-     *
-     * \return constant reference to the desired link object.
-     */
-    const COpendriveLink &get_link(unsigned int index) const;
-    /**
-     * \brief Returns a reference to the link connecting with the given road node
-     *
-     * This function returns a constant reference to the COpendriveLink object that 
-     * connects this road node with another one, identified by its unique index. If 
-     * there exist no connection between the two road nodes, an exception is thrown.
-     *
-     * In this case, only links starting at the road node calling this function can
-     * be accessed.
-     *
-     * \param node_index unique identifier of the road node candidate to check for
-     *                   connectivity.
-     *
-     * \return constant reference to the desired link object, it it exists.
-     */
-    const COpendriveLink &get_link_ending_at(unsigned int node_index) const;
-    /**
-     * \brief Returns a reference to the link connecting with the given road node
-     *
-     * This function returns a constant reference to the COpendriveLink object that 
-     * connects this road node with another one. If there exist no connection between 
-     * the two road nodes, an exception is thrown.
-     *
-     * In this case, only links starting at the road node calling this function can
-     * be accessed.
-     *
-     * \param node COpendriveRoadNode object candidate to check for connectivity.
-     *
-     * \return constant reference to the desired link object, it it exists.
-     */
-    const COpendriveLink &get_link_ending_at(const COpendriveRoadNode &node) const;
-    /**
-     * \brief checks if a link with another road node exists
-     *
-     * This function checks if a road node is connected through a link with another
-     * road node, identified by its unique index.
-     * 
-     * \param node_index unique identifier of the road node candidate to check for
-     *                   connectivity.
-     *
-     * \return true if the two road nodes are connected, or false otherwise.
-     */
-    bool has_link_with(unsigned int node_index) const;
-    /**
-     * \brief checks if a link with another road node exists
-     *
-     * This function checks if a road node is connected through a link with another
-     * road node.
-     * 
-     * \param node COpendriveRoadNode object candidate to check for connectivity.
-     *
-     * \return true if the two road nodes are connected, or false otherwise.
-     */
-    bool has_link_with(const COpendriveRoadNode &node) const;
-    /** 
-     * \brief returns the index of the closest link to a given pose
-     *
-     * This functions finds the closest link starting on this road node to a given 
-     * pose, and returns its index. This function only returns a valid index 
-     * when the given pose coincides with the road node links.
-     *
-     * \param world given pose to find the closest link.
-     * \param distance the link distance between the projection of the given pose to 
-     *                 the closest link geometry, and the position of the starting road node,
-     *                 following the closest link geometry. If the closest link does not exist
-     *                 this argument is set to the maximum double value 
-     *                 (std::numeric_limits<double>::max()) 
-     * \param only_lanes flag to indicate it the search for the closest link should include 
-     *                   only the lane links (true) or all of them (false).
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the link geometry to consider each link as a candidate on the search.
-     *  
-     * \return the index of the closest link or -1 if there is no link close to the given 
-     *         pose.
-     */
-    unsigned int get_closest_link_index(TOpendriveWorldPose &pose,double &distance,bool only_lanes=false,double angle_tol=0.1) const;
-    /** 
-     * \brief returns the closest link to a given pose
-     *
-     * This functions finds the closest link starting on this road node to a given 
-     * pose, and returns it. This function only returns a valid reference when the 
-     * given pose coincides with the road node links. Otherwise, it throws an exception.
-     *
-     * \param world given pose to find the closest link.
-     * \param distance the link distance between the projection of the given pose to 
-     *                 the closest link geometry, and the position of the starting road node,
-     *                 following the closest link geometry. If the closest link does not exist
-     *                 this argument is set to the maximum double value 
-     *                 (std::numeric_limits<double>::max()) 
-     * \param only_lanes flag to indicate it the search for the closest link should include 
-     *                   only the lane links (true) or all of them (false).
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the link geometry to consider any link as a candidate on the search.
-     *  
-     * \return a constant reference to the closest link which can not be modified by the 
-     *         user.
-     */
-    const COpendriveLink &get_closest_link(TOpendriveWorldPose &pose,double &distance,bool only_lanes=false,double angle_tol=0.1) const;
-    /**
-     * \brief returns the closest pose at any link to a given pose
-     *
-     * This functions finds the closest pose (minimum distance) on any link geometry 
-     * to a given pose. The closest pose only exist if the imaginary line between the 
-     * given pose and its projection into any link is orthogonal to the corresponding
-     * link geometry. Otherwise, it is considered that the given pose does not coincide 
-     * with the road node links.
-     * 
-     * \param world given pose to find the closest pose.
-     * \param closest_pose the closest pose to the closest link geometry if the given pose 
-     *                     coincides with the link object. Otherwise the data on this 
-     *                     parameters is not valid.
-     * \param only_lanes flag to indicate it the search for the closest link should include 
-     *                   only the lane links (true) or all of them (false).
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the link geometry to consider any link as a candidate on the search.
-     *
-     * \return the link distance between the projection of the given pose to the 
-     *         closest link geometry, following its geometry. If there is no
-     *         closest link, the maximum double value (std::numeric_limits<double>::max())
-     *         will be returned.
-     */
-    double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,bool only_lanes=false,double angle_tol=0.1) const;
-    /**
-     * \brief Returns a set of poses on any link close to a given pose
-     *
-     * This function is similar to the get_closest_pose(), but in this case, the closest 
-     * pose of any link which is closer to the given pose that a given distance will be 
-     * returned.
-     * 
-     * \param world given pose to find the closest poses.
-     * \param closest_poses the closest poses on any link geometry if the given pose 
-     *                      coincide with the link object. Otherwise an empty vector will
-     *                      be returned.
-     * \param distances the link distance between the projection of the given pose to each  
-     *                  of the closest link geometries, following the corresponding geometry.
-     *                  If the given pose does not coincide with any of the links, an empty 
-     *                  vector will be returned.
-     * \param dist_tol maximum allowed distance from the given position to consider any link 
-     *                 as a candidate on the search.
-     * \param only_lanes flag to indicate it the search for the closest link should include 
-     *                   only the lane links (true) or all of them (false).
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the link geometry to consider any link as a candidate on the search.
-     */
-    void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &distances,double dist_tol,bool only_lanes=false,double angle_tol=0.1) const;
-    /** 
-     * \brief overloaded stream operator
-     * 
-     * This functions streams human readable information about the road node. This information
-     * includes:
-     *  * The node index.
-     *  * Its position and heading.
-     *  * for each parent: the name of the segment and the lane id.
-     *  * The links information.
-     *
-     */
-    friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
-    /**
-     * \brief Destructor
-     */
-    ~COpendriveRoadNode();
-};
-
-#endif
diff --git a/include/opendrive_road_segment.h b/include/opendrive_road_segment.h
deleted file mode 100644
index c9acd26a80fdbb9e7bb46ea24fcfd9a8ed3629c9..0000000000000000000000000000000000000000
--- a/include/opendrive_road_segment.h
+++ /dev/null
@@ -1,439 +0,0 @@
-#ifndef _OPENDRIVE_ROAD_SEGMENT_H
-#define _OPENDRIVE_ROAD_SEGMENT_H
-
-#ifdef _HAVE_XSD
-#include "xml/OpenDRIVE_1.4H.hxx"
-#endif
-
-#include "opendrive_common.h"
-#include "opendrive_lane.h"
-#include "opendrive_signal.h"
-#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
-{
-  friend class COpendriveRoad;
-  friend class COpendriveRoadNode;
-  friend class COpendriveLane;
-  friend class COpendriveSignal;
-  friend class COpendriveObject;
-  private:
-    std::map<int,COpendriveLane *> lanes;
-    std::vector<COpendriveRoadNode *> nodes;
-    std::vector<TOpendriveRoadSegmentGeometry> geometries;
-    COpendriveRoad *parent_road;
-    double resolution;
-    double scale_factor;
-    double min_road_length;
-    int num_right_lanes;
-    int num_left_lanes;
-    std::vector<COpendriveSignal *> signals;
-    std::vector<COpendriveObject *> objects;
-    std::vector<COpendriveRoadSegment *> connecting;
-    std::string name;
-    unsigned int id;
-    opendrive_mark_t center_mark;
-  protected:
-    COpendriveRoadSegment();
-    COpendriveRoadSegment(const COpendriveRoadSegment &object);
-    void load(OpenDRIVE::road_type &road_info);
-    void free(void);
-    void set_resolution(double res);
-    void set_scale_factor(double scale);
-    void set_min_road_length(double length);
-    void set_parent_road(COpendriveRoad *parent);
-    void set_name(std::string &name);
-    void set_id(unsigned int id);
-    void set_center_mark(opendrive_mark_t mark);
-    bool updated(segment_up_ref_t &segment_refs);
-    COpendriveRoadSegment *get_original_segment(segment_up_ref_t &segment_refs);
-    void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
-    void clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
-    void add_lanes(lanes::laneSection_type &lane_section);
-    void add_lane(const ::lane &lane_info);
-    void remove_lane(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) const;
-    void link_neightbour_lanes(lanes::laneSection_type &lane_section);
-    void link_neightbour_lanes(void);
-    void link_segment(COpendriveRoadSegment *segment);
-    void link_segment(COpendriveRoadSegment *segment,int from,bool from_start, int to,bool to_start);
-    bool connects_to(COpendriveRoadSegment *segment);
-    bool connects_segments(COpendriveRoadSegment *segment1,COpendriveRoadSegment *segment2);
-    COpendriveRoadSegment *get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL) const;
-    COpendriveRoadSegment *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref) const;
-    TOpendriveLocalPose transform_to_local_pose(const TOpendriveTrackPose &track);
-    TOpendriveWorldPose transform_to_world_pose(const TOpendriveTrackPose &track);
-  public:
-    /**
-     * \brief Returns the Opendrive name
-     *
-     * \return The Opendrive name of the road segment 
-     */
-    std::string get_name(void) const;
-    /**
-     * \brief Returns the Opendirve numeric identifier
-     *
-     * \return The Opendrive numeric identifier of the road segment.
-     */
-    unsigned int get_id(void) const;
-    /**
-     * \brief Returns the number of right lanes 
-     *
-     * This function returns how many lanes there are on the right side of the road
-     * segment. The number of left and right lanes can be different.
-     *
-     * \return This function returns the number of right lanes. The range of 
-     *         valid indexes goes from -1 to the value returned by this function 
-     *         negated.
-     */
-    unsigned int get_num_right_lanes(void) const;
-    /**
-     * \brief Returns the number of left lanes 
-     *
-     * This function returns how many lanes there are on the left side of the road
-     * segment. The number of left and right lanes can be different.
-     *
-     * \return This function returns the number of left lanes. The range of 
-     *         valid indexes goes from 1 to the value returned by this function.
-     */
-    unsigned int get_num_left_lanes(void) const;
-    /**
-     * \brief Returns a reference to a lane by id
-     *
-     * This function returns a constant reference to a lane of the road segment,
-     * which can not be modified by the user. If the identifier is not valid, this 
-     * function will throw an exception.
-     *
-     * \param id is the Opendrive Lane identifier: negative integers values are used 
-     *           for right lanes, and positive values for left lanes. These values 
-     *           increase (for left lanes) or decrease (for right lanes) the further 
-     *           they are from the center lane of the segment.
-     *
-     * \return constant reference to a lane.
-     */
-    const COpendriveLane &get_lane(int id) const;
-    /**
-     * \brief Returns the number of road nodes
-     *
-     * This function returns how many road nodes belong to the road segment.
-     *
-     * \return This function returns the number of road nodes. The range of 
-     *         valid indexes goes from 0 to the value returned by this function -1.
-     */
-    unsigned int get_num_nodes(void) const;
-    /**
-     * \brief Returns a reference to a road node by local index
-     *
-     * This function returns a constant reference to the desired road node 
-     * object, which can not be modified by the user. If the index is not valid,
-     * this function will throw an exception.
-     *
-     * \param index is the 0 based local index of the desired road node. It is not
-     *              the unique index assigned to the road nodes when they are created.
-     *
-     * \return constant reference to the desired road node object.
-     */
-    const COpendriveRoadNode &get_node(unsigned int index) const;
-    /**
-     * \brief Returns a reference to the parent road
-     *
-     * This function returns a constant reference to the parent road object, which 
-     * can not be modified by the user. If the internal parent road is invalid, this 
-     * function will throw an exception.
-     *
-     * \return a constant reference to the parent road object.
-     */
-    const COpendriveRoad &get_parent_road(void) const;
-    /**
-     * \brief Returns the number of road signals
-     *
-     * This function returns how many road signals are placed at this road segment.
-     *
-     * \return This function returns the number of road signals. The range of 
-     *         valid indexes goes from 0 to the value returned by this function -1.
-     */
-    unsigned int get_num_signals(void) const;
-    /**
-     * \brief Returns a reference to a road signal by local index
-     *
-     * This function returns a constant reference to the desired road signal 
-     * object, which can not be modified by the user. If the index is not valid,
-     * this function will throw an exception.
-     *
-     * \param index is the 0 based local index of the desired road signal. 
-     *
-     * \return constant reference to the desired road signal object.
-     */
-    const COpendriveSignal &get_signal(unsigned int index) const;
-    /**
-     * \brief Returns the number of road objects
-     *
-     * This function returns how many road objects are placed at this road segment.
-     *
-     * \return This function returns the number of road objects. The range of 
-     *         valid indexes goes from 0 to the value returned by this function -1.
-     */
-    unsigned int get_num_objects(void) const;
-    /**
-     * \brief Returns a reference to a road object by local index
-     *
-     * This function returns a constant reference to the desired road object 
-     * object, which can not be modified by the user. If the index is not valid,
-     * this function will throw an exception.
-     *
-     * \param index is the 0 based local index of the desired road object. 
-     *
-     * \return constant reference to the desired road object object.
-     */
-    const COpendriveObject &get_object(unsigned int index) const;
-    /**
-     * \brief Returns the number of connecting road segments
-     *
-     * This function returns how many road segments are connected to this road
-     * segment. Since the road segments have to possible traffic directions, the 
-     * concept of next and previous is only defined when the side of the road is 
-     * selected.
-     *
-     * \return This function returns the number of connecting road segments. The 
-     *         range of valid indexes goes from 0 to the value returned by this 
-     *         function -1.
-     */
-    unsigned int get_num_connecting(void) const;
-    /**
-     * \brief Returns a reference to a connecting road segment by local index
-     *
-     * This function returns a constant reference to the desired connecting road 
-     * segment object, which can not be modified by the user. If the index is 
-     * not valid, this function will throw an exception.
-     *
-     * \param index is the 0 based local index of the desired connecting road
-     *              segment. 
-     *
-     * \return constant reference to the desired connecting road segment object.
-     */
-    const COpendriveRoadSegment &get_connecting(unsigned int index) const;
-    /** 
-     * \brief returns the length of the road segment
-     *
-     * This function returns the length of the road segment geometry. The road 
-     * segment trajectory is represented by Opendive geometry objects and also
-     * by G2 Splines, and it is different from the link and lanes geometries. 
-     * Both representations are equivalent.
-     *
-     * \return length of the road segment in meters.
-     */
-    double get_length(void) const;
-    /** 
-     * \brief returns the pose of the road segment at a given point
-     *
-     * This function returns the pose (x and y coordinates and heading) of the center 
-     * lane geometry of the road segment at a given distance from the origin point 
-     * of the road segment. 
-     *
-     * The road segment trajectory is represented by Opendive geometry objects and also
-     * by G2 Splines, and it is different from the link and lanes geometries. 
-     * Both representations are equivalent.
-     *
-     * \param length desired distance from the origin point of the road segment in meters.
-     *               This distance must be smaller or equal than the actual road segment
-     *               length.
-     *
-     * \return the pose at the desired point in the world reference system.
-     */
-    TOpendriveWorldPose get_pose_at(double length) const;
-    /** 
-     * \brief returns the curvature of the road segment at a given point
-     *
-     * This function returns the curvature of the center geometry of the road segment
-     * at a given distance from the origin point of the road segment. 
-     *
-     * The road segment trajectory is represented by Opendive geometry objects and also
-     * by G2 Splines, and it is different from the link and lanes geometries. 
-     * Both representations are equivalent.
-     *
-     * \param length desired distance from the origin point of the road segment in meters. 
-     *               This distance must be smaller or equal than the actual road segment 
-     *               length.
-     *
-     * \return the curvature at the desired point.
-     */
-    double get_curvature_at(double length) const;
-    /** 
-     * \brief returns the closest node to a given pose
-     *
-     * This functions finds the closest road node belonging to the road segment to a
-     * given pose, and returns it. This function only returns a valid reference when the 
-     * given pose coincides with the road segment. Otherwise, it throws an exception.
-     *
-     * \param world given pose to find the closest road node.
-     * \param distance the node distance between the projection of the given pose to 
-     *                 the lane link geometry of the closest road node, and the start 
-     *                 position of the node, following the corresponding lane link geometry. 
-     *                 If the closest road node does not exist this argument is set to the 
-     *                 maximum double value (std::numeric_limits<double>::max()) 
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the lane link geometry of the closest road node.
-     *  
-     * \return a constant reference to the closest node which can not be modified by the 
-     *         user.
-     */
-    const COpendriveRoadNode &get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
-    /** 
-     * \brief returns the index of the closest node to a given pose
-     *
-     * This functions finds the closest road node belonging to the road segment to a given 
-     * pose, and returns its index. This function only returns a valid index 
-     * when the given pose coincides with the road segment.
-     *
-     * \param world given pose to find the closest road node.
-     * \param distance the node distance between the projection of the given pose to 
-     *                 the lane link geometry of the closest road node, and the start 
-     *                 position of the node, following the corresponding lane link geometry. 
-     *                 If the closest road node does not exist this argument is set to the 
-     *                 maximum double value (std::numeric_limits<double>::max()) 
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the lane link geometry of the closest road node.
-     *  
-     * \return the index of the closest node or -1 if there is no node close to the given 
-     *         pose.
-     */
-    unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
-    /** 
-     * \brief returns the closest lane to a given pose
-     *
-     * This functions finds the closest lane belonging to the road segment to a
-     * given pose, and returns it. This function only returns a valid reference when the 
-     * given pose coincides with the road segment. Otherwise, it throws an exception.
-     *
-     * \param world given pose to find the closest lane.
-     * \param distance the lane distance between the projection of the given pose to 
-     *                 the lane geometry, and the start position of the lane, following 
-     *                 its geometry. If the closest lane does not exist this argument 
-     *                 is set to the maximum double value (std::numeric_limits<double>::max()) 
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the lane geometry.
-     *  
-     * \return a constant reference to the closest lane which can not be modified by the 
-     *         user.
-     */
-    const COpendriveLane &get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
-    /** 
-     * \brief returns the id of the closest lane to a given pose
-     *
-     * This functions finds the closest lane belonging to the road segment to a given 
-     * pose, and returns its id. This function only returns a valid id 
-     * when the given pose coincides with the road segment.
-     *
-     * \param world given pose to find the closest lane.
-     * \param distance the lane distance between the projection of the given pose to 
-     *                 the lane geometry, and the start position of the lane, following 
-     *                 its geometry. If the closest lane does not exist this argument 
-     *                 is set to the maximum double value (std::numeric_limits<double>::max()) 
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the lane geometry.
-     *  
-     * \return the id of the closest node or 0 if there is no lane close to the given 
-     *         pose.
-     */
-    int get_closest_lane_id(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
-    /**
-     * \brief returns the closest pose to a given pose
-     *
-     * This functions finds the closest pose (minimum distance) on the road segment geometry 
-     * to a given pose. The closest pose only exist if the imaginary line between the 
-     * given pose and its projection into the road segment is orthogonal to the segment geometry. 
-     * Otherwise, it is considered that the given pose does not coincide with the road segment.
-     *
-     * \param world given pose to find the closest pose.
-     * \param closest_pose the closest pose to the center lane of the road segment if the 
-     *                     given pose coincides with it. Otherwise the data on this 
-     *                     parameters is not valid.
-     * \param angle_tol maximum heading difference between the given pose and the closest pose
-     *                  on the road segment geometry.
-     *
-     * \return the road segment distance between the projection of the given pose to the 
-     *         center lane  geometry of the road segment, following its geometry. If the 
-     *         closest pose does not exist, the maximum double value 
-     *         (std::numeric_limits<double>::max()) will be returned.
-     */
-    double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
-    /**
-     * \brief returns a reference to the next road segments.
-     *
-     * This function returns a constant reference to the set of road segments that will follow
-     * this road segment, depending on the road side of interest. 
-     *
-     * \param input_side an integer indicating the road side of interest of the current 
-     *                   road segment. It is negative for the right side and positive for
-     *                   the left one (similar to the lane id criteria).
-     * \param output_sides output parameter that indicates the side of the next road segments
-     *                     that connects with the side of interest of the current one. It 
-     *                     is negative for the right side and positive for the left one. If
-     *                     there are no next road segments, this vector will be empty.
-     *
-     * \return a vector with the constant references of the next road segments. If there 
-     *         are no next road segments, this vector will be empty.
-     */
-    std::vector<const COpendriveRoadSegment *> get_next_segments(int input_side,std::vector<int> &output_sides) const;
-    /**
-     * \brief returns a reference to the previous road segments.
-     *
-     * This function returns a constant reference to the set of road segments that preceed
-     * this road segment, depending on the road side of interest. 
-     *
-     * \param input_side an integer indicating the road side of interest of the current 
-     *                   road segment. It is negative for the right side and positive for
-     *                   the left one (similar to the lane id criteria).
-     * \param output_sides output parameter that indicates the side of the previous road segments
-     *                     that connects with the side of interest of the current one. It 
-     *                     is negative for the right side and positive for the left one. If
-     *                     there are no previous road segments, this vector will be empty.
-     *
-     * \return a vector with the constant references of the previous road segments. If there 
-     *         are no previous road segments, this vector will be empty.
-     */
-    std::vector<const COpendriveRoadSegment *> get_prev_segments(int input_side,std::vector<int> &output_sides) const;
-    /**
-     * \brief Returns the road segment side of a given pose
-     *
-     * This function returns the road segment side of a given pose. If the given pose
-     * does not coincide with the road segment, an exception will be thrown.
-     *
-     * \return an integer indicating the road side of the given pose. It is negative for 
-     *         the right side and positive for the left one (similar to the lane id criteria).
-     */
-    int get_pose_side(TOpendriveWorldPose &pose) const;
-    /** 
-     * \brief overloaded stream operator
-     * 
-     * This functions streams human readable information about the road segment. This information
-     * includes:
-     *  * The name of the connecting road segments.
-     *  * The road nodes information.
-     *  * The left lanes information.
-     *  * The right lanes information.
-     *  * The road signals information.
-     *  * The road objects information.
-     *
-     */
-    friend std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment);
-    /**
-     * \brief Destructor 
-     */
-    ~COpendriveRoadSegment();
-};
-
-#endif
diff --git a/include/osm/osm_common.h b/include/osm/osm_common.h
new file mode 100644
index 0000000000000000000000000000000000000000..843ffcc27e1c43f141539c0016cecb165f24d800
--- /dev/null
+++ b/include/osm/osm_common.h
@@ -0,0 +1,15 @@
+#ifndef _OSM_COMMON_H
+#define _OSM_COMMON_H
+
+#define DEFAULT_LANE_WIDTH   4.0
+#define DEFAULT_MIN_RADIUS   5.0
+#define MIN_ROAD_LENGTH      0.5
+
+class COSMMap;
+class COSMNode;
+class COSMWay;
+class COSMJunction;
+class COSMRoadSegment;
+class COSMRestriction;
+
+#endif
diff --git a/include/osm/osm_junction.h b/include/osm/osm_junction.h
new file mode 100644
index 0000000000000000000000000000000000000000..3db8e36ad77ade8400ff927cca3366903d566b73
--- /dev/null
+++ b/include/osm/osm_junction.h
@@ -0,0 +1,50 @@
+#ifndef _OSM_JUNCTION_H
+#define _OSM_JUNCTION_H
+
+#include "osm/osm_common.h"
+#include "osm/osm_node.h"
+#include "osm/osm_road_segment.h"
+
+#include <Eigen/Dense>
+#include <vector>
+
+#include "junction.h"
+
+typedef struct{
+  COSMWay *in_way;
+  int in_lane_id;
+  COSMWay *out_way;
+  int out_lane_id;
+}TOSMLink;
+
+class COSMJunction
+{
+  friend class COSMMap;
+  friend class COSMNode;
+  friend class COSMWay;
+  friend class COSMRoadSegment;
+  private:
+    std::vector<COSMRoadSegment *> in_roads;
+    std::vector<COSMRoadSegment *> out_roads;
+    COSMNode *parent_node;
+    std::vector<std::vector<Eigen::MatrixXi>> connections;
+    std::vector<TOSMLink> links;
+  protected:
+    void create_FF_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix);
+    void create_FB_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lanes,Eigen::MatrixXi &matrix);
+    void create_BF_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int total_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix);
+    void create_BB_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int total_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lines,Eigen::MatrixXi &matrix);
+    void create_initial_connectivity(void);
+    void apply_node_restrictions(void);
+    void apply_way_restrictions(void);
+    void create_links(void);
+    bool add_link(TOSMLink &new_link);
+    void convert(CJunction **junction,osm_road_map_t &road_map);
+  public:
+    COSMJunction(COSMNode *node);
+    COSMNode &get_parent_node(void);
+    friend std::ostream& operator<<(std::ostream& out, COSMJunction &junction);
+    ~COSMJunction();
+};
+
+#endif
diff --git a/include/osm/osm_map.h b/include/osm/osm_map.h
new file mode 100644
index 0000000000000000000000000000000000000000..1bf2f79e51230b43ec870a35a97887dd7592747e
--- /dev/null
+++ b/include/osm/osm_map.h
@@ -0,0 +1,56 @@
+#ifndef _OSM_MAP_H
+#define _OSM_MAP_H
+
+#include "osm/osm_common.h"
+#include "osm/osm_way.h"
+#include "osm/osm_node.h"
+#include "osm/osm_restriction.h"
+
+#include "road_map.h"
+
+#include <osmium/handler.hpp>
+#include <osmium/io/header.hpp>
+#include <osmium/tags/tags_filter.hpp>
+
+class COSMMap : public osmium::handler::Handler
+{
+  friend class COSMWay;
+  friend class COSMNode;
+  friend class COSMRestriction;
+  private:
+    std::vector<COSMRoadSegment *> segments;
+    std::vector<COSMJunction *> junctions;
+    std::vector<COSMNode *> nodes;
+    std::vector<COSMWay *> ways;
+    std::vector<COSMRestriction *> restrictions;
+    osmium::TagsFilter way_filter;
+    osmium::TagsFilter relation_filter;
+    long int max_id;
+    int utm_zone;
+  protected:
+    void free(void);
+    void process_map(void);
+    void create_junctions(void);
+    void create_road_segments(void);
+    void delete_way(COSMWay *way);
+    void add_way(COSMWay *way);
+    COSMWay *get_way_by_id(long int id);
+    void delete_node(COSMNode *node);
+    void add_node(COSMNode *node);
+    COSMNode *get_node_by_id(long int id);
+    void add_restriction(COSMRestriction *restriction);
+    void normalize_locations(std::vector<osmium::Box> &boxes);
+  public:
+    COSMMap();
+    void set_utm_zone(int zone);
+    int get_utm_zone(void);
+    void load(const std::string &filename);
+    void node(const osmium::Node& node);
+    void way(const osmium::Way& way);
+    void relation(const osmium::Relation& relation);
+    void convert(CRoadMap &road);
+    friend std::ostream& operator<<(std::ostream& out, COSMMap &map);
+    ~COSMMap();
+};
+
+#endif
diff --git a/include/osm/osm_node.h b/include/osm/osm_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..532b13b5dd36e2e8de14431fe869b0ab59f541cd
--- /dev/null
+++ b/include/osm/osm_node.h
@@ -0,0 +1,64 @@
+#ifndef _OSM_NODE_H
+#define _OSM_NODE_H
+
+#include "osm/osm_common.h"
+#include "osm/osm_map.h"
+#include "osm/osm_way.h"
+#include "osm/osm_junction.h"
+#include "osm/osm_restriction.h"
+
+#include <vector>
+#include <map>
+
+#include <osmium/osm/node.hpp>
+
+typedef struct
+{
+  double x;
+  double y;
+}TLocation;
+
+class COSMNode
+{
+  friend class COSMMap;
+  friend class COSMWay;
+  friend class COSMJunction;
+  friend class COSMRestriction;
+  private:
+    long int id;
+    std::vector<COSMWay *> ways;
+    TLocation location;
+    COSMJunction *junction;
+    std::vector<COSMRestriction *> restrictions;
+    COSMMap *parent;
+  protected:
+    void set_parent(COSMMap *aprent);
+    void add_way(COSMWay *new_way);
+    void delete_way(COSMWay *way);
+    void update_way(COSMWay *old_way,COSMWay *new_way);
+    bool merge_ways(void);
+    void split_ways(void);
+    void remove_in_junction_nodes(void);
+    void add_restriction(COSMRestriction *new_restriction);
+    void delete_restriction(COSMRestriction *restriction);
+    void clear_restrictions(void);
+    void normalize_location(double center_x,double center_y);
+    COSMNode(const osmium::Node &node,COSMMap *parent);
+  public:
+    COSMNode(const COSMNode &object);
+    long int get_id(void);
+    unsigned int get_num_ways(void);
+    COSMWay &get_way_by_index(unsigned int index); 
+    COSMWay &get_way_by_id(long int id);
+    void get_location(double &x, double &y);
+    double compute_distance(COSMNode &node);
+    double compute_heading(COSMNode &node);
+    double compute_angle(COSMNode &node1,COSMNode &node2);
+    unsigned int get_num_restrictions(void);
+    COSMRestriction &get_restriction(unsigned int index);
+    COSMMap &get_parent(void);
+    friend std::ostream& operator<<(std::ostream& out,COSMNode &node);
+    ~COSMNode();
+};
+
+#endif
diff --git a/include/osm/osm_restriction.h b/include/osm/osm_restriction.h
new file mode 100644
index 0000000000000000000000000000000000000000..88e5a96fac0e5fc42dadd3738d88f591452449c7
--- /dev/null
+++ b/include/osm/osm_restriction.h
@@ -0,0 +1,45 @@
+#ifndef _OSM_RESTRICTION_H
+#define _OSM_RESTRICTION_H
+
+#include "osm/osm_map.h"
+#include "osm/osm_node.h"
+#include "osm/osm_way.h"
+
+#include <osmium/osm/relation.hpp>
+
+typedef enum {RESTRICTION_NONE,RESTRICTION_NO,RESTRICTION_ONLY} restriction_action_t;
+typedef enum {RESTRICTION_LEFT_TURN,RESTRICTION_RIGHT_TURN,RESTRICTION_U_TURN,RESTRICTION_STRAIGHT_ON} restriction_type_t;
+
+class COSMRestriction
+{
+  friend class COSMMap;
+  friend class COSMNode;
+  friend class COSMWay;
+  private:
+    COSMMap *parent;
+    long int id;
+    COSMWay *from;
+    COSMWay *to;
+    COSMNode *via;
+    restriction_action_t action;
+    restriction_type_t type;
+  protected: 
+    void set_parent(COSMMap *parent);
+    void update_to_way(COSMWay *old_way,COSMWay *new_way);
+    void update_from_way(COSMWay *old_way,COSMWay *new_way);
+    COSMRestriction(COSMWay *from,COSMNode *via,COSMWay *to,restriction_action_t action, restriction_type_t type,COSMMap *parent);
+    COSMRestriction(const osmium::Relation &relation,COSMMap *parent);
+    COSMRestriction(const COSMRestriction &object);
+  public:
+    COSMRestriction();
+    long int get_id(void);
+    COSMWay &get_from_way(void);
+    COSMWay &get_to_way(void);
+    COSMNode &get_via_node(void);
+    restriction_action_t get_action(void);
+    restriction_type_t get_type(void);
+    COSMMap &get_parent(void);
+    ~COSMRestriction();
+};
+
+#endif
diff --git a/include/osm/osm_road_segment.h b/include/osm/osm_road_segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..14b0205e71b71557eaf214432a05c54717a19568
--- /dev/null
+++ b/include/osm/osm_road_segment.h
@@ -0,0 +1,42 @@
+#ifndef _OSM_ROAD_SEGMENT_H
+#define _OSM_ROAD_SEGMENT_H
+
+#include "osm/osm_common.h"
+#include "osm/osm_way.h"
+#include "osm/osm_junction.h"
+
+#include "road.h"
+
+#include <iostream>
+
+
+class COSMRoadSegment
+{
+  friend class COSMJunction;
+  friend class COSMMap;
+  friend class COSMWay;
+  friend class CRoad;
+  private:
+    COSMWay *parent_way;
+    COSMJunction *start_junction;
+    COSMJunction *end_junction;
+    double start_distance;
+    double original_start_distance;
+    double end_distance;
+    double original_end_distance;
+  protected:
+    void set_start_distance(double dist);
+    void set_end_distance(double dist);
+    bool generate_fwd_geometry(CRoad *road,double resolution);
+    bool generate_bwd_geometry(CRoad *road,double resolution);
+    void convert(CRoad **left_road,CRoad **right_road,double resolution);
+  public:
+    COSMRoadSegment(COSMWay *way);
+    COSMWay &get_parent_way(void);
+    double get_start_distance(void);
+    double get_end_distance(void);
+    friend std::ostream& operator<<(std::ostream& out, COSMRoadSegment &segment);
+    ~COSMRoadSegment();
+};
+
+#endif
diff --git a/include/osm/osm_way.h b/include/osm/osm_way.h
new file mode 100644
index 0000000000000000000000000000000000000000..3e3bf98904ff9531532a7e7246bac81e1b159c2b
--- /dev/null
+++ b/include/osm/osm_way.h
@@ -0,0 +1,90 @@
+#ifndef _OSM_WAY_H
+#define _OSM_WAY_H
+
+#include "osm/osm_common.h"
+#include "osm/osm_map.h"
+#include "osm/osm_node.h"
+#include "osm/osm_road_segment.h"
+#include "osm/osm_restriction.h"
+
+#include <map>
+#include <vector>
+
+#include <osmium/osm/way.hpp>
+
+#define LAST_SEGMENT -1
+#define FIRST_SEGMENT 0
+
+typedef enum {NO_RESTRICTION=0x00,RESTRICTION_THROUGH=0x01,RESTRICTION_LEFT=0x02,RESTRICTION_RIGHT=0x04} lane_restructions_t;
+
+class COSMWay
+{
+  friend class COSMMap;
+  friend class COSMNode;
+  friend class COSMJunction;
+  friend class COSMRoadSegment;
+  friend class COSMRestriction;
+  private:
+    long int id;
+    std::string name;
+    std::vector<COSMNode *> nodes;
+    unsigned int num_forward_lanes;
+    std::vector<lane_restructions_t> forward_lanes;
+    unsigned int num_backward_lanes;
+    std::vector<lane_restructions_t> backward_lanes;
+    double lane_width;
+    COSMRoadSegment *road_segment;
+    COSMMap *parent;
+    std::vector<COSMRestriction *> restrictions;
+  protected:
+    void set_parent(COSMMap *parent);
+    void add_node(COSMNode *new_node);
+    void delete_node(COSMNode *node);
+    void update_node(COSMNode *old_node,COSMNode *new_node);
+    bool is_node_first(long int id);
+    bool is_node_last(long int id);
+    bool is_node_left(COSMNode &node,bool forward=true,double tol=0.5);
+    bool is_node_right(COSMNode &node,bool forward=true,double tol=0.5);
+    void remove_straight_nodes(void);
+    void add_restriction(COSMRestriction *new_restriction);
+    void delete_restriction(COSMRestriction *restriction);
+    void delete_restriction(long int id);
+    void update_restriction(COSMWay *old_way,COSMWay *new_way);
+    void clear_restrictions(void);    
+    COSMWay *split(long int node_id);
+    void merge(COSMWay *way);
+    COSMWay(const osmium::Way &way,COSMMap *parent);
+    bool is_not_connected(void);
+    bool load_turn_lane_tag(const osmium::Way &way,const std::string &tag,std::vector<lane_restructions_t> &restrictions,unsigned int max_lanes);
+    void load_lane_restrictions(const osmium::Way &way);
+  public:
+    COSMWay(const COSMWay &object);
+    long int get_id(void);
+    std::string get_name(void);
+    unsigned int get_num_nodes(void);
+    COSMNode &get_node_by_index(unsigned int index);
+    COSMNode &get_next_node_by_index(unsigned int index);
+    COSMNode &get_prev_node_by_index(unsigned int index);
+    COSMNode &get_node_by_id(long int id);
+    COSMNode &get_next_node_by_id(long int id);
+    COSMNode &get_prev_node_by_id(long int id);
+    COSMNode &get_closest_node(COSMNode &node);
+    unsigned int get_node_multiplicity(long int id);
+    bool has_node(long int id);
+    unsigned int get_num_segments(void);
+    COSMNode &get_segment_start_node(unsigned int index);
+    COSMNode &get_segment_end_node(unsigned int index);
+    unsigned int get_closest_segment(COSMNode &node);
+    unsigned int get_num_lanes(void);
+    unsigned int get_num_forward_lanes(void);
+    unsigned int get_num_backward_lanes(void);
+    bool is_one_way(void);
+    lane_restructions_t get_lane_restriction(unsigned int index);
+    double get_lane_width(void);
+    COSMRoadSegment &get_road_segment(void);
+    COSMMap &get_parent(void);
+    friend std::ostream& operator<<(std::ostream& out, COSMWay &way);
+    ~COSMWay();
+};
+
+#endif
diff --git a/include/road.h b/include/road.h
new file mode 100644
index 0000000000000000000000000000000000000000..3af83a6a1a6df723a7744e8b117c3ee22c00a878
--- /dev/null
+++ b/include/road.h
@@ -0,0 +1,81 @@
+#ifndef _ROAD_H
+#define _ROAD_H
+
+#include "road_map.h"
+#include "junction.h"
+#include "road_segment.h"
+#include "opendrive/opendrive_road_segment.h"
+
+class CRoad
+{
+  friend class CJunction;
+  friend class CRoadMap;
+  friend class CRoadSegment;
+  friend class COpendriveRoadSegment;
+  friend class COSMRoadSegment;
+  private:
+    unsigned int id;
+    double resolution;
+    /* connectivity */
+    CRoadMap *parent_roadmap;
+    CJunction *prev_junction; 
+    CJunction *next_junction;
+    CRoad *opposite_direction_road;
+    /* geometry */
+    std::vector<CRoadSegment *> segments;
+    TPoint start_point;
+    unsigned int num_lanes;
+    double lane_width;
+    double lane_speed;
+  protected:
+    CRoad(unsigned int id);
+    static unsigned int get_index_by_id(const std::vector<CRoad *> &roads,unsigned int id);
+    void set_id(unsigned int id);
+    /* connectivity */
+    void set_parent_roadmap(CRoadMap *roadmap);
+    void set_prev_junction(CJunction *junction);
+    void set_next_junction(CJunction *junction);
+    void set_opposite_direction_road(CRoad *road);
+    /* geometry */
+    CRoadSegment *get_first_segment(void);
+    CRoadSegment *get_last_segment(void);
+    CRoadSegment *get_segment_by_index(unsigned int index);
+    void merge(CRoad *road);
+  public:
+    CRoad();
+    void set_resolution(double resolution);
+    double get_resolution(void);
+    CRoadMap &get_parent_roadmap(void);
+    unsigned int get_id(void);
+    /* connectivity */
+    bool exist_prev_junction(void);
+    CJunction &get_prev_junction(void);
+    bool exist_next_junction(void);
+    CJunction &get_next_junction(void);
+    bool has_opposite_direction_road(void);
+    CRoad &get_opposite_direction_road(void);
+    /* geometry */
+    void set_num_lanes(unsigned int num);
+    unsigned int get_num_lanes(void);
+    void set_lane_width(double width);
+    double get_lane_width(void);
+    void set_lane_speed(double speed);
+    double get_lane_speed(void);
+    unsigned int get_num_segments(void);
+    CRoadSegment &get_segment_by_id(unsigned int id);
+    void set_start_point(TPoint &start_point);
+    void set_start_point(double x,double y,double heading, double curvature);
+    void add_segment(TPoint &end_point);
+    void add_segment(double x,double y,double heading, double curvature);
+    void remove_segment_by_index(unsigned int index);
+    bool get_point_at(double distance, double lateral_offset,TPoint &point);
+    bool get_closest_point(TPoint &target_point,TPoint &closest_point,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max());
+
+    bool get_closest_segment_id(TPoint &point,unsigned int &segment_id,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max());
+    bool get_segment_id_at(double distance,unsigned int &segment_id);
+    void get_segment_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw);
+    void get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw);
+    ~CRoad();
+};
+
+#endif
diff --git a/include/road_map.h b/include/road_map.h
new file mode 100644
index 0000000000000000000000000000000000000000..f7d8138deb7049858a20aef69ffaa164ea611537
--- /dev/null
+++ b/include/road_map.h
@@ -0,0 +1,87 @@
+#ifndef _ROAD_MAP_H
+#define _ROAD_MAP_H
+
+#include "road.h"
+#include "junction.h"
+#include "road_segment.h"
+#include "common.h"
+
+#include "opendrive/opendrive_road_segment.h"
+#include "opendrive/opendrive_junction.h"
+
+#include <Eigen/Dense>
+#include <vector>
+
+class CRoad;
+class CJunction;
+class CRoadSegment;
+
+typedef struct{
+  unsigned int segment_id;
+  unsigned int lane_id;
+}TLaneID;
+
+class CRoadMap
+{
+  friend class CRoad;
+  friend class CJunction;
+  friend class CRoadSegment;
+  friend class COpendriveRoadSegment;
+  friend class COpendriveJunction;
+  private:
+    unsigned int next_segment_id;
+    unsigned int next_road_id;
+    unsigned int next_junction_id;
+    /* connectivity */
+    std::vector<CRoad *> roads;
+    std::vector<CJunction *> junctions;
+    double resolution;
+  protected:
+    void free(void);
+    unsigned get_next_segment_id(void);
+    unsigned get_next_road_id(void);
+    unsigned get_next_junction_id(void);
+    CRoad *get_road_by_index(unsigned int index);
+    CJunction *get_junction_by_index(unsigned int index);
+    void get_segment_by_id(unsigned int id,CRoadSegment **segment);
+    void remove_road_by_index(unsigned int index);
+    void set_opposite_direction_roads_by_index(unsigned int right_road,unsigned int left_road);
+    void remove_junction_by_index(unsigned int index);
+    void merge_roads(opendrive_road_map_t &road_map);
+  public:
+    CRoadMap();
+    void load_opendrive(const std::string &filename);
+    void save_opendrive(const std::string &filename);
+    void load_osm(const std::string &filename);
+    void set_resolution(double resolution);
+    double get_resolution(void);
+    std::vector<unsigned int> get_path_sub_roadmap(std::vector<unsigned int> &segment_ids,CRoadMap &new_road_map);
+    double get_path_length(std::vector<unsigned int> &segment_ids);
+    /* connectivity */
+    unsigned int get_num_roads(void);
+    CRoad &get_road_by_id(unsigned int id);
+    unsigned int get_num_junctions(void);
+    CJunction &get_junction_by_id(unsigned int id);
+    unsigned int get_num_segments(void);
+    CRoadSegment &get_segment_by_id(unsigned int id);
+    void add_road(CRoad *road);
+    bool has_road(unsigned int id);
+    void remove_road_by_id(unsigned int id);
+    void set_opposite_direction_roads_by_id(unsigned int right_road,unsigned int left_road);
+    void add_junction(CJunction *junction);
+    bool has_junction(unsigned int id);
+    void remove_junction_by_id(unsigned int id);
+    void get_road_connectivity(Eigen::MatrixXi &connectivity,std::vector<unsigned int> &id_map);
+    void get_segment_connectivity(Eigen::MatrixXi &connectivity,std::vector<unsigned int> &id_map);
+    void get_lane_connectivity(Eigen::MatrixXi &connectivity,std::vector<TLaneID> &id_map);
+    /* geometry */
+    unsigned int get_max_num_lanes(void);
+    bool get_closest_segment_id(TPoint &point,unsigned int &segment_id,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max());
+    bool get_closest_segment_ids(TPoint &point,std::vector<unsigned int >&segment_ids,std::vector<double >&distances,std::vector<double >&lateral_offsets,double angle_tol=std::numeric_limits<double>::max(),double offset_tol=std::numeric_limits<double>::max());
+    void get_segment_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw);
+    void get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw);
+
+    ~CRoadMap();
+};
+
+#endif
diff --git a/include/road_segment.h b/include/road_segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..72310e13aed80b3d469d89de93e2fa64109dbf41
--- /dev/null
+++ b/include/road_segment.h
@@ -0,0 +1,74 @@
+#ifndef _ROAD_SEGMENT_H
+#define _ROAD_SEGMENT_H
+
+#include "g2_spline.h"
+#include "road.h"
+#include "junction.h"
+
+#include <Eigen/Dense>
+
+class CRoad;
+class CJunction;
+
+class CRoadSegment
+{
+  friend class CRoad;
+  friend class CJunction;
+  friend class CRoadMap;
+  private:
+    unsigned int id;
+    double resolution;
+    CRoad *parent_road;
+    CJunction *parent_junction;
+    std::vector<CRoadSegment *> prev_segments;
+    std::vector<CRoadSegment *> next_segments;
+    Eigen::MatrixXd connectivity;
+    TPoint start_point;
+    TPoint end_point;
+    CG2Spline spline;
+  protected:
+    static unsigned int get_index_by_id(const std::vector<CRoadSegment *> &segments,unsigned int id);
+    void set_id(unsigned int id);
+    void set_parent_road(CRoad *road);
+    void set_parent_junction(CJunction *junction);
+    void add_prev_segment(CRoadSegment *segment);
+    bool has_prev_segment(CRoadSegment *segment);
+    void remove_prev_segment(CRoadSegment *segment);
+    void add_next_segment(CRoadSegment *segment);
+    bool has_next_segment(CRoadSegment *segment);
+    void remove_next_segment(CRoadSegment *segment);
+  public:
+    CRoadSegment(unsigned int lanes_in,unsigned int lanes_out);
+    unsigned int get_id(void);
+    void set_resolution(double resolution);
+    double get_resolution(void);
+    bool has_parent_road(void);
+    CRoad &get_parent_road(void);
+    bool has_parent_junction(void);
+    CJunction &get_parent_junction(void);
+    unsigned int get_num_prev_segments(void);
+    CRoadSegment &get_prev_segment_by_index(unsigned int index);
+    unsigned int get_num_next_segments(void);
+    CRoadSegment &get_next_segment_by_index(unsigned int index);
+    unsigned int get_num_lanes_in(void);
+    unsigned int get_num_lanes_out(void);
+    double get_lane_width(void);
+    double get_lane_speed(void);
+    unsigned int get_lane(double lateral_offset);
+    void set_geometry(TPoint &start_point,TPoint &end_point);
+    void get_start_point(TPoint &point);
+    void get_end_point(TPoint &point);
+    void set_full_connectivity(void);
+    void link_lanes(unsigned int lane1,unsigned int lane2);
+    void unlink_lanes(unsigned int lane1,unsigned int lane2);
+    bool are_lanes_linked(unsigned int lane1,unsigned int lane2);
+    void get_connectivity_matrix(Eigen::MatrixXd &connectivity);
+    bool is_input_lane_connected(unsigned int lane);
+    double get_length(void);
+    bool get_point_at(double distance, double lateral_offset,TPoint &point);
+    bool get_closest_point(TPoint &target_point,TPoint &closest_point,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max());
+    void get_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double lateral_offset=0.0,double start_length=0.0, double end_length=std::numeric_limits<double>::max());
+    ~CRoadSegment();
+};
+
+#endif
diff --git a/include/vel_profile.h b/include/vel_profile.h
new file mode 100644
index 0000000000000000000000000000000000000000..abde16e31cfe541b12a3fbb84130a8a8c16322df
--- /dev/null
+++ b/include/vel_profile.h
@@ -0,0 +1,44 @@
+#ifndef _VEL_PROFILE_H
+#define _VEL_PROFILE_H
+
+#include <vector>
+#include <ostream>
+
+class CVelProfile
+{
+  protected:
+    double start_vel;
+    double end_vel;
+    double max_vel;
+    double start_acc;
+    double end_acc;
+    double max_acc;
+    double time;
+    double length;
+    bool generated;
+  public:
+    CVelProfile();
+    CVelProfile(double start_vel,double end_vel,double max_vel,double start_acc,double end_acc,double max_acc);
+    void set_start_vel(double vel);
+    double get_start_vel(void);
+    void set_end_vel(double vel);
+    double get_end_vel(void);
+    void set_max_vel(double vel);
+    double get_max_vel(void);
+    void set_start_acc(double acc);
+    double get_start_acc(void);
+    void set_end_acc(double acc);
+    double get_end_acc(void);
+    void set_max_acc(double acc);
+    double get_max_acc(void);
+    double get_time(void);
+    double get_length(void);
+    // evaluation functions
+    virtual void evaluate_at_time(double time,double &vel, double &acc,double &len,double start_len=0.0)=0;
+    virtual void evaluate_at_length(double length,double &vel, double &acc,double &time)=0;
+    virtual void evaluate_all(double resolution,std::vector<double> &vel,std::vector<double> &acc,std::vector<double> &len,double start_len=0.0)=0;
+    friend std::ostream& operator<< (std::ostream& out, const CVelProfile &profile);
+    virtual ~CVelProfile();
+};
+
+#endif
diff --git a/include/vel_spline.h b/include/vel_spline.h
new file mode 100644
index 0000000000000000000000000000000000000000..983c4f563d6ca481871c30682cf49e8b3aeecac6
--- /dev/null
+++ b/include/vel_spline.h
@@ -0,0 +1,38 @@
+#ifndef _VEL_SPLINE_H
+#define _VEL_SPLINE_H
+
+#include "vel_profile.h"
+#include "gradient.h"
+
+#define DEFAULT_MAX_ACC      1.5
+
+class CVelSpline : public CVelProfile
+{
+  private:
+    double coeff[4];
+    // optimization attributes
+    CGradient time_grad;
+    double target_length;
+  protected:
+    double length_pow2(double time);
+    double length_pow2_der(double time);
+    void set_target_length(double length);
+    void compute_max_vel(void);
+  public:
+    CVelSpline();
+    CVelSpline(double start_vel,double end_vel,double max_vel,double start_acc,double end_acc,double max_acc);
+    // generation functions
+    void generate_max_acc(double max_acc);
+    bool generate_max_length(double &max_len);
+    void generate_max_acc(double max_acc,double start_vel,double start_acc,double end_vel,double end_acc=0.0);
+    bool generate_max_length(double &max_len,double start_vel,double start_acc,double end_vel,double end_acc=0.0);
+    void generate_constant_vel(double vel, double length);
+    void update_max_start_vel(double max_acc,double length,double max_end_vel);
+    // evaluation functions
+    void evaluate_at_time(double time,double &vel, double &acc,double &len,double start_len=0.0);
+    void evaluate_at_length(double length,double &vel, double &acc,double &time);
+    void evaluate_all(double resolution,std::vector<double> &vel,std::vector<double> &acc,std::vector<double> &len,double start_len=0.0);
+    ~CVelSpline();
+};
+
+#endif
diff --git a/include/vel_trapezoid.h b/include/vel_trapezoid.h
new file mode 100644
index 0000000000000000000000000000000000000000..6a477ab89da345c85981164a61005ea9675d6b56
--- /dev/null
+++ b/include/vel_trapezoid.h
@@ -0,0 +1,25 @@
+#ifndef _VEL_TRAPEZOID_H
+#define _VEL_TRAPEZOID_H
+
+#include "vel_profile.h"
+
+class CVelTrapezoid : public CVelProfile
+{
+  private:
+    double acc_time;
+    double acc_length;
+    double dec_time;
+    double dec_length;
+  protected:
+    void generate(double start_vel,double end_vel,double max_vel, double max_acc,double length);
+  public:
+    CVelTrapezoid();
+    CVelTrapezoid(double start_vel,double end_vel,double max_vel,double start_acc,double end_acc,double max_acc);
+    bool generate(double start_vel,double end_vel,double max_vel, double max_acc,double length,double &new_start_vel,double &new_max_vel);
+    void evaluate_at_time(double time,double &vel, double &acc,double &len,double start_len=0.0);
+    void evaluate_at_length(double length,double &vel, double &acc,double &time);
+    void evaluate_all(double resolution,std::vector<double> &vel,std::vector<double> &acc,std::vector<double> &len,double start_len=0.0);
+    ~CVelTrapezoid();
+};
+
+#endif
diff --git a/libosmium/FindGem.cmake b/libosmium/FindGem.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..9927ea81438c15d247c1ed07b29d1dfb6feb9ccf
--- /dev/null
+++ b/libosmium/FindGem.cmake
@@ -0,0 +1,155 @@
+# Author thomas.roehr@dfki.de
+#
+# Version 0.31 2017-09-15
+#       - find gem executable gem.cmd on Windows
+# Version 0.3 2013-07-02
+#       - rely on `gem content` to find library and header
+#       - introduce GEM_OS_PKG to allow search via pkgconfig
+# Version 0.2 2010-01-14
+#       - add support for searching for multiple gems
+# Version 0.1 2010-12-15
+# 	- support basic search functionality 
+#       - tested to find rice
+#
+# OUTPUT:
+#
+# GEM_INCLUDE_DIRS	After successful search contains the include directores
+#
+# GEM_LIBRARIES		After successful search contains the full path of each found library
+#
+#
+# Usage: 
+# set(GEM_DEBUG TRUE)
+# find_package(Gem COMPONENTS rice hoe)
+# include_directories(${GEM_INCLUDE_DIRS})
+# target_link_libraries(${GEM_LIBRARIES}
+#
+# in case pkg-config should be used to search for the os pkg, set GEM_OS_PKG, i.e.
+# set(GEM_OS_PKG TRUE)
+#
+# Check for how 'gem' should be called
+include(FindPackageHandleStandardArgs)
+find_program(GEM_EXECUTABLE
+    NAMES "gem.cmd" "gem${RUBY_VERSION_MAJOR}${RUBY_VERSION_MINOR}"
+        "gem${RUBY_VERSION_MAJOR}.${RUBY_VERSION_MINOR}"
+        "gem-${RUBY_VERSION_MAJOR}${RUBY_VERSION_MINOR}"
+        "gem-${RUBY_VERSION_MAJOR}.${RUBY_VERSION_MINOR}"
+        "gem${RUBY_VERSION_MAJOR}${RUBY_VERSION_MINOR}${RUBY_VERSION_PATCH}"
+        "gem${RUBY_VERSION_MAJOR}.${RUBY_VERSION_MINOR}.${RUBY_VERSION_PATCH}"
+        "gem-${RUBY_VERSION_MAJOR}${RUBY_VERSION_MINOR}${RUBY_VERSION_PATCH}"
+        "gem-${RUBY_VERSION_MAJOR}.${RUBY_VERSION_MINOR}.${RUBY_VERSION_PATCH}"
+        "gem")
+
+# Making backward compatible
+if(Gem_DEBUG)
+    set(GEM_DEBUG TRUE)
+endif()
+
+if(NOT GEM_EXECUTABLE)
+	MESSAGE(FATAL_ERROR "Could not find the gem executable - install 'gem' first")
+endif()
+
+if(NOT Gem_FIND_COMPONENTS)
+	MESSAGE(FATAL_ERROR "If searching for a Gem you have to provide COMPONENTS with the name of the gem")
+endif()
+
+foreach(Gem_NAME ${Gem_FIND_COMPONENTS})
+    set(GEM_${Gem_NAME}_FOUND TRUE)
+    list(APPEND components_found_vars GEM_${Gem_NAME}_FOUND)
+    # If the gem is installed as a gem
+    if(NOT GEM_OS_PKG)
+	    set(GEM_HOME ENV{GEM_HOME})
+
+        # Use `gem content <gem-name>` to extract current information about installed gems
+        # Store the information into ${GEM_LOCAL_INFO}
+        EXECUTE_PROCESS(COMMAND ${GEM_EXECUTABLE} content ${Gem_NAME}
+            RESULT_VARIABLE GEM_RUN_RESULT
+            OUTPUT_VARIABLE GEM_LOCAL_INFO)
+
+        if(GEM_RUN_RESULT STREQUAL "0")
+            list(APPEND FOUND_GEMS ${Gem_NAME})
+            set(_library_NAME_PATTERN lib${Gem_NAME}.a
+	        		   lib${Gem_NAME}.so
+	        		   lib${Gem_NAME}.dylib
+	        		   ${Gem_NAME}.a
+	        		   ${Gem_NAME}.so
+	        		   ${Gem_NAME}.dylib
+                       .*.a
+                       .*.so
+                       .*.dylib
+	        )
+
+            set(_header_SUFFIX_PATTERN
+                        .h
+                        .hh
+                        .hpp
+            )
+
+            # Create a list from the output results of the gem command
+            string(REPLACE "\n" ";" GEM_CONTENT_LIST "${GEM_LOCAL_INFO}")
+            foreach(_gem_CONTENT_PATH ${GEM_CONTENT_LIST})
+                
+                # Convert so that only '/' Unix path separator are being using
+                # needed to do proper regex matching
+                FILE(TO_CMAKE_PATH ${_gem_CONTENT_PATH} gem_CONTENT_PATH)
+
+                # Identify library -- checking for a library in the gems 'lib' (sub)directory
+                # Search for an existing library, but only within the gems folder
+                foreach(_library_NAME ${_library_NAME_PATTERN})
+                    STRING(REGEX MATCH ".*${Gem_NAME}.*/lib/.*${_library_NAME}$" GEM_PATH_INFO "${gem_CONTENT_PATH}")
+                    if(NOT "${GEM_PATH_INFO}" STREQUAL "")
+                        list(APPEND GEM_LIBRARIES ${GEM_PATH_INFO})
+                        break()
+                    endif()
+                endforeach()
+
+                # Identify headers
+                # Checking for available headers in an include directory
+                foreach(_header_PATTERN ${_header_SUFFIX_PATTERN})
+                    STRING(REGEX MATCH ".*${Gem_NAME}.*/include/.*${_header_PATTERN}$" GEM_PATH_INFO "${gem_CONTENT_PATH}")
+                    if(NOT "${GEM_PATH_INFO}" STREQUAL "")
+                        STRING(REGEX REPLACE "(.*${Gem_NAME}.*/include/).*${_header_PATTERN}$" "\\1" GEM_PATH_INFO "${gem_CONTENT_PATH}")
+                        list(APPEND GEM_INCLUDE_DIRS ${GEM_PATH_INFO})
+                        break()
+                    endif()
+                endforeach()
+            endforeach()
+        else()
+            set(GEM_${Gem_NAME}_FOUND FALSE)
+        endif()
+    else(NOT GEM_OS_PKG)
+        pkg_check_modules(GEM_PKG ${Gem_NAME})
+        set(GEM_${GEM_NAME}_FOUND GEM_PKG_FOUND)
+        set(GEM_INCLUDE_DIRS ${GEM_PKG_INCLUDE_DIRS})
+        set(GEM_LIBRARIES ${GEM_PKG_LIBRARIES} ${GEM_PKG_STATIC_LIBRARIES})
+        list(APPEND GEM_LIBRARIES ${GEM_PKG_LDFLAGS} ${GEM_PKG_STATIC_LDFLAGS})
+        list(APPEND GEM_LIBRARIES ${GEM_PKG_LDFLAGS_OTHER} ${GEM_PKG_STATIC_LDFLAGS_OTHER})
+
+        if(GEM_DEBUG)
+            message(STATUS "GEM_OS_PKG is defined")
+            message(STATUS "GEM_INCLUDE_DIRS ${GEM_INCLUDE_DIRS}")
+            message(STATUS "GEM_STATIC_LIBRARY_DIRS ${GEM_PKG_STATIC_LIBRARY_DIRS}")
+            message(STATUS "GEM_LIBRARY_DIRS ${GEM_PKG_STATIC_LIBRARY_DIRS}")
+            message(STATUS "GEM_STATIC_LIBRARIES ${GEM_PKG_STATIC_LIBRARIES}")
+            message(STATUS "GEM_LIBRARIES ${GEM_LIBRARIES}")
+        endif()
+    endif()
+
+    if(GEM_DEBUG)
+		message(STATUS "${Gem_NAME} library dir: ${GEM_LIBRARIES}")
+		message(STATUS "${Gem_NAME} include dir: ${GEM_INCLUDE_DIRS}")
+    endif()
+endforeach()
+
+# Compact the lists
+if(DEFINED GEM_LIBRARIES)
+    LIST(REMOVE_DUPLICATES GEM_LIBRARIES)
+endif()
+if(DEFINED GEM_INCLUDE_DIRS)
+    LIST(REMOVE_DUPLICATES GEM_INCLUDE_DIRS)
+endif()
+
+find_package_handle_standard_args(Gem
+    REQUIRED_VARS ${components_found_vars}
+    FAIL_MESSAGE "Could not find all required gems")
+
diff --git a/libosmium/FindLZ4.cmake b/libosmium/FindLZ4.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..8c94e3bcd1e5e6bd236d52e62ae35480947abefe
--- /dev/null
+++ b/libosmium/FindLZ4.cmake
@@ -0,0 +1,38 @@
+find_path(LZ4_INCLUDE_DIR
+  NAMES lz4.h
+  DOC "lz4 include directory")
+mark_as_advanced(LZ4_INCLUDE_DIR)
+find_library(LZ4_LIBRARY
+  NAMES lz4 liblz4
+  DOC "lz4 library")
+mark_as_advanced(LZ4_LIBRARY)
+
+if (LZ4_INCLUDE_DIR)
+  file(STRINGS "${LZ4_INCLUDE_DIR}/lz4.h" _lz4_version_lines
+    REGEX "#define[ \t]+LZ4_VERSION_(MAJOR|MINOR|RELEASE)")
+  string(REGEX REPLACE ".*LZ4_VERSION_MAJOR *\([0-9]*\).*" "\\1" _lz4_version_major "${_lz4_version_lines}")
+  string(REGEX REPLACE ".*LZ4_VERSION_MINOR *\([0-9]*\).*" "\\1" _lz4_version_minor "${_lz4_version_lines}")
+  string(REGEX REPLACE ".*LZ4_VERSION_RELEASE *\([0-9]*\).*" "\\1" _lz4_version_release "${_lz4_version_lines}")
+  set(LZ4_VERSION "${_lz4_version_major}.${_lz4_version_minor}.${_lz4_version_release}")
+  unset(_lz4_version_major)
+  unset(_lz4_version_minor)
+  unset(_lz4_version_release)
+  unset(_lz4_version_lines)
+endif ()
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(LZ4
+  REQUIRED_VARS LZ4_LIBRARY LZ4_INCLUDE_DIR
+  VERSION_VAR LZ4_VERSION)
+
+if (LZ4_FOUND)
+  set(LZ4_INCLUDE_DIRS "${LZ4_INCLUDE_DIR}")
+  set(LZ4_LIBRARIES "${LZ4_LIBRARY}")
+
+  if (NOT TARGET LZ4::LZ4)
+    add_library(LZ4::LZ4 UNKNOWN IMPORTED)
+    set_target_properties(LZ4::LZ4 PROPERTIES
+      IMPORTED_LOCATION "${LZ4_LIBRARY}"
+      INTERFACE_INCLUDE_DIRECTORIES "${LZ4_INCLUDE_DIR}")
+  endif ()
+endif ()
diff --git a/libosmium/FindOsmium.cmake b/libosmium/FindOsmium.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..651c09a72b61f99ce3e452d8605ae69476f14ff8
--- /dev/null
+++ b/libosmium/FindOsmium.cmake
@@ -0,0 +1,366 @@
+#----------------------------------------------------------------------
+#
+#  FindOsmium.cmake
+#
+#  Find the Libosmium headers and, optionally, several components needed
+#  for different Libosmium functions.
+#
+#----------------------------------------------------------------------
+#
+#  Usage:
+#
+#    Copy this file somewhere into your project directory, where cmake can
+#    find it. Usually this will be a directory called "cmake" which you can
+#    add to the CMake module search path with the following line in your
+#    CMakeLists.txt:
+#
+#      list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake")
+#
+#    Then add the following in your CMakeLists.txt:
+#
+#      find_package(Osmium [version] REQUIRED COMPONENTS <XXX>)
+#      include_directories(SYSTEM ${OSMIUM_INCLUDE_DIRS})
+#
+#    The version number is optional. If it is not set, any version of
+#    libosmium will do.
+#
+#    For the <XXX> substitute a space separated list of one or more of the
+#    following components:
+#
+#      pbf        - include libraries needed for PBF input and output
+#      xml        - include libraries needed for XML input and output
+#      io         - include libraries needed for any type of input/output
+#      geos       - include if you want to use any of the GEOS functions
+#      gdal       - include if you want to use any of the OGR functions
+#      proj       - include if you want to use any of the Proj.4 functions
+#      sparsehash - include if you use the sparsehash index (deprecated!)
+#      lz4        - include support for LZ4 compression of PBF files
+#
+#    You can check for success with something like this:
+#
+#      if(NOT OSMIUM_FOUND)
+#          message(WARNING "Libosmium not found!\n")
+#      endif()
+#
+#----------------------------------------------------------------------
+#
+#  Variables:
+#
+#    OSMIUM_FOUND         - True if Osmium found.
+#    OSMIUM_INCLUDE_DIRS  - Where to find include files.
+#    OSMIUM_XML_LIBRARIES - Libraries needed for XML I/O.
+#    OSMIUM_PBF_LIBRARIES - Libraries needed for PBF I/O.
+#    OSMIUM_IO_LIBRARIES  - Libraries needed for XML or PBF I/O.
+#    OSMIUM_LIBRARIES     - All libraries Osmium uses somewhere.
+#
+#----------------------------------------------------------------------
+
+# This is the list of directories where we look for osmium includes.
+set(_osmium_include_path
+        ../libosmium
+        ~/Library/Frameworks
+        /Library/Frameworks
+        /opt/local # DarwinPorts
+        /opt
+)
+
+# Look for the header file.
+find_path(OSMIUM_INCLUDE_DIR osmium/version.hpp
+    PATH_SUFFIXES include
+    PATHS ${_osmium_include_path}
+)
+
+# Check libosmium version number
+if(Osmium_FIND_VERSION)
+    if(NOT EXISTS "${OSMIUM_INCLUDE_DIR}/osmium/version.hpp")
+        message(FATAL_ERROR "Missing ${OSMIUM_INCLUDE_DIR}/osmium/version.hpp. Either your libosmium version is too old, or libosmium wasn't found in the place you said.")
+    endif()
+    file(STRINGS "${OSMIUM_INCLUDE_DIR}/osmium/version.hpp" _libosmium_version_define REGEX "#define LIBOSMIUM_VERSION_STRING")
+    if("${_libosmium_version_define}" MATCHES "#define LIBOSMIUM_VERSION_STRING \"([0-9.]+)\"")
+        set(_libosmium_version "${CMAKE_MATCH_1}")
+    else()
+        set(_libosmium_version "unknown")
+    endif()
+endif()
+
+set(OSMIUM_INCLUDE_DIRS "${OSMIUM_INCLUDE_DIR}")
+
+#----------------------------------------------------------------------
+#
+#  Check for optional components
+#
+#----------------------------------------------------------------------
+if(Osmium_FIND_COMPONENTS)
+    foreach(_component ${Osmium_FIND_COMPONENTS})
+        string(TOUPPER ${_component} _component_uppercase)
+        set(Osmium_USE_${_component_uppercase} TRUE)
+    endforeach()
+endif()
+
+#----------------------------------------------------------------------
+# Component 'io' is an alias for 'pbf' and 'xml'
+if(Osmium_USE_IO)
+    set(Osmium_USE_PBF TRUE)
+    set(Osmium_USE_XML TRUE)
+endif()
+
+#----------------------------------------------------------------------
+# Component 'ogr' is an alias for 'gdal'
+if(Osmium_USE_OGR)
+    set(Osmium_USE_GDAL TRUE)
+endif()
+
+#----------------------------------------------------------------------
+# Component 'pbf'
+if(Osmium_USE_PBF)
+    find_package(ZLIB)
+    find_package(Threads)
+    find_package(Protozero 1.6.3)
+
+    if(Osmium_USE_LZ4)
+        find_package(LZ4 REQUIRED)
+        add_definitions(-DOSMIUM_WITH_LZ4)
+    endif()
+
+    list(APPEND OSMIUM_EXTRA_FIND_VARS ZLIB_FOUND Threads_FOUND PROTOZERO_INCLUDE_DIR)
+    if(ZLIB_FOUND AND Threads_FOUND AND PROTOZERO_FOUND)
+        list(APPEND OSMIUM_PBF_LIBRARIES
+            ${ZLIB_LIBRARIES}
+            ${LZ4_LIBRARIES}
+            ${CMAKE_THREAD_LIBS_INIT}
+        )
+        list(APPEND OSMIUM_INCLUDE_DIRS
+            ${ZLIB_INCLUDE_DIR}
+            ${LZ4_INCLUDE_DIRS}
+            ${PROTOZERO_INCLUDE_DIR}
+        )
+    else()
+        message(WARNING "Osmium: Can not find some libraries for PBF input/output, please install them or configure the paths.")
+    endif()
+endif()
+
+#----------------------------------------------------------------------
+# Component 'xml'
+if(Osmium_USE_XML)
+    find_package(EXPAT)
+    find_package(BZip2)
+    find_package(ZLIB)
+    find_package(Threads)
+
+    list(APPEND OSMIUM_EXTRA_FIND_VARS EXPAT_FOUND BZIP2_FOUND ZLIB_FOUND Threads_FOUND)
+    if(EXPAT_FOUND AND BZIP2_FOUND AND ZLIB_FOUND AND Threads_FOUND)
+        list(APPEND OSMIUM_XML_LIBRARIES
+            ${EXPAT_LIBRARIES}
+            ${BZIP2_LIBRARIES}
+            ${ZLIB_LIBRARIES}
+            ${CMAKE_THREAD_LIBS_INIT}
+        )
+        list(APPEND OSMIUM_INCLUDE_DIRS
+            ${EXPAT_INCLUDE_DIR}
+            ${BZIP2_INCLUDE_DIR}
+            ${ZLIB_INCLUDE_DIR}
+        )
+    else()
+        message(WARNING "Osmium: Can not find some libraries for XML input/output, please install them or configure the paths.")
+    endif()
+endif()
+
+#----------------------------------------------------------------------
+list(APPEND OSMIUM_IO_LIBRARIES
+    ${OSMIUM_PBF_LIBRARIES}
+    ${OSMIUM_XML_LIBRARIES}
+)
+
+list(APPEND OSMIUM_LIBRARIES
+    ${OSMIUM_IO_LIBRARIES}
+)
+
+#----------------------------------------------------------------------
+# Component 'geos'
+if(Osmium_USE_GEOS)
+    find_path(GEOS_INCLUDE_DIR geos/geom.h)
+    find_library(GEOS_LIBRARY NAMES geos)
+
+    list(APPEND OSMIUM_EXTRA_FIND_VARS GEOS_INCLUDE_DIR GEOS_LIBRARY)
+    if(GEOS_INCLUDE_DIR AND GEOS_LIBRARY)
+        SET(GEOS_FOUND 1)
+        list(APPEND OSMIUM_LIBRARIES ${GEOS_LIBRARY})
+        list(APPEND OSMIUM_INCLUDE_DIRS ${GEOS_INCLUDE_DIR})
+    else()
+        message(WARNING "Osmium: GEOS library is required but not found, please install it or configure the paths.")
+    endif()
+endif()
+
+#----------------------------------------------------------------------
+# Component 'gdal' (alias 'ogr')
+if(Osmium_USE_GDAL)
+    find_package(GDAL)
+
+    list(APPEND OSMIUM_EXTRA_FIND_VARS GDAL_FOUND)
+    if(GDAL_FOUND)
+        list(APPEND OSMIUM_LIBRARIES ${GDAL_LIBRARIES})
+        list(APPEND OSMIUM_INCLUDE_DIRS ${GDAL_INCLUDE_DIRS})
+    else()
+        message(WARNING "Osmium: GDAL library is required but not found, please install it or configure the paths.")
+    endif()
+endif()
+
+#----------------------------------------------------------------------
+# Component 'proj'
+if(Osmium_USE_PROJ)
+    find_path(PROJ_INCLUDE_DIR proj_api.h)
+    find_library(PROJ_LIBRARY NAMES proj)
+
+    list(APPEND OSMIUM_EXTRA_FIND_VARS PROJ_INCLUDE_DIR PROJ_LIBRARY)
+    if(PROJ_INCLUDE_DIR AND PROJ_LIBRARY)
+        set(PROJ_FOUND 1)
+        list(APPEND OSMIUM_LIBRARIES ${PROJ_LIBRARY})
+        list(APPEND OSMIUM_INCLUDE_DIRS ${PROJ_INCLUDE_DIR})
+    else()
+        message(WARNING "Osmium: PROJ.4 library is required but not found, please install it or configure the paths.")
+    endif()
+endif()
+
+#----------------------------------------------------------------------
+# Component 'sparsehash'
+if(Osmium_USE_SPARSEHASH)
+    message(WARNING "Osmium: Use of Google SparseHash is deprecated. Please switch to a different index type.")
+    find_path(SPARSEHASH_INCLUDE_DIR google/sparsetable)
+
+    list(APPEND OSMIUM_EXTRA_FIND_VARS SPARSEHASH_INCLUDE_DIR)
+    if(SPARSEHASH_INCLUDE_DIR)
+        # Find size of sparsetable::size_type. This does not work on older
+        # CMake versions because they can do this check only in C, not in C++.
+        if(NOT CMAKE_VERSION VERSION_LESS 3.0)
+           include(CheckTypeSize)
+           set(CMAKE_REQUIRED_INCLUDES ${SPARSEHASH_INCLUDE_DIR})
+           set(CMAKE_EXTRA_INCLUDE_FILES "google/sparsetable")
+           check_type_size("google::sparsetable<int>::size_type" SPARSETABLE_SIZE_TYPE LANGUAGE CXX)
+           set(CMAKE_EXTRA_INCLUDE_FILES)
+           set(CMAKE_REQUIRED_INCLUDES)
+        else()
+           set(SPARSETABLE_SIZE_TYPE ${CMAKE_SIZEOF_VOID_P})
+        endif()
+
+        # Sparsetable::size_type must be at least 8 bytes (64bit), otherwise
+        # OSM object IDs will not fit.
+        if(SPARSETABLE_SIZE_TYPE GREATER 7)
+            set(SPARSEHASH_FOUND 1)
+            add_definitions(-DOSMIUM_WITH_SPARSEHASH=${SPARSEHASH_FOUND})
+            list(APPEND OSMIUM_INCLUDE_DIRS ${SPARSEHASH_INCLUDE_DIR})
+        else()
+            message(WARNING "Osmium: Disabled Google SparseHash library on 32bit system (size_type=${SPARSETABLE_SIZE_TYPE}).")
+        endif()
+    else()
+        message(WARNING "Osmium: Google SparseHash library is required but not found, please install it or configure the paths.")
+    endif()
+endif()
+
+#----------------------------------------------------------------------
+
+list(REMOVE_DUPLICATES OSMIUM_INCLUDE_DIRS)
+
+if(OSMIUM_XML_LIBRARIES)
+    list(REMOVE_DUPLICATES OSMIUM_XML_LIBRARIES)
+endif()
+
+if(OSMIUM_PBF_LIBRARIES)
+    list(REMOVE_DUPLICATES OSMIUM_PBF_LIBRARIES)
+endif()
+
+if(OSMIUM_IO_LIBRARIES)
+    list(REMOVE_DUPLICATES OSMIUM_IO_LIBRARIES)
+endif()
+
+if(OSMIUM_LIBRARIES)
+    list(REMOVE_DUPLICATES OSMIUM_LIBRARIES)
+endif()
+
+#----------------------------------------------------------------------
+#
+#  Check that all required libraries are available
+#
+#----------------------------------------------------------------------
+if(OSMIUM_EXTRA_FIND_VARS)
+    list(REMOVE_DUPLICATES OSMIUM_EXTRA_FIND_VARS)
+endif()
+# Handle the QUIETLY and REQUIRED arguments and the optional version check
+# and set OSMIUM_FOUND to TRUE if all listed variables are TRUE.
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(Osmium
+                                  REQUIRED_VARS OSMIUM_INCLUDE_DIR ${OSMIUM_EXTRA_FIND_VARS}
+                                  VERSION_VAR _libosmium_version)
+unset(OSMIUM_EXTRA_FIND_VARS)
+
+#----------------------------------------------------------------------
+#
+#  A function for setting the -pthread option in compilers/linkers
+#
+#----------------------------------------------------------------------
+function(set_pthread_on_target _target)
+    if(NOT MSVC)
+        set_target_properties(${_target} PROPERTIES COMPILE_FLAGS "-pthread")
+        if(NOT APPLE)
+            set_target_properties(${_target} PROPERTIES LINK_FLAGS "-pthread")
+        endif()
+    endif()
+endfunction()
+
+#----------------------------------------------------------------------
+#
+#  Add compiler flags
+#
+#----------------------------------------------------------------------
+add_definitions(-D_LARGEFILE_SOURCE -D_FILE_OFFSET_BITS=64)
+
+if(MSVC)
+    add_definitions(-wd4996)
+
+    # Disable warning C4068: "unknown pragma" because we want it to ignore
+    # pragmas for other compilers.
+    add_definitions(-wd4068)
+
+    # Disable warning C4715: "not all control paths return a value" because
+    # it generates too many false positives.
+    add_definitions(-wd4715)
+
+    # Disable warning C4351: new behavior: elements of array '...' will be
+    # default initialized. The new behaviour is correct and we don't support
+    # old compilers anyway.
+    add_definitions(-wd4351)
+
+    # Disable warning C4503: "decorated name length exceeded, name was truncated"
+    # there are more than 150 of generated names in libosmium longer than 4096 symbols supported in MSVC
+    add_definitions(-wd4503)
+
+    add_definitions(-DNOMINMAX -DWIN32_LEAN_AND_MEAN -D_CRT_SECURE_NO_WARNINGS)
+endif()
+
+if(APPLE AND "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+# following only available from cmake 2.8.12:
+#   add_compile_options(-stdlib=libc++)
+# so using this instead:
+    add_definitions(-stdlib=libc++)
+    set(LDFLAGS ${LDFLAGS} -stdlib=libc++)
+endif()
+
+#----------------------------------------------------------------------
+
+# This is a set of recommended warning options that can be added when compiling
+# libosmium code.
+if(MSVC)
+    set(OSMIUM_WARNING_OPTIONS "/W3 /wd4514" CACHE STRING "Recommended warning options for libosmium")
+else()
+    set(OSMIUM_WARNING_OPTIONS "-Wall -Wextra -pedantic -Wredundant-decls -Wdisabled-optimization -Wctor-dtor-privacy -Wnon-virtual-dtor -Woverloaded-virtual -Wsign-promo -Wold-style-cast" CACHE STRING "Recommended warning options for libosmium")
+endif()
+
+set(OSMIUM_DRACONIC_CLANG_OPTIONS "-Wdocumentation -Wunused-exception-parameter -Wmissing-declarations -Weverything -Wno-c++98-compat -Wno-c++98-compat-pedantic -Wno-unused-macros -Wno-exit-time-destructors -Wno-global-constructors -Wno-padded -Wno-switch-enum -Wno-missing-prototypes -Wno-weak-vtables -Wno-cast-align -Wno-float-equal")
+
+if(Osmium_DEBUG)
+    message(STATUS "OSMIUM_XML_LIBRARIES=${OSMIUM_XML_LIBRARIES}")
+    message(STATUS "OSMIUM_PBF_LIBRARIES=${OSMIUM_PBF_LIBRARIES}")
+    message(STATUS "OSMIUM_IO_LIBRARIES=${OSMIUM_IO_LIBRARIES}")
+    message(STATUS "OSMIUM_LIBRARIES=${OSMIUM_LIBRARIES}")
+    message(STATUS "OSMIUM_INCLUDE_DIRS=${OSMIUM_INCLUDE_DIRS}")
+endif()
+
diff --git a/libosmium/FindProtozero.cmake b/libosmium/FindProtozero.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..ad16cabeb00e4a2d6cb381cf083491ba24ebeb6f
--- /dev/null
+++ b/libosmium/FindProtozero.cmake
@@ -0,0 +1,63 @@
+#----------------------------------------------------------------------
+#
+#  FindProtozero.cmake
+#
+#  Find the protozero headers.
+#
+#----------------------------------------------------------------------
+#
+#  Usage:
+#
+#    Copy this file somewhere into your project directory, where cmake can
+#    find it. Usually this will be a directory called "cmake" which you can
+#    add to the CMake module search path with the following line in your
+#    CMakeLists.txt:
+#
+#      list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake")
+#
+#    Then add the following in your CMakeLists.txt:
+#
+#      find_package(Protozero [version] [REQUIRED])
+#      include_directories(SYSTEM ${PROTOZERO_INCLUDE_DIR})
+#
+#    The version number is optional. If it is not set, any version of
+#    protozero will do.
+#
+#      if(NOT PROTOZERO_FOUND)
+#          message(WARNING "Protozero not found!\n")
+#      endif()
+#
+#----------------------------------------------------------------------
+#
+#  Variables:
+#
+#    PROTOZERO_FOUND        - True if Protozero was found.
+#    PROTOZERO_INCLUDE_DIR  - Where to find include files.
+#
+#----------------------------------------------------------------------
+
+# find include path
+find_path(PROTOZERO_INCLUDE_DIR protozero/version.hpp
+    PATH_SUFFIXES include
+    PATHS ${CMAKE_SOURCE_DIR}/../protozero
+)
+
+# Check version number
+if(Protozero_FIND_VERSION)
+    file(STRINGS "${PROTOZERO_INCLUDE_DIR}/protozero/version.hpp" _version_define REGEX "#define PROTOZERO_VERSION_STRING")
+    if("${_version_define}" MATCHES "#define PROTOZERO_VERSION_STRING \"([0-9.]+)\"")
+        set(_version "${CMAKE_MATCH_1}")
+    else()
+        set(_version "unknown")
+    endif()
+endif()
+
+#set(PROTOZERO_INCLUDE_DIRS "${PROTOZERO_INCLUDE_DIR}")
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(Protozero
+                                  REQUIRED_VARS PROTOZERO_INCLUDE_DIR
+                                  VERSION_VAR _version)
+
+
+#----------------------------------------------------------------------
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index b584288126167c5b52f30a06c9c342d218d19f69..8600f515f15e66c09b1c9399c18d8c5ccfb14372 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,12 +1,22 @@
 # driver source files
-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)
+SET(sources gradient.cpp g2_spline.cpp dijkstra.cpp common.cpp road.cpp junction.cpp road_segment.cpp road_map.cpp vel_profile.cpp vel_spline.cpp vel_trapezoid.cpp)
+SET(opendrive_sources opendrive/opendrive_geometry.cpp opendrive/opendrive_line.cpp opendrive/opendrive_spiral.cpp opendrive/opendrive_arc.cpp opendrive/opendrive_param_poly3.cpp opendrive/opendrive_signal.cpp opendrive/opendrive_object.cpp opendrive/opendrive_road_segment.cpp opendrive/opendrive_lane.cpp opendrive/opendrive_junction.cpp)
+SET(osm_sources osm/osm_map.cpp osm/osm_node.cpp osm/osm_way.cpp osm/osm_junction.cpp osm/osm_road_segment.cpp osm/osm_restriction.cpp)
 
 # application header files
-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)
+SET(headers ../include/gradient.h ../include/g2_spline.h ../include/dijkstra.h ../include/common.h ../include/road.h ../include/junction.h ../include/road_segment.h ../include/road_map.h ../include/vel_profile.h ../include/vel_spline.h ../include/vel_trapezoid.h)
+SET(opendrive_headers ../include/opendrive/opendrive_common.h ../include/opendrive/opendrive_geometry.h ../include/opendrive/opendrive_line.h ../include/opendrive/opendrive_spiral.h ../include/opendrive/opendrive_arc.h ../include/opendrive/opendrive_param_poly3.h ../include/opendrive/opendrive_signal.h ../include/opendrive/opendrive_object.h ../include/opendrive/opendrive_road_segment.h ../include/opendrive/opendrive_lane.h ../include/opendrive/opendrive_junction.h) 
+SET(osm_headers ../include/osm/osm_map.h ../include/osm/osm_node.h ../include/osm/osm_way.h ../include/osm/osm_junction.h ../include/osm/osm_road_segment.h ../include/osm/osm_restriction.h) 
+
+list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/libosmium")
+
+set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/share/cmake/geographiclib")
 
 # locate the necessary dependencies
 find_package(Eigen3 REQUIRED)
 find_package(iriutils REQUIRED)
+find_package(Osmium REQUIRED COMPONENTS xml io gdal) 
+find_package(GeographicLib REQUIRED)
 
 ADD_SUBDIRECTORY(xml)
 
@@ -15,12 +25,16 @@ INCLUDE_DIRECTORIES(../include)
 INCLUDE_DIRECTORIES(./)
 INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${OSMIUM_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${GeographicLib_INCLUDE_DIRS})
 
 # create the shared library
-ADD_LIBRARY(${PROJECT_NAME} SHARED ${sources} ${XSD_SOURCES})
+ADD_LIBRARY(${PROJECT_NAME} SHARED ${sources} ${opendrive_sources} ${osm_sources} ${XSD_SOURCES})
 
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${XSD_LIBRARY})
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${iriutils_LIBRARY})
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OSMIUM_LIBRARIES})
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GeographicLib_LIBRARIES})
 
 ADD_DEPENDENCIES(${PROJECT_NAME} xsd_files_gen)
 
@@ -32,6 +46,8 @@ INSTALL(TARGETS ${PROJECT_NAME}
         LIBRARY DESTINATION lib/iri/${PROJECT_NAME}
         ARCHIVE DESTINATION lib/iri/${PROJECT_NAME})
 INSTALL(FILES ${headers} DESTINATION include/iri/${PROJECT_NAME})
+INSTALL(FILES ${opendrive_headers} DESTINATION include/iri/${PROJECT_NAME}/opendrive)
+INSTALL(FILES ${osm_headers} DESTINATION include/iri/${PROJECT_NAME}/osm)
 INSTALL(FILES ../Find${PROJECT_NAME}.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
 
 #Install script files if needed
diff --git a/src/opendrive_common.cpp b/src/common.cpp
similarity index 94%
rename from src/opendrive_common.cpp
rename to src/common.cpp
index c81c8ded5b9c5f084a80dadf53f4ef582f6a9c8c..57ab78d02d25937aadd3e109d8dcbf04ab8d6e74 100644
--- a/src/opendrive_common.cpp
+++ b/src/common.cpp
@@ -1,4 +1,5 @@
-#include "opendrive_common.h"
+#include "common.h"
+
 #include <cmath>
 
 double normalize_angle(double angle)
@@ -27,3 +28,4 @@ double diff_angle(double angle1,double angle2)
 
   return std::fmin(diff1,diff2);
 }
+
diff --git a/src/dijkstra.cpp b/src/dijkstra.cpp
index 985df9c6427e7a1b735a7fc9456846f7b71c4f26..e3089e29244c33660356ea4f3ef06a2632dfdcf3 100644
--- a/src/dijkstra.cpp
+++ b/src/dijkstra.cpp
@@ -55,7 +55,7 @@ double CDijkstra::find_shortest_path(Eigen::MatrixXd &graph,unsigned int start_n
 {
   Eigen::MatrixXd w=Eigen::MatrixXd::Zero(graph.rows(),graph.cols());
   Eigen::MatrixXd d=Eigen::MatrixXd(graph.rows(),2);
-  Eigen::MatrixXd d2,new_d2;
+  Eigen::MatrixXd d2,new_d2,int_graph=graph;
   unsigned int i,j,k,l;
 
   path.clear();
@@ -64,24 +64,29 @@ double CDijkstra::find_shortest_path(Eigen::MatrixXd &graph,unsigned int start_n
     path.push_back(end_node);
     return 0.0;
   }
+  // arrange nodes
+  if(end_node==0)
+    end_node=start_node;
+  int_graph.row(0).swap(int_graph.row(start_node));
+  int_graph.col(0).swap(int_graph.col(start_node));
   // set Inf cost to all cells with 0
-  for(i=0;i<graph.rows();i++)
-    for(j=0;j<graph.cols();j++)
-      if(graph(i,j)==0.0)
-        graph(i,j)=std::numeric_limits<double>::max()/2.0;
-  for(i=1;i<graph.rows();i++)
+  for(i=0;i<int_graph.rows();i++)
+    for(j=0;j<int_graph.cols();j++)
+      if(int_graph(i,j)==0.0)
+        int_graph(i,j)=std::numeric_limits<double>::max()/2.0;
+  for(i=1;i<int_graph.rows();i++)
   {
     w(0,i)=i;
-    w(1,i)=graph(0,i);
+    w(1,i)=int_graph(0,i);
   }
-  for(i=0;i<graph.rows();i++)
+  for(i=0;i<int_graph.rows();i++)
   {
-    d(i,0)=graph(0,i);
+    d(i,0)=int_graph(0,i);
     d(i,1)=i;
   }
   d2=d.block(1,0,d.rows()-1,2);
   l=1;
-  while(l<(graph.rows()-1))
+  while(l<(int_graph.rows()-1))
   {
     l++;
     this->sort_rows(d2);
@@ -90,21 +95,23 @@ double CDijkstra::find_shortest_path(Eigen::MatrixXd &graph,unsigned int start_n
     new_d2=d2.block(1,0,d2.rows()-1,2);
     for(i=0;i<new_d2.rows();i++)
     {
-      if(d(new_d2(i,1),0)>(d(k,0)+graph(k,new_d2(i,1))))
+      if(d(new_d2(i,1),0)>(d(k,0)+int_graph(k,new_d2(i,1))))
       {
-        d(new_d2(i,1),0)=d(k,0)+graph(k,new_d2(i,1));
+        d(new_d2(i,1),0)=d(k,0)+int_graph(k,new_d2(i,1));
         new_d2(i,0)=d(new_d2(i,1),0); 
       }
     }
-    for(i=1;i<graph.rows();i++)
+    for(i=1;i<int_graph.rows();i++)
       w(l,i)=d(i,0);
     d2=new_d2;
   }
   path.clear();
-  // find the last reachable cell (preferably on the same target lane)
-  while(w(1,end_node)==w(w.rows()-1,end_node) && w(1,end_node)==std::numeric_limits<double>::max()/2.0)
-    end_node--;
-  path.push_back(end_node); 
+  if(w(1,end_node)==w(w.rows()-1,end_node) && w(1,end_node)==std::numeric_limits<double>::max()/2.0)
+    return std::numeric_limits<double>::max()/2.0;
+  if(end_node==start_node)
+    path.push_back(0); 
+  else
+    path.push_back(end_node); 
   this->get_path(path,w,start_node,end_node);
 
   return w(w.rows()-1,end_node);
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 34527a7335814dc9a28efbfafd257298bb20f1da..2a1815eef6903f5637fe693d3c76663226e7b1b2 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -4,6 +4,16 @@ ADD_EXECUTABLE(dijkstra_test dijkstra_test.cpp)
 TARGET_LINK_LIBRARIES(dijkstra_test ${PROJECT_NAME})
 
 # create an example application
-ADD_EXECUTABLE(opendrive_test opendrive_test.cpp)
+ADD_EXECUTABLE(build_from_scratch_test build_from_scratch_test.cpp)
 # link necessary libraries
-TARGET_LINK_LIBRARIES(opendrive_test ${PROJECT_NAME})
+TARGET_LINK_LIBRARIES(build_from_scratch_test ${PROJECT_NAME})
+
+# create an example application
+ADD_EXECUTABLE(opendrive_import_test opendrive_import_test.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(opendrive_import_test ${PROJECT_NAME})
+
+# create an example application
+ADD_EXECUTABLE(osm_import_test osm_import_test.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(osm_import_test ${PROJECT_NAME})
diff --git a/src/examples/build_from_scratch_test.cpp b/src/examples/build_from_scratch_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8313acd4010b41b71fc5fb7dae45177b2ed4f4b8
--- /dev/null
+++ b/src/examples/build_from_scratch_test.cpp
@@ -0,0 +1,147 @@
+#include "road_map.h"
+#include "exceptions.h"
+
+#include <iostream>
+
+int main(int argc, char *argv[])
+{
+  try{
+    CRoadMap map;
+    CRoad *road0,*road1,*road2,*road3,*road4,*road5;
+    CJunction *junction0,*junction1;
+    Eigen::MatrixXi connectivity;
+    std::vector<unsigned int> id_map;
+    std::vector<double> x,y,heading;
+ 
+    road0=new CRoad();
+    road0->set_num_lanes(2);
+    road0->set_lane_width(4.0);
+    road0->set_start_point(20.0,60.0,3.14159,0.0);
+    road0->add_segment(0.0,60.0,3.14159,0.0);
+    road0->add_segment(-20.0,40.0,4.71238898,0.0);
+    road0->add_segment(-20.0,20.0,4.71238898,0.0);
+    road0->add_segment(0.0,0.0,0.0,0.0);
+    road0->add_segment(20.0,0.0,0.0,0.0);
+    road1=new CRoad();
+    road1->set_num_lanes(2);
+    road1->set_lane_width(4.0);
+    road1->set_start_point(20.0,0.0,3.14159,0.0);
+    road1->add_segment(0.0,0.0,3.14159,0.0);
+    road1->add_segment(-20.0,20.0,1.5707,0.0);
+    road1->add_segment(-20.0,40.0,1.5707,0.0);
+    road1->add_segment(0.0,60.0,0.0,0.0);
+    road1->add_segment(20.0,60.0,0.0,0.0);
+    road2=new CRoad();
+    road2->set_num_lanes(2);
+    road2->set_lane_width(4.0);
+    road2->set_start_point(30.0,50.0,4.71238898,0.0);
+    road2->add_segment(30.0,10.0,4.71238898,0.0);
+    road3=new CRoad();
+    road3->set_num_lanes(2);
+    road3->set_lane_width(4.0);
+    road3->set_start_point(30.0,10.0,1.5707,0.0);
+    road3->add_segment(30.0,50.0,1.5707,0.0);
+    road4=new CRoad();
+    road4->set_num_lanes(2);
+    road4->set_lane_width(4.0);
+    road4->set_start_point(40.0,60.0,0.0,0.0);
+    road4->add_segment(60.0,60.0,0.0,0.0);
+    road4->add_segment(80.0,40.0,4.71238898,0.0);
+    road4->add_segment(80.0,20.0,4.71238898,0.0);
+    road4->add_segment(60.0,0.0,3.14159,0.0);
+    road4->add_segment(40.0,0.0,3.14159,0.0);
+    road5=new CRoad();
+    road5->set_num_lanes(2);
+    road5->set_lane_width(4.0);
+    road5->set_start_point(40.0,0.0,0.0,0.0);
+    road5->add_segment(60.0,0.0,0.0,0.0);
+    road5->add_segment(80.0,20.0,1.5707,0.0);
+    road5->add_segment(80.0,40.0,1.5707,0.0);
+    road5->add_segment(60.0,60.0,3.14159,0.0);
+    road5->add_segment(40.0,60.0,3.14159,0.0);
+
+    junction0=new CJunction();
+    junction1=new CJunction();
+
+    map.add_road(road0);
+    map.add_road(road1);
+    map.add_road(road2);
+    map.add_road(road3);
+    map.add_road(road4);
+    map.add_road(road5);
+
+    map.set_opposite_direction_roads_by_id(road0->get_id(),road1->get_id());
+    map.set_opposite_direction_roads_by_id(road2->get_id(),road3->get_id());
+    map.set_opposite_direction_roads_by_id(road4->get_id(),road5->get_id());
+
+    map.add_junction(junction0);
+    map.add_junction(junction1);
+
+    junction0->add_incomming_road(road1);
+    junction0->add_incomming_road(road3);
+    junction0->add_incomming_road(road5);
+    junction0->add_outgoing_road(road0);
+    junction0->add_outgoing_road(road2);
+    junction0->add_outgoing_road(road4);
+    junction0->link_roads_by_id(road1->get_id(),road2->get_id());
+    junction0->link_same_lanes_by_id(road1->get_id(),road2->get_id());
+    junction0->link_roads_by_id(road1->get_id(),road4->get_id());
+    junction0->link_same_lanes_by_id(road1->get_id(),road4->get_id());
+    junction0->link_roads_by_id(road3->get_id(),road0->get_id());
+    junction0->link_same_lanes_by_id(road3->get_id(),road0->get_id());
+    junction0->link_roads_by_id(road3->get_id(),road4->get_id());
+    junction0->link_same_lanes_by_id(road3->get_id(),road4->get_id());
+    junction0->link_roads_by_id(road5->get_id(),road0->get_id());
+    junction0->link_same_lanes_by_id(road5->get_id(),road0->get_id());
+    junction0->link_roads_by_id(road5->get_id(),road2->get_id());
+    junction0->link_same_lanes_by_id(road5->get_id(),road2->get_id());
+
+    junction1->add_incomming_road(road0);
+    junction1->add_incomming_road(road2);
+    junction1->add_incomming_road(road4);
+    junction1->add_outgoing_road(road1);
+    junction1->add_outgoing_road(road3);
+    junction1->add_outgoing_road(road5);
+    junction1->link_roads_by_id(road0->get_id(),road3->get_id());
+    junction1->link_same_lanes_by_id(road0->get_id(),road3->get_id());
+    junction1->link_roads_by_id(road0->get_id(),road5->get_id());
+    junction1->link_same_lanes_by_id(road0->get_id(),road5->get_id());
+    junction1->link_roads_by_id(road2->get_id(),road1->get_id());
+    junction1->link_same_lanes_by_id(road2->get_id(),road1->get_id());
+    junction1->link_roads_by_id(road2->get_id(),road5->get_id());
+    junction1->link_same_lanes_by_id(road2->get_id(),road5->get_id());
+    junction1->link_roads_by_id(road4->get_id(),road3->get_id());
+    junction1->link_same_lanes_by_id(road4->get_id(),road3->get_id());
+    junction1->link_roads_by_id(road4->get_id(),road1->get_id());
+    junction1->link_same_lanes_by_id(road4->get_id(),road1->get_id());
+
+    map.save_opendrive("test_road.xodr");
+  
+    map.get_lane_geometry(x,y,heading);
+    for(unsigned int i=0;i<x.size();i++)
+      std::cout << x[i] << "," << y[i] << "," << heading[i] << std::endl;
+
+    return 0;
+
+    map.get_segment_connectivity(connectivity,id_map);
+    for(unsigned int i=0;i<id_map.size();i++)
+      std::cout << i << " -> " << id_map[i] << " ";
+    std::cout << std::endl;
+    std::cout << "Initial connectivity:" << std::endl;
+    std::cout << connectivity << std::endl;
+
+    map.remove_road_by_id(road3->get_id());
+
+    map.get_segment_connectivity(connectivity,id_map);
+    for(unsigned int i=0;i<id_map.size();i++)
+      std::cout << i << " -> " << id_map[i] << " ";
+    std::cout << std::endl;
+    std::cout << "Updated connectivity:" << std::endl;
+    std::cout << connectivity << std::endl;
+  }
+  catch (CException &e)
+  {
+    std::cout << "[Exception caught] : " << e.what() << std::endl;
+  }
+  return 0;
+}
diff --git a/src/examples/opendrive_build_road_test.cpp b/src/examples/opendrive_build_road_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..539310119c46877e3162f7bfacc8661ce57468d2
--- /dev/null
+++ b/src/examples/opendrive_build_road_test.cpp
@@ -0,0 +1,201 @@
+#include "opendrive_road.h"
+#include "exceptions.h"
+#include <iostream>
+#include <vector>
+#include <limits>
+
+int main(int argc, char *argv[])
+{
+  std::vector<double> x,y;
+  COpendriveRoad road;
+  COpendriveRoadSegment *new_segment;
+  COpendriveLane new_lane(4.0,50.0,OD_MARK_SOLID);
+  TOpendriveWorldPose start_pose;
+  COpendriveLine *line_geom;
+  COpendriveArc *arc_geom;
+  COpendriveJunction *new_junction;
+  unsigned int road0_index,road1_index,road2_index,road3_index,road4_index,road5_index,road6_index,road7_index,road8_index,road9_index,road10_index,connection_index;
+  
+  try
+  {
+    road.set_resolution(0.1);
+    road.set_scale_factor(1.0);
+    road.set_min_road_length(0.1);
+    new_segment=new COpendriveRoadSegment("road0",OD_MARK_BROKEN,-1,false);
+    new_segment->add_left_lane(new_lane);
+    new_segment->add_right_lane(new_lane);
+    start_pose.x=0.0;
+    start_pose.y=0.0;
+    start_pose.heading=0.0;
+    line_geom=new COpendriveLine(start_pose,20.0);
+    new_segment->add_geometry(line_geom);
+    road0_index=road.add_segment(*new_segment);
+    delete new_segment;
+    delete line_geom;
+    new_segment=new COpendriveRoadSegment("road1",OD_MARK_BROKEN,-1,false);
+    new_segment->add_left_lane(new_lane);
+    new_segment->add_right_lane(new_lane);
+    start_pose.x=40.0;
+    start_pose.y=0.0;
+    start_pose.heading=0.0;
+    line_geom=new COpendriveLine(start_pose,20.0);
+    new_segment->add_geometry(line_geom);
+    road1_index=road.add_segment(*new_segment);
+    delete new_segment;
+    delete line_geom;
+    new_segment=new COpendriveRoadSegment("road2",OD_MARK_BROKEN,-1,false);
+    new_segment->add_left_lane(new_lane);
+    new_segment->add_right_lane(new_lane);
+    start_pose.x=60.0;
+    start_pose.y=0.0;
+    start_pose.heading=0.0;
+    arc_geom=new COpendriveArc(start_pose,20.0*90.0*3.14159/180.0,1.0/20.0);
+    new_segment->add_geometry(arc_geom);
+    road2_index=road.add_segment(*new_segment);
+    road.link_segments_by_index(road1_index,road2_index);
+    delete new_segment;
+    delete arc_geom;
+    new_segment=new COpendriveRoadSegment("road3",OD_MARK_BROKEN,-1,false);
+    new_segment->add_left_lane(new_lane);
+    new_segment->add_right_lane(new_lane);
+    start_pose.x=80.0;
+    start_pose.y=20.0;
+    start_pose.heading=1.5707;
+    line_geom=new COpendriveLine(start_pose,20.0);
+    new_segment->add_geometry(line_geom);
+    road3_index=road.add_segment(*new_segment);
+    road.link_segments_by_index(road2_index,road3_index);
+    delete new_segment;
+    delete line_geom;
+    new_segment=new COpendriveRoadSegment("road4",OD_MARK_BROKEN,-1,false);
+    new_segment->add_left_lane(new_lane);
+    new_segment->add_right_lane(new_lane);
+    start_pose.x=80.0;
+    start_pose.y=40.0;
+    start_pose.heading=1.5707;
+    arc_geom=new COpendriveArc(start_pose,20.0*90.0*3.14159/180.0,1.0/20.0);
+    new_segment->add_geometry(arc_geom);
+    road4_index=road.add_segment(*new_segment);
+    road.link_segments_by_index(road3_index,road4_index);
+    delete new_segment;
+    delete arc_geom;
+    new_segment=new COpendriveRoadSegment("road5",OD_MARK_BROKEN,-1,false);
+    new_segment->add_left_lane(new_lane);
+    new_segment->add_right_lane(new_lane);
+    start_pose.x=60.0;
+    start_pose.y=60.0;
+    start_pose.heading=3.14159;
+    line_geom=new COpendriveLine(start_pose,20.0);
+    new_segment->add_geometry(line_geom);
+    road5_index=road.add_segment(*new_segment);
+    road.link_segments_by_index(road4_index,road5_index);
+    delete new_segment;
+    delete line_geom;
+    new_segment=new COpendriveRoadSegment("road6",OD_MARK_BROKEN,-1,false);
+    new_segment->add_left_lane(new_lane);
+    new_segment->add_right_lane(new_lane);
+    start_pose.x=20.0;
+    start_pose.y=60.0;
+    start_pose.heading=3.14159;
+    line_geom=new COpendriveLine(start_pose,20.0);
+    new_segment->add_geometry(line_geom);
+    road6_index=road.add_segment(*new_segment);
+    delete new_segment;
+    delete line_geom;
+    new_segment=new COpendriveRoadSegment("road7",OD_MARK_BROKEN,-1,false);
+    new_segment->add_left_lane(new_lane);
+    new_segment->add_right_lane(new_lane);
+    start_pose.x=0.0;
+    start_pose.y=60.0;
+    start_pose.heading=3.14159;
+    arc_geom=new COpendriveArc(start_pose,20.0*90.0*3.14159/180.0,1.0/20.0);
+    new_segment->add_geometry(arc_geom);
+    road7_index=road.add_segment(*new_segment);
+    road.link_segments_by_index(road6_index,road7_index);
+    delete new_segment;
+    delete arc_geom;
+    new_segment=new COpendriveRoadSegment("road8",OD_MARK_BROKEN,-1,false);
+    new_segment->add_left_lane(new_lane);
+    new_segment->add_right_lane(new_lane);
+    start_pose.x=-20.0;
+    start_pose.y=40.0;
+    start_pose.heading=4.71229;
+    line_geom=new COpendriveLine(start_pose,20.0);
+    new_segment->add_geometry(line_geom);
+    road8_index=road.add_segment(*new_segment);
+    road.link_segments_by_index(road7_index,road8_index);
+    delete new_segment;
+    delete line_geom;
+    new_segment=new COpendriveRoadSegment("road8",OD_MARK_BROKEN,-1,false);
+    new_segment->add_left_lane(new_lane);
+    new_segment->add_right_lane(new_lane);
+    start_pose.x=-20.0;
+    start_pose.y=20.0;
+    start_pose.heading=4.71229;
+    arc_geom=new COpendriveArc(start_pose,20.0*90.0*3.14159/180.0,1.0/20.0);
+    new_segment->add_geometry(arc_geom);
+    road9_index=road.add_segment(*new_segment);
+    road.link_segments_by_index(road8_index,road9_index);
+    road.link_segments_by_index(road9_index,road0_index);
+    delete new_segment;
+    delete arc_geom;    
+    new_segment=new COpendriveRoadSegment("road10",OD_MARK_BROKEN,-1,false);
+    new_segment->add_right_lane(new_lane);
+    start_pose.x=32.0;
+    start_pose.y=50.0;
+    start_pose.heading=4.71229;
+    line_geom=new COpendriveLine(start_pose,40.0);
+    new_segment->add_geometry(line_geom);
+    road10_index=road.add_segment(*new_segment);
+    delete new_segment;
+    delete line_geom;
+    // create junction roads
+    new_junction=new COpendriveJunction("junction1");
+    connection_index=new_junction->add_connection(road0_index,road1_index,false);
+    new_junction->add_link_to_connection(connection_index,-1,-1);
+    connection_index=new_junction->add_connection(road1_index,road0_index,false);
+    new_junction->add_link_to_connection(connection_index,1,1);
+    connection_index=new_junction->add_connection(road10_index,road0_index,false);
+    new_junction->add_link_to_connection(connection_index,-1,1);
+    connection_index=new_junction->add_connection(road10_index,road1_index,false);
+    new_junction->add_link_to_connection(connection_index,-1,-1);
+    road.add_junction(*new_junction);
+    delete new_junction;
+    new_junction=new COpendriveJunction("junction2");
+    connection_index=new_junction->add_connection(road5_index,road6_index,false);
+    new_junction->add_link_to_connection(connection_index,-1,-1);
+    connection_index=new_junction->add_connection(road6_index,road5_index,false);
+    new_junction->add_link_to_connection(connection_index,1,1);
+    connection_index=new_junction->add_connection(road6_index,road10_index,false);
+    new_junction->add_link_to_connection(connection_index,1,-1);
+    connection_index=new_junction->add_connection(road5_index,road10_index,false);
+    new_junction->add_link_to_connection(connection_index,-1,-1);
+    road.add_junction(*new_junction);
+    delete new_junction;
+
+    road.save("./new_road.xodr");
+
+    for(unsigned int i=0;i<road.get_num_nodes();i++)
+    {
+      const COpendriveRoadNode &node=road.get_node(i);
+      for(unsigned int j=0;j<node.get_num_links();j++)
+      {
+        const COpendriveLink &link=node.get_link(j);
+        link.get_trajectory(x,y);
+        for(unsigned int k=0;k<x.size();k++)
+          std::cout << x[k] << "," << y[k] << std::endl;
+      }
+    }
+
+    return 0;
+    
+    std::cout << road << std::endl;
+    return 0;
+
+  }
+  catch (CException &e)
+  {
+    std::cout << "[Exception caught] : " << e.what() << std::endl;
+  }
+  return 0;
+}
diff --git a/src/examples/opendrive_import_test.cpp b/src/examples/opendrive_import_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4e3af3241634f516d235b3442b56c8d379401ced
--- /dev/null
+++ b/src/examples/opendrive_import_test.cpp
@@ -0,0 +1,44 @@
+#include "road_map.h"
+#include "exceptions.h"
+
+#include <iostream>
+
+//const std::string opendrive_file="/home/sergi/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/src/xml/adc_road.xodr";
+const std::string opendrive_file="/home/sergi/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/build/test_road.xodr";
+
+int main(int argc, char *argv[])
+{
+  try{
+    CRoadMap map,new_map;
+    Eigen::MatrixXi connectivity;
+    std::vector<unsigned int> id_map,new_path,old_path;    
+    std::vector<double> x,y,heading;
+ 
+    map.load_opendrive(opendrive_file);
+
+
+    map.get_lane_geometry(x,y,heading);
+    for(unsigned int i=0;i<x.size();i++)
+      std::cout << x[i] << "," << y[i] << "," << heading[i] << std::endl;
+
+    return 0;
+
+//    old_path.push_back(3);
+//    old_path.push_back(4);
+    old_path.push_back(22);
+//    old_path.push_back(10);
+    new_path=map.get_path_sub_roadmap(old_path,new_map);
+
+    new_map.get_segment_connectivity(connectivity,id_map);
+    for(unsigned int i=0;i<id_map.size();i++)
+      std::cout << i << " -> " << id_map[i] << " ";
+    std::cout << std::endl;
+    std::cout << "Initial connectivity:" << std::endl;
+    std::cout << connectivity << std::endl;
+  }
+  catch (CException &e)
+  {
+    std::cout << "[Exception caught] : " << e.what() << std::endl;
+  }
+  return 0;
+}
diff --git a/src/examples/opendrive_test.cpp b/src/examples/opendrive_test.cpp
deleted file mode 100644
index cff26ce8f6c5113ab29133f5cac4441e1f0628fd..0000000000000000000000000000000000000000
--- a/src/examples/opendrive_test.cpp
+++ /dev/null
@@ -1,217 +0,0 @@
-#include "opendrive_road.h"
-#include "exceptions.h"
-#include <iostream>
-#include <vector>
-#include <limits>
-
-int main(int argc, char *argv[])
-{
-//  TOpendriveWorldPose closest_pose;
-  std::vector<double> x,y;
-  COpendriveRoad road,new_road,new_road2;
-  std::vector<unsigned int> path;
-  TOpendriveWorldPose end_pose,start_pose;
-  double length=5.0;
-//  std::string xml_file="/home/shernand/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/add_road_full.xodr";
-  std::string xml_file="/home/shernand/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/adc_road.xodr";
-  
-  try
-  {
-    road.set_resolution(0.1);
-    road.set_scale_factor(1.0);
-    road.set_min_road_length(0.1);
-    road.load(xml_file);
-//    std::cout << road << std::endl;
-
-    for(unsigned int i=0;i<road.get_num_nodes();i++)
-    {
-      const COpendriveRoadNode &node=road.get_node(i);
-      for(unsigned int j=0;j<node.get_num_links();j++)
-      {
-        const COpendriveLink &link=node.get_link(j);
-        link.get_trajectory(x,y);
-        for(unsigned int k=0;k<x.size();k++)
-          std::cout << x[k] << "," << y[k] << std::endl;
-      }
-    }
-    return 0;
-/*
-    path.push_back(4);
-    start_pose.x=10.0;
-    start_pose.y=0.0;
-    start_pose.heading=0.0;
-    end_pose.x=17.0;
-    end_pose.y=0.0;
-    end_pose.heading=0.0;
-    road.get_sub_road(path,end_pose,new_road);
-    std::cout << "road1" << std::endl;
-    std::cout << new_road << std::endl;
-*/
-    /*
-    path.clear();
-    path.push_back(4);
-    path.push_back(6);
-    path.push_back(8);
-    path.push_back(150);
-    path.push_back(67);
-    path.push_back(69);
-    path.push_back(71);
-    start_pose.x=15.0;
-    start_pose.y=0.0;
-    start_pose.heading=0.0;
-    end_pose.x=18.5;
-    end_pose.y=22.0;
-    end_pose.heading=3.14159;
-    */
-    /*
-    path.clear();
-    path.push_back(61);
-    path.push_back(63);
-    path.push_back(65);
-    path.push_back(153);
-    path.push_back(10);
-    path.push_back(12);
-    path.push_back(14);
-    end_pose.x=15.0;
-    end_pose.y=0.0;
-    end_pose.heading=3.14159;
-    start_pose.x=18.5;
-    start_pose.y=22.0;
-    start_pose.heading=0.0;
-    */
-   
-    path.clear();
-    path.push_back(0);
-    path.push_back(98);
-    path.push_back(100);
-    path.push_back(16);
-    path.push_back(17);
-    path.push_back(18);
-    path.push_back(133);
-    path.push_back(134);
-    path.push_back(59);
-    path.push_back(157);
-    path.push_back(158);
-    path.push_back(11);
-    path.push_back(13);
-    path.push_back(15);
-    end_pose.x=9.5;
-    end_pose.y=0.6;
-    end_pose.heading=3.14159;
-    start_pose.x=6.7;
-    start_pose.y=2.3;
-    start_pose.heading=1.5707;
-    
-    road.get_sub_road(path,end_pose,new_road);
-//    std::cout << "road2" << std::endl;
-//    std::cout << new_road << std::endl;
-/*
-    for(unsigned int i=0;i<new_road.get_num_nodes();i++)
-    {
-      const COpendriveRoadNode &node=new_road.get_node(i);
-      for(unsigned int j=0;j<node.get_num_links();j++)
-      {
-        const COpendriveLink &link=node.get_link(j);
-        link.get_trajectory(x,y);
-        for(unsigned int k=0;k<x.size();k++)
-          std::cout << x[k] << "," << y[k] << std::endl;
-      }
-    }
-*/
-
-    length=21.0;
-    new_road.get_sub_road(start_pose,length,1.0,new_road2);
-//    std::cout << "road2" << std::endl;
-//    std::cout << new_road2 << std::endl;
-    for(unsigned int i=0;i<new_road2.get_num_nodes();i++)
-    {
-      const COpendriveRoadNode &node=new_road2.get_node(i);
-      for(unsigned int j=0;j<node.get_num_links();j++)
-      {
-        const COpendriveLink &link=node.get_link(j);
-        link.get_trajectory(x,y);
-        for(unsigned int k=0;k<x.size();k++)
-          std::cout << x[k] << "," << y[k] << std::endl;
-      }
-    }
-/*
-    if(argc!=4)
-    {
-      std::cout << "Invalid number of parameters" << std::endl;
-      std::cout << argv[0] << " x y heading" << std::endl;
-      return -1;
-    }
-    end_pose.x=std::stod(argv[1]);
-    end_pose.y=std::stod(argv[2]);
-    end_pose.heading=std::stod(argv[3]);
-    length=road.get_closest_pose(end_pose,closest_pose);
-    std::cout << "closest pose to: " << x << "," << y << "," << heading << " -> " << closest_pose.x << "," << closest_pose.y << "," << closest_pose.heading << " at " << length << " m" << std::endl;
-*/  
-/*
-    path.push_back(0);
-    start_pose.x=1.0;
-    start_pose.y=0.0;
-    start_pose.heading=0.0;
-    end_pose.x=2.0;
-    end_pose.y=0.0;
-    end_pose.heading=0.0;
-    road.get_sub_road(path,end_pose,new_road);
-    std::cout << "road1" << std::endl;
-    std::cout << new_road << std::endl;
-*/
-/*
-    path.clear();
-    path.push_back(0);
-    path.push_back(22);
-    path.push_back(2);
-    start_pose.x=1.0;
-    start_pose.y=0.0;
-    start_pose.heading=0.0;
-    end_pose.x=5.0;
-    end_pose.y=0.0;
-    end_pose.heading=0.0;
-    road.get_sub_road(path,end_pose,new_road);
-    std::cout << "road2" << std::endl;
-    std::cout << new_road << std::endl;
-    path.clear();
-    path.push_back(0);
-    path.push_back(22);
-    path.push_back(2);
-    path.push_back(4);
-    path.push_back(6);
-    path.push_back(8);
-    path.push_back(10);
-    path.push_back(25);
-    path.push_back(21);
-    path.push_back(24);
-    path.push_back(1);
-    start_pose.x=1.0;
-    start_pose.y=0.0;
-    start_pose.heading=0.0;
-    end_pose.x=2.0;
-    end_pose.y=0.0;
-    end_pose.heading=0.0;
-    road.get_sub_road(path,end_pose,new_road);
-    std::cout << "road3" << std::endl;
-    std::cout << new_road << std::endl;
-*/
-/*
-    for(unsigned int i=0;i<new_road.get_num_nodes();i++)
-    {
-      const COpendriveRoadNode &node=new_road.get_node(i);
-      for(unsigned int j=0;j<node.get_num_links();j++)
-      {
-        const COpendriveLink &link=node.get_link(j);
-        link.get_trajectory(x,y);
-        for(unsigned int k=0;k<x.size();k++)
-          std::cout << x[k] << "," << y[k] << std::endl;
-      }
-    }
-*/
-  }
-  catch (CException &e)
-  {
-    std::cout << "[Exception caught] : " << e.what() << std::endl;
-  }
-  return 0;
-}
diff --git a/src/examples/osm_import_test.cpp b/src/examples/osm_import_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4d15f586aabc7f31fac924ad24233caf2f9ed2d1
--- /dev/null
+++ b/src/examples/osm_import_test.cpp
@@ -0,0 +1,51 @@
+#include "road_map.h"
+#include "exceptions.h"
+
+#include <iostream>
+
+//const std::string osm_file="/home/shernand/Downloads/test_osm.osm";
+//const std::string osm_file="/home/shernand/Downloads/test_osm2.osm";
+const std::string osm_file="/home/shernand/Downloads/diagonal2.osm";
+
+int main(int argc, char *argv[])
+{
+  try{
+    CRoadMap map,new_map;
+    Eigen::MatrixXi connectivity;
+    std::vector<unsigned int> id_map,new_path,old_path;    
+    std::vector<double> x,y,heading;
+ 
+    map.load_osm(osm_file);
+
+    /*
+    map.get_segment_geometry(x,y,heading);
+    for(unsigned int i=0;i<x.size();i++)
+      std::cout << x[i] << "," << y[i] << "," << heading[i] << std::endl;
+
+    return 0;
+*/
+    map.get_lane_geometry(x,y,heading);
+    for(unsigned int i=0;i<x.size();i++)
+      std::cout << x[i] << "," << y[i] << "," << heading[i] << std::endl;
+
+    return 0;
+
+//    old_path.push_back(3);
+//    old_path.push_back(4);
+    old_path.push_back(22);
+//    old_path.push_back(10);
+    new_path=map.get_path_sub_roadmap(old_path,new_map);
+
+    new_map.get_segment_connectivity(connectivity,id_map);
+    for(unsigned int i=0;i<id_map.size();i++)
+      std::cout << i << " -> " << id_map[i] << " ";
+    std::cout << std::endl;
+    std::cout << "Initial connectivity:" << std::endl;
+    std::cout << connectivity << std::endl;
+  }
+  catch (CException &e)
+  {
+    std::cout << "[Exception caught] : " << e.what() << std::endl;
+  }
+  return 0;
+}
diff --git a/src/g2_spline.cpp b/src/g2_spline.cpp
index 9f21ff78878ac5c743a1560be2ec4f9090d98d49..2cd609be84289562c88e05ca1f9b157439572157 100644
--- a/src/g2_spline.cpp
+++ b/src/g2_spline.cpp
@@ -124,8 +124,8 @@ double CG2Spline::find_parameter(double length)
 
   index=length/this->resolution;
   
-  if(this->length[index]>length)
-    while(index>0 && this->length[index]>length)
+  if(this->length[index]-length>this->resolution)
+    while(index>0 && this->length[index]-length>this->resolution)
       index--;
   else
     while(index<this->num_points && this->length[index]<length)
@@ -584,40 +584,42 @@ void CG2Spline::get_end_point(TPoint &point)
 
 void CG2Spline::generate(double resolution,unsigned int iterations)
 {
-  double l_c_start,l2_k_s_start,l_s_start,l2_k_c_start;
-  double l_c_end,l2_k_s_end,l_s_end,l2_k_c_end;
+  double n1_c_start,n1_2_k_s_start,n1_s_start,n1_2_k_c_start;
+  double n2_c_end,n2_2_k_s_end,n2_s_end,n2_2_k_c_end;
   double u,pow_u,new_length,last_length;
-  unsigned int i;
+  unsigned int i,max_num_points;
 
   this->resolution=resolution;
   last_length=0.0;
   new_length=sqrt(pow(this->end.x-this->start.x,2)+pow(this->end.y-this->start.y,2));
+  max_num_points=10.0*(new_length/resolution);
+  this->num_points=0;
   if(new_length>this->resolution)
   {
-    while(iterations>0 && fabs(new_length-last_length)>this->resolution)
+    while(iterations>0 && fabs(new_length-last_length)>this->resolution && this->num_points<max_num_points)
     {
       last_length=new_length;
       this->num_points=ceil(last_length/resolution);
-      l_c_start=last_length*cos(this->start.heading);
-      l2_k_s_start=pow(last_length,2)*this->start.curvature*sin(this->start.heading);
-      l_c_end=last_length*cos(this->end.heading);
-      l2_k_s_end=pow(last_length,2)*this->end.curvature*sin(this->end.heading);
+      n1_c_start=last_length*cos(this->start.heading);
+      n1_2_k_s_start=pow(last_length,2)*this->start.curvature*sin(this->start.heading);
+      n2_c_end=last_length*cos(this->end.heading);
+      n2_2_k_s_end=pow(last_length,2)*this->end.curvature*sin(this->end.heading);
       this->x_coeff[0]=this->start.x;
-      this->x_coeff[1]=l_c_start;
-      this->x_coeff[2]=-0.5*l2_k_s_start;
-      this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*l_c_start-4.0*l_c_end+1.5*l2_k_s_start-0.5*l2_k_s_end;
-      this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*l_c_start+7.0*l_c_end-1.5*l2_k_s_start+l2_k_s_end;
-      this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*l_c_start-3.0*l_c_end+0.5*l2_k_s_start-0.5*l2_k_s_end;
-      l_s_start=last_length*sin(this->start.heading);
-      l2_k_c_start=pow(last_length,2)*this->start.curvature*cos(this->start.heading);
-      l_s_end=last_length*sin(this->end.heading);
-      l2_k_c_end=pow(last_length,2)*this->end.curvature*cos(this->end.heading);
+      this->x_coeff[1]=n1_c_start;
+      this->x_coeff[2]=-0.5*n1_2_k_s_start;
+      this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*n1_c_start-4.0*n2_c_end+1.5*n1_2_k_s_start-0.5*n2_2_k_s_end;
+      this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*n1_c_start+7.0*n2_c_end-1.5*n1_2_k_s_start+n2_2_k_s_end;
+      this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*n1_c_start-3.0*n2_c_end+0.5*n1_2_k_s_start-0.5*n2_2_k_s_end;
+      n1_s_start=last_length*sin(this->start.heading);
+      n1_2_k_c_start=pow(last_length,2)*this->start.curvature*cos(this->start.heading);
+      n2_s_end=last_length*sin(this->end.heading);
+      n2_2_k_c_end=pow(last_length,2)*this->end.curvature*cos(this->end.heading);
       this->y_coeff[0]=this->start.y;
-      this->y_coeff[1]=l_s_start;
-      this->y_coeff[2]=0.5*l2_k_c_start;
-      this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*l_s_start-4.0*l_s_end-1.5*l2_k_c_start+0.5*l2_k_c_end;
-      this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*l_s_start+7.0*l_s_end+1.5*l2_k_c_start-l2_k_c_end;
-      this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*l_s_start-3.0*l_s_end-0.5*l2_k_c_start+0.5*l2_k_c_end;
+      this->y_coeff[1]=n1_s_start;
+      this->y_coeff[2]=0.5*n1_2_k_c_start;
+      this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*n1_s_start-4.0*n2_s_end-1.5*n1_2_k_c_start+0.5*n2_2_k_c_end;
+      this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*n1_s_start+7.0*n2_s_end+1.5*n1_2_k_c_start-n2_2_k_c_end;
+      this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*n1_s_start-3.0*n2_s_end-0.5*n1_2_k_c_start+0.5*n2_2_k_c_end;
       new_length=0.0;
       if(this->x!=NULL)
         delete[] this->x;
@@ -677,40 +679,42 @@ void CG2Spline::generate(double resolution,unsigned int iterations)
 
 void CG2Spline::generate(double resolution,double length,unsigned int iterations)
 {
-  double l_c_start,l2_k_s_start,l_s_start,l2_k_c_start;
-  double l_c_end,l2_k_s_end,l_s_end,l2_k_c_end;
+  double n1_c_start,n1_2_k_s_start,n1_s_start,n1_2_k_c_start;
+  double n2_c_end,n2_2_k_s_end,n2_s_end,n2_2_k_c_end;
   double u,pow_u,new_length,last_length;
-  unsigned int i;
+  unsigned int i,max_num_points;
 
   this->resolution=resolution;
   last_length=0.0;
   new_length=length;
+  max_num_points=10.0*(length/resolution);
+  this->num_points=0;
   if(new_length>this->resolution)
   {
-    while(iterations>0 && fabs(new_length-last_length)>this->resolution)
+    while(iterations>0 && fabs(new_length-last_length)>this->resolution && this->num_points<max_num_points)
     {
       last_length=new_length;
       this->num_points=ceil(last_length/resolution);
-      l_c_start=last_length*cos(this->start.heading);
-      l2_k_s_start=pow(last_length,2)*this->start.curvature*sin(this->start.heading);
-      l_c_end=last_length*cos(this->end.heading);
-      l2_k_s_end=pow(last_length,2)*this->end.curvature*sin(this->end.heading);
+      n1_c_start=last_length*cos(this->start.heading);
+      n1_2_k_s_start=pow(last_length,2)*this->start.curvature*sin(this->start.heading);
+      n2_c_end=last_length*cos(this->end.heading);
+      n2_2_k_s_end=pow(last_length,2)*this->end.curvature*sin(this->end.heading);
       this->x_coeff[0]=this->start.x;
-      this->x_coeff[1]=l_c_start;
-      this->x_coeff[2]=-0.5*l2_k_s_start;
-      this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*l_c_start-4.0*l_c_end+1.5*l2_k_s_start-0.5*l2_k_s_end;
-      this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*l_c_start+7.0*l_c_end-1.5*l2_k_s_start+l2_k_s_end;
-      this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*l_c_start-3.0*l_c_end+0.5*l2_k_s_start-0.5*l2_k_s_end;
-      l_s_start=last_length*sin(this->start.heading);
-      l2_k_c_start=pow(last_length,2)*this->start.curvature*cos(this->start.heading);
-      l_s_end=last_length*sin(this->end.heading);
-      l2_k_c_end=pow(last_length,2)*this->end.curvature*cos(this->end.heading);
+      this->x_coeff[1]=n1_c_start;
+      this->x_coeff[2]=-0.5*n1_2_k_s_start;
+      this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*n1_c_start-4.0*n2_c_end+1.5*n1_2_k_s_start-0.5*n2_2_k_s_end;
+      this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*n1_c_start+7.0*n2_c_end-1.5*n1_2_k_s_start+n2_2_k_s_end;
+      this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*n1_c_start-3.0*n2_c_end+0.5*n1_2_k_s_start-0.5*n2_2_k_s_end;
+      n1_s_start=last_length*sin(this->start.heading);
+      n1_2_k_c_start=pow(last_length,2)*this->start.curvature*cos(this->start.heading);
+      n2_s_end=last_length*sin(this->end.heading);
+      n2_2_k_c_end=pow(last_length,2)*this->end.curvature*cos(this->end.heading);
       this->y_coeff[0]=this->start.y;
-      this->y_coeff[1]=l_s_start;
-      this->y_coeff[2]=0.5*l2_k_c_start;
-      this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*l_s_start-4.0*l_s_end-1.5*l2_k_c_start+0.5*l2_k_c_end;
-      this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*l_s_start+7.0*l_s_end+1.5*l2_k_c_start-l2_k_c_end;
-      this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*l_s_start-3.0*l_s_end-0.5*l2_k_c_start+0.5*l2_k_c_end;
+      this->y_coeff[1]=n1_s_start;
+      this->y_coeff[2]=0.5*n1_2_k_c_start;
+      this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*n1_s_start-4.0*n2_s_end-1.5*n1_2_k_c_start+0.5*n2_2_k_c_end;
+      this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*n1_s_start+7.0*n2_s_end+1.5*n1_2_k_c_start-n2_2_k_c_end;
+      this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*n1_s_start-3.0*n2_s_end-0.5*n1_2_k_c_start+0.5*n2_2_k_c_end;
       new_length=0.0;
       if(this->x!=NULL)
         delete[] this->x;
@@ -768,9 +772,113 @@ void CG2Spline::generate(double resolution,double length,unsigned int iterations
   this->generated=true;
 }
 
-TPoint CG2Spline::evaluate(double length)
+void CG2Spline::generate(double &resolution,double n1,double n2, double n3, double n4)
 {
-  return this->evaluate_parameter(this->find_parameter(length));
+  double n1_c_start,n1_2_k_s_start,n1_s_start,n1_2_k_c_start;
+  double n3_c_start,n3_s_start;
+  double n2_c_end,n2_2_k_s_end,n2_s_end,n2_2_k_c_end;
+  double n4_c_end,n4_s_end;
+  double u,pow_u,length,new_length;
+  unsigned int i;
+
+  this->resolution=resolution;
+  length=sqrt(pow(this->end.x-this->start.x,2)+pow(this->end.y-this->start.y,2));
+  if(length>this->resolution)
+  {
+    this->num_points=ceil(length/resolution);
+    n1_c_start=n1*cos(this->start.heading);
+    n3_c_start=n3*cos(this->start.heading);
+    n1_2_k_s_start=pow(n1,2)*this->start.curvature*sin(this->start.heading);
+    n2_c_end=n2*cos(this->end.heading);
+    n4_c_end=n4*cos(this->end.heading);
+    n2_2_k_s_end=pow(n2,2)*this->end.curvature*sin(this->end.heading);
+    this->x_coeff[0]=this->start.x;
+    this->x_coeff[1]=n1_c_start;
+    this->x_coeff[2]=0.5*n3_c_start-0.5*n1_2_k_s_start;
+    this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*n1_c_start-1.5*n3_c_start-4.0*n2_c_end+0.5*n4_c_end+1.5*n1_2_k_s_start-0.5*n2_2_k_s_end;
+    this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*n1_c_start+1.5*n3_c_start+7.0*n2_c_end-n4_c_end-1.5*n1_2_k_s_start+n2_2_k_s_end;
+    this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*n1_c_start-0.5*n3_c_start-3.0*n2_c_end+0.5*n4_c_end+0.5*n1_2_k_s_start-0.5*n2_2_k_s_end;
+    n1_s_start=n1*sin(this->start.heading);
+    n3_s_start=n3*sin(this->start.heading);
+    n1_2_k_c_start=pow(n1,2)*this->start.curvature*cos(this->start.heading);
+    n2_s_end=n2*sin(this->end.heading);
+    n4_s_end=n4*sin(this->end.heading);
+    n2_2_k_c_end=pow(n2,2)*this->end.curvature*cos(this->end.heading);
+    this->y_coeff[0]=this->start.y;
+    this->y_coeff[1]=n1_s_start;
+    this->y_coeff[2]=0.5*n3_s_start+0.5*n1_2_k_c_start;
+    this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*n1_s_start-1.5*n3_s_start-4.0*n2_s_end+0.5*n4_s_end-1.5*n1_2_k_c_start+0.5*n2_2_k_c_end;
+    this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*n1_s_start+1.5*n3_s_start+7.0*n2_s_end-n4_s_end+1.5*n1_2_k_c_start-n2_2_k_c_end;
+    this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*n1_s_start-0.5*n3_s_start-3.0*n2_s_end+0.5*n4_s_end-0.5*n1_2_k_c_start+0.5*n2_2_k_c_end;
+    if(this->x!=NULL)
+      delete[] this->x;
+    this->x=new double[num_points];
+    if(this->y!=NULL)
+      delete[] this->y;
+    this->y=new double[num_points];
+    if(this->length!=NULL)
+      delete[] this->length;
+    this->length=new double[num_points];
+    this->x[0]=this->x_coeff[0];
+    this->y[0]=this->y_coeff[0];
+    this->length[0]=0.0;
+    new_length=0.0;
+    for(i=1,u=(1.0/(num_points-1));i<num_points;i++,u+=(1.0/(num_points-1)))
+    {
+      this->x[i]=this->x_coeff[0];
+      this->y[i]=this->y_coeff[0];
+      pow_u=u;
+      this->x[i]+=this->x_coeff[1]*pow_u;
+      this->y[i]+=this->y_coeff[1]*pow_u;
+      pow_u*=u;
+      this->x[i]+=this->x_coeff[2]*pow_u;
+      this->y[i]+=this->y_coeff[2]*pow_u;
+      pow_u*=u;
+      this->x[i]+=this->x_coeff[3]*pow_u;
+      this->y[i]+=this->y_coeff[3]*pow_u;
+      pow_u*=u;
+      this->x[i]+=this->x_coeff[4]*pow_u;
+      this->y[i]+=this->y_coeff[4]*pow_u;
+      pow_u*=u;
+      this->x[i]+=this->x_coeff[5]*pow_u;
+      this->y[i]+=this->y_coeff[5]*pow_u;
+      new_length+=sqrt(pow(this->x[i]-this->x[i-1],2)+pow(this->y[i]-this->y[i-1],2));
+      this->length[i]=new_length;
+    }
+    this->resolution=new_length/num_points;
+    resolution=this->resolution;
+  }
+  else
+  {
+    this->num_points=1;
+    if(this->x!=NULL)
+      delete[] this->x;
+    this->x=new double[num_points];
+    x[0]=this->start.x;
+    if(this->y!=NULL)
+      delete[] this->y;
+    this->y=new double[num_points];
+    y[0]=this->start.y;
+    if(this->length!=NULL)
+      delete[] this->length;
+    this->length=new double[num_points];
+    this->length[0]=0.0;
+  }
+  this->generated=true;
+}
+
+bool CG2Spline::evaluate(double length, TPoint& point)
+{
+  if(!this->generated)
+    this->generate(DEFAULT_RESOLUTION);
+  if (length < -this->resolution || length > (this->length[this->num_points-1]+this->resolution))
+    return false;
+  else if(length<0.0)
+    length=0.0;
+  else if(length>this->length[this->num_points-1])
+    length=this->length[this->num_points-1];
+  point = this->evaluate_parameter(this->find_parameter(length));
+  return true;
 }
 
 void CG2Spline::evaluate_all(std::vector<double> &x, std::vector<double> &y,std::vector<double> &curvature,std::vector<double> &heading)
@@ -872,6 +980,7 @@ double CG2Spline::get_max_curvature_der(double max_error,unsigned int max_iter)
 
 double CG2Spline::find_closest_point(double x, double y, TPoint &point,double max_error,unsigned int max_iter)
 {
+  unsigned int point_index;
   double start_point;
   bool beyond_limits;
 
@@ -885,8 +994,14 @@ double CG2Spline::find_closest_point(double x, double y, TPoint &point,double ma
     if(!beyond_limits)
     {
       point=this->evaluate_parameter(this->distance_grad.get_coordinate());
+      point_index=(unsigned int)ceil(this->distance_grad.get_coordinate()*(this->num_points-1));
       if(this->num_points>0)
-        return this->length[(unsigned int)ceil(this->distance_grad.get_coordinate()*(this->num_points-1))];
+      {
+        if(point_index>=this->num_points)
+          return this->length[this->num_points-1];
+        else
+          return this->length[point_index];
+      }
       else
         return 0.0;
     }
@@ -897,6 +1012,7 @@ double CG2Spline::find_closest_point(double x, double y, TPoint &point,double ma
 
 double CG2Spline::find_closest_point(double x, double y,double max_error,unsigned int max_iter)
 {
+  unsigned int point_index;
   double start_point;
   bool beyond_limits;
 
@@ -909,8 +1025,14 @@ double CG2Spline::find_closest_point(double x, double y,double max_error,unsigne
   {
     if(!beyond_limits)
     {
+      point_index=(unsigned int)ceil(this->distance_grad.get_coordinate()*(this->num_points-1));
       if(this->num_points)
-        return this->length[(unsigned int)ceil(this->distance_grad.get_coordinate()*(this->num_points-1))];
+      {
+        if(point_index>=this->num_points)
+          return this->length[this->num_points-1];
+        else
+          return this->length[point_index];
+      }
       else
         return 0.0;
     }
@@ -919,17 +1041,29 @@ double CG2Spline::find_closest_point(double x, double y,double max_error,unsigne
   }
 }
 
-void CG2Spline::get_part(CG2Spline *spline,double start_length, double end_length)
+bool CG2Spline::get_part(CG2Spline *spline,double start_length, double end_length)
 {
+  TPoint p;
+
   if(start_length==0.0)
     spline->set_start_point(this->start);
   else
-    spline->set_start_point(this->evaluate(start_length));
-  if(end_length==-1.0)
+  {
+    if (!this->evaluate(start_length, p))
+      return false;
+    spline->set_start_point(p);
+  }
+
+  if(end_length==std::numeric_limits<double>::max())
     spline->set_end_point(this->end);
   else
-    spline->set_end_point(this->evaluate(end_length));
+  {
+    if (!this->evaluate(end_length, p))
+      return false;
+    spline->set_end_point(p);
+  }
   spline->generate(this->resolution);
+  return true;
 }
 
 double CG2Spline::curvature(double u)
diff --git a/src/junction.cpp b/src/junction.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bc5c9a24c617822fda61ddaf6f0a54ed4161b201
--- /dev/null
+++ b/src/junction.cpp
@@ -0,0 +1,743 @@
+#include "junction.h"
+#include "exceptions.h"
+
+CJunction::CJunction()
+{
+  this->id=-1;
+  this->resolution=0.1;
+  this->parent_roadmap=NULL;
+  this->incomming_roads.clear();
+  this->outgoing_roads.clear();
+  this->segments.clear();
+}
+
+CJunction::CJunction(unsigned int id)
+{
+  this->set_id(id);
+  this->resolution=0.1;
+  this->parent_roadmap=NULL;
+  this->incomming_roads.clear();
+  this->outgoing_roads.clear();
+  this->segments.clear();
+}
+
+void CJunction::set_parent_roadmap(CRoadMap *roadmap)
+{
+  if(roadmap==NULL)
+    throw CException(_HERE_,"Invalid roadmap reference");
+  this->parent_roadmap=roadmap;
+}
+
+void CJunction::set_id(unsigned int id)
+{
+  this->id=id;
+}
+
+unsigned int CJunction::get_index_by_id(const std::vector<CJunction *> &junctions,unsigned int id)
+{
+  std::vector<CJunction *>::const_iterator it;
+  unsigned int index;
+
+  for(it=junctions.begin(),index=0;it!=junctions.end();it++,index++)
+    if((*it)->id==id)
+      return index;
+
+  return -1;
+}
+
+CRoadMap &CJunction::get_parent_roadmap(void)
+{
+  if(this->parent_roadmap==NULL)
+    throw CException(_HERE_,"No parent road map has been defined");
+  return *this->parent_roadmap;
+}
+
+unsigned int CJunction::get_id(void)
+{
+  return this->id;
+}
+
+void CJunction::set_resolution(double resolution)
+{
+  this->resolution=resolution;
+  for(unsigned int i=0;i<this->segments.size();i++)
+    this->segments[i]->set_resolution(resolution);
+}
+
+double CJunction::get_resolution(double resolution)
+{
+  return this->resolution;
+}
+
+unsigned int CJunction::get_num_incomming_roads(void)
+{
+  return this->incomming_roads.size();
+}
+
+CRoad *CJunction::get_incomming_road_by_index(unsigned int index)
+{
+  if(index<0 || index >= this->incomming_roads.size())
+    throw CException(_HERE_,"Invalid incomming road index");
+  return this->incomming_roads[index];
+}
+
+CRoad &CJunction::get_incomming_road_by_id(unsigned int id)
+{
+  unsigned int index;
+
+  index=CRoad::get_index_by_id(this->incomming_roads,id);
+  return *this->get_incomming_road_by_index(index);
+}
+
+unsigned int CJunction::get_incomming_road_index_by_id(unsigned int id)
+{
+  return CRoad::get_index_by_id(this->incomming_roads,id);
+}
+
+bool CJunction::has_incomming_road(unsigned int id)
+{
+  if(CRoad::get_index_by_id(this->incomming_roads,id)==(unsigned int)-1)
+    return false;
+  else
+    return true;
+}
+
+unsigned int CJunction::get_num_outgoing_roads(void)
+{
+  return this->outgoing_roads.size();
+}
+
+CRoad *CJunction::get_outgoing_road_by_index(unsigned int index)
+{
+  if(index<0 || index >= this->outgoing_roads.size())
+    throw CException(_HERE_,"Invalid outgoing road index");
+  return this->outgoing_roads[index];
+}
+
+unsigned int CJunction::get_outgoing_road_index_by_id(unsigned int id)
+{
+  return CRoad::get_index_by_id(this->outgoing_roads,id);
+}
+
+CRoad &CJunction::get_outgoing_road_by_id(unsigned int id)
+{
+  unsigned int index;
+
+  index=CRoad::get_index_by_id(this->outgoing_roads,id);
+  return *this->get_outgoing_road_by_index(index);
+}
+
+bool CJunction::has_outgoing_road(unsigned int id)
+{
+  if(CRoad::get_index_by_id(this->outgoing_roads,id)==(unsigned int)-1)
+    return false;
+  else
+    return true;
+}
+
+void CJunction::add_incomming_road(CRoad *road)
+{
+  unsigned int num_rows;
+  unsigned int num_cols;
+
+  if(road==NULL)
+    throw CException(_HERE_,"Invalid incomming road reference");
+  if(this->has_incomming_road(road->get_id()))
+    throw CException(_HERE_,"Incomming road already present");
+  if(road->get_num_segments()==0)
+    throw CException(_HERE_,"Incomming road does not have any geometry");
+  if(road->get_num_lanes()==0)
+    throw CException(_HERE_,"Incomming road does not have any lane");
+  this->incomming_roads.push_back(road);
+  /* add a row to the connectivity matrix */
+  if(this->incomming_roads.size()>0 && this->outgoing_roads.size()>0)
+  {
+    num_rows=this->incomming_roads.size();
+    num_cols=this->outgoing_roads.size();
+    this->connectivity.conservativeResize(num_rows,num_cols);
+    for(unsigned int i=0;i<num_cols;i++)
+      this->connectivity(num_rows-1,i)=0;
+  }
+  /* update road information */
+  road->set_next_junction(this);
+}
+
+void CJunction::remove_incomming_road_by_index(unsigned int index)
+{
+  unsigned int num_rows=this->connectivity.rows();
+  unsigned int num_cols=this->connectivity.cols();
+  std::vector<CRoad *>::iterator it;
+  unsigned int i;
+
+  if(index<0 || index >= this->incomming_roads.size())
+    throw CException(_HERE_,"Invalid incomming road index");
+  for(it=this->incomming_roads.begin(),i=0;it!=this->incomming_roads.end();it++,i++)
+  {
+    if(i==index)
+    {
+      /* update road information */
+      (*it)->set_next_junction(NULL);
+      this->incomming_roads.erase(it);
+      break;
+    }
+  }
+  /* remove the corresponding row from the connectivity matrix */
+  this->connectivity.block(index,0,num_rows-index-1,num_cols) = this->connectivity.block(index+1,0,num_rows-index-1,num_cols);
+  this->connectivity.conservativeResize(num_rows-1,num_cols);
+}
+
+void CJunction::remove_incomming_road_by_id(unsigned int id)
+{
+  unsigned int index;
+
+  index=CRoad::get_index_by_id(this->incomming_roads,id);
+  this->remove_incomming_road_by_index(index);
+}
+
+void CJunction::add_outgoing_road(CRoad *road)
+{
+  unsigned int num_rows;
+  unsigned int num_cols;
+
+  if(road==NULL)
+    throw CException(_HERE_,"Invalid outgoing road reference");
+  if(this->has_outgoing_road(road->get_id()))
+    throw CException(_HERE_,"Outgoing road already present");
+  if(road->get_num_segments()==0)
+    throw CException(_HERE_,"Outgoing road does not have any geometry");
+  if(road->get_num_lanes()==0)
+    throw CException(_HERE_,"Outgoing road does not have any lane");
+  this->outgoing_roads.push_back(road);
+  /* add a column to the connectivity matrix */
+  if(this->incomming_roads.size()>0 && this->outgoing_roads.size()>0)
+  {
+    num_rows=this->incomming_roads.size();
+    num_cols=this->outgoing_roads.size();
+    this->connectivity.conservativeResize(num_rows,num_cols);
+    for(unsigned int i=0;i<num_rows;i++)
+      this->connectivity(i,num_cols-1)=0;
+  }
+  /* update road information */
+  road->set_prev_junction(this);
+}
+
+void CJunction::remove_outgoing_road_by_index(unsigned int index)
+{
+  unsigned int num_rows=this->connectivity.rows();
+  unsigned int num_cols=this->connectivity.cols();
+  std::vector<CRoad *>::iterator it;
+  unsigned int i;
+
+  if(index<0 || index >= this->outgoing_roads.size())
+    throw CException(_HERE_,"Invalid outgoing road index");
+  for(it=this->outgoing_roads.begin(),i=0;it!=this->outgoing_roads.end();it++,i++)
+  {
+    if(i==index)
+    {
+      /* update road information */
+      (*it)->set_prev_junction(NULL);
+      this->outgoing_roads.erase(it);
+      break;
+    }
+  }
+  /* remove the corresponding column from the connectivity matrix */
+  this->connectivity.block(0,index,num_rows,num_cols-index-1) = this->connectivity.block(0,index+1,num_rows,num_cols-index-1);
+  this->connectivity.conservativeResize(num_rows,num_cols-1);
+}
+
+void CJunction::remove_outgoing_road_by_id(unsigned int id)
+{
+  unsigned int index;
+
+  index=CRoad::get_index_by_id(this->outgoing_roads,id);
+  this->remove_outgoing_road_by_index(index);
+}
+
+void CJunction::link_roads_by_index(unsigned int incomming_road,unsigned int outgoing_road)
+{
+  CRoadSegment *new_segment,*in_last_segment,*out_first_segment;
+  TPoint start_point,end_point;
+
+  if(incomming_road<0 || incomming_road >= this->incomming_roads.size())
+    throw CException(_HERE_,"Invalid incomming road index");
+  if(outgoing_road<0 || outgoing_road >= this->outgoing_roads.size())
+    throw CException(_HERE_,"Invalid outgoing road index");
+  /* update the connectivity matrix */
+  this->connectivity(incomming_road,outgoing_road)=1;
+  /* create a new road segment */
+  in_last_segment=this->incomming_roads[incomming_road]->get_last_segment();
+  out_first_segment=this->outgoing_roads[outgoing_road]->get_first_segment();
+  in_last_segment->get_end_point(start_point);
+  out_first_segment->get_start_point(end_point);
+  new_segment=new CRoadSegment(this->incomming_roads[incomming_road]->get_num_lanes(),this->outgoing_roads[outgoing_road]->get_num_lanes());
+  new_segment->set_resolution(this->resolution);
+  new_segment->set_parent_junction(this);
+  new_segment->set_geometry(start_point,end_point);
+  in_last_segment->add_next_segment(new_segment);
+  new_segment->add_prev_segment(in_last_segment);
+  new_segment->add_next_segment(out_first_segment);
+  out_first_segment->add_prev_segment(new_segment);
+  if(this->parent_roadmap!=NULL)
+    new_segment->id=this->parent_roadmap->get_next_segment_id();
+  this->segments.push_back(new_segment);
+}
+
+void CJunction::link_roads_by_id(unsigned int incomming_road,unsigned int outgoing_road)
+{
+  unsigned int incomming_index,outgoing_index;
+
+  incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road);
+  outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road);
+  this->link_roads_by_index(incomming_index,outgoing_index);
+}
+
+void CJunction::unlink_roads_by_index(unsigned int incomming_road,unsigned int outgoing_road)
+{
+  CRoadSegment *in_last_segment,*out_first_segment;
+  std::vector<CRoadSegment *>::iterator it;
+
+  if(incomming_road<0 || incomming_road >= this->incomming_roads.size())
+    throw CException(_HERE_,"Invalid incomming road index");
+  if(outgoing_road<0 || outgoing_road >= this->outgoing_roads.size())
+    throw CException(_HERE_,"Invalid outgoing road index");
+  /* update the connectivity matrix */
+  this->connectivity(incomming_road,outgoing_road)=0;
+  /* delete road segment */
+  in_last_segment=this->incomming_roads[incomming_road]->get_last_segment();
+  out_first_segment=this->outgoing_roads[outgoing_road]->get_first_segment();
+  for(it=this->segments.begin();it!=this->segments.end();it++)
+  {
+    if((*it)->has_prev_segment(in_last_segment) && (*it)->has_next_segment(out_first_segment))
+    {
+      in_last_segment->remove_next_segment(*it);
+      out_first_segment->remove_prev_segment(*it);
+      delete *it;
+      this->segments.erase(it);
+      break;
+    }
+  }
+}
+
+void CJunction::unlink_roads_by_id(unsigned int incomming_road,unsigned int outgoing_road)
+{
+  unsigned int incomming_index,outgoing_index;
+
+  incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road);
+  outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road);
+  this->unlink_roads_by_index(incomming_index,outgoing_index);
+}
+
+bool CJunction::are_roads_linked_by_index(unsigned int incomming_road,unsigned int outgoing_road)
+{
+  if(incomming_road<0 || incomming_road >= this->incomming_roads.size())
+    throw CException(_HERE_,"Invalid incomming road index");
+  if(outgoing_road<0 || outgoing_road >= this->outgoing_roads.size())
+    throw CException(_HERE_,"Invalid outgoing road index");
+  /* update the connectivity matrix */
+  if(this->connectivity(incomming_road,outgoing_road)==1)
+    return true;
+  else
+    return false;
+}
+
+bool CJunction::are_roads_linked_by_id(unsigned int incomming_road,unsigned int outgoing_road)
+{
+  unsigned int incomming_index,outgoing_index;
+
+  incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road);
+  outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road);
+  return this->are_roads_linked_by_index(incomming_index,outgoing_index);
+}
+
+CRoadSegment *CJunction::link_road_to_point(unsigned int incomming_road,TPoint &outgoing_point,unsigned int outgoing_num_lanes)
+{
+  CRoadSegment *new_segment,*in_last_segment;
+  TPoint start_point,end_point;
+  unsigned int incomming_index;
+
+  incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road);
+  if(incomming_index<0 || incomming_index >= this->incomming_roads.size())
+    throw CException(_HERE_,"Invalid incomming road index");
+  /* connectivity not updated */
+  /* create a new road segment */
+  in_last_segment=this->incomming_roads[incomming_index]->get_last_segment();
+  in_last_segment->get_end_point(start_point);
+  end_point=outgoing_point;
+  new_segment=new CRoadSegment(this->incomming_roads[incomming_index]->get_num_lanes(),outgoing_num_lanes);
+  new_segment->set_resolution(this->resolution);
+  new_segment->set_parent_junction(this);
+  new_segment->set_geometry(start_point,end_point);
+  in_last_segment->add_next_segment(new_segment);
+  new_segment->add_prev_segment(in_last_segment);
+  if(this->parent_roadmap!=NULL)
+    new_segment->id=this->parent_roadmap->get_next_segment_id();
+  this->segments.push_back(new_segment);
+
+  return new_segment;
+}
+
+CRoadSegment *CJunction::link_point_to_road(TPoint &incomming_point,unsigned int incomming_num_lanes,unsigned int outgoing_road)
+{
+  CRoadSegment *new_segment,*out_first_segment;
+  TPoint start_point,end_point;
+  unsigned int outgoing_index;
+
+  outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road);
+  if(outgoing_index<0 || outgoing_index >= this->outgoing_roads.size())
+    throw CException(_HERE_,"Invalid outgoing road index");
+  /* connectivity not updated */
+  /* create a new road segment */
+  out_first_segment=this->outgoing_roads[outgoing_index]->get_first_segment();
+  start_point=incomming_point;
+  out_first_segment->get_start_point(end_point);
+  new_segment=new CRoadSegment(incomming_num_lanes,this->outgoing_roads[outgoing_index]->get_num_lanes());
+  new_segment->set_resolution(this->resolution);
+  new_segment->set_parent_junction(this);
+  new_segment->set_geometry(start_point,end_point);
+  new_segment->add_next_segment(out_first_segment);
+  out_first_segment->add_prev_segment(new_segment);
+  if(this->parent_roadmap!=NULL)
+    new_segment->id=this->parent_roadmap->get_next_segment_id();
+  this->segments.push_back(new_segment);
+
+  return new_segment;
+}
+
+CRoadSegment *CJunction::link_point_to_point(TPoint &incomming_point,unsigned int incomming_num_lanes,TPoint &outgoing_point,unsigned int outgoing_num_lanes)
+{
+  CRoadSegment *new_segment;
+  TPoint start_point,end_point;
+
+  /* connectivity not updated */
+  /* create a new road segment */
+  start_point=incomming_point;
+  end_point=outgoing_point;
+  new_segment=new CRoadSegment(incomming_num_lanes,outgoing_num_lanes);
+  new_segment->set_resolution(this->resolution);
+  new_segment->set_parent_junction(this);
+  new_segment->set_geometry(start_point,end_point);
+  if(this->parent_roadmap!=NULL)
+    new_segment->id=this->parent_roadmap->get_next_segment_id();
+  this->segments.push_back(new_segment);
+
+  return new_segment;
+}
+
+void CJunction::link_lanes_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out)
+{
+  CRoadSegment *segment;
+
+  if(incomming_road<0 || incomming_road >= this->incomming_roads.size())
+    throw CException(_HERE_,"Invalid incomming road index");
+  if(outgoing_road<0 || outgoing_road >= this->outgoing_roads.size())
+    throw CException(_HERE_,"Invalid outgoing road index");
+  if(lane_in>=this->incomming_roads[incomming_road]->get_num_lanes())
+    throw CException(_HERE_,"Incomming road does not have that many lanes");
+  if(lane_out>=this->outgoing_roads[outgoing_road]->get_num_lanes())
+    throw CException(_HERE_,"Outgoing road does not have that many lanes");
+  segment=this->get_segment_between_by_index(incomming_road,outgoing_road);
+  if(segment==NULL)
+    throw CException(_HERE_,"No segment links the desired roads");
+  segment->link_lanes(lane_in,lane_out);
+}
+
+void CJunction::link_lanes_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out)
+{
+  unsigned int incomming_index,outgoing_index;
+
+  incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road);
+  outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road);
+  this->link_lanes_by_index(incomming_index,lane_in,outgoing_index,lane_out);
+}
+
+void CJunction::link_same_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road)
+{
+  unsigned int num_lanes;
+  unsigned int incomming_index,outgoing_index;
+
+  incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road);
+  outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road);
+
+  num_lanes=std::min(this->incomming_roads[incomming_index]->get_num_lanes(),this->outgoing_roads[outgoing_index]->get_num_lanes());
+  for(unsigned int i=0;i<num_lanes;i++)
+    this->link_lanes_by_index(incomming_index,i,outgoing_index,i);
+}
+
+void CJunction::link_full_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road)
+{
+  unsigned int incomming_index,outgoing_index;
+
+  incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road);
+  outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road);
+
+  for(unsigned int i=0;i<this->incomming_roads[incomming_index]->get_num_lanes();i++)
+    for(unsigned int j=0;j<this->outgoing_roads[outgoing_index]->get_num_lanes();j++)
+      this->link_lanes_by_index(incomming_index,i,outgoing_index,j);
+}
+
+void CJunction::unlink_lanes_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out)
+{
+  CRoadSegment *segment;
+
+  if(incomming_road<0 || incomming_road >= this->incomming_roads.size())
+    throw CException(_HERE_,"Invalid incomming road index");
+  if(outgoing_road<0 || outgoing_road >= this->outgoing_roads.size())
+    throw CException(_HERE_,"Invalid outgoing road index");
+  if(lane_in>=this->incomming_roads[incomming_road]->get_num_lanes())
+    throw CException(_HERE_,"Incomming road does not have that many lanes");
+  if(lane_out>=this->outgoing_roads[outgoing_road]->get_num_lanes())
+    throw CException(_HERE_,"Outgoing road does not have that many lanes");
+  segment=this->get_segment_between_by_index(incomming_road,outgoing_road);
+  if(segment==NULL)
+    throw CException(_HERE_,"No segment links the desired roads");
+  segment->unlink_lanes(lane_in,lane_out);
+}
+
+void CJunction::unlink_lanes_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out)
+{
+  unsigned int incomming_index,outgoing_index;
+
+  incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road);
+  outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road);
+  this->unlink_lanes_by_index(incomming_index,lane_in,outgoing_index,lane_out);
+}
+
+void CJunction::unlink_same_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road)
+{
+  unsigned int num_lanes;
+  unsigned int incomming_index,outgoing_index;
+
+  incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road);
+  outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road);
+
+  num_lanes=std::min(this->incomming_roads[incomming_index]->get_num_lanes(),this->outgoing_roads[outgoing_index]->get_num_lanes());
+  for(unsigned int i=0;i<num_lanes;i++)
+    this->unlink_lanes_by_index(incomming_index,i,outgoing_index,i);
+}
+
+void CJunction::unlink_full_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road)
+{
+  unsigned int incomming_index,outgoing_index;
+
+  incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road);
+  outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road);
+
+  for(unsigned int i=0;i<this->incomming_roads[incomming_index]->get_num_lanes();i++)
+    for(unsigned int j=0;j<this->outgoing_roads[outgoing_index]->get_num_lanes();j++)
+      this->unlink_lanes_by_index(incomming_index,i,outgoing_index,j);
+}
+
+bool CJunction::are_lanes_linked_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out)
+{
+  CRoadSegment *segment;
+
+  if(incomming_road<0 || incomming_road >= this->incomming_roads.size())
+    throw CException(_HERE_,"Invalid incomming road index");
+  if(outgoing_road<0 || outgoing_road >= this->outgoing_roads.size())
+    throw CException(_HERE_,"Invalid outgoing road index");
+  if(lane_in>=this->incomming_roads[incomming_road]->get_num_lanes())
+    throw CException(_HERE_,"Incomming road does not have that many lanes");
+  if(lane_out>=this->outgoing_roads[outgoing_road]->get_num_lanes())
+    throw CException(_HERE_,"Outgoing road does not have that many lanes");
+  segment=this->get_segment_between_by_index(incomming_road,outgoing_road);
+  if(segment==NULL)
+    throw CException(_HERE_,"No segment links the desired roads");
+  return segment->are_lanes_linked(lane_in,lane_out);
+}
+
+bool CJunction::are_lanes_linked_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out)
+{
+  unsigned int incomming_index,outgoing_index;
+
+  incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road);
+  outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road);
+  return this->are_lanes_linked_by_index(incomming_index,lane_in,outgoing_index,lane_out);
+}
+
+void CJunction::get_connectivity_matrix(Eigen::MatrixXi &connectivity,std::vector<unsigned int> &incomming_id_map,std::vector<unsigned int> &outgoing_id_map)
+{
+  incomming_id_map.clear();
+  for(unsigned int i=0;i<this->incomming_roads.size();i++)
+    incomming_id_map.push_back(this->incomming_roads[i]->get_id());
+  outgoing_id_map.clear();
+  for(unsigned int i=0;i<this->outgoing_roads.size();i++)
+    outgoing_id_map.push_back(this->outgoing_roads[i]->get_id());
+  connectivity=this->connectivity;
+}
+
+/* geometry */
+unsigned int CJunction::get_num_segments(void)
+{
+  return this->segments.size();
+}
+
+CRoadSegment *CJunction::get_segment_by_index(unsigned int index)
+{
+  if(index<0 || index >= this->segments.size())
+    throw CException(_HERE_,"Invalid segment index");
+  return this->segments[index];
+}
+
+CRoadSegment &CJunction::get_segment_by_id(unsigned int id)
+{
+  unsigned int index;
+
+  index=CRoadSegment::get_index_by_id(this->segments,id);
+  return *this->get_segment_by_index(index);
+}
+
+CRoadSegment &CJunction::get_segment_between_by_id(unsigned int incomming_road,unsigned int outgoing_road)
+{
+  unsigned int incomming_index,outgoing_index;
+
+  incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road);
+  outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road);
+  return *this->get_segment_between_by_index(incomming_index,outgoing_index);
+}
+
+CRoadSegment *CJunction::get_segment_between_by_index(unsigned int incomming_road,unsigned int outgoing_road)
+{
+  CRoadSegment *in_last_segment,*out_first_segment;
+  std::vector<CRoadSegment *>::const_iterator it;
+
+  if(incomming_road<0 || incomming_road >= this->incomming_roads.size())
+    throw CException(_HERE_,"Invalid incomming road index");
+  if(outgoing_road<0 || outgoing_road >= this->outgoing_roads.size())
+    throw CException(_HERE_,"Invalid outgoing road index");
+  in_last_segment=this->incomming_roads[incomming_road]->get_last_segment();
+  out_first_segment=this->outgoing_roads[outgoing_road]->get_first_segment();
+  for(it=this->segments.begin();it!=this->segments.end();it++)
+  {
+    if((*it)->has_prev_segment(in_last_segment) && (*it)->has_next_segment(out_first_segment))
+    {
+      return *it;
+    }
+  }
+
+  return NULL;
+}
+
+bool CJunction::get_closest_segment_id(TPoint &point,unsigned int &segment_id,double &distance,double &lateral_offset,double angle_tol)
+{
+  double min_offset=std::numeric_limits<double>::max(),tmp_offset,tmp_distance;
+  TPoint tmp_point;
+
+  for(unsigned int i=0;i<this->segments.size();i++)
+  {
+    if(this->segments[i]->get_closest_point(point,tmp_point,tmp_distance,tmp_offset,angle_tol))
+    {
+      if(fabs(tmp_offset)<min_offset)
+      {
+        min_offset=fabs(tmp_offset);
+        distance=tmp_distance;
+        lateral_offset=tmp_offset;
+        segment_id=this->segments[i]->get_id();
+      }
+    }
+  }
+  if(min_offset!=std::numeric_limits<double>::max())
+    return true;
+  else
+    return false;
+}
+
+bool CJunction::get_closest_segment_ids(TPoint &point,std::vector<unsigned int >&segment_ids,std::vector<double >&distances,std::vector<double> &lateral_offsets,double angle_tol,double offset_tol)
+{
+  double tmp_offset,tmp_distance;
+  TPoint tmp_point;
+
+  segment_ids.clear();
+  distances.clear();
+  lateral_offsets.clear();
+  for(unsigned int i=0;i<this->segments.size();i++)
+  {
+    if(this->segments[i]->get_closest_point(point,tmp_point,tmp_distance,tmp_offset,angle_tol))
+    {
+      if(fabs(tmp_offset)<offset_tol)
+      {
+        distances.push_back(tmp_distance);
+        lateral_offsets.push_back(tmp_offset);
+        segment_ids.push_back(this->segments[i]->get_id());
+      }
+    }
+  }
+  if(segment_ids.size()>0)
+    return true;
+  else
+    return false;
+
+}
+
+void CJunction::get_segment_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw)
+{
+  std::vector<double> partial_x,partial_y,partial_heading;
+
+  x.clear();
+  y.clear();
+  yaw.clear();
+  for(unsigned int i=0;i<this->segments.size();i++)
+  {
+    this->segments[i]->get_geometry(partial_x,partial_y,partial_heading);
+    x.insert(x.end(),partial_x.begin(),partial_x.end());
+    y.insert(y.end(),partial_y.begin(),partial_y.end());
+    yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end());
+  }
+}
+
+void CJunction::get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw)
+{
+  std::vector<double> partial_x,partial_y,partial_heading,curvature;
+  TPoint start_point,end_point;
+  double lateral_offset_in,lateral_offset_out,width;
+  CG2Spline *segment_spline;
+
+  x.clear();
+  y.clear();
+  yaw.clear();
+  for(unsigned int i=0;i<this->segments.size();i++)
+  {
+    width=this->segments[i]->get_lane_width();
+    if(width==0.0)
+      continue;
+    
+    lateral_offset_in=-width/2.0;
+    for(unsigned int j=0;j<this->segments[i]->get_num_lanes_in();j++)
+    {
+      this->segments[i]->get_point_at(0.0,lateral_offset_in,start_point);
+      lateral_offset_out=-width/2.0;
+      for(unsigned int k=0;k<this->segments[i]->get_num_lanes_out();k++)
+      {
+        this->segments[i]->get_point_at(this->segments[i]->get_length(),lateral_offset_out,end_point);
+        if(this->segments[i]->are_lanes_linked(j,k))
+        {
+          segment_spline=new CG2Spline(start_point,end_point);
+          segment_spline->evaluate_all(partial_x,partial_y,curvature,partial_heading);
+          x.insert(x.end(),partial_x.begin(),partial_x.end());
+          y.insert(y.end(),partial_y.begin(),partial_y.end());
+          yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end());
+          delete segment_spline;
+        }
+        lateral_offset_out-=width;
+      }
+      lateral_offset_in-=width;
+    }
+  }
+}
+
+CJunction::~CJunction()
+{
+  this->id=-1;
+  this->parent_roadmap=NULL;
+  this->incomming_roads.clear();
+  this->outgoing_roads.clear();
+  for(unsigned int i=0;i<this->segments.size();i++)
+  {
+    if(this->segments[i]!=NULL)
+    {
+      delete this->segments[i];
+      this->segments[i]=NULL;
+    }
+  }
+  this->segments.clear();
+}
+
diff --git a/src/opendrive_arc.cpp b/src/opendrive/opendrive_arc.cpp
similarity index 65%
rename from src/opendrive_arc.cpp
rename to src/opendrive/opendrive_arc.cpp
index 4d18947e7b54670e93488c37bbb7447aad5ac224..371a49cfea1d104cf67539f84f4cafa9a012da55 100644
--- a/src/opendrive_arc.cpp
+++ b/src/opendrive/opendrive_arc.cpp
@@ -1,4 +1,5 @@
-#include "opendrive_arc.h"
+#include "opendrive/opendrive_arc.h"
+#include "common.h"
 #include <cmath>
 
 COpendriveArc::COpendriveArc()
@@ -11,13 +12,18 @@ COpendriveArc::COpendriveArc(const COpendriveArc &object) : COpendriveGeometry(o
   this->curvature=object.curvature;
 }
 
+COpendriveArc::COpendriveArc(TOpendriveWorldPose &start_pose,double length,double curvature):COpendriveGeometry(start_pose,length)
+{
+  this->curvature=curvature;
+}
+
 bool COpendriveArc::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
 {
   double alpha;
 
-  alpha = track.s*this->curvature*this->scale_factor;
-  local.u = std::sin(alpha)/(this->curvature*this->scale_factor) - track.t*std::sin(alpha);
-  local.v = (1 - std::cos(alpha))/(this->curvature*this->scale_factor) + track.t*std::cos(alpha);
+  alpha = track.s*this->curvature;
+  local.u = std::sin(alpha)/(this->curvature) - track.t*std::sin(alpha);
+  local.v = (1 - std::cos(alpha))/(this->curvature) + track.t*std::cos(alpha);
   local.heading = normalize_angle(track.heading + alpha);
   return true;
 }
@@ -41,6 +47,14 @@ 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);
 }
 
+void COpendriveArc::save_params(planView::geometry_type &geometry_info)
+{
+  ::arc arc;
+
+  arc.curvature(this->curvature);
+  geometry_info.arc(arc);
+}
+
 COpendriveGeometry *COpendriveArc::clone(void)
 {
   COpendriveArc *new_arc=new COpendriveArc(*this);
@@ -50,8 +64,13 @@ COpendriveGeometry *COpendriveArc::clone(void)
 
 void COpendriveArc::get_curvature(double &start,double &end)
 {
-  start=this->curvature*this->scale_factor;
-  end=this->curvature*this->scale_factor;
+  start=this->curvature;
+  end=this->curvature;
+}
+
+void COpendriveArc::get_curvature_at(double length,double &curvature)
+{
+  curvature=this->curvature;
 }
 
 void COpendriveArc::operator=(const COpendriveArc &object)
diff --git a/src/opendrive_geometry.cpp b/src/opendrive/opendrive_geometry.cpp
similarity index 74%
rename from src/opendrive_geometry.cpp
rename to src/opendrive/opendrive_geometry.cpp
index f3380799a6ecf86d299f20045e4a54e5d2cb0c1b..4f7357c9e8f2e22f722a37464e740fa9b2e46241 100644
--- a/src/opendrive_geometry.cpp
+++ b/src/opendrive/opendrive_geometry.cpp
@@ -1,4 +1,5 @@
-#include "opendrive_geometry.h"
+#include "opendrive/opendrive_geometry.h"
+#include "common.h"
 #include <cmath>
 
 COpendriveGeometry::COpendriveGeometry()
@@ -8,7 +9,6 @@ COpendriveGeometry::COpendriveGeometry()
   this->pose.x = 0.0;
   this->pose.y = 0.0;
   this->pose.heading = 0.0;
-  this->scale_factor=DEFAULT_SCALE_FACTOR;
 }
 
 COpendriveGeometry::COpendriveGeometry(const COpendriveGeometry &object)
@@ -18,7 +18,13 @@ COpendriveGeometry::COpendriveGeometry(const COpendriveGeometry &object)
   this->pose.x = object.pose.x;
   this->pose.y = object.pose.y;
   this->pose.heading = object.pose.heading;
-  this->scale_factor = object.scale_factor;
+}
+
+COpendriveGeometry::COpendriveGeometry(TOpendriveWorldPose &start_pose,double length)
+{
+  this->min_s=0.0;
+  this->set_max_s(length);
+  this->set_start_pose(start_pose);
 }
 
 void COpendriveGeometry::print(std::ostream& out)
@@ -40,15 +46,20 @@ void COpendriveGeometry::load(const planView::geometry_type &geometry_info)
   this->load_params(geometry_info);
 }
 
-void COpendriveGeometry::set_scale_factor(double scale)
+void COpendriveGeometry::save(planView::geometry_type &geometry_info)
 {
-  this->scale_factor=scale;
+  geometry_info.s(this->min_s);
+  geometry_info.x(this->pose.x);
+  geometry_info.y(this->pose.y);
+  geometry_info.hdg(this->pose.heading);
+  geometry_info.length(this->max_s);
+  this->save_params(geometry_info);
 }
 
 void COpendriveGeometry::set_start_pose(TOpendriveWorldPose &pose)
 {
-  this->pose.x=pose.x*this->scale_factor;
-  this->pose.y=pose.y*this->scale_factor;
+  this->pose.x=pose.x;
+  this->pose.y=pose.y;
   this->pose.heading=pose.heading;
 }
 
@@ -57,7 +68,7 @@ void COpendriveGeometry::set_max_s(double s)
   if(s<this->min_s)
     this->max_s=this->min_s;
   else
-    this->max_s=s*this->scale_factor;
+    this->max_s=s;
 }
 
 void COpendriveGeometry::set_min_s(double s)
@@ -65,7 +76,7 @@ void COpendriveGeometry::set_min_s(double s)
   if(s>this->max_s)
     this->min_s=this->max_s;
   else
-    this->min_s=s*this->scale_factor;
+    this->min_s=s;
 }
 
 bool COpendriveGeometry::get_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
@@ -80,8 +91,8 @@ bool COpendriveGeometry::get_world_pose(const TOpendriveTrackPose &track,TOpendr
   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/this->scale_factor;
-    world.y=local.u*std::sin(this->pose.heading)+local.v*std::cos(this->pose.heading)+this->pose.y/this->scale_factor;
+    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
@@ -90,7 +101,7 @@ bool COpendriveGeometry::get_world_pose(const TOpendriveTrackPose &track,TOpendr
 
 bool COpendriveGeometry::in_range(double s) const
 {
-  if((s<(this->max_s/this->scale_factor)) && (s>=(this->min_s/this->scale_factor)))
+  if((s<this->max_s) && (s>=this->min_s))
     return true;
   else 
     return false;
@@ -98,25 +109,25 @@ bool COpendriveGeometry::in_range(double s) const
 
 double COpendriveGeometry::get_length(void) const
 {
-  return (this->max_s-this->min_s)/this->scale_factor;
+  return (this->max_s-this->min_s);
 }
 
 double COpendriveGeometry::get_max_s(void) const
 {
-  return this->max_s/this->scale_factor;
+  return this->max_s;
 }
 
 double COpendriveGeometry::get_min_s(void) const
 {
-  return this->min_s/this->scale_factor;
+  return this->min_s;
 }
 
 TOpendriveWorldPose COpendriveGeometry::get_start_pose(void) const
 {
   TOpendriveWorldPose tmp_pose;
   
-  tmp_pose.x=this->pose.x/this->scale_factor;
-  tmp_pose.y=this->pose.y/this->scale_factor;
+  tmp_pose.x=this->pose.x;
+  tmp_pose.y=this->pose.y;
   tmp_pose.heading=this->pose.heading;
 
   return tmp_pose;
@@ -135,6 +146,22 @@ TOpendriveWorldPose COpendriveGeometry::get_end_pose(void) const
   return world_pose;
 }
 
+bool COpendriveGeometry::get_pose_at(double length,TOpendriveWorldPose &pose) const
+{
+  TOpendriveTrackPose track_pose;
+
+  if(length >= 0.0 && length <= this->get_length())
+  {
+    track_pose.s=length;
+    track_pose.t=0.0;
+    track_pose.heading=0.0;
+    this->get_world_pose(track_pose,pose);
+    return true;
+  }
+  else
+    return false;
+}
+
 void COpendriveGeometry::operator=(const COpendriveGeometry &object)
 {
   this->min_s = object.min_s;
@@ -142,7 +169,6 @@ void COpendriveGeometry::operator=(const COpendriveGeometry &object)
   this->pose.x = object.pose.x;
   this->pose.y = object.pose.y;
   this->pose.heading = object.pose.heading;
-  this->scale_factor = object.scale_factor;
 }
 
 std::ostream& operator<<(std::ostream& out, COpendriveGeometry &geom)
@@ -158,6 +184,5 @@ COpendriveGeometry::~COpendriveGeometry()
   this->pose.x = 0.0;
   this->pose.y = 0.0;
   this->pose.heading = 0.0;
-  this->scale_factor = DEFAULT_SCALE_FACTOR;
 }
 
diff --git a/src/opendrive/opendrive_junction.cpp b/src/opendrive/opendrive_junction.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9651b7ffb977807990e060ea354c76b475b0ba53
--- /dev/null
+++ b/src/opendrive/opendrive_junction.cpp
@@ -0,0 +1,347 @@
+#include "opendrive/opendrive_junction.h"
+#include "opendrive/opendrive_param_poly3.h"
+
+#include "exceptions.h"
+
+#include <sstream>
+
+COpendriveJunction::COpendriveJunction()
+{
+  this->id=-1;
+  this->name="";
+  this->connections.clear();
+}
+
+void COpendriveJunction::set_name(const std::string &name)
+{
+  this->name=name;
+}
+
+void COpendriveJunction::set_id(unsigned int id)
+{
+  this->id=id;
+}
+
+void COpendriveJunction::load(OpenDRIVE::junction_type &junction_info)
+{
+  std::stringstream error;
+  TJunctionConnection new_connection;
+  TJunctionLink new_link;
+
+  this->set_name(junction_info.name().get());
+  this->set_id(std::stol(junction_info.id().get()));  
+  for(junction::connection_iterator connection_it(junction_info.connection().begin()); connection_it!=junction_info.connection().end();++connection_it)
+  {
+    new_connection.id=std::stol(connection_it->id().get());
+    if(connection_it->incomingRoad().present())
+      new_connection.incomming_road_id=std::stol(connection_it->incomingRoad().get());
+    else
+    {
+      error << "Connectivity information missing for junction " << this->name << ": No incomming segment";
+      throw CException(_HERE_,error.str());
+    }
+    if(connection_it->connectingRoad().present())
+      new_connection.connecting_road_id=std::stol(connection_it->connectingRoad().get());
+    else
+    {
+      error << "Connectivity information missing for junction " << this->name << ": No connecting segment";
+      throw CException(_HERE_,error.str());
+    }
+    if(connection_it->contactPoint().present())
+    {
+      if(connection_it->contactPoint().get().compare("end")==0)
+        new_connection.end_contact_point=true;
+      else
+        new_connection.end_contact_point=false;
+    }
+    else
+    {
+      error << "Connectivity information missing for junction " << this->name << ": No contact point";
+      throw CException(_HERE_,error.str());
+    }
+    new_connection.links.clear();
+    for(connection::laneLink_iterator lane_link_it(connection_it->laneLink().begin()); lane_link_it!=connection_it->laneLink().end();++lane_link_it)
+    {
+      if(lane_link_it->from().present())
+         new_link.from_lane_id=lane_link_it->from().get();
+      else
+      {
+        error << "Connectivity information missing for junction " << this->name << ": lane link missing from identifier";
+        throw CException(_HERE_,error.str());
+      }
+      if(lane_link_it->to().present())
+        new_link.to_lane_id=lane_link_it->to().get();
+      else
+      {
+        error << "Connectivity information missing for junction " << this->name << ": lane link missing to identifier";
+        throw CException(_HERE_,error.str());
+      }
+      new_connection.links.push_back(new_link);
+    }
+    this->connections.push_back(new_connection);
+  }
+}
+
+void COpendriveJunction::save(OpenDRIVE::junction_type &junction_info)
+{
+  junction_info.id(std::to_string(this->id));
+  junction_info.name(this->name);
+  for(unsigned int i=0;i<this->connections.size();i++)
+  {
+    ::connection new_connection;
+    new_connection.id(std::to_string(this->connections[i].id));
+    new_connection.incomingRoad(std::to_string(this->connections[i].incomming_road_id));
+    new_connection.connectingRoad(std::to_string(this->connections[i].connecting_road_id));
+    if(this->connections[i].end_contact_point)
+      new_connection.contactPoint("end");
+    else
+      new_connection.contactPoint("start");
+    for(unsigned int j=0;j<this->connections[i].links.size();j++)
+    {
+      ::laneLink new_link;
+      new_link.from(this->connections[i].links[j].from_lane_id);
+      new_link.to(this->connections[i].links[j].to_lane_id);
+      new_connection.laneLink().push_back(new_link);
+    }
+    junction_info.connection().push_back(new_connection);
+  }
+}
+
+void COpendriveJunction::convert(CJunction **junction,opendrive_road_map_t &road_map)
+{
+  CRoad *in=NULL,*out=NULL;
+  unsigned int from_index,to_index;
+
+  (*junction)=new CJunction();
+
+  // add incomming and outgoing roads
+  for(unsigned int i=0;i<this->connections.size();i++)
+  { 
+    for(opendrive_road_map_t::iterator it=road_map.begin();it!=road_map.end();it++)
+    {
+      if(it->first->get_id()==this->connections[i].incomming_road_id)
+      {
+        if(it->first->has_successor() && it->first->is_successor_junction() && it->first->get_successor_id()==this->id)
+        {
+          if((it->second).size()>0)
+          {
+            in=(it->second)[0];
+            if(!(*junction)->has_incomming_road(in->get_id()))
+              (*junction)->add_incomming_road(in);
+          }
+        }
+        else if(it->first->has_predecessor() && it->first->is_predecessor_junction() && it->first->get_predecessor_id()==this->id)
+        {
+          if((it->second).size()>1)
+          {
+            in=(it->second)[1];
+            if(!(*junction)->has_incomming_road(in->get_id()))
+              (*junction)->add_incomming_road(in);
+          }
+        }
+
+      }
+      if(it->first->get_id()==this->connections[i].connecting_road_id)
+      {
+        if(it->first->has_predecessor() && it->first->is_predecessor_junction() && it->first->get_predecessor_id()==this->id)
+        {
+          if((it->second).size()>0)
+          {
+            out=(it->second)[0];
+            if(!(*junction)->has_outgoing_road(out->get_id()))
+              (*junction)->add_outgoing_road(out);
+          }
+        }
+        else if(it->first->has_successor() && it->first->is_successor_junction() && it->first->get_successor_id()==this->id)
+        {
+          if((it->second).size()>1)
+          {
+            out=(it->second)[1];
+            if(!(*junction)->has_outgoing_road(out->get_id()))
+              (*junction)->add_outgoing_road(out);
+          }
+        }
+      }
+    }
+    if(in!=NULL && out!=NULL)
+      (*junction)->link_roads_by_id(in->get_id(),out->get_id());
+    /* craete the connectivity */
+    for(unsigned int j=0;j<this->connections[i].links.size();j++)
+    {
+      from_index=abs(this->connections[i].links[j].from_lane_id)-1;
+      to_index=abs(this->connections[i].links[j].to_lane_id)-1;
+      (*junction)->link_lanes_by_id(in->get_id(),from_index,out->get_id(),to_index); 
+    }
+  }  
+}
+
+bool COpendriveJunction::get_road_data(unsigned int id,opendrive_road_inverse_map_t &road_map,COpendriveRoadSegment **road,bool incomming)
+{
+  for(unsigned int i=0;i<road_map.size();i++)
+  {
+    if(road_map[i].first.size()>0)
+    {
+      if(road_map[i].first[0]->get_id()==id)
+      {
+        if(incomming)
+          (*road)=road_map[i].second[road_map[i].second.size()-1];
+        else
+          (*road)=road_map[i].second[0];
+        return true;   
+      }
+    }
+    if(road_map[i].first.size()>1)
+    {
+      if(road_map[i].first[1]->get_id()==id)
+      {
+        if(incomming)
+          (*road)=road_map[i].second[0];
+        else
+          (*road)=road_map[i].second[road_map[i].second.size()-1];
+        return false;   
+      }
+    }
+  }
+  throw CException(_HERE_,"No road data for the given ID");
+}
+
+void COpendriveJunction::generate_road_segment(CRoadSegment *segment,unsigned int &current_id,unsigned int road_in,int lane_in,unsigned int road_out,int lane_out,COpendriveRoadSegment *new_segment)
+{
+  std::stringstream txt;
+  double lateral_offset_in,lateral_offset_out;
+  TPoint start_point,end_point;
+  TOpendriveWorldPose start_pose,end_pose;
+  COpendriveParamPoly3 *new_geometry;
+  COpendriveLane *new_lane;
+
+  txt << "junction_" << road_in << "_" << lane_in << "_" << road_out << "_" << lane_out;
+  new_segment->set_name(txt.str());
+  new_segment->set_id(current_id);
+  current_id++;
+  new_segment->set_center_mark(OD_MARK_NONE);
+  new_segment->set_junction_id(segment->get_parent_junction().get_id());
+  // add geometry
+  lateral_offset_in=-(abs(lane_in)-1)*segment->get_lane_width();
+  segment->get_point_at(0.0,lateral_offset_in,start_point);
+  start_pose.x=start_point.x;
+  start_pose.y=start_point.y;
+  start_pose.heading=start_point.heading;
+  lateral_offset_out=-(abs(lane_out)-1)*segment->get_lane_width();
+  segment->get_point_at(segment->get_length(),lateral_offset_out,end_point);
+  end_pose.x=end_point.x;
+  end_pose.y=end_point.y;
+  end_pose.heading=end_point.heading;
+  new_geometry=new COpendriveParamPoly3(start_pose,end_pose);
+  new_segment->add_geometry(new_geometry,segment->get_resolution());
+  // add a single right lane
+  new_lane=new COpendriveLane();
+  new_lane->set_speed(segment->get_lane_speed());
+  new_lane->set_width(segment->get_lane_width());
+  new_lane->set_id(-1);
+  new_lane->set_right_mark(OD_MARK_NONE);
+  new_segment->add_right_lane(new_lane);
+}
+
+void COpendriveJunction::convert(CJunction *junction_in,unsigned int &current_id,COpendriveJunction *junction_out,std::vector<COpendriveRoadSegment *> &segments,opendrive_road_inverse_map_t &road_map)
+{
+  std::stringstream txt;
+  CRoadSegment *segment;
+  COpendriveRoadSegment *incomming_segment,*outgoing_segment,*new_segment;
+  TJunctionConnection new_connection;
+  TJunctionLink new_link;
+  bool incomming_right,outgoing_right;
+  int incomming_lane,outgoing_lane;
+  unsigned int ids=0,incomming_id,outgoing_id;
+
+  if(junction_in==NULL)
+    throw CException(_HERE_,"Invalid junction reference");
+  segments.clear();
+  txt << "junction" << junction_in->get_id();
+  junction_out->set_name(txt.str());
+  junction_out->set_id(junction_in->get_id());
+  for(unsigned int i=0;i<junction_in->get_num_incomming_roads();i++)
+  {
+    // get incomming opendrive road info
+    incomming_id=junction_in->get_incomming_road_by_index(i)->get_id();
+    incomming_right=COpendriveJunction::get_road_data(incomming_id,road_map,&incomming_segment,true);
+    new_connection.incomming_road_id=incomming_segment->get_id();
+    new_connection.end_contact_point=false;
+    for(unsigned int j=0;j<junction_in->get_num_outgoing_roads();j++)
+    {
+      outgoing_id=junction_in->get_outgoing_road_by_index(j)->get_id();
+      outgoing_right=COpendriveJunction::get_road_data(outgoing_id,road_map,&outgoing_segment,false);
+      new_connection.connecting_road_id=outgoing_segment->get_id();
+      new_connection.links.clear();
+      if(junction_in->are_roads_linked_by_id(incomming_id,outgoing_id))
+      {
+        new_connection.id=ids;
+        ids++;
+        segment=&junction_in->get_segment_between_by_id(incomming_id,outgoing_id);
+        // create road from segment
+        for(unsigned int k=0;k<segment->get_num_lanes_in();k++)
+        {
+          if(incomming_right)
+            incomming_lane=-(k+1);
+          else
+            incomming_lane=k+1;
+          for(unsigned int l=0;l<segment->get_num_lanes_out();l++)
+          {
+            if(outgoing_right)
+              outgoing_lane=-(k+1);
+            else
+              outgoing_lane=k+1;
+            if(segment->are_lanes_linked(k,l))
+            {
+              new_link.from_lane_id=incomming_lane;
+              new_link.to_lane_id=outgoing_lane;
+              new_connection.links.push_back(new_link);
+              // create links
+              new_segment=new COpendriveRoadSegment();  
+              COpendriveJunction::generate_road_segment(segment,current_id,incomming_segment->get_id(),incomming_lane,outgoing_segment->get_id(),outgoing_lane,new_segment);
+              segments.push_back(new_segment);
+            }
+          }
+        }
+        if(new_connection.links.size()>0)
+          junction_out->connections.push_back(new_connection);
+      }
+    }
+  }
+}
+
+std::string COpendriveJunction::get_name(void) const
+{
+  return this->name;
+}
+
+unsigned int COpendriveJunction::get_id(void) const
+{
+  return this->id;
+}
+
+unsigned int COpendriveJunction::get_num_connections(void) const
+{
+  return this->connections.size();
+}
+
+const TJunctionConnection &COpendriveJunction::get_connection(unsigned int index) const
+{
+  std::stringstream error;
+
+  if(index>=0 && index<this->connections.size())
+    return this->connections[index];
+  else
+  {
+    error << "Invalid connection index " << index;
+    throw CException(_HERE_,error.str());
+  }
+}
+
+COpendriveJunction::~COpendriveJunction()
+{
+  this->connections.clear();
+  this->name="";
+  this->id=-1;
+}
+
+
diff --git a/src/opendrive/opendrive_lane.cpp b/src/opendrive/opendrive_lane.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..733f44909f3a49aea7817d46d3be041bc9064e0e
--- /dev/null
+++ b/src/opendrive/opendrive_lane.cpp
@@ -0,0 +1,176 @@
+#include "opendrive/opendrive_lane.h"
+#include "exceptions.h"
+#include <math.h>
+
+COpendriveLane::COpendriveLane()
+{
+  this->right_mark=OD_MARK_NONE;
+  this->width=0.0;
+  this->speed=0.0;
+  this->id=0;
+}
+
+void COpendriveLane::set_right_mark(opendrive_mark_t mark)
+{
+  this->right_mark=mark;
+}
+
+void COpendriveLane::set_width(double width)
+{
+  this->width=width;
+}
+
+void COpendriveLane::set_speed(double speed)
+{
+  this->speed=speed;
+}
+
+void COpendriveLane::set_id(int id)
+{
+  this->id=id;
+}
+
+void COpendriveLane::load(const ::lane &lane_info)
+{
+  std::stringstream error;
+  opendrive_mark_t mark;
+
+  this->right_mark=OD_MARK_NONE;
+  this->set_id(lane_info.id().get());
+  // import lane width
+  if(lane_info.width().size()<1)
+  {
+    error << "Lane " << this->id << " has no width record";
+    throw CException(_HERE_,error.str());
+  }
+  else if(lane_info.width().size()>1)
+  {
+    error << "Lane " << this->id << " has more than one width record";
+    throw CException(_HERE_,error.str());
+  }
+  this->set_width(lane_info.width().begin()->a().get());
+  // import lane speed
+  if(lane_info.speed().size()<1)
+    this->set_speed(60.0);
+  else if(lane_info.speed().size()>1)
+  {
+    error << "Lane " << this->id << " has more than one speed record";
+    throw CException(_HERE_,error.str());
+  }
+  else
+    this->set_speed(lane_info.speed().begin()->max().get());
+  //lane mark
+  if(lane_info.roadMark().size()==0)
+    mark=OD_MARK_NONE;
+  else if(lane_info.roadMark().size()>1)
+  {
+    error << "Lane " << this->id << " has more than one road mark record";
+    throw CException(_HERE_,error.str());
+  }
+  else if(lane_info.roadMark().size()==1)
+  {
+    if(lane_info.roadMark().begin()->type1().present())
+    {
+      if(lane_info.roadMark().begin()->type1().get()=="none")
+        mark=OD_MARK_NONE;
+      else if(lane_info.roadMark().begin()->type1().get()=="solid")
+        mark=OD_MARK_SOLID;
+      else if(lane_info.roadMark().begin()->type1().get()=="broken")
+        mark=OD_MARK_BROKEN;
+      else if(lane_info.roadMark().begin()->type1().get()=="solid solid")
+        mark=OD_MARK_SOLID_SOLID;
+      else if(lane_info.roadMark().begin()->type1().get()=="solid broken")
+        mark=OD_MARK_SOLID_BROKEN;
+      else if(lane_info.roadMark().begin()->type1().get()=="broken solid")
+        mark=OD_MARK_BROKEN_SOLID;
+      else if(lane_info.roadMark().begin()->type1().get()=="broken broken")
+        mark=OD_MARK_BROKEN_BROKEN;
+      else
+        mark=OD_MARK_NONE;
+    }
+    else
+      mark=OD_MARK_NONE;
+  }
+  this->right_mark=mark;
+}
+
+void COpendriveLane::save(::lane &lane_info)
+{
+  ::width lane_width;
+  ::speed lane_speed;
+  ::roadMark lane_mark;
+
+  lane_info.id(this->id);
+  lane_info.type("driving");
+  lane_info.level("false");
+  lane_width.sOffset(0.0);
+  lane_width.a(this->width);
+  lane_width.b(0.0);
+  lane_width.c(0.0);
+  lane_width.d(0.0);
+  lane_info.width().push_back(lane_width);
+  lane_speed.sOffset(0.0);
+  lane_speed.max(this->speed);
+  lane_info.speed().push_back(lane_speed);
+  switch(this->right_mark)
+  {
+    case OD_MARK_NONE:
+      lane_mark.type1("none");
+      break;
+    case OD_MARK_SOLID:
+      lane_mark.type1("solid");
+      break;
+    case OD_MARK_BROKEN:
+      lane_mark.type1("broken");
+      break;
+    case OD_MARK_SOLID_SOLID:
+      lane_mark.type1("solid solid");
+      break;
+    case OD_MARK_SOLID_BROKEN:
+      lane_mark.type1("solid broken");
+      break;
+    case OD_MARK_BROKEN_SOLID:
+      lane_mark.type1("broken solid");
+      break;
+    case OD_MARK_BROKEN_BROKEN:
+      lane_mark.type1("broken broken");
+      break;
+    default:
+      lane_mark.type1("none");
+      break;
+  }
+  lane_mark.sOffset(0.0);
+  lane_mark.weight("standard");
+  lane_mark.color("standard");
+  lane_mark.width(0.3);
+  lane_mark.laneChange("both");
+  lane_info.roadMark().push_back(lane_mark);
+}
+
+opendrive_mark_t COpendriveLane::get_right_road_mark(void) const
+{
+  return this->right_mark;
+}
+
+double COpendriveLane::get_width(void) const
+{
+  return this->width;
+}
+
+double COpendriveLane::get_speed(void) const
+{
+  return this->speed;
+}
+
+int COpendriveLane::get_id(void) const
+{
+  return this->id;
+}
+
+COpendriveLane::~COpendriveLane()
+{
+  this->right_mark=OD_MARK_NONE;
+  this->width=0.0;
+  this->speed=0.0;
+  this->id=0;
+}
diff --git a/src/opendrive_line.cpp b/src/opendrive/opendrive_line.cpp
similarity index 71%
rename from src/opendrive_line.cpp
rename to src/opendrive/opendrive_line.cpp
index 9b0fa5eb1cb0b722d8bd3f45651cb25f760325ed..469a97a8f936cee5e2c8bfc28b7bfa95064f69dd 100644
--- a/src/opendrive_line.cpp
+++ b/src/opendrive/opendrive_line.cpp
@@ -1,4 +1,4 @@
-#include "opendrive_line.h"
+#include "opendrive/opendrive_line.h"
 
 COpendriveLine::COpendriveLine()
 {
@@ -10,6 +10,11 @@ COpendriveLine::COpendriveLine(const COpendriveGeometry &object) : COpendriveGeo
 
 }
 
+COpendriveLine::COpendriveLine(TOpendriveWorldPose &start_pose,double length):COpendriveGeometry(start_pose,length)
+{
+
+}
+
 bool COpendriveLine::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
 {
   local.u=track.s;
@@ -34,6 +39,13 @@ void COpendriveLine::load_params(const planView::geometry_type &geometry_info)
 
 }
 
+void COpendriveLine::save_params(planView::geometry_type &geometry_info)
+{
+  ::line1 new_line;
+
+  geometry_info.line(new_line);
+}
+
 COpendriveGeometry *COpendriveLine::clone(void)
 {
   COpendriveLine *new_line=new COpendriveLine(*this);
@@ -47,6 +59,11 @@ void COpendriveLine::get_curvature(double &start,double &end)
   end=0.0;
 }
 
+void COpendriveLine::get_curvature_at(double length,double &curvature)
+{
+  curvature=0.0;
+}
+
 void COpendriveLine::operator=(const COpendriveLine &object)
 {
   COpendriveGeometry::operator=(object);
diff --git a/src/opendrive_object.cpp b/src/opendrive/opendrive_object.cpp
similarity index 82%
rename from src/opendrive_object.cpp
rename to src/opendrive/opendrive_object.cpp
index 580d41f8df3471bf34a6d82182179ce75aec981e..75fe964e2ee1c6a2768189de7e2adaf76ac5bc35 100644
--- a/src/opendrive_object.cpp
+++ b/src/opendrive/opendrive_object.cpp
@@ -1,22 +1,19 @@
-#include "opendrive_object.h"
+#include "opendrive/opendrive_object.h"
 #include "exceptions.h"
 #include <cmath>
 
 COpendriveObject::COpendriveObject()
 {
-  this->segment=NULL;
   this->pose.s=0.0;
   this->pose.t=0.0;
   this->pose.heading=0.0;
   this->type="";
   this->name="";
   this->object_type=OD_NONE;
-  this->scale_factor=DEFAULT_SCALE_FACTOR;
 }
 
 COpendriveObject::COpendriveObject(const COpendriveObject& object)
 {
-  this->segment=object.segment;
   this->pose.s=object.pose.s;
   this->pose.s=object.pose.t;
   this->pose.heading=object.pose.heading;
@@ -45,10 +42,9 @@ COpendriveObject::COpendriveObject(const COpendriveObject& object)
     case OD_NONE:
       break;
   }
-  this->scale_factor=object.scale_factor;
 }
 
-void COpendriveObject::load(objects::object_type &object_info,COpendriveRoadSegment *segment)
+void COpendriveObject::load(objects::object_type &object_info)
 {
   double u,v;
   unsigned int i;
@@ -105,49 +101,19 @@ void COpendriveObject::load(objects::object_type &object_info,COpendriveRoadSegm
   }
   else
     this->object_type=OD_NONE;
-  this->segment=segment;
-}
-
-void COpendriveObject::update_references(segment_up_ref_t &segment_refs)
-{
-  if(segment_refs.find(this->segment)!=segment_refs.end())
-    this->segment=segment_refs[this->segment];
-}
-
-void COpendriveObject::set_scale_factor(double scale)
-{
-  this->scale_factor=scale;
-}
-
-double COpendriveObject::get_scale_factor(void)
-{
-  return this->scale_factor;
 }
 
 TOpendriveTrackPose COpendriveObject::get_track_pose(void) const
 {
   TOpendriveTrackPose track;
 
-  track.s=this->pose.s/this->scale_factor;
-  track.t=this->pose.t/this->scale_factor;
+  track.s=this->pose.s;
+  track.t=this->pose.t;
   track.heading=this->pose.heading;
 
   return track;
 }
 
-TOpendriveWorldPose COpendriveObject::get_world_pose(void) const
-{
-  TOpendriveWorldPose world;
-
-  if(this->segment!=NULL)
-  {
-    world=segment->transform_to_world_pose(this->get_track_pose());
-    return world;
-  }
-  else
-    throw CException(_HERE_,"Invalid parent segment reference");
-}
-
 int COpendriveObject::get_id(void) const
 {
   return this->id;
@@ -199,9 +165,9 @@ TOpendriveBox COpendriveObject::get_box_data(void) const
 {
   TOpendriveBox data;
 
-  data.length=this->object.box.length/this->scale_factor;
-  data.width=this->object.box.width/this->scale_factor;
-  data.height=this->object.box.height/this->scale_factor;
+  data.length=this->object.box.length;
+  data.width=this->object.box.width;
+  data.height=this->object.box.height;
 
   return data;
 }
@@ -210,8 +176,8 @@ TOpendriveCylinder COpendriveObject::get_cylinder_data(void) const
 {
   TOpendriveCylinder data;
 
-  data.radius=this->object.cylinder.radius/this->scale_factor;
-  data.height=this->object.cylinder.height/this->scale_factor;
+  data.radius=this->object.cylinder.radius;
+  data.height=this->object.cylinder.height;
 
   return data;
 }
@@ -222,9 +188,9 @@ TOpendrivePolygon COpendriveObject::get_polygon_data(void) const
 
   for(unsigned int i=0;i<this->object.polygon.num_vertices;i++)
   {
-    data.height[i]=this->object.polygon.height[i]/this->scale_factor;
-    data.vertices[i].s=this->object.polygon.vertices[i].s/this->scale_factor;
-    data.vertices[i].t=this->object.polygon.vertices[i].t/this->scale_factor;
+    data.height[i]=this->object.polygon.height[i];
+    data.vertices[i].s=this->object.polygon.vertices[i].s;
+    data.vertices[i].t=this->object.polygon.vertices[i].t;
     data.vertices[i].heading=this->object.polygon.vertices[i].heading;
   }
   return data;
diff --git a/src/opendrive_param_poly3.cpp b/src/opendrive/opendrive_param_poly3.cpp
similarity index 53%
rename from src/opendrive_param_poly3.cpp
rename to src/opendrive/opendrive_param_poly3.cpp
index e57b486a43ab74db4fafcf64902e4e8eaded21f7..bbb3354bb1a7423782396212724aaaf09ec430a7 100644
--- a/src/opendrive_param_poly3.cpp
+++ b/src/opendrive/opendrive_param_poly3.cpp
@@ -1,6 +1,9 @@
-#include "opendrive_param_poly3.h"
+#include "opendrive/opendrive_param_poly3.h"
+#include "common.h"
 #include <cmath>
 
+#include <Eigen/Dense>
+
 COpendriveParamPoly3::COpendriveParamPoly3()
 {
   this->u.a=0.0;  
@@ -27,14 +30,84 @@ COpendriveParamPoly3::COpendriveParamPoly3(const COpendriveParamPoly3 &object) :
   this->normalized=object.normalized;
 }
 
+COpendriveParamPoly3::COpendriveParamPoly3(TOpendriveWorldPose &start_pose,TOpendriveWorldPose &end_pose)
+{
+  TOpendriveWorldPose end_pose_zero,end_pose_rot;
+  Eigen::MatrixXd A=Eigen::MatrixXd::Zero(6,8),pinv;
+  Eigen::VectorXd b=Eigen::VectorXd::Zero(6,1),sol;
+  double length=0.0,x,prev_x,y,prev_y,p,p2,p3;
+
+  end_pose_zero.x=end_pose.x-start_pose.x;
+  end_pose_zero.y=end_pose.y-start_pose.y;
+  end_pose_zero.heading=end_pose.heading;
+  end_pose_rot.x=end_pose_zero.x*std::cos(start_pose.heading)+end_pose_zero.y*std::sin(start_pose.heading);
+  end_pose_rot.y=-end_pose_zero.x*std::sin(start_pose.heading)+end_pose_zero.y*std::cos(start_pose.heading);
+  end_pose_rot.heading=end_pose.heading-start_pose.heading;
+  A(0,0)=1.0;
+  A(1,4)=1.0;
+  A(2,0)=1.0;
+  A(2,1)=1.0;
+  A(2,2)=1.0;
+  A(2,3)=1.0;
+  A(3,4)=1.0;
+  A(3,5)=1.0;
+  A(3,6)=1.0;
+  A(3,7)=1.0;
+  A(4,1)=-tan(0.0);
+  A(4,5)=1.0;
+  A(5,1)=-tan(end_pose_rot.heading);
+  A(5,2)=-2.0*tan(end_pose_rot.heading);
+  A(5,3)=-3.0*tan(end_pose_rot.heading);
+  A(5,5)=1.0;
+  A(5,6)=2.0;
+  A(5,7)=3.0;
+  pinv=A.completeOrthogonalDecomposition().pseudoInverse();
+  b(0)=0.0;
+  b(1)=0.0;
+  b(2)=end_pose_rot.x;
+  b(3)=end_pose_rot.y;
+  sol=pinv*b;
+  this->u.a=sol(0);
+  this->u.b=sol(1);
+  this->u.c=sol(2);
+  this->u.d=sol(3);
+  this->v.a=sol(4);
+  this->v.b=sol(5);
+  this->v.c=sol(6);
+  this->v.d=sol(7);
+
+  this->normalized=true;
+  // compute start pose
+  COpendriveGeometry::set_start_pose(start_pose);
+  // compute length
+  prev_x=this->u.a;
+  prev_y=this->v.a;
+  for(unsigned int i=1;i<=100;i++)
+  {
+    p=i*0.01;
+    p2=p*p;
+    p3=p2*p;
+    x=this->u.a + this->u.b*p + this->u.c*p2 + this->u.d*p3;
+    y=this->v.a + this->v.b*p + this->v.c*p2 + this->v.d*p3;
+    length+=sqrt(pow(x-prev_x,2.0)+pow(y-prev_y,2.0));
+    prev_x=x;
+    prev_y=y;
+  }
+  this->set_max_s(length);
+}
+
 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 p = (this->normalized ? track.s/(this->max_s - this->min_s):track.s);
   double p2 = p*p;
   double p3 = p2*p;
   double du = this->u.b + 2*this->u.c*p + 3*this->u.d*p2;
   double dv = this->v.b + 2*this->v.c*p + 3*this->v.d*p2;
-  double alpha = std::atan2(dv, du);
+  double alpha;
+  if(p==0.0)
+    alpha = 0.0;
+  else
+    alpha = std::atan2(dv,du);
   local.u = this->u.a + this->u.b*p + this->u.c*p2 + this->u.d*p3 - track.t*std::sin(alpha);
   local.v = this->v.a + this->v.b*p + this->v.c*p2 + this->v.d*p3 + track.t*std::cos(alpha);
   local.heading = normalize_angle(track.heading + alpha);
@@ -73,6 +146,55 @@ void COpendriveParamPoly3::load_params(const planView::geometry_type &geometry_i
     this->normalized = false;
 }
 
+void COpendriveParamPoly3::save_params(planView::geometry_type &geometry_info)
+{
+  ::paramPoly3 poly;
+
+  poly.aU(this->u.a);
+  poly.bU(this->u.b);
+  poly.cU(this->u.c);
+  poly.dU(this->u.d);
+  poly.aV(this->v.a);
+  poly.bV(this->v.b);
+  poly.cV(this->v.c);
+  poly.dV(this->v.d);
+  if(this->normalized)
+    poly.pRange(pRange::normalized);
+  else
+    poly.pRange(pRange::arcLength);
+  geometry_info.paramPoly3(poly);
+}
+
+COpendriveParamPoly3::COpendriveParamPoly3(TOpendriveWorldPose &start_pose,TOpendrivePoly3Params &u_params,TOpendrivePoly3Params &v_params)
+{
+  double length=0.0,x,prev_x=u_params.a,y,prev_y=v_params.a,p,p2,p3;
+
+  this->u.a=u_params.a;
+  this->u.b=u_params.b;
+  this->u.c=u_params.c;
+  this->u.d=u_params.d;
+  this->v.a=v_params.a;
+  this->v.b=v_params.b;
+  this->v.c=v_params.c;
+  this->v.d=v_params.d;
+  this->normalized=true;
+  // compute start pose
+  COpendriveGeometry::set_start_pose(start_pose);
+  // compute length
+  for(unsigned int i=1;i<=100;i++)
+  {
+    p=i*0.01;
+    p2=p*p;
+    p3=p2*p;
+    x=this->u.a + this->u.b*p + this->u.c*p2 + this->u.d*p3;
+    y=this->v.a + this->v.b*p + this->v.c*p2 + this->v.d*p3;
+    length+=sqrt(pow(x-prev_x,2.0)+pow(y-prev_y,2.0));
+    prev_x=x;
+    prev_y=y;
+  }
+  this->set_max_s(length);
+}
+
 COpendriveGeometry *COpendriveParamPoly3::clone(void)
 {
   COpendriveParamPoly3 *new_poly=new COpendriveParamPoly3(*this);
@@ -86,6 +208,11 @@ void COpendriveParamPoly3::get_curvature(double &start,double &end)
   end=0.0;
 }
 
+void COpendriveParamPoly3::get_curvature_at(double length,double &curvature)
+{
+  curvature=0.0;  
+}
+
 TOpendrivePoly3Params COpendriveParamPoly3::get_u_params(void)
 {
   return this->u;
diff --git a/src/opendrive/opendrive_road_segment.cpp b/src/opendrive/opendrive_road_segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..61e28b2c5af3e64ea95a1f386b288573b7fc9ba5
--- /dev/null
+++ b/src/opendrive/opendrive_road_segment.cpp
@@ -0,0 +1,822 @@
+#include "opendrive/opendrive_road_segment.h"
+#include "opendrive/opendrive_line.h"
+#include "opendrive/opendrive_spiral.h"
+#include "opendrive/opendrive_arc.h"
+#include "opendrive/opendrive_param_poly3.h"
+#include "road_segment.h"
+#include "common.h"
+#include "exceptions.h"
+#include <math.h>
+
+COpendriveRoadSegment::COpendriveRoadSegment()
+{
+  this->name="";
+  this->id=-1;
+  this->center_mark=OD_MARK_NONE;
+  this->left_lanes.clear();
+  this->right_lanes.clear();
+  this->signals.clear();
+  this->objects.clear();
+  this->geometries.clear();
+  this->predecessor_exists=false;
+  this->successor_exists=false;
+  this->junction_id=-1;
+}
+
+void COpendriveRoadSegment::free(void)
+{
+  for(unsigned int i=0;i<this->geometries.size();i++)
+  {
+    delete this->geometries[i];
+    this->geometries[i]=NULL;
+  }
+  this->geometries.clear();
+  for(unsigned int i=0;i<this->right_lanes.size();i++)
+  {
+    delete this->right_lanes[i];
+    this->right_lanes[i]=NULL;
+  }
+  this->right_lanes.clear();
+  for(unsigned int i=0;i<this->left_lanes.size();i++)
+  {
+    delete this->left_lanes[i];
+    this->left_lanes[i]=NULL;
+  }
+  this->left_lanes.clear();
+  for(unsigned int i=0;i<this->signals.size();i++)
+  {
+    delete this->signals[i];
+    this->signals[i]=NULL;
+  }
+  this->signals.clear();
+  for(unsigned int i=0;i<this->objects.size();i++)
+  {
+    delete this->objects[i];
+    this->objects[i]=NULL;
+  }
+}
+
+void COpendriveRoadSegment::set_name(const std::string &name)
+{
+  this->name=name;
+}
+
+void COpendriveRoadSegment::set_id(unsigned int id)
+{
+  this->id=id;
+}
+
+void COpendriveRoadSegment::set_center_mark(opendrive_mark_t mark)
+{
+  this->center_mark=mark;
+}
+
+bool COpendriveRoadSegment::add_geometries(OpenDRIVE::road_type &road_info)
+{
+  planView::geometry_iterator geom_info;
+  COpendriveGeometry *geometry;
+
+  for(unsigned int i=0;i<this->geometries.size();i++)
+    delete this->geometries[i];
+  this->geometries.clear();
+  for(geom_info=road_info.planView().geometry().begin(); geom_info!=road_info.planView().geometry().end(); ++geom_info)
+  {
+    // import geometry
+    if(geom_info->line().present())
+      geometry=new COpendriveLine();
+    else if(geom_info->spiral().present())
+      geometry=new COpendriveSpiral();
+    else if(geom_info->arc().present())
+      geometry=new COpendriveArc();
+    else if(geom_info->paramPoly3().present())
+      geometry=new COpendriveParamPoly3();
+    else
+      throw CException(_HERE_,"Unknown geometry");
+    geometry->load(*geom_info);
+    this->geometries.push_back(geometry);
+  }
+  if(this->geometries.size()==0)
+    return false;
+  else
+    return true;
+}
+
+void COpendriveRoadSegment::add_geometry(COpendriveGeometry *geometry,double resolution)
+{
+  TOpendriveWorldPose start_pose,end_pose;
+
+  if(geometry==NULL)
+    throw CException(_HERE_,"Invalid geometry reference");
+  if(this->geometries.size()==0)
+    this->geometries.push_back(geometry);
+  else
+  {
+    start_pose=geometry->get_start_pose();
+    end_pose=this->geometries[this->geometries.size()-1]->get_end_pose();
+    if(sqrt(pow(start_pose.x-end_pose.x,2.0)+pow(start_pose.y-end_pose.y,2.0))<=resolution)
+      this->geometries.push_back(geometry);
+    else
+      throw CException(_HERE_,"geometries do not coincide");
+  }
+}
+
+void COpendriveRoadSegment::set_junction_id(unsigned int id)
+{
+  this->junction_id=id;
+}
+
+void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
+{
+  right::lane_iterator r_lane_it;
+  left::lane_iterator l_lane_it;
+
+  // right lanes
+  if(lane_section.right().present())
+  {
+    for(r_lane_it=lane_section.right()->lane().begin();r_lane_it!=lane_section.right()->lane().end();r_lane_it++)
+      this->add_right_lane(*r_lane_it);
+  }
+  // left lanes
+  if(lane_section.left().present())
+  {
+    for(l_lane_it=lane_section.left()->lane().begin();l_lane_it!=lane_section.left()->lane().end();l_lane_it++)
+      this->add_left_lane(*l_lane_it);
+  }
+}
+
+void COpendriveRoadSegment::add_right_lane(const ::lane &lane_info)
+{
+  COpendriveLane *new_lane;
+
+  try{
+    new_lane=new COpendriveLane();
+    new_lane->load(lane_info);
+    for(unsigned int i=0;i<this->right_lanes.size();i++)
+      if(this->right_lanes[i]->get_id()==new_lane->get_id())
+        throw CException(_HERE_,"A lane with the same ID has already been added");
+    this->right_lanes.push_back(new_lane);
+  }catch(CException &e){
+    delete new_lane;
+    throw e;
+  }
+}
+
+void COpendriveRoadSegment::add_right_lane(COpendriveLane *lane)
+{
+  if(lane==NULL)
+    throw CException(_HERE_,"Invalid right lane reference");
+  for(unsigned int i=0;i<this->right_lanes.size();i++)
+    if(this->right_lanes[i]->get_id()==lane->get_id())
+      throw CException(_HERE_,"A lane with the same ID has already been added");
+  this->right_lanes.push_back(lane);
+}
+
+void COpendriveRoadSegment::add_left_lane(const ::lane &lane_info)
+{
+  COpendriveLane *new_lane;
+
+  try{
+    new_lane=new COpendriveLane();
+    new_lane->load(lane_info);
+    for(unsigned int i=0;i<this->left_lanes.size();i++)
+      if(this->left_lanes[i]->get_id()==new_lane->get_id())
+        throw CException(_HERE_,"A lane with the same ID has already been added");
+    this->left_lanes.push_back(new_lane);
+  }catch(CException &e){
+    delete new_lane;
+    throw e;
+  }
+}
+
+void COpendriveRoadSegment::add_left_lane(COpendriveLane *lane)
+{
+  if(lane==NULL)
+    throw CException(_HERE_,"Invalid left lane reference");
+  for(unsigned int i=0;i<this->left_lanes.size();i++)
+    if(this->left_lanes[i]->get_id()==lane->get_id())
+      throw CException(_HERE_,"A lane with the same ID has already been added");
+  this->left_lanes.push_back(lane);
+}
+
+bool COpendriveRoadSegment::is_junction(void) const
+{
+  if(this->junction_id==(unsigned int)-1)
+    return false;
+  else
+    return true;
+}
+
+bool COpendriveRoadSegment::has_predecessor(void)
+{
+  return this->predecessor_exists;
+}
+
+bool COpendriveRoadSegment::is_predecessor_road(void)
+{
+  if(this->predecessor_exists)
+    return this->predecessor.road;
+  else
+    throw CException(_HERE_,"Road segment has no predecessor");
+}
+
+bool COpendriveRoadSegment::is_predecessor_junction(void)
+{
+  if(this->predecessor_exists)
+    return !this->predecessor.road;
+  else
+    throw CException(_HERE_,"Road segment has no predecessor");
+}
+
+unsigned int COpendriveRoadSegment::get_predecessor_id(void)
+{
+  if(this->predecessor_exists)
+    return this->predecessor.id;
+  else
+    throw CException(_HERE_,"Road segment has no predecessor");
+}
+
+bool COpendriveRoadSegment::is_predecessor_contact_end(void)
+{
+  if(this->predecessor_exists)
+    return this->predecessor.end_contact;
+  else
+    throw CException(_HERE_,"Road segment has no predecessor");
+}
+
+bool COpendriveRoadSegment::is_predecessor_contact_start(void)
+{
+  if(this->predecessor_exists)
+    return !this->predecessor.end_contact;
+  else
+    throw CException(_HERE_,"Road segment has no predecessor");
+}
+
+bool COpendriveRoadSegment::has_successor(void)
+{ 
+  return this->successor_exists;
+}
+
+bool COpendriveRoadSegment::is_successor_road(void)
+{
+  if(this->successor_exists)
+    return this->successor.road;
+  else
+    throw CException(_HERE_,"Road segment has no successor");
+}
+
+bool COpendriveRoadSegment::is_successor_junction(void)
+{
+  if(this->successor_exists)
+    return !this->successor.road;
+  else
+    throw CException(_HERE_,"Road segment has no successor");
+}
+
+unsigned int COpendriveRoadSegment::get_successor_id(void)
+{
+  if(this->successor_exists)
+    return this->successor.id;
+  else
+    throw CException(_HERE_,"Road segment has no successor");
+}
+
+bool COpendriveRoadSegment::is_successor_contact_end(void)
+{ 
+  if(this->successor_exists)
+    return this->successor.end_contact;
+  else
+    throw CException(_HERE_,"Road segment has no successor");
+}
+
+bool COpendriveRoadSegment::is_successor_contact_start(void)
+{
+  if(this->successor_exists)
+    return !this->successor.end_contact;
+  else
+    throw CException(_HERE_,"Road segment has no successor");
+}
+
+void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
+{
+  signals::signal_iterator signal_it;
+  objects::object_iterator object_it;
+  lanes::laneSection_iterator lane_section;
+  COpendriveSignal *new_signal;
+  COpendriveObject *new_object;
+  std::stringstream error;
+
+  this->free();
+  this->set_name(road_info.name().get());
+  this->set_id(std::stol(road_info.id().get()));
+
+  // read link information
+  if(road_info.lane_link().present())
+  {
+    if(road_info.lane_link().get().predecessor().present())// predecessor present
+    {
+      this->predecessor_exists=true;
+      if(road_info.lane_link().get().predecessor().get().elementType().get()=="road")// previous segment is a road
+        this->predecessor.road=true;
+      else
+        this->predecessor.road=false;
+      this->predecessor.id=std::stol(road_info.lane_link().get().predecessor().get().elementId().get());
+      if(road_info.lane_link().get().predecessor().get().contactPoint().get()=="end")
+        this->predecessor.end_contact=true;
+      else
+        this->predecessor.end_contact=false;
+    }
+    else
+      this->predecessor_exists=false;
+    if(road_info.lane_link().get().successor().present())// successor present
+    {
+      this->successor_exists=true;
+      if(road_info.lane_link().get().successor().get().elementType().get()=="road")// previous segment is a road
+        this->successor.road=true;
+      else
+        this->successor.road=false;
+      this->successor.id=std::stol(road_info.lane_link().get().successor().get().elementId().get());
+      if(road_info.lane_link().get().successor().get().contactPoint().get()=="end")
+        this->successor.end_contact=true;
+      else
+        this->successor.end_contact=false;
+    }
+    else
+      this->successor_exists=false;
+  }
+  // only one lane section is supported
+  if(road_info.lanes().laneSection().size()<1)
+  {
+    error << "No lane sections defined for segment " << this->name;
+    throw CException(_HERE_,error.str());
+  }
+  else if(road_info.lanes().laneSection().size()>1)
+  {
+    error << "Segment " << this->name << " has more than one lane section";
+    throw CException(_HERE_,error.str());
+  }
+  else
+    lane_section=road_info.lanes().laneSection().begin();
+
+  try{
+    // add lanes
+    this->add_lanes(*lane_section);
+    // add geometries
+    if(!this->add_geometries(road_info))
+    {
+      error << "Segment " << this->name << " has no valid geometry";
+      throw CException(_HERE_,error.str());
+    }
+    // add road signals
+    if(road_info.signals().present())
+    {
+      for(signal_it=road_info.signals().get().signal().begin();signal_it!=road_info.signals().get().signal().end();++signal_it)
+      {
+        new_signal=new COpendriveSignal();
+        new_signal->load(*signal_it);
+        this->signals.push_back(new_signal);
+      }
+    }
+    // add road objects
+    if(road_info.objects().present())
+    {
+      for(object_it=road_info.objects().get().object().begin();object_it!=road_info.objects().get().object().end();++object_it)
+      {
+        new_object=new COpendriveObject();
+        new_object->load(*object_it);
+        this->objects.push_back(new_object);
+      }
+    }
+    this->junction_id=std::stol(road_info.junction().get());
+  }catch(CException &e){
+    this->free();
+    throw e;
+  }
+}
+
+void COpendriveRoadSegment::convert(CRoad **left_road,CRoad **right_road,double resolution)
+{
+  double avg_width,heading;
+  TOpendriveWorldPose start_point,end_point;
+  opendrive_mark_t mark;
+  CRoadSegment *segment;
+  bool first;
+
+  if(this->right_lanes.size()>0)
+  {
+    (*right_road)=new CRoad();
+    (*right_road)->set_num_lanes(this->right_lanes.size());
+    avg_width=0.0;
+    for(unsigned int i=0;i<this->right_lanes.size();i++)
+      avg_width+=this->right_lanes[i]->get_width();
+    avg_width/=this->right_lanes.size();
+    (*right_road)->set_lane_width(avg_width);
+    first=true;
+    for(unsigned int i=0;i<this->geometries.size();i++)
+    {
+      if(this->geometries[i]->get_length()>resolution)
+      {
+        if(first)
+        {
+          start_point=this->geometries[i]->get_start_pose();
+          (*right_road)->set_start_point(start_point.x,start_point.y,start_point.heading,0.0);
+          first=false;
+        }
+        end_point=this->geometries[i]->get_end_pose();
+        (*right_road)->add_segment(end_point.x,end_point.y,end_point.heading,0.0);
+        segment=(*right_road)->get_last_segment();
+        for(unsigned int j=0;j<this->right_lanes.size();j++)
+        {
+          mark=this->right_lanes[j]->get_right_road_mark();
+          if(mark==OD_MARK_SOLID || mark==OD_MARK_SOLID_SOLID)
+          {
+            for(unsigned int k=abs(this->right_lanes[j]->get_id());k<this->right_lanes.size();k++)
+            {
+              segment->unlink_lanes(j,k);
+              segment->unlink_lanes(k,j);
+            }
+          }
+          else if(mark==OD_MARK_SOLID_BROKEN)
+          {
+            for(unsigned int k=abs(this->right_lanes[j]->get_id());k<this->right_lanes.size();k++)
+              segment->unlink_lanes(j,k);
+          }
+          else if(mark==OD_MARK_BROKEN_SOLID)
+          {
+            for(unsigned int k=abs(this->right_lanes[j]->get_id());k<this->right_lanes.size();k++)
+              segment->unlink_lanes(k,j);
+          }
+        }   
+      }
+    }
+  }
+  else
+    (*right_road)=NULL;
+  if(this->left_lanes.size()>0)
+  {
+    (*left_road)=new CRoad();
+    (*left_road)->set_num_lanes(this->left_lanes.size());
+    avg_width=0.0;
+    for(unsigned int i=0;i<this->left_lanes.size();i++)
+      avg_width+=this->left_lanes[i]->get_width();
+    avg_width/=this->left_lanes.size();
+    (*left_road)->set_lane_width(avg_width);
+    first=true;
+    for(unsigned int i=0;i<this->geometries.size();i++)
+    { 
+      if(this->geometries[this->geometries.size()-i-1]->get_length()>resolution)
+      {
+        if(first)
+        { 
+          start_point=this->geometries[this->geometries.size()-i-1]->get_end_pose();
+          heading=normalize_angle(start_point.heading+3.14159);
+          (*left_road)->set_start_point(start_point.x,start_point.y,heading,0.0);
+          first=false;
+        }
+        end_point=this->geometries[this->geometries.size()-i-1]->get_start_pose();
+        heading=normalize_angle(end_point.heading+3.14159);
+        (*left_road)->add_segment(end_point.x,end_point.y,heading,0.0);
+        segment=(*left_road)->get_last_segment();
+        for(unsigned int j=0;j<this->left_lanes.size();j++)
+        { 
+          mark=this->left_lanes[j]->get_right_road_mark();
+          if(mark==OD_MARK_SOLID || mark==OD_MARK_SOLID_SOLID)
+          { 
+            for(unsigned int k=this->left_lanes[j]->get_id();k<this->left_lanes.size();k++)
+            {
+              segment->unlink_lanes(j,k);
+              segment->unlink_lanes(k,j);
+            }
+          }
+          else if(mark==OD_MARK_SOLID_BROKEN)
+          {
+            for(unsigned int k=this->left_lanes[j]->get_id();k<this->left_lanes.size();k++)
+              segment->unlink_lanes(j,k);
+          }
+          else if(mark==OD_MARK_BROKEN_SOLID)
+          {
+            for(unsigned int k=this->left_lanes[j]->get_id();k<this->left_lanes.size();k++)
+              segment->unlink_lanes(k,j);
+          }
+        }
+      }
+    }
+  } 
+  else
+    (*left_road)=NULL;
+  if((*right_road)!=NULL && (*left_road)!=NULL)
+  {
+    (*left_road)->set_opposite_direction_road((*right_road));
+    (*right_road)->set_opposite_direction_road((*left_road));
+  }
+}
+
+void COpendriveRoadSegment::convert(CRoad *left_road,CRoad *right_road,unsigned int &current_id,std::vector<COpendriveRoadSegment *> &segments)
+{
+  COpendriveLane *new_lane;
+  COpendriveRoadSegment *new_segment;
+  CRoadSegment *right_segment,*left_segment;
+  std::stringstream txt;
+  TPoint start_point,end_point;
+  TOpendriveWorldPose start_pose,end_pose;
+  COpendriveParamPoly3 *new_geometry;
+
+  if(right_road==NULL)
+    throw CException(_HERE_,"Invalid right road reference");
+  segments.clear();
+  for(unsigned int i=0;i<right_road->get_num_segments();i++)
+  {
+    right_segment=right_road->get_segment_by_index(i);
+    new_segment=new COpendriveRoadSegment();
+    txt.str("");
+    txt << "road" << current_id;
+    new_segment->set_name(txt.str());
+    new_segment->set_id(current_id);
+    new_segment->set_center_mark(OD_MARK_SOLID);
+    if(right_segment->has_parent_junction())
+      new_segment->set_junction_id(right_segment->get_parent_junction().get_id());
+    else 
+      new_segment->set_junction_id(-1);
+    // add geometry
+    right_segment->get_start_point(start_point);
+    start_pose.x=start_point.x;
+    start_pose.y=start_point.y;
+    start_pose.heading=start_point.heading;
+    right_segment->get_end_point(end_point);
+    end_pose.x=end_point.x;
+    end_pose.y=end_point.y;
+    end_pose.heading=end_point.heading;
+    new_geometry=new COpendriveParamPoly3(start_pose,end_pose);
+    new_segment->add_geometry(new_geometry,right_road->get_resolution());
+    // add right lanes
+    for(unsigned int j=0;j<right_road->get_num_lanes();j++)
+    {
+      new_lane=new COpendriveLane();
+      new_lane->set_speed(right_road->get_lane_speed());
+      new_lane->set_width(right_road->get_lane_width());
+      new_lane->set_id(-(j+1));
+      if(j==(right_road->get_num_lanes()-1))
+        new_lane->set_right_mark(OD_MARK_SOLID);
+      else
+      {
+        if(right_segment->are_lanes_linked(j,j+1))
+          new_lane->set_right_mark(OD_MARK_BROKEN);
+        else
+          new_lane->set_right_mark(OD_MARK_SOLID);
+      }
+      new_segment->add_right_lane(new_lane);
+    }
+    // add left lanes
+    if(left_road!=NULL)
+    {
+      left_segment=left_road->get_segment_by_index(left_road->get_num_segments()-1-i);
+      for(unsigned int j=0;j<left_road->get_num_lanes();j++)
+      {
+        new_lane=new COpendriveLane();
+        new_lane->set_speed(left_road->get_lane_speed());
+        new_lane->set_width(left_road->get_lane_width());
+        new_lane->set_id(j+1);
+        if(j==(left_road->get_num_lanes()-1))
+          new_lane->set_right_mark(OD_MARK_SOLID);
+        else
+        {
+          if(left_segment->are_lanes_linked(j,j+1))
+            new_lane->set_right_mark(OD_MARK_BROKEN);
+          else
+            new_lane->set_right_mark(OD_MARK_SOLID);
+        }
+        new_segment->add_left_lane(new_lane);
+      }
+    }
+    // add connectivity
+    if(right_segment==right_road->get_first_segment())
+    {
+      if(right_road->exist_prev_junction())
+      {
+        new_segment->predecessor_exists=true;
+        new_segment->predecessor.road=false;
+        new_segment->predecessor.id=right_road->get_prev_junction().get_id();
+        new_segment->predecessor.end_contact=false;
+      }
+      else
+        new_segment->predecessor_exists=false;
+    }
+    else
+    {
+      new_segment->predecessor_exists=true;
+      new_segment->predecessor.road=true;
+      new_segment->predecessor.id=current_id-1;
+      new_segment->predecessor.end_contact=false;
+    }
+    if(right_segment==right_road->get_last_segment())
+    {
+      if(right_road->exist_next_junction())
+      {
+        new_segment->successor_exists=true;
+        new_segment->successor.road=false;
+        new_segment->successor.id=right_road->get_next_junction().get_id();
+        new_segment->successor.end_contact=true;
+      }
+      else
+        new_segment->successor_exists=false;
+    }
+    else
+    {
+      new_segment->successor_exists=true;
+      new_segment->successor.road=true;
+      new_segment->successor.id=current_id+1;
+      new_segment->successor.end_contact=true;
+    }
+    segments.push_back(new_segment);
+    current_id++;
+  }
+}
+
+void COpendriveRoadSegment::save(OpenDRIVE::road_type **road_info)
+{
+  ::lanes lanes;
+  ::laneSection *section;
+  ::center center;
+  ::centerLane center_lane;
+  ::roadMark1 lane_mark;
+  ::left left_lanes;
+  ::right right_lanes;
+  ::type2 road_type;
+  ::planView plan_view;
+  ::link2 link;
+  ::predecessor1 predecessor;
+  ::successor1 successor;
+
+  // center lane
+  center_lane.id(0);
+  center_lane.type("driving");
+  center_lane.level("false");
+  switch(this->center_mark)
+  {
+    case OD_MARK_NONE:
+      lane_mark.type1("none");
+      break;
+    case OD_MARK_SOLID:
+      lane_mark.type1("solid");
+      break;
+    case OD_MARK_BROKEN:
+      lane_mark.type1("broken");
+      break;
+    case OD_MARK_SOLID_SOLID:
+      lane_mark.type1("solid solid");
+      break;
+    case OD_MARK_SOLID_BROKEN:
+      lane_mark.type1("solid broken");
+      break;
+    case OD_MARK_BROKEN_SOLID:
+      lane_mark.type1("broken solid");
+      break;
+    case OD_MARK_BROKEN_BROKEN:
+      lane_mark.type1("broken broken");
+      break;
+    default:
+      lane_mark.type1("none");
+      break;
+  }  
+  lane_mark.sOffset(0.0);
+  lane_mark.weight("standard");
+  lane_mark.color("standard");
+  lane_mark.width(0.2);
+  lane_mark.laneChange("both");
+  center_lane.roadMark().push_back(lane_mark);  
+  center.lane(center_lane);
+  section=new laneSection(center);
+  // lane section
+  section->s(0.0);
+  for(unsigned int i=0;i<this->left_lanes.size();i++)
+  {
+    ::lane new_lane;
+    this->left_lanes[i]->save(new_lane);
+    left_lanes.lane().push_back(new_lane);
+  }
+  section->left(left_lanes);
+  for(unsigned int i=0;i<this->right_lanes.size();i++)
+  {
+    ::lane new_lane;
+    this->right_lanes[i]->save(new_lane);
+    right_lanes.lane().push_back(new_lane);
+  }
+  section->right(right_lanes);
+  lanes.laneSection().push_back(*section);
+  delete section; 
+  // geometry
+  for(unsigned int i=0;i<this->geometries.size();i++)
+  {
+    ::geometry new_geometry;
+    this->geometries[i]->save(new_geometry);
+    plan_view.geometry().push_back(new_geometry);
+  }
+  (*road_info)=new OpenDRIVE::road_type(plan_view,lanes);
+  (*road_info)->name(this->name);
+  (*road_info)->id(std::to_string(this->id));
+  (*road_info)->length(this->get_length());
+  // road type
+  road_type.s(0.0);
+  road_type.type("town");
+  (*road_info)->type().push_back(road_type);
+  // link information
+  (*road_info)->junction(std::to_string((int)this->junction_id));
+  /* get connectivity info from junction object */
+  if(this->predecessor.road)
+    predecessor.elementType("road");
+  else
+    predecessor.elementType("junction");
+  predecessor.elementId(std::to_string(this->predecessor.id));
+  if(this->predecessor.end_contact)
+    predecessor.contactPoint("end");
+  else
+    predecessor.contactPoint("start");
+  link.predecessor(predecessor);
+  if(this->successor.road)
+    successor.elementType("road");
+  else
+    successor.elementType("junction");
+  successor.elementId(std::to_string(this->successor.id));
+  if(this->successor.end_contact)
+    successor.contactPoint("end");
+  else
+    successor.contactPoint("start");
+  link.successor(successor);
+  (*road_info)->lane_link(link);
+}
+
+std::string COpendriveRoadSegment::get_name(void) const
+{
+  return this->name;
+}
+
+unsigned int COpendriveRoadSegment::get_id(void) const
+{
+  return this->id;
+}
+
+unsigned int COpendriveRoadSegment::get_num_right_lanes(void) const
+{
+  return this->right_lanes.size();
+}
+
+unsigned int COpendriveRoadSegment::get_num_left_lanes(void) const
+{
+  return this->left_lanes.size();
+}
+
+unsigned int COpendriveRoadSegment::get_num_signals(void) const
+{
+  return this->signals.size();  
+}
+
+const COpendriveSignal &COpendriveRoadSegment::get_signal(unsigned int index) const
+{
+  std::stringstream error;
+
+  if(index>=0 && index<this->signals.size())
+    return *this->signals[index];
+  else
+  {
+    error << "Invalid signal index (" << index << ") for segment " << this->name;
+    throw CException(_HERE_,error.str());
+  }
+}
+
+unsigned int COpendriveRoadSegment::get_num_objects(void) const
+{
+  return this->objects.size();
+}
+
+const COpendriveObject &COpendriveRoadSegment::get_object(unsigned int index) const
+{
+  std::stringstream error;
+
+  if(index>=0 && index<this->objects.size())
+    return *this->objects[index];
+  else
+  {
+    error << "Invalid object index (" << index << ") for segment " << this->name;
+    throw CException(_HERE_,error.str());
+  }
+}
+
+double COpendriveRoadSegment::get_length(void)
+{
+  double length=0.0;
+
+  for(unsigned int i=0;i<this->geometries.size();i++)
+    length+=this->geometries[i]->get_length();
+
+  return length;
+}
+
+COpendriveRoadSegment::~COpendriveRoadSegment()
+{
+  this->free();
+  this->objects.clear();
+  this->name="";
+  this->id=-1;
+  this->center_mark=OD_MARK_NONE;
+  this->junction_id=-1;
+}
+
diff --git a/src/opendrive_signal.cpp b/src/opendrive/opendrive_signal.cpp
similarity index 70%
rename from src/opendrive_signal.cpp
rename to src/opendrive/opendrive_signal.cpp
index bea1acdf228e146f63b376d20800fd2430acd059..9ae4ff2a017b5f38d525514b319544a478bde3bf 100644
--- a/src/opendrive_signal.cpp
+++ b/src/opendrive/opendrive_signal.cpp
@@ -1,10 +1,10 @@
-#include "opendrive_signal.h"
+#include "opendrive/opendrive_signal.h"
+#include "common.h"
 #include "exceptions.h"
 #include <cmath>
 
 COpendriveSignal::COpendriveSignal()
 {
-  this->segment=NULL;
   this->id=-1;
   this->pose.s=0.0;
   this->pose.t=0.0;
@@ -13,12 +13,10 @@ COpendriveSignal::COpendriveSignal()
   this->sub_type="";
   this->value=0;
   this->text="";
-  this->scale_factor=DEFAULT_SCALE_FACTOR;
 }
 
 COpendriveSignal::COpendriveSignal(const COpendriveSignal &object)
 {
-  this->segment=object.segment;
   this->id=object.id;
   this->pose.s=object.pose.s;
   this->pose.t=object.pose.t;
@@ -27,10 +25,9 @@ COpendriveSignal::COpendriveSignal(const COpendriveSignal &object)
   this->sub_type=object.sub_type;
   this->value=object.value;
   this->text=object.text;
-  this->scale_factor=object.scale_factor;
 }
 
-void COpendriveSignal::load(signals::signal_type &signal_info,COpendriveRoadSegment *segment)
+void COpendriveSignal::load(signals::signal_type &signal_info)
 {
   std::stringstream ss(signal_info.id().get());
   ss >> this->id;
@@ -45,18 +42,6 @@ void COpendriveSignal::load(signals::signal_type &signal_info,COpendriveRoadSegm
   if (signal_info.orientation().present() && signal_info.orientation().get() == orientation::cxx_1)
     reverse = true;
   this->pose.heading = normalize_angle(this->pose.heading + (reverse ? M_PI : 0.0));
-  this->segment=segment;
-}
-
-void COpendriveSignal::update_references(segment_up_ref_t &segment_refs)
-{
-  if(segment_refs.find(this->segment)!=segment_refs.end())
-    this->segment=segment_refs[this->segment];
-}
-
-void COpendriveSignal::set_scale_factor(double scale)
-{
-  this->scale_factor=scale;
 }
 
 int COpendriveSignal::get_id(void) const
@@ -64,35 +49,17 @@ int COpendriveSignal::get_id(void) const
   return this->id;
 }
 
-double COpendriveSignal::get_scale_factor(void)
-{ 
-  return this->scale_factor;
-}
-
 TOpendriveTrackPose COpendriveSignal::get_track_pose(void) const
 {
   TOpendriveTrackPose track;
  
-  track.s=this->pose.s/this->scale_factor;
-  track.t=this->pose.t/this->scale_factor;
+  track.s=this->pose.s;
+  track.t=this->pose.t;
   track.heading=this->pose.heading;
 
   return track;
 }
 
-TOpendriveWorldPose COpendriveSignal::get_world_pose(void) const
-{
-  TOpendriveWorldPose world;
-
-  if(this->segment!=NULL)
-  {
-    world=segment->transform_to_world_pose(this->get_track_pose());
-    return world;
-  }
-  else
-    throw CException(_HERE_,"Invalid parent segment reference");
-}
-
 void COpendriveSignal::get_type(std::string &type, std::string &sub_type) const
 {
   type=this->type;
@@ -125,7 +92,6 @@ std::ostream& operator<<(std::ostream& out, COpendriveSignal &signal)
 
 COpendriveSignal::~COpendriveSignal()
 {
-  this->segment=NULL;
   this->id=-1;
   this->pose.s=0.0;
   this->pose.t=0.0;
@@ -134,6 +100,5 @@ COpendriveSignal::~COpendriveSignal()
   this->sub_type="";
   this->value=0;
   this->text="";
-  this->scale_factor=DEFAULT_SCALE_FACTOR;
 }
 
diff --git a/src/opendrive_spiral.cpp b/src/opendrive/opendrive_spiral.cpp
similarity index 71%
rename from src/opendrive_spiral.cpp
rename to src/opendrive/opendrive_spiral.cpp
index 089ad95045e80fd433633b0d09531642d50a8af4..ef648bb2b70f253a02e304cdc3d3e38c6a28661e 100644
--- a/src/opendrive_spiral.cpp
+++ b/src/opendrive/opendrive_spiral.cpp
@@ -1,4 +1,4 @@
-#include "opendrive_spiral.h"
+#include "opendrive/opendrive_spiral.h"
 
 COpendriveSpiral::COpendriveSpiral()
 {
@@ -12,6 +12,12 @@ COpendriveSpiral::COpendriveSpiral(const COpendriveSpiral &object) : COpendriveG
   this->end_curvature=object.end_curvature;
 }
 
+COpendriveSpiral::COpendriveSpiral(TOpendriveWorldPose &start_pose,double length,double start_curvature,double end_curvature) : COpendriveGeometry(start_pose,length)
+{
+  this->start_curvature=start_curvature;
+  this->end_curvature=end_curvature;
+}
+
 bool COpendriveSpiral::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
 {
   return true;
@@ -32,6 +38,15 @@ void COpendriveSpiral::load_params(const planView::geometry_type &geometry_info)
   this->end_curvature = (geometry_info.spiral().get().curvEnd().present() ? geometry_info.spiral().get().curvEnd().get() : 0.0);
 }
 
+void COpendriveSpiral::save_params(planView::geometry_type &geometry_info)
+{
+  ::spiral spiral;
+
+  spiral.curvStart(this->start_curvature);
+  spiral.curvEnd(this->end_curvature);
+  geometry_info.spiral(spiral);
+}
+
 std::string COpendriveSpiral::get_name(void)
 {
   return std::string("spiral");
@@ -46,8 +61,13 @@ COpendriveGeometry *COpendriveSpiral::clone(void)
 
 void COpendriveSpiral::get_curvature(double &start,double &end)
 {
-  start=this->start_curvature*this->scale_factor;
-  end=this->end_curvature*this->scale_factor;
+  start=this->start_curvature;
+  end=this->end_curvature;
+}
+
+void COpendriveSpiral::get_curvature_at(double length,double &curvature)
+{
+  curvature=0.0;
 }
 
 void COpendriveSpiral::operator=(const COpendriveSpiral &object)
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
deleted file mode 100644
index ddda090bae8d56c1654a5e53b98b0a50a3532c08..0000000000000000000000000000000000000000
--- a/src/opendrive_lane.cpp
+++ /dev/null
@@ -1,993 +0,0 @@
-#include "opendrive_lane.h"
-#include "exceptions.h"
-#include <math.h>
-
-COpendriveLane::COpendriveLane()
-{
-  this->nodes.clear();
-  this->next.clear();
-  this->prev.clear();
-  this->left_lane=NULL;
-  this->left_mark=OD_MARK_NONE;
-  this->right_lane=NULL;
-  this->right_mark=OD_MARK_NONE;
-  this->segment=NULL;
-  this->resolution=DEFAULT_RESOLUTION;
-  this->scale_factor=DEFAULT_SCALE_FACTOR;
-  this->width=0.0;
-  this->speed=0.0;
-  this->offset=0.0;
-  this->id=0;
-  this->index=-1;
-}
-
-COpendriveLane::COpendriveLane(const COpendriveLane &object)
-{
-  this->nodes=object.nodes;
-  this->next=object.next;
-  this->prev=object.prev;
-  this->left_lane=object.left_lane;
-  this->left_mark=object.left_mark;
-  this->right_lane=object.right_lane;
-  this->right_mark=object.right_mark;
-  this->segment=object.segment;
-  this->resolution=object.resolution;
-  this->scale_factor=object.scale_factor;
-  this->width=object.width;
-  this->speed=object.speed;
-  this->offset=object.offset;
-  this->id=object.id;
-  this->index=object.index;
-}
-
-void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
-{
-  unsigned int min_num_nodes;
-
-  if(lane!=NULL)
-  {
-    if(this->get_num_nodes()<lane->get_num_nodes())
-      min_num_nodes=this->get_num_nodes();
-    else
-      min_num_nodes=lane->get_num_nodes();
-    for(unsigned int i=0;i<min_num_nodes-1;i++)
-    {
-      this->nodes[i]->add_link(lane->nodes[i+1],lane->right_mark,this->segment,NULL);
-      lane->nodes[i]->add_link(this->nodes[i+1],lane->right_mark,this->segment,NULL);
-      this->left_mark=lane->right_mark;
-    } 
-    if(min_num_nodes>0)
-    {
-      this->left_lane=lane;
-      lane->right_lane=this;
-    }
-  }
-}
-
-void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start, bool to_start,bool belongs_to_lane)
-{
-  COpendriveRoadNode *start,*end;
-  std::stringstream error;
-
-  if(lane!=NULL)
-  {
-    if(this->get_num_nodes()>0 && lane->get_num_nodes()>0)
-    {
-      if(from_start)
-      {
-        if(this->id<0)
-          start=this->nodes[0];
-        else
-          start=this->nodes[this->nodes.size()-1];
-        if(to_start)
-        {
-          if(lane->id<0)
-            end=lane->nodes[0];
-          else
-            end=lane->nodes[lane->nodes.size()-1];
-        }
-        else
-        {
-          if(lane->id<0)
-            end=lane->nodes[lane->nodes.size()-1];
-          else
-            end=lane->nodes[0];
-        }
-      }
-      else
-      {
-        if(this->id<0)
-          start=this->nodes[this->nodes.size()-1];
-        else
-          start=this->nodes[0];
-        if(to_start)
-        {
-          if(lane->id<0)
-            end=lane->nodes[0];
-          else
-            end=lane->nodes[lane->nodes.size()-1];
-        }
-        else
-        {
-          if(lane->id<0)
-            end=lane->nodes[lane->nodes.size()-1];
-          else
-            end=lane->nodes[0];
-        }
-      }
-      if(belongs_to_lane)
-        start->add_link(end,mark,this->segment,this);
-      else
-        start->add_link(end,mark,this->segment,NULL);
-      // link lane
-      this->add_next_lane(lane);
-      lane->add_prev_lane(this);
-    }
-    else 
-    {
-      error << "Lane " << this->id << " of segment " << this->segment->get_name() << " or lane " << lane->get_id() << " of segment " << lane->get_segment().get_name() << " has no nodes";
-      throw CException(_HERE_,error.str());
-    }
-  }
-}
-
-void COpendriveLane::add_next_lane(COpendriveLane *lane)
-{
-  for(unsigned int i=0;i<this->next.size();i++)
-    if(this->next[i]==lane)// lane is already linked
-      return;
-
-  // add the lane
-  this->next.push_back(lane);
-}
-
-void COpendriveLane::add_prev_lane(COpendriveLane *lane)
-{
-  for(unsigned int i=0;i<this->prev.size();i++)
-    if(this->prev[i]==lane)// lane is already linked
-      return;
-
-  // add the lane
-  this->prev.push_back(lane);
-}
-
-void COpendriveLane::remove_lane(COpendriveLane *lane)
-{
-  std::vector<COpendriveLane *>::iterator lane_it;
-
-  // remove the reference from neightbour lanes
-  if(this->right_lane==lane)
-    this->right_lane=NULL;
-  if(this->left_lane==lane)
-    this->left_lane=NULL;
-  // remove the reference from next lanes
-  for(lane_it=this->next.begin();lane_it!=this->next.end();)
-  {
-    if((*lane_it)==lane)
-      lane_it=this->next.erase(lane_it);
-     else
-      lane_it++;
-  }
-  // remove the reference from next lanes
-  for(lane_it=this->prev.begin();lane_it!=this->prev.end();)
-  {
-    if((*lane_it)==lane)
-      lane_it=this->prev.erase(lane_it);
-     else
-      lane_it++;
-  }
-}
-
-void COpendriveLane::add_node(COpendriveRoadNode *node)
-{
-  if(this->has_node(node))
-    return;
-  // link the new node with the previous one in the current lane, if any
-  if(this->nodes.size()>0)
-    this->nodes[this->nodes.size()-1]->add_link(node,OD_MARK_NONE,this->segment,this);
-  // add the new node
-  this->nodes.push_back(node);
-}
-
-bool COpendriveLane::has_node(COpendriveRoadNode *node)
-{
-  for(unsigned int i=0;i<this->nodes.size();i++)
-    if(this->nodes[i]==node)
-      return true;
-
-  return false;
-}
-
-bool COpendriveLane::has_node_with_index(unsigned int index)
-{
-  for(unsigned int i=0;i<this->nodes.size();i++)
-    if(this->nodes[i]->get_index()==index)
-      return true;
-
-  return false;
-}
-
-void COpendriveLane::set_parent_segment(COpendriveRoadSegment *segment)
-{
-  this->segment=segment;
-}
-
-void COpendriveLane::set_left_lane(COpendriveLane *lane,opendrive_mark_t mark)
-{
-  this->left_lane=lane;
-  this->left_mark=mark;
-}
-
-void COpendriveLane::set_right_lane(COpendriveLane *lane,opendrive_mark_t mark)
-{
-  this->right_lane=lane;
-  this->right_mark=mark;
-}
-
-void COpendriveLane::set_resolution(double res)
-{
-  this->resolution=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_width(double width)
-{
-  this->width=width;
-}
-
-void COpendriveLane::set_speed(double speed)
-{
-  this->speed=speed;
-}
-
-void COpendriveLane::set_offset(double offset)
-{
-  this->offset=offset;
-}
-
-void COpendriveLane::set_id(int id)
-{
-  this->id=id;
-}
-
-void COpendriveLane::set_index(unsigned int index)
-{
-  this->index=index;
-}
-
-bool COpendriveLane::updated(lane_up_ref_t &refs)
-{
-  lane_up_ref_t::iterator updated_it;
-
-  for(updated_it=refs.begin();updated_it!=refs.end();updated_it++)
-    if(updated_it->second==this)
-      return true;
-
-  return false;
-}
-
-void COpendriveLane::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
-{
-  std::vector<COpendriveLane *>::iterator lane_it;
-  std::vector<COpendriveRoadNode *>::iterator node_it;
- 
-  if(this->updated(lane_refs))
-  {
-    for(node_it=this->nodes.begin();node_it!=this->nodes.end();node_it++)
-    {
-      if(node_refs.find(*node_it)!=node_refs.end())
-      {
-        (*node_it)=node_refs[*node_it];
-        (*node_it)->update_references(segment_refs,lane_refs,node_refs);
-      }
-      else if((*node_it)->updated(node_refs))
-        (*node_it)->update_references(segment_refs,lane_refs,node_refs);
-    }
-    for(lane_it=this->next.begin();lane_it!=this->next.end();lane_it++)
-      if(lane_refs.find(*lane_it)!=lane_refs.end())
-        (*lane_it)=lane_refs[*lane_it];
-    for(lane_it=this->prev.begin();lane_it!=this->prev.end();lane_it++)
-      if(lane_refs.find(*lane_it)!=lane_refs.end())
-        (*lane_it)=lane_refs[*lane_it];
-    if(lane_refs.find(this->left_lane)!=lane_refs.end())
-      this->left_lane=lane_refs[this->left_lane];
-    if(lane_refs.find(this->right_lane)!=lane_refs.end())
-      this->right_lane=lane_refs[this->right_lane];
-    if(segment_refs.find(this->segment)!=segment_refs.end())
-      this->segment=segment_refs[this->segment];
-  }
-}
-
-void COpendriveLane::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
-{
-  std::vector<COpendriveLane *>::iterator lane_it;
-  std::vector<COpendriveRoadNode *>::iterator node_it;
-
-  if(this->updated(lane_refs))
-  {
-    for(node_it=this->nodes.begin();node_it!=this->nodes.end();)
-    {
-      if((*node_it)->updated(node_refs))
-      {
-        (*node_it)->clean_references(segment_refs,lane_refs,node_refs);
-        node_it++;
-      }
-      else
-        node_it=this->nodes.erase(node_it);
-    }
-    for(lane_it=this->next.begin();lane_it!=this->next.end();)
-    {
-      if(!(*lane_it)->updated(lane_refs))
-        lane_it=this->next.erase(lane_it);
-      else
-        lane_it++;
-    }
-    for(lane_it=this->prev.begin();lane_it!=this->prev.end();)
-    {
-      if(!(*lane_it)->updated(lane_refs))
-        lane_it=this->prev.erase(lane_it);
-      else
-        lane_it++;
-    }
-    if(lane_refs.find(this->left_lane)!=lane_refs.end())
-      this->left_lane=lane_refs[this->left_lane];
-    else if(!this->left_lane->updated(lane_refs))
-      this->left_lane=NULL;
-    if(lane_refs.find(this->right_lane)!=lane_refs.end())
-      this->right_lane=lane_refs[this->right_lane];
-    else if(!this->right_lane->updated(lane_refs))
-      this->right_lane=NULL;
-  }
-}
-
-void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start)
-{
-  std::vector<COpendriveRoadNode *>::iterator it;
-  segment_up_ref_t new_segment_ref;
-  unsigned int start_node_index,i;
-  TOpendriveWorldPose start_pose;
-  COpendriveRoadNode *new_node;
-  std::stringstream error;
-  double distance;
-  
-  if(start==NULL)
-    return;
-  start_node_index=this->get_closest_node_index(*start,distance,3.14159);
-  if(start_node_index==(unsigned int)-1)
-  {
-    error << "No node close to (x=" << start->x << ",y=" << start->y << ",heading=" << start->heading << ") in lane " << this->id << " of segment " << this->segment->get_name();
-    throw CException(_HERE_,error.str());
-  }
-  // get the actual start position on the lane
-  this->get_closest_pose(*start,start_pose,3.14159);
-  // eliminate all the node before the start one
-  for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
-  {
-    if(i<start_node_index)
-    {
-      if((*it)->updated(new_node_ref))
-      {
-        new_node_ref.erase((*it)->get_original_node(new_node_ref));
-        delete *it;
-      }
-      it=this->nodes.erase(it);
-    }
-    else
-    {
-      if(!(*it)->updated(new_node_ref))
-      {
-        new_node=(*it)->clone(new_link_ref);
-        new_node_ref[*it]=new_node;
-      }
-      it++;
-    }
-  }
-  this->prev.clear();// it is no longer connected to any previous lane
-  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
-  this->nodes[0]->update_start_pose(this,start_pose,distance);
-}
-
-void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *end)
-{
-  std::vector<COpendriveRoadNode *>::iterator it;
-  segment_up_ref_t new_segment_ref;
-  unsigned int end_node_index,i;
-  TOpendriveWorldPose end_pose;
-  COpendriveRoadNode *new_node;
-  std::stringstream error;
-  double distance;
-    
-  if(end==NULL)
-    return;
-  end_node_index=this->get_closest_node_index(*end,distance,3.14159);
-  if(end_node_index==(unsigned int)-1)
-  {
-    error << "No node close to (x=" << end->x << ",y=" << end->y << ",heading=" << end->heading << ") in lane " << this->id << " of segment " << this->segment->get_name();
-    throw CException(_HERE_,error.str());
-  }
-  this->get_closest_pose(*end,end_pose,3.14159);
-  // eliminate all the node before the start one
-  for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
-  {
-    if(i>end_node_index)
-    {
-      if((*it)->updated(new_node_ref))
-      {
-        new_node_ref.erase((*it)->get_original_node(new_node_ref));
-        delete *it;
-      }
-      it=this->nodes.erase(it);
-    }
-    else
-    {
-      if(!(*it)->updated(new_node_ref))
-      {
-        new_node=(*it)->clone(new_link_ref);
-        new_node_ref[*it]=new_node;
-      }
-      it++;
-    }
-  }
-  this->next.clear();// it is no longer connected to any next lane
-  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
-  this->nodes[this->nodes.size()-1]->update_end_pose(this,end_pose,distance);
-}
-
-COpendriveLane *COpendriveLane::get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start,TOpendriveWorldPose *end)
-{
-  COpendriveLane *new_lane;
-
-  if(start==NULL && end==NULL)
-    return this->clone(new_node_ref,new_lane_ref,new_link_ref);
-  new_lane=new COpendriveLane(*this);
-  new_lane_ref[this]=new_lane;
-  if(this->id<0)
-  {
-    new_lane->update_start_node(new_node_ref,new_lane_ref,new_link_ref,start);
-    new_lane->update_end_node(new_node_ref,new_lane_ref,new_link_ref,end);
-  }
-  else
-  {
-    new_lane->update_start_node(new_node_ref,new_lane_ref,new_link_ref,end);
-    new_lane->update_end_node(new_node_ref,new_lane_ref,new_link_ref,start);
-  }
-
-  return new_lane;
-}
-
-COpendriveLane *COpendriveLane::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref)
-{
-  COpendriveLane *new_lane;
-  std::vector<COpendriveRoadNode *>::iterator it;
-  COpendriveRoadNode *new_node;
-  segment_up_ref_t new_segment_ref;
-
-  new_lane=new COpendriveLane(*this);
-  new_lane_ref[this]=new_lane;
-  for(it=new_lane->nodes.begin();it!=new_lane->nodes.end();it++)
-  {
-    new_node=(*it)->clone(new_link_ref);
-    new_node_ref[*it]=new_node;
-  }
-  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
-
-  return new_lane;
-}
-
-void COpendriveLane::load(const ::lane &lane_info,COpendriveRoadSegment *segment)
-{
-  std::stringstream error;
-  opendrive_mark_t mark;
-
-  this->nodes.clear();
-  this->prev.clear();
-  this->next.clear();
-  this->left_lane=NULL;
-  this->left_mark=OD_MARK_NONE;
-  this->right_lane=NULL;
-  this->right_mark=OD_MARK_NONE;
-  this->set_id(lane_info.id().get());
-  // import lane width
-  if(lane_info.width().size()<1)
-  {
-    error << "Lane " << this->id << " of segment " << segment->get_name() << " has no width record";
-    throw CException(_HERE_,error.str());
-  }
-  else if(lane_info.width().size()>1)
-  {
-    error << "Lane " << this->id << " of segment " << segment->get_name() << " has more than one width record";
-    throw CException(_HERE_,error.str());
-  }
-  this->set_width(lane_info.width().begin()->a().get());
-  // import lane speed
-  if(lane_info.speed().size()<1)
-    this->set_speed(60.0);
-  else if(lane_info.speed().size()>1)
-  {
-    error << "Lane " << this->id << " of segment " << segment->get_name() << " has more than one speed record";
-    throw CException(_HERE_,error.str());
-  }
-  else
-    this->set_speed(lane_info.speed().begin()->max().get());
-  //lane mark
-  if(lane_info.roadMark().size()==0)
-    mark=OD_MARK_NONE;
-  else if(lane_info.roadMark().size()>1)
-  {
-    error << "Lane " << this->id << " of segment " << segment->get_name() << " has more than one road mark record";
-    throw CException(_HERE_,error.str());
-  }
-  else if(lane_info.roadMark().size()==1)
-  {
-    if(lane_info.roadMark().begin()->type1().present())
-    {
-      if(lane_info.roadMark().begin()->type1().get()=="none")
-        mark=OD_MARK_NONE;
-      else if(lane_info.roadMark().begin()->type1().get()=="solid")
-        mark=OD_MARK_SOLID;
-      else if(lane_info.roadMark().begin()->type1().get()=="broken")
-        mark=OD_MARK_BROKEN;
-      else if(lane_info.roadMark().begin()->type1().get()=="solid solid")
-        mark=OD_MARK_SOLID_SOLID;
-      else if(lane_info.roadMark().begin()->type1().get()=="solid broken")
-        mark=OD_MARK_SOLID_BROKEN;
-      else if(lane_info.roadMark().begin()->type1().get()=="broken solid")
-        mark=OD_MARK_BROKEN_SOLID;
-      else if(lane_info.roadMark().begin()->type1().get()=="broken broken")
-        mark=OD_MARK_BROKEN_BROKEN;
-      else
-        mark=OD_MARK_NONE;
-    }
-    else
-      mark=OD_MARK_NONE;
-  }
-  this->right_mark=mark;
-
-  this->set_parent_segment(segment);
-}
-
-unsigned int COpendriveLane::get_num_nodes(void) const
-{
-  return this->nodes.size();
-}
-
-const COpendriveRoadNode &COpendriveLane::get_node(unsigned int index) const
-{
-  std::stringstream error;
-
-  if(index>=0 && index<this->nodes.size())
-    return *this->nodes[index];
-  else
-  {
-    error << "Invalid node index (" << index << ") for segment " << this->segment->get_name() << " (lane " << this->id << ")"; 
-    throw CException(_HERE_,error.str());
-  }
-}
-
-unsigned int COpendriveLane::get_num_next_lanes(void) const
-{
-  return this->next.size();
-}
-
-const COpendriveLane &COpendriveLane::get_next_lane(unsigned int index) const
-{
-  std::stringstream error;
-
-  if(index>=0 && index<this->next.size())
-    return *this->next[index];
-  else
-  {
-    error << "Invalid next lane index (" << index << ") for segment " << this->segment->get_name() << " (lane " << this->id << ")"; 
-    throw CException(_HERE_,error.str());
-  }
-}
-
-unsigned int COpendriveLane::get_num_prev_lanes(void) const
-{
-  return this->prev.size();
-}
-
-const COpendriveLane &COpendriveLane::get_prev_lane(unsigned int index) const
-{
-  std::stringstream error;
-
-  if(index>=0 && index<this->prev.size())
-    return *this->prev[index];
-  else
-  {
-    error << "Invalid previous lane index (" << index << ") for segment " << this->segment->get_name() << " (lane " << this->id << ")"; 
-    throw CException(_HERE_,error.str());
-  }
-}
-
-const COpendriveRoadSegment &COpendriveLane::get_segment(void) const
-{
-  return *this->segment;
-}
-
-bool COpendriveLane::exist_left_lane(void)
-{
-  if(this->left_lane!=NULL)
-    return true;
-  else
-    return false;
-}
-
-const COpendriveLane &COpendriveLane::get_left_lane(void) const
-{
-  std::stringstream error;
-
-  if(this->left_lane!=NULL)
-    return *this->left_lane;
-
-  error << "Left lane not defined for segment " << this->segment->get_name() << " (lane " << this->id << ")"; 
-  
-  throw CException(_HERE_,error.str());
-}
-
-bool COpendriveLane::exist_right_lane(void)
-{
-  if(this->right_lane!=NULL)
-    return true;
-  else
-    return false;
-}
-
-const COpendriveLane &COpendriveLane::get_right_lane(void) const
-{
-  std::stringstream error;
-
-  if(this->right_lane!=NULL)
-    return *this->right_lane;
-
-  error << "Right lane not defined for segment " << this->segment->get_name() << " (lane " << this->id << ")";  
-
-  throw CException(_HERE_,error.str());
-}
-
-opendrive_mark_t COpendriveLane::get_left_road_mark(void) const
-{
-  return this->left_mark;
-}
-
-opendrive_mark_t COpendriveLane::get_right_road_mark(void) const
-{
-  return this->right_mark;
-}
-
-double COpendriveLane::get_width(void) const
-{
-  return this->width/this->scale_factor;
-}
-
-double COpendriveLane::get_speed(void) const
-{
-  return this->speed;
-}
-
-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;
-}
-
-int COpendriveLane::get_id(void) const
-{
-  return this->id;
-}
-
-TOpendriveWorldPose COpendriveLane::get_start_pose(void) const
-{
-  TOpendriveTrackPose track_pose;
-  TOpendriveWorldPose world_pose;
-  std::stringstream error;
-
-  track_pose.t=0.0;
-  if(this->id<0)// right lane
-  {
-    track_pose.s=0.0;
-    track_pose.heading=0.0;
-  }
-  else
-  {
-    track_pose.s=this->segment->get_length();
-    track_pose.heading=3.14159;
-  }
-  world_pose=this->transform_to_world_pose(track_pose);
-
-  return world_pose;
-}
-
-TOpendriveWorldPose COpendriveLane::get_end_pose(void) const
-{
-  TOpendriveTrackPose track_pose;
-  TOpendriveWorldPose world_pose;
-  std::stringstream error;
-  
-  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.s=0.0;
-    track_pose.heading=3.14159;
-  }
-  world_pose=this->transform_to_world_pose(track_pose);
-
-  return world_pose;
-}
-
-double COpendriveLane::get_length(void) const
-{
-  std::stringstream error;
-  double length=0.0;
-
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  {
-    for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
-    {
-      if(&this->nodes[i]->get_link(j).get_parent_lane()==this)
-        length+=this->nodes[i]->links[j]->get_length();
-    }
-  }
-
-  return length;
-}
-
-TOpendriveWorldPose COpendriveLane::get_pose_at(double length) const
-{
-  TOpendriveWorldPose world_pose={0};
-
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  {
-    for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
-    {
-      if(&this->nodes[i]->get_link(j).get_parent_lane()==this)
-      {
-        if((length-this->nodes[i]->links[j]->get_length())<0.0)
-        {
-          world_pose=this->nodes[i]->links[j]->get_pose_at(length);
-          return world_pose;
-        }
-        else
-          length-=this->nodes[i]->links[j]->get_length();
-      }
-    }
-  }
-
-  return world_pose;
-}
-
-double COpendriveLane::get_curvature_at(double length) const
-{
-  double curvature;
-
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  {
-    for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
-    {
-      if(&this->nodes[i]->get_link(j).get_parent_lane()==this)
-      {
-        if((length-this->nodes[i]->links[j]->get_length())<0.0)
-        {
-          curvature=this->nodes[i]->links[j]->get_curvature_at(length);
-          return curvature;
-        }
-        else
-          length-=this->nodes[i]->links[j]->get_length();
-      }
-    }
-  }
-
-  return 0.0;
-}
-
-TOpendriveLocalPose COpendriveLane::transform_to_local_pose(TOpendriveTrackPose &track) const
-{
-  TOpendriveTrackPose pose;
-
-  pose=track;
-  pose.t+=this->get_center_offset();
-  return this->segment->transform_to_local_pose(pose);
-}
-
-TOpendriveWorldPose COpendriveLane::transform_to_world_pose(TOpendriveTrackPose &track) const
-{ 
-  TOpendriveTrackPose pose;
-
-  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 &distance,double angle_tol) const
-{
-  double dist,min_dist=std::numeric_limits<double>::max(),length;
-  TOpendriveWorldPose found_pose;
-  unsigned int closest_index=-1;
-
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  {
-    length=this->nodes[i]->get_closest_pose(pose,found_pose,true,angle_tol);
-    if(length<std::numeric_limits<double>::max())
-    {
-      dist=sqrt(pow(found_pose.x-pose.x,2.0)+pow(found_pose.y-pose.y,2.0));
-      if(dist<min_dist)
-      {
-        min_dist=dist;
-        closest_index=i;
-        distance=length;
-      }
-    }
-  }
-
-  return closest_index;
-}
-
-const COpendriveRoadNode &COpendriveLane::get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
-{
-  unsigned int closest_index;
-
-  closest_index=this->get_closest_node_index(pose,distance,angle_tol);
-  if(closest_index==(unsigned int)-1)
-    throw CException(_HERE_,"Impossible to find the closest node");
-  return *this->nodes[closest_index];
-}
-
-double COpendriveLane::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
-{
-  double dist,min_dist=std::numeric_limits<double>::max(),distance,length;
-  TOpendriveWorldPose found_pose;
-  unsigned int closest_index=-1;
-  COpendriveLink *link;
-
-  closest_pose.x=std::numeric_limits<double>::max();
-  closest_pose.y=std::numeric_limits<double>::max();
-  closest_pose.heading=std::numeric_limits<double>::max();
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  {
-    length=this->nodes[i]->get_closest_pose(pose,found_pose,true,angle_tol);
-    if(length<std::numeric_limits<double>::max())
-    {
-      dist=sqrt(pow(found_pose.x-pose.x,2.0)+pow(found_pose.y-pose.y,2.0));
-      if(dist<min_dist)
-      {
-        min_dist=dist;
-        closest_index=i;
-        closest_pose=found_pose;
-        distance=length;
-      }
-    }
-  }
-
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  {
-    if(i<closest_index)
-    {
-      link=this->nodes[i]->get_link_with(this->nodes[i+1]);
-      if(link!=NULL)
-        distance+=link->get_length();
-    }
-    else
-      break;
-  }
-
-  return distance;
-}
-
-std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
-{
-  out << "  Lane: " << lane.id << " (offset: " << lane.offset << ")" << std::endl;
-  out << "    width: " << lane.get_width() << std::endl;
-  out << "    speed: " << lane.get_speed() << std::endl;
-  out << "    Previous lanes: " << std::endl;
-  for(unsigned int i=0;i<lane.prev.size();i++)
-    out << "      Lane " << lane.prev[i]->id << " in road segment " << lane.prev[i]->segment->get_name() << std::endl;
-  out << "    Next lanes: " << std::endl;
-  for(unsigned int i=0;i<lane.next.size();i++)
-    out << "      Lane " << lane.next[i]->id << " in road segment " << lane.next[i]->segment->get_name() << std::endl;
-  if(lane.segment!=NULL)
-    out << "    Parent road segment: " << lane.segment->get_name() << std::endl;
-  else 
-    out << "    No parent segment" << std::endl;
-  if(lane.left_lane==NULL)
-    out << "    No left lane (";
-  else
-    out << "    Left lane " << lane.left_lane->get_id() << " (";
-  switch(lane.left_mark)
-  {
-    case OD_MARK_NONE:
-      out << "no mark)" << std::endl;
-      break;
-    case OD_MARK_SOLID:
-      out << "solid)" << std::endl;
-      break;
-    case OD_MARK_BROKEN:
-      out << "broken)" << std::endl;
-      break;
-    case OD_MARK_SOLID_SOLID:
-      out << "solid solid)" << std::endl;
-      break;
-    case OD_MARK_SOLID_BROKEN:
-      out << "solid broken)" << std::endl;
-      break;
-    case OD_MARK_BROKEN_SOLID:
-      out << "broken solid)" << std::endl;
-      break;
-    case OD_MARK_BROKEN_BROKEN:
-      out << "broken broken)" << std::endl;
-      break;
-  }
-  if(lane.right_lane==NULL)
-    out << "    No right lane (";
-  else
-    out << "    Right lane " << lane.right_lane->get_id() << " (";
-  switch(lane.right_mark)
-  {
-    case OD_MARK_NONE:
-      out << "no mark)" << std::endl;
-      break;
-    case OD_MARK_SOLID:
-      out << "solid)" << std::endl;
-      break;
-    case OD_MARK_BROKEN:
-      out << "broken)" << std::endl;
-      break;
-    case OD_MARK_SOLID_SOLID:
-      out << "solid solid)" << std::endl;
-      break;
-    case OD_MARK_SOLID_BROKEN:
-      out << "solid broken)" << std::endl;
-      break;
-    case OD_MARK_BROKEN_SOLID:
-      out << "broken solid)" << std::endl;
-      break;
-    case OD_MARK_BROKEN_BROKEN:
-      out << "broken broken)" << std::endl;
-      break;
-  }
-  out << "    Number of nodes: " << lane.nodes.size() << std::endl;
-  for(unsigned int i=0;i<lane.nodes.size();i++)
-    out << *lane.nodes[i];
-
-  return out;
-}
-
-COpendriveLane::~COpendriveLane()
-{
-  this->nodes.clear();
-  this->prev.clear();
-  this->next.clear();
-  this->left_lane=NULL;
-  this->left_mark=OD_MARK_NONE;
-  this->right_lane=NULL;
-  this->right_mark=OD_MARK_NONE;
-  this->segment=NULL;
-  this->resolution=DEFAULT_RESOLUTION;
-  this->scale_factor=DEFAULT_SCALE_FACTOR;
-  this->width=0.0;
-  this->speed=0.0;
-  this->offset=0.0;
-  this->id=0;
-  this->index=-1;
-}
diff --git a/src/opendrive_link.cpp b/src/opendrive_link.cpp
deleted file mode 100644
index 4243d05e6817746bb4667beb526355bf1671c681..0000000000000000000000000000000000000000
--- a/src/opendrive_link.cpp
+++ /dev/null
@@ -1,359 +0,0 @@
-#include "opendrive_link.h"
-#include "opendrive_road_node.h"
-#include "exceptions.h"
-
-COpendriveLink::COpendriveLink()
-{
-  this->prev=NULL;
-  this->next=NULL;
-  this->segment=NULL;
-  this->lane=NULL;
-  this->spline=NULL;
-  this->mark=OD_MARK_NONE;
-  this->resolution=DEFAULT_RESOLUTION;
-  this->scale_factor=DEFAULT_SCALE_FACTOR;
-}
-
-COpendriveLink::COpendriveLink(const COpendriveLink &object)
-{
-  this->prev=object.prev;
-  this->next=object.next;
-  this->segment=object.segment;
-  this->lane=object.lane;
-  this->spline=new CG2Spline(*object.spline);
-  this->mark=object.mark;
-  this->resolution=object.resolution;
-  this->scale_factor=object.scale_factor;
-}
-
-void COpendriveLink::set_prev(COpendriveRoadNode *node)
-{
-  this->prev=node;
-}
-
-void COpendriveLink::set_next(COpendriveRoadNode *node)
-{
-  this->next=node;
-}
-
-void COpendriveLink::set_road_mark(opendrive_mark_t mark)
-{
-  this->mark=mark;
-}
-
-void COpendriveLink::set_parent_segment(COpendriveRoadSegment *segment)
-{
-  this->segment=segment;
-}
-
-void COpendriveLink::set_parent_lane(COpendriveLane *lane)
-{
-  this->lane=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)
-{
-  TOpendriveWorldPose node_start,node_end;
-  TPoint start,end;
-
-  node_start=this->prev->get_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_pose();
-  end.x=node_end.x;
-  end.y=node_end.y;
-  end.heading=node_end.heading;
-  end.curvature=end_curvature;
-  this->spline=new CG2Spline(start,end);
-  this->spline->generate(this->resolution);
-}
-
-void COpendriveLink::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
-{
-  if(node_refs.find(this->prev)!=node_refs.end())
-    this->prev=node_refs[this->prev];
-  if(node_refs.find(this->next)!=node_refs.end())
-    this->next=node_refs[this->next];
-  if(segment_refs.find(this->segment)!=segment_refs.end())
-    this->segment=segment_refs[this->segment];
-  if(lane_refs.find(this->lane)!=lane_refs.end())
-    this->lane=lane_refs[this->lane];
-}
-
-bool COpendriveLink::clean_references(node_up_ref_t &refs)
-{
-  if(refs.find(this->prev)!=refs.end())
-    this->prev=refs[this->prev];
-  else if(!this->prev->updated(refs))
-    return true;
-  if(refs.find(this->next)!=refs.end())
-    this->next=refs[this->next];
-  else if(!this->next->updated(refs))
-    return true;
-
-  return false;
-}
-
-void COpendriveLink::update_start_pose(TOpendriveWorldPose &start,double curvature)
-{
-  TPoint start_pose;
-
-  if(this->spline!=NULL)
-  {
-    start_pose.x=start.x;
-    start_pose.y=start.y;
-    start_pose.heading=start.heading;
-    start_pose.curvature=curvature;
-    this->spline->set_start_point(start_pose);
-    this->spline->generate(this->resolution);
-  }
-}
-
-void COpendriveLink::update_end_pose(TOpendriveWorldPose &end,double curvature)
-{
-  TPoint end_pose;
-
-  if(this->spline!=NULL)
-  {
-    end_pose.x=end.x;
-    end_pose.y=end.y;
-    end_pose.heading=end.heading;
-    end_pose.curvature=curvature;
-    this->spline->set_end_point(end_pose);
-    this->spline->generate(this->resolution);
-  }
-}
-
-const COpendriveRoadNode &COpendriveLink::get_prev(void) const
-{
-  if(this->prev!=NULL)
-    return *this->prev;
-
-  throw CException(_HERE_,"Invalid previous road node reference. Link is not properly defined");
-}
-
-const COpendriveRoadNode &COpendriveLink::get_next(void) const
-{
-  if(this->next!=NULL)
-    return *this->next;
-
-  throw CException(_HERE_,"Invalid next road node reference. Link is not properly defined");
-}
-
-opendrive_mark_t COpendriveLink::get_road_mark(void) const
-{
-  return this->mark;
-}
-
-const COpendriveRoadSegment &COpendriveLink::get_parent_segment(void) const
-{
-  if(this->segment!=NULL)
-    return *this->segment;
-
-  throw CException(_HERE_,"Invalid parent road segment reference. Link is not properky defined");
-}
-
-const COpendriveLane &COpendriveLink::get_parent_lane(void) const
-{
-  std::stringstream error;
-
-  if(this->lane!=NULL)
-    return *this->lane;
-  else
-  {
-    error << "Link from node " << this->prev->get_index() << " to node " << this->next->get_index() << " has no parent lane";
-    throw CException(_HERE_,error.str());
-  }
-}
-
-bool COpendriveLink::is_lane_link(void) const
-{
-  if(this->lane==NULL)
-    return false;
-  else
-    return true;
-}
-
-double COpendriveLink::get_resolution(void) const
-{
-  return this->resolution;
-}
-
-double COpendriveLink::get_scale_factor(void) const
-{
-  return this->scale_factor;
-}
-
-double COpendriveLink::find_closest_world_pose(TOpendriveWorldPose &world,TOpendriveWorldPose &pose) const
-{
-  TPoint spline_pose;
-  double length;
-
-  if(this->spline!=NULL)
-  {
-    length=this->spline->find_closest_point(world.x,world.y,spline_pose);
-    pose.x=spline_pose.x;
-    pose.y=spline_pose.y;
-    pose.heading=normalize_angle(spline_pose.heading);
-  }
-  else
-    length=std::numeric_limits<double>::max();
-
-  return length;
-}
-
-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;
-}
-
-TOpendriveWorldPose COpendriveLink::get_pose_at(double length)
-{
-  TPoint spline_pose;
-  TOpendriveWorldPose world_pose={0};
-
-  if(this->spline!=NULL)
-  {
-    spline_pose=this->spline->evaluate(length);
-    world_pose.x=spline_pose.x;
-    world_pose.y=spline_pose.y;
-    world_pose.heading=spline_pose.heading;
-  }
-
-  return world_pose;
-}
-
-double COpendriveLink::get_curvature_at(double length)
-{
-  TPoint spline_pose;
-  double curvature=0.0;
-
-  if(this->spline!=NULL)
-  {
-    spline_pose=this->spline->evaluate(length);
-    curvature=spline_pose.curvature;
-  }
-
-  return curvature;
-}
-
-std::ostream& operator<<(std::ostream& out, COpendriveLink &link)
-{
-  out << "        Previous node: " << link.get_prev().get_index() << std::endl;
-  out << "        Next node: " << link.get_next().get_index() << std::endl;
-  if(link.lane!=NULL)
-    out << "        Parent segment: " << link.get_parent_segment().get_name() << " (lane " << link.get_parent_lane().get_id() << ")" << std::endl;
-  else
-    out << "        Parent segment: " << link.get_parent_segment().get_name() << " (no lane)" << std::endl;
-  out << "        Road mark: ";
-  switch(link.get_road_mark())
-  {
-    case OD_MARK_NONE:
-      out << "no mark" << std::endl;
-      break;
-    case OD_MARK_SOLID:
-      out << "solid" << std::endl;
-      break;
-    case OD_MARK_BROKEN:
-      out << "broken" << std::endl;
-      break;
-    case OD_MARK_SOLID_SOLID:
-      out << "solid solid" << std::endl;
-      break;
-    case OD_MARK_SOLID_BROKEN:
-      out << "solid broken" << std::endl;
-      break;
-    case OD_MARK_BROKEN_SOLID:
-      out << "broken solid" << std::endl;
-      break;
-    case OD_MARK_BROKEN_BROKEN:
-      out << "broken broken" << std::endl;
-      break;
-  }
-
-  return out;
-}
-
-COpendriveLink::~COpendriveLink()
-{
-  this->prev=NULL;
-  this->next=NULL;
-  this->segment=NULL;
-  this->lane=NULL;
-  if(this->spline!=NULL)
-  {
-    delete this->spline;
-    this->spline=NULL;
-  }
-  this->mark=OD_MARK_NONE;
-  this->resolution=DEFAULT_RESOLUTION;
-  this->scale_factor=DEFAULT_SCALE_FACTOR;
-}
-
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
deleted file mode 100644
index bb740b7e84fbb7af6e122ac865ee184e15139723..0000000000000000000000000000000000000000
--- a/src/opendrive_road.cpp
+++ /dev/null
@@ -1,1150 +0,0 @@
-#include "opendrive_road.h"
-#include "exceptions.h"
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <math.h>
-
-COpendriveRoad::COpendriveRoad()
-{
-  this->resolution=DEFAULT_RESOLUTION;
-  this->scale_factor=DEFAULT_SCALE_FACTOR;
-  this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
-  this->segments.clear();
-  this->nodes.clear();
-  this->lanes.clear();
-}
-
-COpendriveRoad::COpendriveRoad(const COpendriveRoad& object)
-{
-  COpendriveRoadSegment *segment;
-  COpendriveRoadNode *node;
-  COpendriveLane *lane;
-  segment_up_ref_t new_segment_ref;
-  node_up_ref_t new_node_ref;
-  lane_up_ref_t new_lane_ref;
-
-  this->free();
-  this->resolution=object.resolution;
-  this->scale_factor=object.scale_factor;
-  this->min_road_length=object.min_road_length;
-  for(unsigned int i=0;i<object.lanes.size();i++)
-  {
-    lane=new COpendriveLane(*object.lanes[i]);
-    this->lanes.push_back(lane);
-    new_lane_ref[object.lanes[i]]=lane;
-  }
-  for(unsigned int i=0;i<object.nodes.size();i++)
-  {
-    node=new COpendriveRoadNode(*object.nodes[i]);
-    this->nodes.push_back(node);
-    new_node_ref[object.nodes[i]]=node;
-  }
-  for(unsigned int i=0;i<object.segments.size();i++)
-  {
-    segment=new COpendriveRoadSegment(*object.segments[i]);
-    this->segments.push_back(segment);
-    new_segment_ref[object.segments[i]]=segment;
-  }
-  // update references
-  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
-}
-
-void COpendriveRoad::free(void)
-{
-  for(unsigned int i=0;i<this->segments.size();i++)
-  {
-    delete this->segments[i];
-    this->segments[i]=NULL;
-  }
-  this->segments.clear();
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  {
-    delete this->nodes[i];
-    this->nodes[i]=NULL;
-  }
-  this->nodes.clear();
-  for(unsigned int i=0;i<this->lanes.size();i++)
-  {
-    delete this->lanes[i];
-    this->lanes[i]=NULL;
-  }
-  this->lanes.clear();
-}
-
-void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
-{
-  std::string predecessor_id,successor_id;
-  std::string predecessor_contact,successor_contact;
-  COpendriveRoadSegment *prev_road,*road,*next_road;
-  std::stringstream error;
-
-  for(OpenDRIVE::road_iterator road_it(open_drive.road().begin());road_it!=open_drive.road().end();++road_it)
-  {
-    // get current segment
-    road=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id((unsigned int)std::stoi(road_it->id().get())));
-    // get predecessor and successor
-    if(road_it->lane_link().present())
-    {
-      if(road_it->lane_link().get().predecessor().present())// predecessor present
-      {
-        if(road_it->lane_link().get().predecessor().get().elementType().get()=="road")// previous segment is a road
-        {
-          predecessor_id=road_it->lane_link().get().predecessor().get().elementId().get();
-          predecessor_contact=road_it->lane_link().get().predecessor().get().contactPoint().get();
-        }
-      }
-      if(road_it->lane_link().get().successor().present())// successor present
-      {
-        if(road_it->lane_link().get().successor().get().elementType().get()=="road")
-        {
-          successor_id=road_it->lane_link().get().successor().get().elementId().get();
-          successor_contact=road_it->lane_link().get().successor().get().contactPoint().get();
-        }
-      }
-    }
-    if(std::stoi(road_it->junction().get())==-1)// non junction road segments
-    {
-      if(!predecessor_id.empty())
-      {
-        prev_road=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id((unsigned int)std::stoi(predecessor_id)));
-        prev_road->link_segment(road);
-        predecessor_id.clear();
-      }
-      if(!successor_id.empty())
-      {
-        next_road=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id((unsigned int)std::stoi(successor_id)));
-        road->link_segment(next_road);
-        successor_id.clear();
-      }
-    }
-    else// junction segment
-    {
-      for(OpenDRIVE::junction_iterator junction_it(open_drive.junction().begin());junction_it!=open_drive.junction().end();++junction_it)
-      {
-        for(junction::connection_iterator connection_it(junction_it->connection().begin()); connection_it!=junction_it->connection().end();++connection_it)
-        {
-          std::string incoming_road_id;
-          std::string connecting_road_id;
-          if(connection_it->incomingRoad().present())
-            incoming_road_id=connection_it->incomingRoad().get();
-          else
-          {
-            error << "Connectivity information missing for segment " << road->get_name() << ": No incomming segment";
-            throw CException(_HERE_,error.str());
-          }
-          if(connection_it->connectingRoad().present())
-            connecting_road_id=connection_it->connectingRoad().get();
-          else
-          {
-            error << "Connectivity information missing for segment " << road->get_name() << ": No connecting segment";
-            throw CException(_HERE_,error.str());
-          }
-          if(predecessor_id.compare(incoming_road_id)==0 && successor_id.compare(connecting_road_id)==0)// this is the connection
-          {
-            prev_road=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id((unsigned int)std::stoi(predecessor_id)));
-            next_road=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id((unsigned int)std::stoi(successor_id)));
-            for(connection::laneLink_iterator lane_link_it(connection_it->laneLink().begin()); lane_link_it!=connection_it->laneLink().end();++lane_link_it)
-            {
-              int from_lane_id;
-              int to_lane_id;
-              if(lane_link_it->from().present())
-                from_lane_id=lane_link_it->from().get();
-              else
-              {
-                error << "Connectivity information missing for segment " << road->get_name() << ": lane link missing from identifier";
-                throw CException(_HERE_,error.str());
-              }
-              if(lane_link_it->to().present())
-                to_lane_id=lane_link_it->to().get();
-              else
-              {
-                error << "Connectivity information missing for segment " << road->get_name() << ": lane link missing to identifier";
-                throw CException(_HERE_,error.str());
-              }
-              if(predecessor_contact=="end")
-                prev_road->link_segment(road,from_lane_id,false,-1,true);
-              else 
-                prev_road->link_segment(road,from_lane_id,true,-1,true);
-              TOpendriveWorldPose end=road->get_lane(-1).get_end_pose();
-              TOpendriveWorldPose start;
-              if(successor_contact=="end")
-              {
-                if(to_lane_id<0)
-                  start=next_road->get_lane(to_lane_id).get_end_pose();
-                else
-                  start=next_road->get_lane(to_lane_id).get_start_pose();
-                if(fabs(end.x-start.x)<this->resolution && fabs(end.y-start.y)<this->resolution)
-                  road->link_segment(next_road,-1,false,to_lane_id,false);
-              }
-              else
-              {
-                if(to_lane_id<0)
-                  start=next_road->get_lane(to_lane_id).get_start_pose();
-                else
-                  start=next_road->get_lane(to_lane_id).get_end_pose();
-                if(fabs(end.x-start.x)<this->resolution && fabs(end.y-start.y)<this->resolution)
-                  road->link_segment(next_road,-1,false,to_lane_id,true);
-              }
-            }
-          }
-        }
-      }
-    }
-  }
-}
-
-unsigned int COpendriveRoad::add_node(COpendriveRoadNode *node)
-{
-  std::stringstream error;
-
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  {
-    if(this->nodes[i]==node)
-    {
-      error << "Node " << node->get_index() << " already present";
-      throw CException(_HERE_,error.str());
-    }
-  }
-  this->nodes.push_back(node);
-
-  return this->nodes.size()-1;
-}
-
-bool COpendriveRoad::remove_node(COpendriveRoadNode *node)
-{
-  std::vector<COpendriveRoadNode *>::iterator it;
-
-  for(it=this->nodes.begin();it!=this->nodes.end();)
-  {
-    if((*it)->get_index()==node->get_index())
-    {
-      delete *it;
-      it=this->nodes.erase(it);
-      return true;
-    }
-    else
-      it++;
-  }
-
-  return false;
-}
-
-unsigned int COpendriveRoad::add_lane(COpendriveLane *lane)
-{
-  std::stringstream error;
-
-  for(unsigned int i=0;i<this->lanes.size();i++)
-  {
-    if(this->lanes[i]==lane)
-    {
-      error << "Lane " << lane->get_index() << " already present";
-      throw CException(_HERE_,error.str());
-    }
-  }
-  this->lanes.push_back(lane);
-
-  return this->lanes.size()-1;
-}
-
-bool COpendriveRoad::remove_lane(COpendriveLane *lane)
-{
-  std::vector<COpendriveLane *>::iterator it;
-
-  for(it=this->lanes.begin();it!=this->lanes.end();)
-  {
-    if((*it)->get_index()==lane->get_index())
-    {
-      delete *it;
-      it=this->lanes.erase(it);
-      return true;
-    }
-    else
-      it++;
-  }
-
-  return false;
-}
-
-void COpendriveRoad::complete_open_lanes(void)
-{
-  std::vector<COpendriveLane *>::iterator lane_it;
-  TOpendriveWorldPose end_pose;
-
-  // remove all nodes and lanes not present in the path
-  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
-  {
-    if((*lane_it)->next.empty())// lane is not connected
-    {
-      try{
-        end_pose=(*lane_it)->get_end_pose();
-        if(!this->node_exists_at(end_pose))// there is no node at the end pose
-        {
-          (*lane_it)->segment->add_empty_node(end_pose,(*lane_it));
-          (*lane_it)->segment->link_neightbour_lanes();
-        }
-        else
-          lane_it++;
-      }catch(CException &e){
-        lane_it++;
-      }
-    }
-    else
-      lane_it++;
-  }   
-}
-
-bool COpendriveRoad::has_segment(COpendriveRoadSegment *segment)
-{
-  for(unsigned int i=0;i<this->segments.size();i++)
-    if(this->segments[i]==segment)
-      return true;
-  
-  return false;
-}
-
-void COpendriveRoad::add_segment(COpendriveRoadSegment *segment)
-{
-  for(unsigned int i=0;i<this->segments.size();i++)
-    if(this->segments[i]->get_id()==segment->get_id())// segment is already present
-      return;
-  // add the new segment
-  this->segments.push_back(segment);
-  // add the lanes
-  for(unsigned int i=1;i<=segment->get_num_right_lanes();i++)
-    this->lanes.push_back(segment->lanes[-i]);
-  for(unsigned int i=1;i<=segment->get_num_left_lanes();i++)
-    this->lanes.push_back(segment->lanes[i]);
-  // add the nodes
-  for(unsigned int i=0;i<segment->get_num_nodes();i++)
-    this->nodes.push_back(segment->nodes[i]);
-  // update the road reference
-  segment->set_parent_road(this);
-}
-
-std::vector<unsigned int> COpendriveRoad::update_path(node_up_ref_t &node_refs,std::vector<unsigned int> &path)
-{
-  std::vector<unsigned int> new_path;
-  node_up_ref_t::iterator node_it;
-
-  for(unsigned int i=0;i<path.size();i++)
-  {
-    for(node_it=node_refs.begin();node_it!=node_refs.end();node_it++)
-    {
-      if(node_it->first->get_index()==path[i])
-      {
-        new_path.push_back(node_it->second->get_index());
-        break;
-      }
-    }
-  }
-
-  return new_path;
-}
-
-void COpendriveRoad::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
-{
-  std::vector<COpendriveRoadSegment *>::iterator seg_it;
-  std::vector<COpendriveLane *>::iterator lane_it;
-  std::vector<COpendriveRoadNode *>::iterator node_it;
-
-  for(seg_it=this->segments.begin();seg_it!=this->segments.end();seg_it++)
-  {
-    if(segment_refs.find(*seg_it)!=segment_refs.end())
-    {
-      (*seg_it)=segment_refs[*seg_it];
-      (*seg_it)->update_references(segment_refs,lane_refs,node_refs);
-    }
-    else if((*seg_it)->updated(segment_refs))
-      (*seg_it)->update_references(segment_refs,lane_refs,node_refs);
-  }
-  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
-    if(lane_refs.find(*lane_it)!=lane_refs.end())
-      (*lane_it)=lane_refs[*lane_it];
-  for(node_it=this->nodes.begin();node_it!=this->nodes.end();node_it++)
-    if(node_refs.find(*node_it)!=node_refs.end())
-      (*node_it)=node_refs[*node_it];
-}
-
-void COpendriveRoad::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
-{
-  std::vector<COpendriveRoadSegment *>::iterator seg_it;
-  std::vector<COpendriveLane *>::iterator lane_it;
-  std::vector<COpendriveRoadNode *>::iterator node_it;
-
-  for(seg_it=this->segments.begin();seg_it!=this->segments.end();)
-  {
-    if((*seg_it)->updated(segment_refs))
-    {
-      (*seg_it)->clean_references(segment_refs,lane_refs,node_refs);
-      seg_it++;
-    }
-    else
-      seg_it=this->segments.erase(seg_it);
-  }
-  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
-  {
-    if(!(*lane_it)->updated(lane_refs))
-      lane_it=this->lanes.erase(lane_it);
-    else
-      lane_it++;
-  }
-  for(node_it=this->nodes.begin();node_it!=this->nodes.end();)
-  {
-    if(!(*node_it)->updated(node_refs))
-      node_it=this->nodes.erase(node_it);
-    else
-      node_it++;
-  }
-}
-
-void COpendriveRoad::reindex(void)
-{
-  // reindex lanes
-  for(unsigned int i=0;i<this->get_num_lanes();i++)
-    this->lanes[i]->set_index(i);
-  // reindex nodes
-  for(unsigned int i=0;i<this->get_num_nodes();i++)
-    this->nodes[i]->set_index(i);
-}
-
-void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
-{
-  COpendriveLane *neightbour_lane;
-  std::vector<COpendriveLane *>::iterator lane_it;
-  bool present;
-
-  // remove all nodes and lanes not present in the path
-  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
-  {
-    present=false;
-    for(unsigned int i=0;i<path_nodes.size();i++)
-    {
-      if((*lane_it)->has_node_with_index(path_nodes[i]))
-      {
-        present=true;
-        break;
-      }
-    } 
-    if(!present)
-    {
-      neightbour_lane=(*lane_it)->left_lane;
-      while(neightbour_lane!=NULL)
-      {
-        for(unsigned int i=0;i<path_nodes.size();i++)
-        {
-          if(neightbour_lane->has_node_with_index(path_nodes[i]))
-          {
-            present=true;
-            break;
-          }
-        }
-        if(present)
-          break;
-        neightbour_lane=neightbour_lane->left_lane;
-      }
-      neightbour_lane=(*lane_it)->right_lane;
-      while(neightbour_lane!=NULL)
-      {
-        for(unsigned int i=0;i<path_nodes.size();i++)
-        {
-          if(neightbour_lane->has_node_with_index(path_nodes[i]))
-          {
-            present=true;
-            break;
-          }
-        }
-        if(present)
-          break;
-        neightbour_lane=neightbour_lane->right_lane;
-      }
-      if(!present)
-      {
-        (*lane_it)->segment->remove_lane(*lane_it);
-        for(unsigned int i=0;i<(*lane_it)->nodes.size();i++)
-          this->remove_node((*lane_it)->nodes[i]);
-        lane_it=this->lanes.erase(lane_it);
-      }
-      else
-        lane_it++;
-    }
-    else
-      lane_it++;
-  }
-  // remove links to non-consecutive nodes
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  {
-    for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
-    {
-      if(!this->has_segment(this->nodes[i]->links[j]->segment))
-        this->nodes[i]->remove_link(this->nodes[i]->links[j]);
-    }
-  } 
-}
-
-bool COpendriveRoad::node_exists_at(TOpendriveWorldPose &pose)
-{
-  TOpendriveWorldPose node_pose;
-
-  for(unsigned int i=0;i<nodes.size();i++)
-  {
-    node_pose=this->nodes[i]->get_pose();
-    if(std::abs(pose.x-node_pose.x)<0.01 && std::abs(pose.y-node_pose.y)<0.01)
-      return true;
-  }
-
-  return false;
-}
-
-COpendriveRoadNode* COpendriveRoad::get_node_at(TOpendriveWorldPose &pose)
-{
-  TOpendriveWorldPose node_pose;
-
-  for(unsigned int i=0;i<nodes.size();i++)
-  {
-    node_pose=this->nodes[i]->get_pose();
-    if(std::abs(pose.x-node_pose.x)<0.01 && std::abs(pose.y-node_pose.y)<0.01)
-      return this->nodes[i];
-  }
-
-  return NULL;
-}
-
-void COpendriveRoad::load(const std::string &filename)
-{
-  struct stat buffer;
-
-  if(stat(filename.c_str(),&buffer)==0)
-  {
-    // delete any previous data
-    this->free();
-    // try to open the specified file
-    try{
-      std::unique_ptr<OpenDRIVE> open_drive(OpenDRIVE_(filename.c_str(), xml_schema::flags::dont_validate));
-      for (OpenDRIVE::road_iterator road_it(open_drive->road().begin()); road_it != open_drive->road().end(); ++road_it)
-      {
-        try{
-          COpendriveRoadSegment *segment=new COpendriveRoadSegment();
-          segment->set_resolution(this->resolution);
-          segment->set_scale_factor(this->scale_factor);
-          segment->set_min_road_length(this->min_road_length);
-          segment->set_parent_road(this);
-          segment->load(*road_it);
-          this->segments.push_back(segment);
-        }catch(CException &e){
-          std::cout << e.what() << std::endl;
-        }
-      }
-      // link segments
-      this->link_segments(*open_drive);
-    }catch (const xml_schema::exception& e){
-      std::ostringstream os;
-      os << e;
-      /* handle exceptions */
-      throw CException(_HERE_,os.str());
-    }
-  }
-  else
-    throw CException(_HERE_,"The .xodr file does not exist");
-}
-
-void COpendriveRoad::set_resolution(double res)
-{
-  this->resolution=res;
-
-  // change the resolution of all current nodes
-  for(unsigned int i=0;i<this->segments.size();i++)
-    this->segments[i]->set_resolution(res);
-}
-
-double COpendriveRoad::get_resolution(void) const
-{
-  return this->resolution;
-}
-
-void COpendriveRoad::set_scale_factor(double scale)
-{
-  this->scale_factor=scale;
-
-  // change the resolution of all current nodes
-  for(unsigned int i=0;i<this->segments.size();i++)
-    this->segments[i]->set_scale_factor(scale);
-}
-
-double COpendriveRoad::get_scale_factor(void) const
-{
-  return this->scale_factor;
-}
-
-void COpendriveRoad::set_min_road_length(double length)
-{
-  this->min_road_length=length;
-
-  for(unsigned int i=0;i<this->segments.size();i++)
-    this->segments[i]->set_min_road_length(length);
-}
-
-double COpendriveRoad::get_min_road_length(void) const
-{
-  return this->min_road_length;
-}
-
-unsigned int COpendriveRoad::get_num_segments(void) const
-{
-  return this->segments.size();
-}
-
-const COpendriveRoadSegment& COpendriveRoad::get_segment(unsigned int index) const
-{
-  std::stringstream error;
-
-  if(index>=0 && index<this->segments.size())
-    return *this->segments[index];
-  else
-  {
-    error << "Invalid segment index " << index;
-    throw CException(_HERE_,error.str());
-  }
-}
-
-const COpendriveRoadSegment &COpendriveRoad::get_segment_by_id(unsigned int id) const
-{
-  std::stringstream error;
-
-  for(unsigned int i=0;i<this->segments.size();i++)
-  {
-    if(this->segments[i]->get_id()==id)
-      return *this->segments[i];
-  }
-
-  error << "No road segment with the " << id << " ID";
-  throw CException(_HERE_,error.str());
-}
-
-const COpendriveRoadSegment &COpendriveRoad::get_segment_by_name(std::string &name) const
-{
-  std::stringstream error;
-
-  for(unsigned int i=0;i<this->segments.size();i++)
-  {
-    if(this->segments[i]->get_name()==name)
-      return *this->segments[i];
-  }
-
-  error << "No road segment with the " << name << " name";
-  throw CException(_HERE_,error.str());
-}
-
-unsigned int COpendriveRoad::get_num_lanes(void) const
-{
-  return this->lanes.size();
-}
-
-const COpendriveLane &COpendriveRoad::get_lane(unsigned int index) const
-{
-  std::stringstream error;
-
-  if(index>=0 && index<this->lanes.size())
-    return *this->lanes[index];
-  else
-  {
-    error << "Invalid lane index " << index;
-    throw CException(_HERE_,error.str());
-  }
-}
-
-unsigned int COpendriveRoad::get_num_nodes(void) const
-{
-  return this->nodes.size();
-}
-
-const COpendriveRoadNode& COpendriveRoad::get_node(unsigned int index) const
-{
-  std::stringstream error;
-
-  if(index>=0 && index<this->nodes.size())
-    return *this->nodes[index];
-  else
-  {
-    error << "Invalid node index " << index;
-    throw CException(_HERE_,error.str());
-  }
-}
-
-const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
-{
-  std::stringstream error;
-
-  if(index>=0 && index<this->segments.size())
-    return *this->segments[index];
-  else
-  {
-    error << "Invalid node index " << index;
-    throw CException(_HERE_,error.str());
-  }
-}
-
-unsigned int COpendriveRoad::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
-{
-  double dist,min_dist=std::numeric_limits<double>::max(),length;
-  TOpendriveWorldPose closest_pose;
-  unsigned int closest_index=-1;
-
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  {
-    length=this->nodes[i]->get_closest_pose(pose,closest_pose,false,angle_tol);
-    if(length<std::numeric_limits<double>::max())
-    {
-      dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
-      if(dist<min_dist)
-      {
-        min_dist=dist;
-        closest_index=i;
-        distance=length;
-      }
-    }
-  }
-
-  return closest_index;
-}
-
-const COpendriveRoadNode &COpendriveRoad::get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
-{
-  unsigned int closest_index;
-
-  closest_index=this->get_closest_node_index(pose,distance,angle_tol);
-  if(closest_index==(unsigned int)-1)
-    throw CException(_HERE_,"Impossible to find the closest node");
-  return *this->nodes[closest_index];
-}
-
-unsigned int COpendriveRoad::get_closest_lane_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
-{
-  double dist,min_dist=std::numeric_limits<double>::max(),length;
-  TOpendriveWorldPose closest_pose;
-  unsigned int closest_index=-1;
-
-  for(unsigned int i=0;i<this->lanes.size();i++)
-  { 
-    length=this->lanes[i]->get_closest_pose(pose,closest_pose,angle_tol);
-    if(length<std::numeric_limits<double>::max())
-    {
-      dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
-      if(dist<min_dist)
-      { 
-        min_dist=dist;
-        closest_index=i;
-        distance=length;
-      }
-    }
-  }
-
-  return closest_index;
-}
-
-const COpendriveLane &COpendriveRoad::get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
-{
-  unsigned int closest_index;
-
-  closest_index=this->get_closest_lane_index(pose,distance,angle_tol);
-  if(closest_index==(unsigned int)-1)
-    throw CException(_HERE_,"Impossible to find the closest lane");
-  return *this->lanes[closest_index];
-}
-
-unsigned int COpendriveRoad::get_closest_segment_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
-{
-  double dist,min_dist=std::numeric_limits<double>::max(),length;
-  TOpendriveWorldPose closest_pose;
-  unsigned int closest_index=-1;
-
-  for(unsigned int i=0;i<this->segments.size();i++)
-  { 
-    length=this->segments[i]->get_closest_pose(pose,closest_pose,angle_tol);
-    if(length<std::numeric_limits<double>::max())
-    {
-      dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
-      if(dist<min_dist)
-      { 
-        min_dist=dist;
-        closest_index=i;
-        distance=length;
-      }
-    }
-  }
-
-  return closest_index;
-}
-
-const COpendriveRoadSegment &COpendriveRoad::get_closest_segment(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
-{
-  unsigned int closest_index;
-
-  closest_index=this->get_closest_segment_index(pose,distance,angle_tol);
-  if(closest_index==(unsigned int)-1)
-    throw CException(_HERE_,"Impossible to find the closest segment");
-  return *this->segments[closest_index];
-}
-
-double COpendriveRoad::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
-{
-  double dist,min_dist=std::numeric_limits<double>::max();
-  TOpendriveWorldPose pose_found;
-  double length,closest_length;
-
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  {
-    length=this->nodes[i]->get_closest_pose(pose,pose_found,false,angle_tol);
-    if(length<std::numeric_limits<double>::max())
-    {
-      dist=sqrt(pow(pose_found.x-pose.x,2.0)+pow(pose_found.y-pose.y,2.0));
-      if(dist<min_dist)
-      {
-        min_dist=dist;
-        closest_pose=pose_found;
-        closest_length=length;
-      }
-    }
-  }
-
-  return closest_length;
-}
-
-void COpendriveRoad::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &distances,double dist_tol,double angle_tol) const
-{
-  std::vector<TOpendriveWorldPose> poses;
-  std::vector<double> lengths;
-  bool already_added;
-
-  closest_poses.clear();
-  distances.clear();
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  {
-    this->nodes[i]->get_closest_poses(pose,poses,lengths,dist_tol,false,angle_tol);
-    for(unsigned int j=0;j<poses.size();j++)
-    {
-      already_added=false;
-      for(unsigned int k=0;k<closest_poses.size();k++)
-        if(fabs(closest_poses[k].x-poses[j].x)<this->resolution &&
-           fabs(closest_poses[k].y-poses[j].y)<this->resolution)
-        {
-          already_added=true;
-          break;
-        }
-      if(!already_added)
-      {
-        closest_poses.push_back(poses[j]);
-        distances.push_back(lengths[j]);
-      }
-    }
-  }
-}
-
-std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPose &end_pose,COpendriveRoad &new_road)
-{
-  segment_up_ref_t new_segment_ref;
-  lane_up_ref_t new_lane_ref;
-  node_up_ref_t new_node_ref;
-  link_up_ref_t new_link_ref;
-  COpendriveRoadNode *node,*next_node;
-  COpendriveRoadSegment *segment,*new_segment;
-//  CopendriveRoadSegment *original_seg1,*original_seg2;
-  COpendriveLink *link;
-  std::vector<unsigned int> new_path_nodes;
-  unsigned int link_index;
-  double distance;
-
-  new_path_nodes.resize(path_nodes.size());
-
-  new_road.free();
-  new_road.set_resolution(this->resolution);
-  new_road.set_scale_factor(this->scale_factor);
-  new_road.set_min_road_length(this->min_road_length);
-
-  for(unsigned int i=0;i<path_nodes.size()-1;i++)
-  {
-    node=this->nodes[path_nodes[i]];
-    next_node=this->nodes[path_nodes[i+1]];
-    link=node->get_link_with(next_node);
-    if(link==NULL)
-      throw CException(_HERE_,"The provided path has unconnected nodes");
-    segment=link->segment;
-    if(new_segment_ref.find(segment)==new_segment_ref.end())
-    {
-      new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref);
-      new_road.add_segment(new_segment);
-      new_segment_ref[segment]=new_segment;
-    }
-  }
-  // add the last segment
-  node=this->nodes[path_nodes[path_nodes.size()-1]];
-  link_index=node->get_closest_link_index(end_pose,distance);
-  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())
-  {
-    new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref);
-    new_road.add_segment(new_segment);
-    new_segment_ref[segment]=new_segment;
-  }
-
-  // add additional nodes not explicitly in the path
-/*
-  for(unsigned int i=0;i<this->segments.size();i++)
-  {
-    for(unsigned int j=0;j<new_road.segments.size();j++)
-    {
-      original_seg1=new_road.segments[j]->get_original_segment(new_segment_ref);
-      for(unsigned int k=j+1;k<new_road.segments.size();k++)
-      {
-        original_seg2=new_road.segments[k]->get_original_segment(new_segment_ref);
-        if(this->segments[i]->connects_segments(original_seg1,original_seg2)
-        {
-          if(!new_road.has_segment(new_segment_ref[this->segments[i]]))
-          {
-            new_segment=this->segment[k]->clone(new_node_ref,new_lane_ref,new_link_ref);
-            new_road.add_segment(new_segment);
-            new_segment_ref[segment]=new_segment;
-          }
-        }
-      }
-    }
-  }
-*/
-  new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);
-  new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref);
-  // remove unconnected elements
-  new_road.prune(path_nodes);
-  new_road.reindex();
-  new_road.complete_open_lanes();
-
-  return new_road.update_path(new_node_ref,path_nodes);
-}
-
-void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double &length,double margin,COpendriveRoad &new_road)
-{
-  segment_up_ref_t new_segment_ref;
-  lane_up_ref_t new_lane_ref;
-  node_up_ref_t new_node_ref;
-  link_up_ref_t new_link_ref;
-  std::vector<const COpendriveRoadSegment *> cand_segments;
-  std::vector<int> cand_sides;
-  unsigned int segment_index;
-  TOpendriveWorldPose end_pose,int_pose;
-  const COpendriveRoadSegment *segment;
-  COpendriveRoadSegment *new_segment;
-  double rem_length=length+2.0*margin,distance,start_length;
-  int node_side;
-
-  new_road.free();
-  new_road.set_resolution(this->resolution);
-  new_road.set_scale_factor(this->scale_factor);
-  new_road.set_min_road_length(this->min_road_length);
-  // get the first segment
-  segment_index=this->get_closest_segment_index(start_pose,distance);
-  if(segment_index==(unsigned int)-1)
-    throw CException(_HERE_,"Start position does not belong to the road");
-  segment=this->segments[segment_index];
-  node_side=segment->get_pose_side(start_pose);
-  if(margin>0.0)
-  {
-    if(node_side<0)
-    {
-      rem_length=distance-margin;
-      if(rem_length<0.0)// get the previous segment
-      {
-        cand_segments=segment->get_prev_segments(node_side,cand_sides);
-        if(cand_segments.size()>1)
-          throw CException(_HERE_,"Road has multiple path, impossible to generate new road");
-        if(cand_segments.size()==0)
-        {
-          int_pose=segment->get_pose_at(0.0);
-          distance=0.0;
-        }
-        else
-        {
-          node_side=cand_sides[0];
-          segment=cand_segments[0];
-          if(node_side<0)
-          {
-            int_pose=segment->get_pose_at(segment->get_length()+rem_length);
-            distance=segment->get_length()+rem_length;
-          }
-          else
-          {
-            int_pose=segment->get_pose_at(-rem_length);
-            distance=-rem_length;
-          }
-        }
-      }
-      else
-      {
-        int_pose=segment->get_pose_at(rem_length);
-        distance-=margin;
-      }
-    }
-    else
-    {
-      rem_length=distance+margin-segment->get_length();
-      if(rem_length>0.0)// get the prev segment
-      {
-        cand_segments=segment->get_prev_segments(node_side,cand_sides);
-        if(cand_segments.size()>1)
-          throw CException(_HERE_,"Road has multiple path, impossible to generate new road");
-        if(cand_segments.size()==0)
-        {
-          int_pose=segment->get_pose_at(segment->get_length());
-          distance=0.0;
-        }
-        else 
-        {
-          node_side=cand_sides[0];
-          segment=cand_segments[0];
-          if(node_side<0)
-          {
-            int_pose=segment->get_pose_at(segment->get_length()-rem_length);
-            distance=segment->get_length()-rem_length;
-          }
-          else
-          {
-            int_pose=segment->get_pose_at(rem_length);
-            distance=rem_length;
-          }
-        }
-      }
-      else
-      {
-        int_pose=segment->get_pose_at(distance+margin);
-        distance+=margin;
-      }
-    }
-  }
-  else
-    int_pose=start_pose;
-  rem_length=length+2.0*margin;
-  new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,&int_pose,NULL);
-  if(new_segment->get_length()<this->min_road_length)
-  {
-    delete new_segment;
-    cand_segments=segment->get_next_segments(node_side,cand_sides);
-    if(cand_segments.size()>1)
-      throw CException(_HERE_,"Road has multiple path, impossible to generate new road");
-    if(cand_segments.size()!=0)
-    {
-      node_side=cand_sides[0];
-      segment=cand_segments[0];
-      new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref);
-      distance=0.0;
-    }
-    else
-      return;
-  }
-  start_length=new_segment->get_length()-distance;
-  if(rem_length<start_length)
-  {
-    if(node_side<0)
-      end_pose=new_segment->get_pose_at(rem_length);
-    else
-      end_pose=new_segment->get_pose_at(new_segment->get_length()-rem_length);
-    delete new_segment;
-    new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,&start_pose,&end_pose);
-  }
-  rem_length-=new_segment->get_length();
-  new_road.add_segment(new_segment);
-  new_segment_ref[(COpendriveRoadSegment *)segment]=new_segment;
-  while(rem_length>this->resolution)
-  {
-    cand_segments=segment->get_next_segments(node_side,cand_sides);
-    if(cand_segments.size()>1)
-      throw CException(_HERE_,"Road has multiple path, impossible to generate new road");
-    if(cand_segments.size()==0)
-      break;
-    else
-    {
-      node_side=cand_sides[0];
-      segment=cand_segments[0];
-      if((rem_length-segment->get_length())<0.0)
-      {
-        if(node_side<0)
-          end_pose=segment->get_pose_at(rem_length);
-        else
-          end_pose=segment->get_pose_at(segment->get_length()-rem_length);
-        new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,NULL,&end_pose);
-      }
-      else
-        new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref);
-      if(new_segment->get_length()>this->min_road_length)
-      {
-        rem_length-=new_segment->get_length();
-        new_road.add_segment(new_segment);
-        new_segment_ref[(COpendriveRoadSegment *)segment]=new_segment;
-      }
-      else
-        break;
-    }
-  }
-  length-=rem_length;
-  new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);
-  new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref);
-  new_road.reindex();
-  new_road.complete_open_lanes();
-}
-
-void COpendriveRoad::operator=(const COpendriveRoad& object)
-{
-  COpendriveRoadSegment *segment;
-  COpendriveRoadNode *node;
-  COpendriveLane *lane;
-  segment_up_ref_t new_segment_ref;
-  node_up_ref_t new_node_ref;
-  lane_up_ref_t new_lane_ref;
-
-  this->free();
-  this->resolution=object.resolution;
-  this->scale_factor=object.scale_factor;
-  this->min_road_length=object.min_road_length;
-  for(unsigned int i=0;i<object.lanes.size();i++)
-  {
-    lane=new COpendriveLane(*object.lanes[i]);
-    this->lanes.push_back(lane);
-    new_lane_ref[object.lanes[i]]=lane;
-  }
-
-  for(unsigned int i=0;i<object.nodes.size();i++)
-  {
-    node=new COpendriveRoadNode(*object.nodes[i]);
-    this->nodes.push_back(node);
-    new_node_ref[object.nodes[i]]=node;
-  }
-
-  for(unsigned int i=0;i<object.segments.size();i++)
-  {
-    segment=new COpendriveRoadSegment(*object.segments[i]);
-    this->segments.push_back(segment);
-    new_segment_ref[object.segments[i]]=segment;
-  }
-  // update references
-  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
-}
-
-std::ostream& operator<<(std::ostream& out, COpendriveRoad &road)
-{
-  out << "Resolution: " << road.resolution << std::endl;
-  out << "Scale factor: " << road.scale_factor << std::endl;
-  out << "Minimum road lenght: " << road.min_road_length << std::endl;
-  out << "Total number of nodes: " << road.nodes.size() << std::endl;
-  for(unsigned int i=0;i<road.segments.size();i++)
-    std::cout << *road.segments[i];
-
-  return out;
-}
-
-COpendriveRoad::~COpendriveRoad()
-{
-  this->free();
-  this->resolution=DEFAULT_RESOLUTION;
-  this->scale_factor=DEFAULT_SCALE_FACTOR;
-  this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
-}
-
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
deleted file mode 100644
index 6551616292dd51f096af8d6e7f13368c5a6cf105..0000000000000000000000000000000000000000
--- a/src/opendrive_road_node.cpp
+++ /dev/null
@@ -1,614 +0,0 @@
-#include "opendrive_road_node.h"
-#include "opendrive_road.h"
-#include "exceptions.h"
-#include <math.h>
-
-COpendriveRoadNode::COpendriveRoadNode()
-{
-  this->resolution=DEFAULT_RESOLUTION;
-  this->scale_factor=DEFAULT_SCALE_FACTOR;
-  this->pose.x=0.0;
-  this->pose.y=0.0;
-  this->pose.heading=0.0;
-  this->parents.clear();
-  this->links.clear();
-  this->index=-1;
-}
-
-COpendriveRoadNode *COpendriveRoadNode::clone(link_up_ref_t &new_link_ref)
-{
-  COpendriveRoadNode *new_node;
-
-  new_node=new COpendriveRoadNode();
-  new_node->resolution=this->resolution;
-  new_node->scale_factor=this->scale_factor;
-  new_node->pose=this->pose;
-  new_node->parents=this->parents;
-  new_node->links.resize(this->links.size());
-  for(unsigned int i=0;i<this->links.size();i++)
-  {
-    if(new_link_ref.find(this->links[i])!=new_link_ref.end())
-      new_node->links[i]=new_link_ref[this->links[i]];
-    else
-    {
-      new_node->links[i]=new COpendriveLink(*this->links[i]);
-      new_link_ref[this->links[i]]=new_node->links[i];
-    }
-  }
-  new_node->index=this->index;
-
-  return new_node;
-}
-
-bool COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane)
-{
-  TOpendriveRoadNodeParent *parent;
-  TOpendriveWorldPose node_pose;
-  double start_curvature,end_curvature;
-  COpendriveLink *new_link;
-
-  if(this->has_link_with(*node) || node->has_link_with(*this))
-    return false;
-  new_link=new COpendriveLink();
-  new_link->set_prev(this);
-  new_link->set_next(node);
-  new_link->set_resolution(this->resolution);
-  new_link->set_scale_factor(this->scale_factor);
-  new_link->set_road_mark(mark);
-  new_link->set_parent_segment(segment);
-  new_link->set_parent_lane(lane);
-  // get the curvature
-  node_pose=node->get_pose();
-  parent=this->get_parent_with_lane(lane);
-  if(parent!=NULL)
-  {
-    start_curvature=parent->start_curvature;
-    end_curvature=parent->end_curvature;
-  }
-  else
-  {
-    if(this->get_num_parents()==1)
-    {
-      start_curvature=this->parents[0].start_curvature;
-      end_curvature=this->parents[0].end_curvature;
-    }
-    else
-    {
-      start_curvature=0.0;
-      end_curvature=0.0;
-    }
-  }
-  new_link->generate(start_curvature,end_curvature);
-  this->add_link(new_link);
-  node->add_link(new_link);
-
-  return true;
-}
-
-void COpendriveRoadNode::add_link(COpendriveLink *link)
-{
-  if(!this->has_link(link))
-    this->links.push_back(link);
-}
-
-void COpendriveRoadNode::remove_link(COpendriveLink *link)
-{
-  std::vector<COpendriveLink *>::iterator it;
-
-  for(it=this->links.begin();it!=this->links.end();)
-  {
-    if((*it)==link)
-    {
-      if((*it)->prev==this)
-        delete (*it);
-      it=this->links.erase(it);
-      break;
-    }
-    else
-      it++;
-  }
-}
-
-bool COpendriveRoadNode::has_link(COpendriveLink *link)
-{
-  for(unsigned int i=0;i<this->links.size();i++)
-    if(this->links[i]==link)
-      return true;
-
-  return false;
-}
-
-bool COpendriveRoadNode::has_link_with(const COpendriveRoadNode &node) const
-{
-  for(unsigned int i=0;i<this->links.size();i++)
-    if(this->links[i]->prev==this && this->links[i]->next==&node)
-      return true;
-
-  return false;
-}
-
-bool COpendriveRoadNode::has_link_with(unsigned int node_index) const
-{
-  for(unsigned int i=0;i<this->links.size();i++)
-    if(this->links[i]->prev==this && this->links[i]->next->get_index()==node_index)
-      return true;
-
-  return false;
-}
-
-COpendriveLink *COpendriveRoadNode::get_link_with(COpendriveRoadNode *node)
-{
-  for(unsigned int i=0;i<this->links.size();i++)
-  {
-    if(this->links[i]->prev==node || this->links[i]->next==node)
-      return this->links[i];
-  }
-
-  return NULL;
-}
-
-void COpendriveRoadNode::set_resolution(double res)
-{
-  this->resolution=res;
-
-  for(unsigned int i=0;i<this->links.size();i++)
-    this->links[i]->set_resolution(res);
-}
-
-void COpendriveRoadNode::set_scale_factor(double scale)
-{
-  this->scale_factor=scale;
-
-  for(unsigned int i=0;i<this->links.size();i++)
-    this->links[i]->set_scale_factor(scale);
-
-  for(unsigned int i=0;i<this->parents.size();i++)
-  {
-    this->parents[i].start_curvature*=scale/this->scale_factor;
-    this->parents[i].end_curvature*=scale/this->scale_factor;
-  }
-}
-
-void COpendriveRoadNode::set_index(unsigned int index)
-{
-  this->index=index;
-}
-
-void COpendriveRoadNode::set_pose(TOpendriveWorldPose &start)
-{
-  this->pose=start;
-}
-
-void COpendriveRoadNode::add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double start_curvature,double end_curvature)
-{
-  TOpendriveRoadNodeParent new_parent;
-
-  for(unsigned int i=0;i<this->parents.size();i++)
-    if(this->parents[i].lane==lane && this->parents[i].segment==segment)
-      return;
-  new_parent.lane=lane;
-  new_parent.segment=segment;
-  new_parent.start_curvature=start_curvature;
-  new_parent.end_curvature=end_curvature;
-  this->parents.push_back(new_parent);
-}
-
-TOpendriveRoadNodeParent *COpendriveRoadNode::get_parent_with_lane(const COpendriveLane *lane)
-{
-  for(unsigned int i=0;i<this->parents.size();i++)
-    if(this->parents[i].lane==lane)
-      return &this->parents[i];
-
-  return NULL;
-}
-
-TOpendriveRoadNodeParent *COpendriveRoadNode::get_parent_with_segment(const COpendriveRoadSegment *segment)
-{
-  for(unsigned int i=0;i<this->parents.size();i++)
-    if(this->parents[i].segment==segment)
-      return &this->parents[i];
-
-  return NULL;
-}
-
-
-void COpendriveRoadNode::free(void)
-{
-  this->parents.clear();
-  for(unsigned int i=0;i<this->links.size();i++)
-  {
-    if(this->links[i]->prev==this)
-    {
-      delete this->links[i];
-      this->links[i]=NULL;
-    }
-  }
-  this->links.clear();
-}
-
-bool COpendriveRoadNode::updated(node_up_ref_t &refs)
-{
-  node_up_ref_t::iterator updated_it;
-
-  for(updated_it=refs.begin();updated_it!=refs.end();updated_it++)
-    if(updated_it->second==this)
-      return true;
-
-  return false;
-}
-
-COpendriveRoadNode *COpendriveRoadNode::get_original_node(node_up_ref_t &node_refs)
-{
-  node_up_ref_t::iterator updated_it;
-
-  for(updated_it=node_refs.begin();updated_it!=node_refs.end();updated_it++)
-    if(updated_it->second==this)
-      return updated_it->first;
-
-  return NULL;
-}
-
-void COpendriveRoadNode::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
-{
-  std::vector<COpendriveLink *>::iterator link_it;
-
-  if(this->updated(node_refs))
-  {
-    for(link_it=this->links.begin();link_it!=this->links.end();link_it++)
-      (*link_it)->update_references(segment_refs,lane_refs,node_refs);
-    for(unsigned int i=0;i<this->parents.size();i++)
-    {
-      if(segment_refs.find(this->parents[i].segment)!=segment_refs.end())
-        this->parents[i].segment=segment_refs[this->parents[i].segment];
-      if(lane_refs.find(this->parents[i].lane)!=lane_refs.end())
-        this->parents[i].lane=lane_refs[this->parents[i].lane];
-    }
-  }
-}
-
-void COpendriveRoadNode::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
-{
-  std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
-  std::vector<COpendriveLink *>::iterator link_it;
-
-  if(this->updated(node_refs))
-  {
-    for(link_it=this->links.begin();link_it!=this->links.end();)
-    {
-      if((*link_it)->clean_references(node_refs))
-        link_it=this->links.erase(link_it);
-      else
-        link_it++;
-    }
-    for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
-    {
-      if(!parent_it->segment->updated(segment_refs) || !parent_it->lane->updated(lane_refs))
-        parent_it=this->parents.erase(parent_it);
-      else
-        parent_it++;
-    }
-  }
-}
-
-void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldPose &start,double distance)
-{
-  std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
-  std::vector<COpendriveLink *>::iterator link_it;
-  TOpendriveRoadNodeParent *parent;
-
-  this->pose=start;
-  // remove the references to all lanes and segments except for lane
-  for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
-  {
-    if(parent_it->lane!=lane)
-      parent_it=this->parents.erase(parent_it);
-    else
-    {
-      parent=&(*parent_it);
-      parent_it++;
-    }
-  }
-  // update the links
-  for(link_it=this->links.begin();link_it!=this->links.end();)
-  {
-    if((*link_it)->next==this)
-    {
-      delete *link_it;
-      link_it=this->links.erase(link_it); 
-    }
-    else
-    {
-      if((*link_it)->lane==lane)
-      {
-        parent->start_curvature=(*link_it)->get_curvature_at(distance);
-        (*link_it)->update_start_pose(start,parent->start_curvature);
-      }
-      else
-        (*link_it)->update_start_pose(start,0.0);
-      link_it++;
-    }
-  }
-}
-
-void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPose &end,double distance)
-{
-  std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
-  std::vector<COpendriveLink *>::iterator link_it;
-
-  // remove the references to all lanes and segments except for lane
-  for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
-  { 
-    if(parent_it->lane!=lane)
-      parent_it=this->parents.erase(parent_it);
-    else
-      parent_it++;
-  }
-  // update the links
-  for(link_it=this->links.begin();link_it!=this->links.end();)
-  {
-    if((*link_it)->prev==this)
-    {
-      delete *link_it;
-      link_it=this->links.erase(link_it);
-    }
-    else
-      link_it++;
-  }
-}
-
-double COpendriveRoadNode::get_resolution(void) const
-{
-  return this->resolution;
-}
-
-double COpendriveRoadNode::get_scale_factor(void) const
-{
-  return this->scale_factor;
-}
-
-unsigned int COpendriveRoadNode::get_index(void) const
-{
- return this->index;
-}
-
-unsigned int COpendriveRoadNode::get_num_parents(void) const
-{
-  return this->parents.size();
-}
-
-const COpendriveRoadSegment &COpendriveRoadNode::get_parent_segment(unsigned int index) const
-{
-  std::stringstream text;
-
-  if(index>=0 && index<this->parents.size())
-    return *this->parents[index].segment;
-  else
-  {
-    text << "Invalid parent index " << index << " for node " << this->index << " (has " << this->parents.size() << " parents)";
-    throw CException(_HERE_,text.str());
-  }
-}
-
-const COpendriveLane &COpendriveRoadNode::get_parent_lane(unsigned int index) const
-{
-  std::stringstream text;
-
-  if(index>=0 && index<this->parents.size())
-    return *this->parents[index].lane;
-  else
-  {
-    text << "Invalid parent index " << index << " for node " << this->index << " (has " << this->parents.size() << " parents)";
-    throw CException(_HERE_,text.str());
-  }
-}
-
-TOpendriveWorldPose COpendriveRoadNode::get_pose(void) const
-{
-  return this->pose;
-}
-
-unsigned int COpendriveRoadNode::get_num_links(void) const
-{
-  return this->links.size();
-}
-
-const COpendriveLink &COpendriveRoadNode::get_link(unsigned int index) const
-{
-  std::stringstream text;
-
-  if(index>=0 && index<this->links.size())
-    return *this->links[index];
-  else
-  {
-    text << "Invalid link index " << index << " for node " << this->index << " (has " << this->links.size() << " links)";
-    throw CException(_HERE_,text.str());
-  }
-}
-
-const COpendriveLink &COpendriveRoadNode::get_link_ending_at(unsigned int node_index) const
-{
-  std::stringstream text;
-
-  for(unsigned int i=0;i<this->links.size();i++)
-  {
-    if(this->links[i]->get_next().get_index()==node_index)
-      return *this->links[i];
-  }
-
-  text << "No link in node " << this->index << " ends at node " << node_index;
-  throw CException(_HERE_,text.str());
-}
-
-const COpendriveLink &COpendriveRoadNode::get_link_ending_at(const COpendriveRoadNode &node) const
-{
-  std::stringstream text;
-
-  for(unsigned int i=0;i<this->links.size();i++)
-  {
-    if(&this->links[i]->get_next()==&node)
-      return *this->links[i];
-  }
-
-  text << "No link in node " << this->index << " ends at node " << node.get_index();
-  throw CException(_HERE_,text.str());
-}
-
-unsigned int COpendriveRoadNode::get_closest_link_index(TOpendriveWorldPose &pose,double &distance,bool only_lanes,double angle_tol) const 
-{
-  double dist,min_dist=std::numeric_limits<double>::max();
-  unsigned int closest_index=-1;
-  double angle,length,min_length=std::numeric_limits<double>::max();
-  TOpendriveWorldPose closest_pose;
-
-  for(unsigned int i=0;i<this->links.size();i++)
-  {
-    if(&this->links[i]->get_prev()==this)// links starting at this node
-    {
-      if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
-      {
-        length=this->links[i]->find_closest_world_pose(pose,closest_pose);
-        if(length<std::numeric_limits<double>::max())
-        {
-          angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
-          if(fabs(angle)<angle_tol)
-          {
-            dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
-            if(dist<min_dist)
-            {
-              min_dist=dist;
-              min_length=length;
-              closest_index=i;
-            }
-          }
-        }
-      }
-    }
-  }
-
-  distance=min_length;
-  return closest_index;
-}
-
-const COpendriveLink &COpendriveRoadNode::get_closest_link(TOpendriveWorldPose &pose,double &distance,bool only_lanes,double angle_tol)const
-{
-  unsigned int closest_index;
-
-  closest_index=this->get_closest_link_index(pose,distance,only_lanes,angle_tol);
-  if(closest_index==(unsigned int)-1)
-    throw CException(_HERE_,"Impossible to find the closest link");
-  return *this->links[closest_index];
-}
-
-double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,bool only_lanes,double angle_tol) const
-{
-  double dist,min_dist=std::numeric_limits<double>::max();
-  double angle;
-  double length,closest_length=std::numeric_limits<double>::max();
-  TOpendriveWorldPose tmp_pose;
-
-  closest_pose.x=std::numeric_limits<double>::max();
-  closest_pose.y=std::numeric_limits<double>::max();
-  closest_pose.heading=std::numeric_limits<double>::max();
-  for(unsigned int i=0;i<this->links.size();i++)
-  {
-    if(&this->links[i]->get_prev()==this)// links starting at this node
-    {
-      if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
-      {
-        length=this->links[i]->find_closest_world_pose(pose,tmp_pose);
-        if(length<std::numeric_limits<double>::max())
-        {
-          angle=diff_angle(normalize_angle(pose.heading),tmp_pose.heading);
-          if(fabs(angle)<angle_tol)
-          {
-            dist=sqrt(pow(tmp_pose.x-pose.x,2.0)+pow(tmp_pose.y-pose.y,2.0));
-            if(dist<min_dist)
-            {
-              min_dist=dist;
-              closest_length=length;
-              closest_pose=tmp_pose;
-            }
-          }
-        }
-      }
-    }
-  }
-
-  return closest_length;
-}
-
-void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &distances,double dist_tol,bool only_lanes,double angle_tol) const
-{
-  double dist;
-  double angle;
-  double length;
-  TOpendriveWorldPose closest_pose;
-  bool already_added;
-
-  closest_poses.clear();
-  distances.clear();
-  for(unsigned int i=0;i<this->links.size();i++)
-  {
-    if(&this->links[i]->get_prev()==this)// links starting at this node
-    {
-      if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
-      {
-        length=this->links[i]->find_closest_world_pose(pose,closest_pose);
-        if(length<std::numeric_limits<double>::max())
-        {
-          angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
-          if(fabs(angle)<angle_tol)
-          {
-            dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
-            if(dist<=dist_tol)
-            {
-              already_added=false;
-              for(unsigned int j=0;j<closest_poses.size();j++)
-                if(fabs(closest_poses[j].x-closest_pose.x)<this->resolution && 
-                   fabs(closest_poses[j].y-closest_pose.y)<this->resolution)
-                {
-                  already_added=true;
-                  break;
-                }
-              if(!already_added)
-              {
-                closest_poses.push_back(closest_pose);
-                distances.push_back(length);
-              }
-            }
-          }
-        }
-      }
-    }
-  }
-}
-
-std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node)
-{
-  out << "    Node: " << node.get_index() << std::endl;
-  out << "      start pose: x: " << node.pose.x << " y: " << node.pose.y << " heading: " << node.pose.heading << std::endl;
-  out << "      Number of parent: " << node.get_num_parents() << std::endl;
-  for(unsigned int i=0;i<node.get_num_parents();i++)
-  {
-    out << "      Parent " << i << ":" << std::endl;
-    out << "        segment " << node.get_parent_segment(i).get_name() << " (lane " << node.get_parent_lane(i).get_id() << ")" << std::endl; 
-  }
-  out << "      Number of links: " << node.links.size() << std::endl;
-  for(unsigned int i=0;i<node.links.size();i++)
-  {
-    out << "      Link " << i << ":" << std::endl;
-    out << *node.links[i];
-  }
-  return out;
-}
-
-COpendriveRoadNode::~COpendriveRoadNode()
-{
-  this->resolution=DEFAULT_RESOLUTION;
-  this->scale_factor=DEFAULT_SCALE_FACTOR;
-  this->pose.x=0.0;
-  this->pose.y=0.0;
-  this->pose.heading=0.0;
-  this->free();
-}
-
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
deleted file mode 100644
index bfd6b041e88d7cf8a39a289c190883adafcd6f79..0000000000000000000000000000000000000000
--- a/src/opendrive_road_segment.cpp
+++ /dev/null
@@ -1,1336 +0,0 @@
-#include "opendrive_road_segment.h"
-#include "exceptions.h"
-#include <math.h>
-
-COpendriveRoadSegment::COpendriveRoadSegment()
-{
-  this->name="";
-  this->id=-1;
-  this->center_mark=OD_MARK_NONE;
-  this->lanes.clear();
-  this->num_left_lanes=0;
-  this->num_right_lanes=0;
-  this->nodes.clear();
-  this->signals.clear();
-  this->objects.clear();
-  this->connecting.clear();
-  this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
-  this->resolution=DEFAULT_RESOLUTION;
-  this->scale_factor=DEFAULT_SCALE_FACTOR;
-  this->geometries.clear();
-}
-
-COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object)
-{
-  this->name=object.name;
-  this->id=object.id;
-  this->lanes=object.lanes;
-  this->num_right_lanes=object.num_right_lanes;
-  this->num_left_lanes=object.num_left_lanes;
-  this->nodes=object.nodes;
-  this->parent_road=object.parent_road;
-  this->signals.clear();
-  this->signals.resize(object.signals.size());
-  for(unsigned int i=0;i<object.signals.size();i++)
-    this->signals[i]=new COpendriveSignal(*object.signals[i]);
-  this->objects.resize(object.objects.size());
-  for(unsigned int i=0;i<object.signals.size();i++)
-    this->objects[i]=new COpendriveObject(*object.objects[i]);
-  this->connecting=object.connecting;
-  this->resolution=object.resolution;
-  this->scale_factor=object.scale_factor;
-  this->min_road_length=object.min_road_length;
-  this->center_mark=object.center_mark;
-  this->geometries.resize(object.geometries.size());
-  for(unsigned int i=0;i<object.geometries.size();i++)
-  {
-    this->geometries[i].opendrive=object.geometries[i].opendrive->clone();
-    this->geometries[i].spline=new CG2Spline(*object.geometries[i].spline);
-  }
-}
-
-void COpendriveRoadSegment::free(void)
-{
-  this->lanes.clear();
-  this->num_left_lanes=0;
-  this->num_right_lanes=0;
-  for(unsigned int i=0;i<this->signals.size();i++)
-  {
-    delete this->signals[i];
-    this->signals[i]=NULL;
-  }
-  this->nodes.clear();
-  this->signals.clear();
-  for(unsigned int i=0;i<this->objects.size();i++)
-  {
-    delete this->objects[i];
-    this->objects[i]=NULL;
-  }
-  this->objects.clear();
-  this->connecting.clear();
-  for(unsigned int i=0;i<this->geometries.size();i++)
-  {
-    delete this->geometries[i].opendrive;
-    this->geometries[i].opendrive=NULL;
-    delete this->geometries[i].spline;
-    this->geometries[i].spline=NULL;
-  }
-  this->geometries.clear();
-}
-
-void COpendriveRoadSegment::set_resolution(double res)
-{
-  std::map<int,COpendriveLane *>::const_iterator lane_it;
-
-  this->resolution=res;
-
-  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
-    lane_it->second->set_resolution(res);
-
-  for(unsigned int i=0;i<this->geometries.size();i++)
-    this->geometries[i].spline->generate(res);
-}
-
-void COpendriveRoadSegment::set_scale_factor(double scale)
-{
-  std::map<int,COpendriveLane *>::const_iterator lane_it;
-  TPoint spline_start,spline_end;
-
-  this->scale_factor=scale;
-
-  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
-    lane_it->second->set_scale_factor(scale);
-  for(unsigned int i=0;i<this->signals.size();i++)
-    this->signals[i]->set_scale_factor(scale);
-  for(unsigned int i=0;i<this->objects.size();i++)
-    this->objects[i]->set_scale_factor(scale);
-
-  for(unsigned int i=0;i<this->geometries.size();i++)
-  {
-    this->geometries[i].opendrive->set_scale_factor(scale);
-    this->geometries[i].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->geometries[i].spline->set_start_point(spline_start);
-    this->geometries[i].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->geometries[i].spline->set_end_point(spline_end);
-    this->geometries[i].spline->generate(this->resolution);
-  }
-}
-
-void COpendriveRoadSegment::set_min_road_length(double length)
-{
-  this->min_road_length=length;
-}
-
-void COpendriveRoadSegment::set_parent_road(COpendriveRoad *parent)
-{
-  this->parent_road=parent;
-}
-
-void COpendriveRoadSegment::set_name(std::string &name)
-{
-  this->name=name;
-}
-
-void COpendriveRoadSegment::set_id(unsigned int id)
-{
-  this->id=id;
-}
-
-void COpendriveRoadSegment::set_center_mark(opendrive_mark_t mark)
-{
-  this->center_mark=mark;
-}
-
-void COpendriveRoadSegment::add_geometries(OpenDRIVE::road_type &road_info)
-{
-  planView::geometry_iterator geom_info;
-  TOpendriveRoadSegmentGeometry geometry;
-  TOpendriveWorldPose start_pose,end_pose;
-  TPoint spline_start,spline_end;
-  double start_curvature,end_curvature;
-  std::stringstream error;
-
-  for(unsigned int i=0;i<this->geometries.size();i++)
-  {
-    delete this->geometries[i].opendrive;
-    delete this->geometries[i].spline;
-  }
-  this->geometries.clear();
-  for(geom_info=road_info.planView().geometry().begin(); geom_info!=road_info.planView().geometry().end(); ++geom_info)
-  {
-    // import geometry
-    if(geom_info->line().present())
-      geometry.opendrive=new COpendriveLine();
-    else if(geom_info->spiral().present())
-      geometry.opendrive=new COpendriveSpiral();
-    else if(geom_info->arc().present())
-      geometry.opendrive=new COpendriveArc();
-    else if(geom_info->paramPoly3().present())
-      geometry.opendrive=new COpendriveParamPoly3();
-    else
-    {
-      error << "Unsupported or missing geometry in road " << this->name;
-      throw CException(_HERE_,error.str());
-    }
-    geometry.opendrive->set_scale_factor(this->scale_factor);
-    geometry.opendrive->load(*geom_info);
-    if(geometry.opendrive->get_length()>this->min_road_length)
-    {
-      // create spline
-      geometry.spline=new CG2Spline();
-      start_pose=geometry.opendrive->get_start_pose();
-      end_pose=geometry.opendrive->get_end_pose();
-      geometry.opendrive->get_curvature(start_curvature,end_curvature);
-      spline_start.x=start_pose.x;
-      spline_start.y=start_pose.y;
-      spline_start.heading=start_pose.heading;
-      spline_start.curvature=start_curvature;
-      geometry.spline->set_start_point(spline_start);
-      spline_end.x=end_pose.x;
-      spline_end.y=end_pose.y;
-      spline_end.heading=end_pose.heading;
-      spline_end.curvature=end_curvature;
-      geometry.spline->set_end_point(spline_end);
-      geometry.spline->generate(this->resolution,geometry.opendrive->get_length());
-      this->geometries.push_back(geometry);
-    }
-  }
-}
-
-bool COpendriveRoadSegment::updated(segment_up_ref_t &refs)
-{
-  segment_up_ref_t::iterator updated_it;
-
-  for(updated_it=refs.begin();updated_it!=refs.end();updated_it++)
-    if(updated_it->second==this)
-      return true;
-
-  return false;
-}
-
-COpendriveRoadSegment *COpendriveRoadSegment::get_original_segment(segment_up_ref_t &segment_refs)
-{
-  segment_up_ref_t::iterator updated_it;
-
-  for(updated_it=segment_refs.begin();updated_it!=segment_refs.end();updated_it++)
-    if(updated_it->second==this)
-      return updated_it->first;
-
-  return NULL;
-}
-
-void COpendriveRoadSegment::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
-{
-  std::vector<COpendriveRoadSegment *>::iterator seg_it;
-  std::map<int,COpendriveLane *>::iterator lane_it;
-  std::vector<COpendriveRoadNode *>::iterator node_it;
-
-  if(this->updated(segment_refs))
-  {
-    for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
-    {
-      if(lane_refs.find((*lane_it).second)!=lane_refs.end())
-      {
-        lane_it->second=lane_refs[lane_it->second];
-        lane_it->second->update_references(segment_refs,lane_refs,node_refs);
-      }
-      else if(lane_it->second->updated(lane_refs))
-        lane_it->second->update_references(segment_refs,lane_refs,node_refs);
-    }
-    for(node_it=this->nodes.begin();node_it!=this->nodes.end();node_it++)
-      if(node_refs.find(*node_it)!=node_refs.end())
-        (*node_it)=node_refs[*node_it];
-    for(seg_it=this->connecting.begin();seg_it!=this->connecting.end();seg_it++)
-      if(segment_refs.find(*seg_it)!=segment_refs.end())
-        (*seg_it)=segment_refs[*seg_it];
-    for(unsigned int i=0;i<this->signals.size();i++)
-      this->signals[i]->update_references(segment_refs);
-    for(unsigned int i=0;i<this->objects.size();i++)
-      this->objects[i]->update_references(segment_refs);
-  }
-}
-
-void COpendriveRoadSegment::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
-{
-  std::vector<COpendriveRoadSegment *>::iterator seg_it;
-  std::map<int,COpendriveLane *>::iterator lane_it;
-  std::vector<COpendriveRoadNode *>::iterator node_it;
-
-  if(this->updated(segment_refs))
-  {
-    for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
-    {
-      if(lane_it->second->updated(lane_refs))
-      {
-        lane_it->second->clean_references(segment_refs,lane_refs,node_refs);
-        lane_it++;
-      }
-      else
-        lane_it=this->lanes.erase(lane_it);
-    }
-    for(node_it=this->nodes.begin();node_it!=this->nodes.end();)
-    {
-      if(!(*node_it)->updated(node_refs))
-        node_it=this->nodes.erase(node_it);
-      else
-        node_it++;
-    }
-    for(seg_it=this->connecting.begin();seg_it!=this->connecting.end();)
-    {
-      if(!(*seg_it)->updated(segment_refs))
-        seg_it=this->connecting.erase(seg_it);
-      else
-        seg_it++;
-    }
-  }
-}
-
-void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
-{
-  right::lane_iterator r_lane_it;
-  left::lane_iterator l_lane_it;
-
-  // right lanes
-  if(lane_section.right().present())
-  {
-    for(r_lane_it=lane_section.right()->lane().begin();r_lane_it!=lane_section.right()->lane().end();r_lane_it++)
-    {
-      this->add_lane(*r_lane_it);
-      this->num_right_lanes++;
-    }
-  }
-  // left lanes
-  if(lane_section.left().present())
-  {
-    for(l_lane_it=lane_section.left()->lane().begin();l_lane_it!=lane_section.left()->lane().end();l_lane_it++)
-    {
-      this->add_lane(*l_lane_it);
-      this->num_left_lanes++;
-    }
-  }
-}
-
-void COpendriveRoadSegment::add_lane(const ::lane &lane_info)
-{
-  std::map<int,COpendriveLane *>::iterator lane_it; 
-  COpendriveLane *new_lane;
-  unsigned int lane_index;
-  bool exist=false;
-
-  try{
-    new_lane=new COpendriveLane();
-    new_lane->set_resolution(this->resolution);
-    new_lane->set_scale_factor(this->scale_factor);
-    new_lane->load(lane_info,this);
-    for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
-      if(lane_it->first==new_lane->get_id())
-      {
-        exist=true;
-        break;
-      }
-    if(!exist)
-      this->lanes[new_lane->get_id()]=new_lane;
-    lane_index=parent_road->add_lane(new_lane);
-    new_lane->set_index(lane_index);
-  }catch(CException &e){
-    this->free();
-    if(!this->parent_road->remove_lane(new_lane))
-      delete new_lane;
-    throw e;
-  }
-}
-
-void COpendriveRoadSegment::remove_lane(COpendriveLane *lane)
-{
-  std::map<int,COpendriveLane *>::iterator lane_it;
-  std::vector<COpendriveLane *>::iterator other_lane_it;
-
-  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
-  {
-    if(lane_it->second==lane)
-    {
-      if(lane_it->first>0)
-        this->num_left_lanes--;
-      else
-        this->num_right_lanes--;
-      // remove associated nodes
-      for(unsigned int i=0;i<lane->nodes.size();i++)
-        this->remove_node(lane->nodes[i]);
-      // remove the reference from neightbour lanes
-      if(lane_it->second->left_lane!=NULL)
-        lane_it->second->left_lane->remove_lane(lane);
-      if(lane_it->second->right_lane!=NULL)
-        lane_it->second->right_lane->remove_lane(lane);
-      // remove the reference from next lanes
-      for(unsigned int i=0;i<lane_it->second->next.size();i++)
-        lane_it->second->next[i]->remove_lane(lane);
-      // remove the reference from prev lanes
-      for(unsigned int i=0;i<lane_it->second->prev.size();i++)
-        lane_it->second->prev[i]->remove_lane(lane);
-      lane_it=this->lanes.erase(lane_it);
-      break;
-    }
-    else
-      lane_it++;
-  }
-}
-
-void COpendriveRoadSegment::add_nodes(void)
-{
-  double lane_offset;
-
-  for(unsigned int j=0;j<this->geometries.size();j++)
-  {
-    lane_offset=0.0;
-    for(int i=-1;i>=-this->num_right_lanes;i--)
-    {
-      this->lanes[i]->set_offset(lane_offset);
-      this->add_node(this->geometries[j],this->lanes[i]);
-      lane_offset-=this->lanes[i]->width;
-    }
-  }
-  for(unsigned int j=0;j<this->geometries.size();j++)
-  {
-    lane_offset=0.0;
-    for(int i=1;i<=this->num_left_lanes;i++)
-    {
-      this->lanes[i]->set_offset(lane_offset);
-      this->add_node(this->geometries[this->geometries.size()-1-j],this->lanes[i]);
-      lane_offset+=this->lanes[i]->width;
-    }
-  }
-}
-
-void COpendriveRoadSegment::add_node(TOpendriveRoadSegmentGeometry &geometry,COpendriveLane *lane)
-{
-  TOpendriveTrackPose track_pose;
-  double start_curvature,end_curvature;
-  COpendriveRoadNode *new_node;
-  TOpendriveWorldPose node_pose;
-  unsigned int node_index;
-  bool exist=false;
-
-  try{
-    new_node=new COpendriveRoadNode();
-    new_node->set_resolution(this->resolution);
-    new_node->set_scale_factor(this->scale_factor);
-    // import start position
-    track_pose.t=lane->get_center_offset();
-    track_pose.heading=0.0;
-    if(lane->get_id()<0)
-    {
-      track_pose.s=0.0;
-      geometry.opendrive->get_world_pose(track_pose,node_pose);
-    }
-    else
-    {
-      track_pose.s=geometry.opendrive->get_length();
-      geometry.opendrive->get_world_pose(track_pose,node_pose);
-      node_pose.heading=normalize_angle(node_pose.heading+3.14159);
-    }
-    new_node->set_pose(node_pose);
-    if(this->parent_road->node_exists_at(node_pose))
-    {
-      delete new_node;
-      new_node=this->parent_road->get_node_at(node_pose);
-    }
-    else
-    {
-      node_index=this->parent_road->add_node(new_node);
-      new_node->set_index(node_index);
-    }
-    geometry.opendrive->get_curvature(start_curvature,end_curvature);
-    if(start_curvature>0)
-      start_curvature=1.0/((1.0/start_curvature)-lane->get_center_offset());
-    else
-      start_curvature=1.0/((1.0/start_curvature)+lane->get_center_offset());
-    if(end_curvature>0)
-      end_curvature=1.0/((1.0/end_curvature)-lane->get_center_offset());
-    else
-      end_curvature=1.0/((1.0/end_curvature)+lane->get_center_offset());
-    if(lane->get_id()<0)
-      new_node->add_parent(lane,this,start_curvature,end_curvature);
-    else
-      new_node->add_parent(lane,this,-end_curvature,-start_curvature);
-    for(unsigned int i=0;i<this->nodes.size();i++)
-      if(this->nodes[i]==new_node)
-      {
-        exist=true;
-        break;
-      }
-    if(!exist)
-      this->nodes.push_back(new_node);
-    lane->add_node(new_node);
-  }catch(CException &e){
-    this->free();
-    if(!this->parent_road->remove_node(new_node))
-      delete new_node;
-    throw e;
-  }
-}
-
-void COpendriveRoadSegment::add_empty_node(TOpendriveWorldPose &pose,COpendriveLane *lane)
-{
-  COpendriveRoadNode *new_node;
-  unsigned int node_index;
-  bool exist=false;
-
-  try{
-    new_node=new COpendriveRoadNode();
-    new_node->set_resolution(this->resolution);
-    new_node->set_scale_factor(this->scale_factor);
-    new_node->set_pose(pose);
-    node_index=this->parent_road->add_node(new_node);
-    new_node->set_index(node_index);
-    for(unsigned int i=0;i<this->nodes.size();i++)
-      if(this->nodes[i]==new_node)
-      {
-        exist=true;
-        break;
-      }
-    if(!exist)
-      this->nodes.push_back(new_node);
-    lane->add_node(new_node);
-  }catch(CException &e){
-    this->free();
-    if(!this->parent_road->remove_node(new_node))
-      delete new_node;
-    throw e;
-  }
-}
-
-void COpendriveRoadSegment::remove_node(COpendriveRoadNode *node)
-{
-  std::vector<COpendriveRoadNode *>::iterator it;
-
-  for(it=this->nodes.begin();it!=this->nodes.end();)
-  {
-    if((*it)==node)
-    {
-      it=this->nodes.erase(it);
-      break;
-    }
-    else
-      it++;
-  }
-}
-
-bool COpendriveRoadSegment::has_node(COpendriveRoadNode *node) const
-{
-  for(unsigned int i=0;i<this->nodes.size();i++)
-    if(this->nodes[i]==node)
-      return true;
-
-  return false;
-}
-
-int COpendriveRoadSegment::get_pose_side(TOpendriveWorldPose &pose) const
-{
-  const COpendriveRoadNode *node;
-  std::map<int,COpendriveLane *>::const_iterator it;
-  std::stringstream error;
-  double distance;
-
-  node=&this->get_closest_node(pose,distance,3.14159);
-  for(it=this->lanes.begin();it!=this->lanes.end();it++)
-    if(it->second->has_node((COpendriveRoadNode *)node))
-      return it->first;
-
-  error << "Segment " << this->name << " does not include node " << node->get_index();
-  throw CException(_HERE_,error.str());
-}
-
-void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_section)
-{
-  center::lane_type center_lane;
-
-  if(lane_section.center().lane().present())
-  {
-    center_lane=lane_section.center().lane().get();
-    if(center_lane.roadMark().size()>1)
-      throw CException(_HERE_,"Only one road mark supported for lane");
-    else if(center_lane.roadMark().size()==0)
-      this->set_center_mark(OD_MARK_NONE);
-    else if(center_lane.roadMark().size()==1)
-    {
-      if(center_lane.roadMark().begin()->type1().present())
-      {
-        if(center_lane.roadMark().begin()->type1().get()=="none")
-          this->set_center_mark(OD_MARK_NONE);
-        else if(center_lane.roadMark().begin()->type1().get()=="solid")
-          this->set_center_mark(OD_MARK_SOLID);
-        else if(center_lane.roadMark().begin()->type1().get()=="broken")
-          this->set_center_mark(OD_MARK_BROKEN);
-        else if(center_lane.roadMark().begin()->type1().get()=="solid solid")
-          this->set_center_mark(OD_MARK_SOLID_SOLID);
-        else if(center_lane.roadMark().begin()->type1().get()=="solid broken")
-          this->set_center_mark(OD_MARK_SOLID_BROKEN);
-        else if(center_lane.roadMark().begin()->type1().get()=="broken solid")
-          this->set_center_mark(OD_MARK_BROKEN_SOLID);
-        else if(center_lane.roadMark().begin()->type1().get()=="broken broken")
-          this->set_center_mark(OD_MARK_BROKEN_BROKEN);
-        else
-          this->set_center_mark(OD_MARK_NONE);
-      }
-      else
-        this->set_center_mark(OD_MARK_NONE);
-    }
-  }
-  this->link_neightbour_lanes();
-}
-
-void COpendriveRoadSegment::link_neightbour_lanes(void)
-{
-  for(int i=-this->num_right_lanes;i<0;i++)
-  {
-    if(this->lanes.find(i+1)!=this->lanes.end())// if the lane exists 
-      this->lanes[i]->link_neightbour_lane(this->lanes[i+1]);
-    else
-      this->lanes[i]->set_left_lane(NULL,this->center_mark);
-  }
-  for(int i=this->num_left_lanes;i>0;i--)
-  {
-    if(this->lanes.find(i-1)!=this->lanes.end())// if the lane exists 
-      this->lanes[i]->link_neightbour_lane(this->lanes[i-1]);
-    else
-      this->lanes[i]->set_left_lane(NULL,this->center_mark);
-  }
-}
-
-void COpendriveRoadSegment::link_segment(COpendriveRoadSegment *segment)
-{
-  if(this->connects_to(segment) || segment->connects_to(this))
-    return;
-  this->connecting.push_back(segment);
-  segment->connecting.push_back(this);
-  // link lanes
-  for(int i=-this->num_right_lanes;i<0;i++)
-  {
-    for(int j=i-1;j<=i+1;j++)
-    {
-      if(segment->lanes.find(j)!=segment->lanes.end())
-      {
-        if(j==i-1)
-          this->lanes[i]->link_lane(segment->lanes[j],this->lanes[i]->right_mark,false,true,false);
-        else if(j==i)
-          this->lanes[i]->link_lane(segment->lanes[j],OD_MARK_NONE,false,true,true);
-        else
-          this->lanes[i]->link_lane(segment->lanes[j],this->lanes[i]->left_mark,false,true,false);
-      }
-    }
-  }
-  for(int i=1;i<=segment->num_left_lanes;i++)
-  {
-    for(int j=i-1;j<=i+1;j++)
-    {
-      if(this->lanes.find(j)!=this->lanes.end())
-      {
-        if(j==i-1)
-          segment->lanes[i]->link_lane(this->lanes[j],segment->lanes[i]->right_mark,false,true,false);
-        else if(j==i)
-          segment->lanes[i]->link_lane(this->lanes[j],OD_MARK_NONE,false,true,true);
-        else
-          segment->lanes[i]->link_lane(this->lanes[j],segment->lanes[i]->left_mark,false,true,false);
-      }
-    }
-  }
-}
-
-void COpendriveRoadSegment::link_segment(COpendriveRoadSegment *segment,int from,bool from_start,int to,bool to_start)
-{
-  if(!this->connects_to(segment))
-    this->connecting.push_back(segment);
-  if(!segment->connects_to(this))
-    segment->connecting.push_back(this);
-  // link lanes
-  if(segment->lanes.find(to)!=segment->lanes.end())
-  {
-    if(this->lanes.find(from-1)!=this->lanes.end())
-      this->lanes[from-1]->link_lane(segment->lanes[to],this->lanes[from-1]->right_mark,from_start,to_start,false);
-    if(this->lanes.find(from)!=this->lanes.end())
-      this->lanes[from]->link_lane(segment->lanes[to],OD_MARK_NONE,from_start,to_start,true);
-    if(this->lanes.find(from+1)!=this->lanes.end())
-      this->lanes[from+1]->link_lane(segment->lanes[to],this->lanes[from+1]->left_mark,from_start,to_start,false);
-  }
-/*
-  if(segment.lanes.find(to-1)!=segment.lanes.end())
-    if(this->lanes.find(from)!=this->lanes.end())
-      this->lanes[from]->link_lane(segment.lanes[to-1],this->lanes[from]->right_mark,from_start,to_start,false);
-  if(segment.lanes.find(to+1)!=segment.lanes.end())
-    if(this->lanes.find(from)!=this->lanes.end())
-      this->lanes[from]->link_lane(segment.lanes[to+1],this->lanes[from]->left_mark,from_start,to_start,false);
-*/
-}
-
-bool COpendriveRoadSegment::connects_to(COpendriveRoadSegment* segment)
-{
-  for(unsigned int i=0;i<this->connecting.size();i++)
-    if(this->connecting[i]->get_id()==segment->get_id())
-      return true;
-
-  return false;
-}
-
-bool COpendriveRoadSegment::connects_segments(COpendriveRoadSegment *segment1,COpendriveRoadSegment *segment2)
-{
-  if(this->connects_to(segment1) && this->connects_to(segment2))
-    return true;
-  else
-    return false;
-}
-
-COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start,TOpendriveWorldPose *end) const
-{
-  TOpendriveWorldPose *start_pose,*end_pose;
-  std::map<int,COpendriveLane *>::iterator lane_it;
-  lane_up_ref_t::iterator lane_ref_it;
-  COpendriveLane *new_lane;
-  std::vector<COpendriveRoadSegment *>::iterator seg_it;
-  segment_up_ref_t new_segment_ref;
-  COpendriveRoadSegment *new_segment;
-  std::vector<COpendriveRoadNode *>::iterator node_it;
-  node_up_ref_t::iterator node_ref_it;
-  std::vector<TOpendriveRoadSegmentGeometry>::iterator geom_it;
-  TPoint new_point;
-  double length;
-
-  if(start==NULL && end==NULL)
-    return this->clone(new_node_ref,new_lane_ref,new_link_ref);
-  new_segment=new COpendriveRoadSegment(*this);
-  new_segment_ref[(COpendriveRoadSegment *)this]=new_segment;
-  if(node_side<0)
-  {
-    start_pose=start;
-    end_pose=end;
-  }
-  else
-  {
-    start_pose=end;
-    end_pose=start;
-  }
-  /* get the sublanes */
-  for(lane_it=new_segment->lanes.begin();lane_it!=new_segment->lanes.end();lane_it++)
-  {
-    new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,new_link_ref,start_pose,end_pose);
-    new_lane_ref[lane_it->second]=new_lane;
-  }
-  new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);
-  // update geometry
-  if(start_pose!=NULL)
-  {
-    for(geom_it=new_segment->geometries.begin();geom_it!=new_segment->geometries.end();)
-    {
-      length=geom_it->spline->find_closest_point(start_pose->x,start_pose->y,new_point);
-      if(length<std::numeric_limits<double>::max())
-      {
-        geom_it->spline->set_start_point(new_point);
-        geom_it->spline->generate(this->resolution);
-        geom_it->opendrive->set_start_pose(*start_pose);
-        geom_it->opendrive->set_max_s(geom_it->opendrive->get_max_s()-length);
-        break;
-      }
-      else
-        geom_it=new_segment->geometries.erase(geom_it);
-    }
-    for(unsigned int i=1;i<new_segment->geometries.size();i++)
-    {
-      new_segment->geometries[i].opendrive->set_min_s(new_segment->geometries[i].opendrive->get_min_s()-length);
-      new_segment->geometries[i].opendrive->set_max_s(new_segment->geometries[i].opendrive->get_max_s()-length);
-    }  
-  }
-  if(end_pose!=NULL)
-  {
-    for(geom_it=new_segment->geometries.begin();geom_it!=new_segment->geometries.end();)
-    {
-      length=geom_it->spline->find_closest_point(end_pose->x,end_pose->y,new_point);
-      if(length<std::numeric_limits<double>::max())
-      {
-        geom_it->spline->set_end_point(new_point);
-        geom_it->spline->generate(this->resolution);
-        geom_it->opendrive->set_max_s(geom_it->opendrive->get_min_s()+length);;
-        geom_it=new_segment->geometries.erase(++geom_it,new_segment->geometries.end());
-        break;
-      }
-      else
-        geom_it++;
-    }
-  }
-
-  return new_segment; 
-}
-
-COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref) const
-{
-  std::map<int,COpendriveLane *>::iterator lane_it;
-  lane_up_ref_t::iterator lane_ref_it;
-  COpendriveLane *new_lane;
-  segment_up_ref_t new_segment_ref;
-  COpendriveRoadSegment *new_segment;
-  std::vector<COpendriveRoadNode *>::iterator node_it;
-  node_up_ref_t::iterator node_ref_it;
-
-  new_segment=new COpendriveRoadSegment(*this);
-  new_segment_ref[(COpendriveRoadSegment *)this]=new_segment;
-  /* get the sublanes */
-  for(lane_it=new_segment->lanes.begin();lane_it!=new_segment->lanes.end();lane_it++)
-  {
-    new_lane=lane_it->second->clone(new_node_ref,new_lane_ref,new_link_ref);
-    new_lane_ref[lane_it->second]=new_lane;
-  }
-  new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);
-
-  return new_segment;
-}
-
-std::vector<const COpendriveRoadSegment *> COpendriveRoadSegment::get_next_segments(int input_side,std::vector<int> &output_sides) const
-{
-  std::vector<const COpendriveRoadSegment *> segment_candidates;
-  bool already_present;
-
-  output_sides.clear();
-  if(input_side<0)
-  {
-    for(unsigned int i=1;i<=this->get_num_right_lanes();i++)
-    {
-      const COpendriveLane &lane=this->get_lane(-i);
-      for(unsigned int j=0;j<lane.get_num_next_lanes();j++)
-      {
-        already_present=false;
-        for(unsigned int k=0;k<segment_candidates.size();k++)
-          if(segment_candidates[k]==&lane.get_next_lane(j).get_segment())
-          {
-            already_present=true;
-            break;
-          }
-        if(!already_present)
-        {
-          segment_candidates.push_back(&lane.get_next_lane(j).get_segment());
-          output_sides.push_back(lane.get_next_lane(j).get_id());
-        }
-      }
-    }
-  }
-  else
-  {
-    for(unsigned int i=1;i<=this->get_num_left_lanes();i++)
-    {
-      const COpendriveLane &lane=this->get_lane(i);
-      for(unsigned int j=0;j<lane.get_num_next_lanes();j++)
-      {
-        already_present=false;
-        for(unsigned int k=0;k<segment_candidates.size();k++)
-          if(segment_candidates[k]==&lane.get_next_lane(j).get_segment())
-          {
-            already_present=true;
-            break;
-          }
-        if(!already_present)
-        {
-          segment_candidates.push_back(&lane.get_next_lane(j).get_segment());
-          output_sides.push_back(lane.get_next_lane(j).get_id());
-        }
-      }
-    }
-  }
-
-  return segment_candidates;
-}
-
-std::vector<const COpendriveRoadSegment *> COpendriveRoadSegment::get_prev_segments(int input_side,std::vector<int> &output_sides) const
-{
-  std::vector<const COpendriveRoadSegment *> segment_candidates;
-  bool already_present;
-
-  output_sides.clear();
-  if(input_side<0)
-  {
-    for(unsigned int i=1;i<=this->get_num_right_lanes();i++)
-    {
-      const COpendriveLane &lane=this->get_lane(-i);
-      for(unsigned int j=0;j<lane.get_num_prev_lanes();j++)
-      {
-        already_present=false;
-        for(unsigned int k=0;k<segment_candidates.size();k++)
-          if(segment_candidates[k]==&lane.get_prev_lane(j).get_segment())
-          {
-            already_present=true;
-            break;
-          }
-        if(!already_present)
-        {
-          segment_candidates.push_back(&lane.get_prev_lane(j).get_segment());
-          output_sides.push_back(lane.get_prev_lane(j).get_id());
-        }
-      }
-    }
-  }
-  else
-  {
-    for(unsigned int i=1;i<=this->get_num_left_lanes();i++)
-    {
-      const COpendriveLane &lane=this->get_lane(i);
-      for(unsigned int j=0;j<lane.get_num_prev_lanes();j++)
-      {
-        already_present=false;
-        for(unsigned int k=0;k<segment_candidates.size();k++)
-          if(segment_candidates[k]==&lane.get_prev_lane(j).get_segment())
-          {
-            already_present=true;
-            break;
-          }
-        if(!already_present)
-        {
-          segment_candidates.push_back(&lane.get_prev_lane(j).get_segment());
-          output_sides.push_back(lane.get_prev_lane(j).get_id());
-        }
-      }
-    }
-  }
-
-  return segment_candidates;
-}
-
-
-void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
-{
-  std::map<int,COpendriveLane *>::const_iterator lane_it;
-  signals::signal_iterator signal_it;
-  objects::object_iterator object_it;
-  lanes::laneSection_iterator lane_section;
-  COpendriveSignal *new_signal;
-  COpendriveObject *new_object;
-  std::stringstream error;
-
-  this->free();
-  this->set_name(road_info.name().get());
-  this->set_id(std::stoi(road_info.id().get()));
-  // only one lane section is supported
-  if(road_info.lanes().laneSection().size()<1)
-  {
-    error << "No lane sections defined for segment " << this->name;
-    throw CException(_HERE_,error.str());
-  }
-  else if(road_info.lanes().laneSection().size()>1)
-  {
-    error << "Segment " << this->name << " has more than one lane section";
-    throw CException(_HERE_,error.str());
-  }
-  else
-    lane_section=road_info.lanes().laneSection().begin();
-
-  try{
-    // add lanes
-    this->add_lanes(*lane_section);
-    // add geometries
-    this->add_geometries(road_info);
-    // add road nodes
-    this->add_nodes();
-    // link lanes
-    this->link_neightbour_lanes(*lane_section);
-    // add road signals
-    if(road_info.signals().present())
-    {
-      for(signal_it=road_info.signals().get().signal().begin();signal_it!=road_info.signals().get().signal().end();++signal_it)
-      {
-        new_signal=new COpendriveSignal();
-        new_signal->load(*signal_it,this);
-        new_signal->set_scale_factor(this->scale_factor);
-        this->signals.push_back(new_signal);
-      }
-    }
-    // add road objects
-    if(road_info.objects().present())
-    {
-      for(object_it=road_info.objects().get().object().begin();object_it!=road_info.objects().get().object().end();++object_it)
-      {
-        new_object=new COpendriveObject();
-        new_object->load(*object_it,this);
-        new_object->set_scale_factor(this->scale_factor);
-        this->objects.push_back(new_object);
-      }
-    }
-  }catch(CException &e){
-    this->free();
-    throw e;
-  }
-}
-
-std::string COpendriveRoadSegment::get_name(void) const
-{
-  return this->name;
-}
-
-unsigned int COpendriveRoadSegment::get_id(void) const
-{
-  return this->id;
-}
-
-unsigned int COpendriveRoadSegment::get_num_right_lanes(void) const
-{
-  return this->num_right_lanes;
-}
-
-unsigned int COpendriveRoadSegment::get_num_left_lanes(void) const
-{
-  return this->num_left_lanes;
-}
-
-const COpendriveLane &COpendriveRoadSegment::get_lane(int id) const
-{
-  std::map<int,COpendriveLane *>::const_iterator lane_it;
-  std::stringstream error;
-
-  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
-  {
-    if(lane_it->second->get_id()==id)
-      return *lane_it->second;
-  }
-
-  error << "Invalid lane id (" << id << ") for segment " << this->name;
-  throw CException(_HERE_,error.str());
-}
-
-unsigned int COpendriveRoadSegment::get_num_nodes(void) const
-{
-  return this->nodes.size();
-}
-
-const COpendriveRoadNode &COpendriveRoadSegment::get_node(unsigned int index) const
-{
-  std::stringstream error;
-
-  if(index>=0 && index<this->nodes.size())
-    return *this->nodes[index];
-  else
-  {
-    error << "Invalid node index (" << index << ") for segment " << this->name;
-    throw CException(_HERE_,error.str());
-  }
-}
-
-const COpendriveRoad &COpendriveRoadSegment::get_parent_road(void) const
-{
-  return *this->parent_road;
-}
-
-unsigned int COpendriveRoadSegment::get_num_signals(void) const
-{
-  return this->signals.size();  
-}
-
-const COpendriveSignal &COpendriveRoadSegment::get_signal(unsigned int index) const
-{
-  std::stringstream error;
-
-  if(index>=0 && index<this->signals.size())
-    return *this->signals[index];
-  else
-  {
-    error << "Invalid signal index (" << index << ") for segment " << this->name;
-    throw CException(_HERE_,error.str());
-  }
-}
-
-unsigned int COpendriveRoadSegment::get_num_objects(void) const
-{
-  return this->objects.size();
-}
-
-const COpendriveObject &COpendriveRoadSegment::get_object(unsigned int index) const
-{
-  std::stringstream error;
-
-  if(index>=0 && index<this->objects.size())
-    return *this->objects[index];
-  else
-  {
-    error << "Invalid object index (" << index << ") for segment " << this->name;
-    throw CException(_HERE_,error.str());
-  }
-}
-
-unsigned int COpendriveRoadSegment::get_num_connecting(void) const
-{
-  return this->connecting.size();
-}
-
-const COpendriveRoadSegment &COpendriveRoadSegment::get_connecting(unsigned int index) const
-{
-  std::stringstream error;
-
-  if(index>=0 && index<this->connecting.size())
-    return *this->connecting[index];
-  else
-  {
-    error << "Invalid connecting segment index (" << index << ") for segment " << this->name;
-    throw CException(_HERE_,error.str());
-  }
-}
-
-double COpendriveRoadSegment::get_length(void) const
-{
-  double length=0.0;
-
-  for(unsigned int i=0;i<this->geometries.size();i++)
-    length+=this->geometries[i].spline->get_length();
-
-  return length;
-}
-
-TOpendriveWorldPose COpendriveRoadSegment::get_pose_at(double length) const
-{
-  TOpendriveWorldPose world_pose{0};
-  TPoint spline_pose;
-
-  for(unsigned int i=0;i<this->geometries.size();i++)
-  {
-    if((length-this->geometries[i].spline->get_length())<0.0)
-    {
-      spline_pose=this->geometries[i].spline->evaluate(length);
-      world_pose.x=spline_pose.x;
-      world_pose.y=spline_pose.y;
-      world_pose.heading=spline_pose.heading;
-      return world_pose;
-    }
-    else
-      length-=this->geometries[i].spline->get_length();
-  }
-
-  return world_pose;
-}
-
-double COpendriveRoadSegment::get_curvature_at(double length) const
-{
-  double curvature;
-  TPoint spline_pose;
-
-  for(unsigned int i=0;i<this->geometries.size();i++)
-  {
-    if((length-this->geometries[i].spline->get_length())<0.0)
-    {
-      spline_pose=this->geometries[i].spline->evaluate(length);
-      curvature=spline_pose.curvature;
-      return curvature;
-    }
-    else
-      length-=this->geometries[i].spline->get_length();
-  }
-
-  return 0.0;
-}
-
-TOpendriveLocalPose COpendriveRoadSegment::transform_to_local_pose(const TOpendriveTrackPose &track)
-{
-  TOpendriveTrackPose pose;
-  TOpendriveLocalPose local;
-  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());
-  }
-  pose=track;
-  for(unsigned int i=0;i<this->geometries.size();i++)
-  {
-    if((pose.s-this->geometries[i].opendrive->get_length())<=this->resolution)
-    {
-      if(!this->geometries[i].opendrive->get_local_pose(pose,local))
-      {
-        error << "Impossible to transform to local coordinates pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") in segment " << this->name;
-        throw CException(_HERE_,error.str());
-      }
-      return local;
-    }
-    else
-      pose.s-=this->geometries[i].opendrive->get_length();
-  }
-
-  error << "Track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") does not belong to segment " << this->name;
-  throw CException(_HERE_,error.str());
-}
-
-TOpendriveWorldPose COpendriveRoadSegment::transform_to_world_pose(const TOpendriveTrackPose &track)
-{
-  TOpendriveTrackPose pose;
-  TOpendriveWorldPose world;
-  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());
-  }
-  pose=track;
-  for(unsigned int i=0;i<this->geometries.size();i++)
-  {
-    if((pose.s-this->geometries[i].opendrive->get_length())<=this->resolution)
-    {
-      if(!this->geometries[i].opendrive->get_world_pose(pose,world))
-      {
-        error << "Impossible to transform to world coordinates pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") in segment " << this->name;
-        throw CException(_HERE_,error.str());
-      }
-      return world;
-    }
-    else
-      pose.s-=this->geometries[i].opendrive->get_length();
-  }
-
-  error << "Track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") does not belong to segment " << this->name;
-  throw CException(_HERE_,error.str());
-}
-
-unsigned int COpendriveRoadSegment::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
-{
-  double dist,min_dist=std::numeric_limits<double>::max(),length;
-  TOpendriveWorldPose closest_pose;
-  unsigned int closest_index=-1;
-
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  {
-    length=this->nodes[i]->get_closest_pose(pose,closest_pose,false,angle_tol);
-    if(length<std::numeric_limits<double>::max())
-    {
-      dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
-      if(dist<min_dist)
-      {
-        min_dist=dist;
-        closest_index=i;
-        distance=length;
-      }
-    }
-  }
-
-  return closest_index;
-}
-
-const COpendriveRoadNode &COpendriveRoadSegment::get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
-{
-  unsigned int closest_index;
-
-  closest_index=this->get_closest_node_index(pose,distance,angle_tol);
-  if(closest_index==(unsigned int)-1)
-    throw CException(_HERE_,"Impossible to find the closest node");
-  return *this->nodes[closest_index];
-}
-
-int COpendriveRoadSegment::get_closest_lane_id(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
-{
-  double dist,min_dist=std::numeric_limits<double>::max(),length;
-  TOpendriveWorldPose closest_pose;
-  int closest_id=0;
-  std::map<int,COpendriveLane *>::const_iterator lane_it;
-
-  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
-  {
-    length=lane_it->second->get_closest_pose(pose,closest_pose,angle_tol);
-    if(length<std::numeric_limits<double>::max())
-    {
-      dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
-      if(dist<min_dist)
-      {
-        min_dist=dist;
-        closest_id=lane_it->first;
-        distance=length;
-      }
-    }
-  }
-
-  return closest_id;
-}
-
-const COpendriveLane &COpendriveRoadSegment::get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
-{
-  std::map<int,COpendriveLane *>::const_iterator lane_it;
-
-  int closest_id=this->get_closest_lane_id(pose,distance,angle_tol);
-  if(closest_id==0)
-    throw CException(_HERE_,"Impossible to find the closest lane");
-  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
-    if(lane_it->first==closest_id)
-      return *lane_it->second;
-
-  throw CException(_HERE_,"Impossible to find the closest lane");
-}
-
-double COpendriveRoadSegment::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
-{
-  double dist,min_dist=std::numeric_limits<double>::max(),distance,length;
-  unsigned int closest_index=-1;
-  TPoint closest_spline_point;
-
-  closest_pose.x=std::numeric_limits<double>::max();
-  closest_pose.y=std::numeric_limits<double>::max();
-  closest_pose.heading=std::numeric_limits<double>::max();
-  for(unsigned int i=0;i<this->geometries.size();i++)
-  {
-    length=this->geometries[i].spline->find_closest_point(pose.x,pose.y,closest_spline_point);
-    if(length<std::numeric_limits<double>::max())
-    {
-      dist=sqrt(pow(closest_spline_point.x-pose.x,2.0)+pow(closest_spline_point.y-pose.y,2.0));
-      if(dist<min_dist)
-      {
-        min_dist=dist;
-        closest_index=i;
-        closest_pose.x=closest_spline_point.x;
-        closest_pose.y=closest_spline_point.y;
-        closest_pose.heading=normalize_angle(closest_spline_point.heading);
-        distance=length;
-      }
-    }
-  }
-
-  if(closest_index!=(unsigned int)-1)
-  {
-    for(unsigned int i=0;i<closest_index;i++)
-      distance+=this->geometries[i].spline->get_length();
-  }
-  else
-    distance=std::numeric_limits<double>::max();
-
-  return distance;
-}
-
-std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment)
-{
-  out << "Road " << segment.get_name() << " (" << segment.get_id() << ")" << std::endl;
-  out << "  Connecting road segments: " << segment.connecting.size() << std::endl;
-  for(unsigned int i=0;i<segment.connecting.size();i++)
-    out << "    " << segment.connecting[i]->get_name() << std::endl;
-  out << "  Number of nodes: " << segment.get_num_nodes() << std::endl;
-  for(unsigned int i=0;i<segment.get_num_nodes();i++)
-    out << "    " << segment.get_node(i).get_index() << std::endl;
-  out << "  Number of right lanes: " << segment.num_right_lanes << std::endl;
-  for(int i=-1;i>=-segment.num_right_lanes;i--)
-    out << *segment.lanes[i];
-  out << "  Number of left lanes: " << segment.num_left_lanes << std::endl;
-  for(int i=1;i<=segment.num_left_lanes;i++)
-    out << *segment.lanes[i];
-  out << "  Number of signals: " << segment.signals.size() << std::endl;
-  for(unsigned int i=0;i<segment.signals.size();i++)
-    out << *segment.signals[i];
-  out << "  Number of objects: " << segment.objects.size() << std::endl;
-  for(unsigned int i=0;i<segment.objects.size();i++)
-    out << *segment.objects[i];
-
-  return out;
-}
-
-COpendriveRoadSegment::~COpendriveRoadSegment()
-{
-  this->free();
-  this->name="";
-  this->id=-1;
-  this->center_mark=OD_MARK_NONE;
-  this->resolution=DEFAULT_RESOLUTION;
-  this->scale_factor=DEFAULT_SCALE_FACTOR;
-  this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
-}
-
diff --git a/src/osm/osm_junction.cpp b/src/osm/osm_junction.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9e5ec0f299d54916f32557aa6a5b7a7eaf4f0ff5
--- /dev/null
+++ b/src/osm/osm_junction.cpp
@@ -0,0 +1,807 @@
+#include "osm/osm_junction.h"
+
+#include <math.h>
+
+#include <iostream>
+
+COSMJunction::COSMJunction(COSMNode *node)
+{
+  std::vector<COSMWay *>::iterator it;
+
+  this->parent_node=node;
+  node->junction=this;
+
+  for(it=node->ways.begin();it!=node->ways.end();it++)
+  {
+    if((*it)->is_node_first(node->id))
+    {
+      this->out_roads.push_back((*it)->road_segment);
+      if(!(*it)->is_one_way())
+        this->in_roads.push_back((*it)->road_segment);
+      (*it)->road_segment->start_junction=this;
+    }
+    else if((*it)->is_node_last(node->id))
+    {
+      this->in_roads.push_back((*it)->road_segment);
+      if(!(*it)->is_one_way())
+        this->out_roads.push_back((*it)->road_segment);
+      (*it)->road_segment->end_junction=this;
+    }
+  }
+  this->create_initial_connectivity();
+  this->apply_node_restrictions();
+  this->apply_way_restrictions();
+  this->create_links();
+}
+
+void COSMJunction::create_FF_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix)
+{
+  COSMNode *check_node;
+
+  check_node=&out_way.get_next_node_by_id(this->parent_node->id);
+  if(in_way.is_node_left(*check_node,true,0.2))
+  {
+    if((num_in_lanes-in_lane_index-1)==(num_out_lanes-out_lane_index-1))
+      matrix(in_lane_index,out_lane_index)=1;
+    else if(((num_out_lanes-out_lane_index-1)>=num_in_lanes) && in_lane_index==0)
+      matrix(in_lane_index,out_lane_index)=1;
+  }
+  else if(in_way.is_node_right(*check_node,true,0.2))
+  {
+    if(in_lane_index==out_lane_index)
+      matrix(in_lane_index,out_lane_index)=1;
+    else if((out_lane_index>=num_in_lanes) && in_lane_index==(num_in_lanes-1))
+      matrix(in_lane_index,out_lane_index)=1;
+  }
+  else
+  {
+    if(in_lane_index==out_lane_index)
+      matrix(in_lane_index,out_lane_index)=1;
+    else if(in_lane_index>=num_out_lanes && out_lane_index==(num_out_lanes-1))
+      matrix(in_lane_index,out_lane_index)=1;
+    else if((out_lane_index>=num_in_lanes) && in_lane_index==(num_in_lanes-1))
+      matrix(in_lane_index,out_lane_index)=1;
+  }
+}
+
+void COSMJunction::create_FB_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lanes,Eigen::MatrixXi &matrix)
+{
+  COSMNode *check_node;
+
+  check_node=&out_way.get_prev_node_by_id(this->parent_node->id);
+  if(in_way.is_node_left(*check_node,true,0.2))
+  {
+    if((num_in_lanes-in_lane_index-1)==(out_lane_index-(total_out_lanes-num_out_lanes)))
+      matrix(in_lane_index,out_lane_index)=1;
+    else if(((out_lane_index-(total_out_lanes-num_out_lanes))>=num_in_lanes) && in_lane_index==0)
+      matrix(in_lane_index,out_lane_index)=1;
+  }
+  else if(in_way.is_node_right(*check_node,true,0.2))
+  {
+    if(in_lane_index==(total_out_lanes-out_lane_index-1))
+      matrix(in_lane_index,out_lane_index)=1;
+    else if(((total_out_lanes-out_lane_index-1)>=num_in_lanes) && in_lane_index==(num_in_lanes-1))
+      matrix(in_lane_index,out_lane_index)=1;
+  }
+  else
+  {
+    if(in_lane_index==(total_out_lanes-out_lane_index-1))
+      matrix(in_lane_index,out_lane_index)=1;
+    else if(in_lane_index>=num_out_lanes && (total_out_lanes-out_lane_index-1)==(num_out_lanes-1))
+      matrix(in_lane_index,out_lane_index)=1;
+    else if(((total_out_lanes-out_lane_index-1)>=num_in_lanes) && in_lane_index==(num_in_lanes-1))
+      matrix(in_lane_index,out_lane_index)=1;
+  }
+}
+
+void COSMJunction::create_BF_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int total_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix)
+{
+  COSMNode *check_node;
+
+  check_node=&out_way.get_next_node_by_id(this->parent_node->id);
+  if(in_way.is_node_left(*check_node,false,0.2))
+  {
+    if((in_lane_index-(total_in_lanes-num_in_lanes))==(num_out_lanes-out_lane_index-1))
+      matrix(in_lane_index,out_lane_index)=1;
+    else if(((signed int)out_lane_index<((signed int)num_out_lanes-(signed int)num_in_lanes)) && (total_in_lanes-in_lane_index-1)==0)
+      matrix(in_lane_index,out_lane_index)=1;
+  }
+  else if(in_way.is_node_right(*check_node,false,0.2))
+  {
+    if((total_in_lanes-in_lane_index-1)==out_lane_index)
+      matrix(in_lane_index,out_lane_index)=1;
+    else if((out_lane_index>=num_in_lanes) && (total_in_lanes-in_lane_index-1)==(num_in_lanes-1))
+      matrix(in_lane_index,out_lane_index)=1;
+  }
+  else
+  {
+    if((total_in_lanes-in_lane_index-1)==out_lane_index)
+      matrix(in_lane_index,out_lane_index)=1;
+    else if((total_in_lanes-in_lane_index-1)>=num_out_lanes && out_lane_index==(num_out_lanes-1))
+      matrix(in_lane_index,out_lane_index)=1;
+    else if((out_lane_index>=num_in_lanes) && (total_in_lanes-in_lane_index-1)==(num_in_lanes-1))
+      matrix(in_lane_index,out_lane_index)=1;
+  }
+}
+
+void COSMJunction::create_BB_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int total_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lanes,Eigen::MatrixXi &matrix)
+{
+  COSMNode *check_node;
+
+  check_node=&out_way.get_prev_node_by_id(this->parent_node->id);
+  if(in_way.is_node_left(*check_node,false,0.2))
+  {
+    if((in_lane_index-(total_in_lanes-num_in_lanes))==out_lane_index)
+      matrix(in_lane_index,out_lane_index)=1;
+    else if((out_lane_index>=num_in_lanes) && (total_in_lanes-in_lane_index-1)==0)
+      matrix(in_lane_index,out_lane_index)=1;
+  }
+  else if(in_way.is_node_right(*check_node,false,0.2))
+  {
+    if((total_in_lanes-in_lane_index-1)==(total_out_lanes-out_lane_index-1))
+      matrix(in_lane_index,out_lane_index)=1;
+    else if(((total_out_lanes-out_lane_index-1)>=num_in_lanes) && (total_in_lanes-in_lane_index-1)==(num_in_lanes-1))
+      matrix(in_lane_index,out_lane_index)=1;
+  }
+  else
+  {
+    if((total_in_lanes-in_lane_index-1)==(total_out_lanes-out_lane_index-1))
+      matrix(in_lane_index,out_lane_index)=1;
+    else if((total_in_lanes-in_lane_index-1)>=num_out_lanes && (total_out_lanes-out_lane_index-1)==(num_out_lanes-1))
+      matrix(in_lane_index,out_lane_index)=1;
+    else if(((total_out_lanes-out_lane_index-1)>=num_in_lanes) && (total_in_lanes-in_lane_index-1)==(num_in_lanes-1))
+      matrix(in_lane_index,out_lane_index)=1;
+  }
+}
+
+void COSMJunction::create_initial_connectivity(void)
+{
+  this->connections.resize(this->in_roads.size());
+  for(unsigned int i=0;i<this->in_roads.size();i++)
+  {
+    COSMWay &in_way=this->in_roads[i]->get_parent_way();
+    this->connections[i].resize(this->out_roads.size());
+    for(unsigned int j=0;j<this->out_roads.size();j++)
+    {
+      COSMWay &out_way=this->out_roads[j]->get_parent_way();
+      this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+      for(unsigned int k=0;k<in_way.get_num_lanes();k++)
+      {
+        for(unsigned int l=0;l<out_way.get_num_lanes();l++)
+        {
+          if(in_way.get_id()!=out_way.get_id())
+          {
+            /*
+            if(out_way.is_node_first(this->parent_node->id))
+            {
+              if(k>=in_way.get_num_forward_lanes() && l<out_way.get_num_forward_lanes())
+              {
+                if((k-in_way.get_num_forward_lanes())==(out_way.get_num_forward_lanes()-l-1))
+                  this->connections[i][j](k,l)=1;
+                else if(((out_way.get_num_forward_lanes()-l-1)>in_way.get_num_backward_lanes()) && k==(in_way.get_num_lanes()-1))
+                  this->connections[i][j](k,l)=1;
+              }
+            }
+            else
+            {
+              if(k<in_way.get_num_forward_lanes() && l>=out_way.get_num_forward_lanes())
+              {
+                if((in_way.get_num_forward_lanes()-k-1)==(l-out_way.get_num_forward_lanes()))
+                  this->connections[i][j](k,l)=1;
+                else if(((l-out_way.get_num_forward_lanes())>=in_way.get_num_forward_lanes()) && k==0)
+                  this->connections[i][j](k,l)=1;
+              }
+            }
+          }
+          else
+          {
+          */
+            if(in_way.is_one_way())
+            {
+              if(out_way.is_one_way())
+                this->create_FF_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_lanes(),out_way.get_num_lanes(),this->connections[i][j]);
+              else
+              {
+                if(out_way.is_node_first(this->parent_node->id))
+                {
+                  if(l<out_way.get_num_forward_lanes())
+                    this->create_FF_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_lanes(),out_way.get_num_forward_lanes(),this->connections[i][j]);
+                }
+                else
+                {
+                  if(l>=out_way.get_num_forward_lanes())
+                    this->create_FB_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_lanes(),out_way.get_num_backward_lanes(),out_way.get_num_lanes(),this->connections[i][j]);
+                }
+              }
+            }
+            else
+            {
+              if(out_way.is_one_way())
+              {
+                if(in_way.is_node_last(this->parent_node->id))
+                {
+                  if(k<in_way.get_num_forward_lanes())
+                    this->create_FF_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_forward_lanes(),out_way.get_num_lanes(),this->connections[i][j]);
+                }
+                else
+                {
+                  if(k>=in_way.get_num_forward_lanes())
+                    this->create_BF_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_backward_lanes(),in_way.get_num_lanes(),out_way.get_num_lanes(),this->connections[i][j]);
+                }
+              }
+              else
+              {
+                if(in_way.is_node_last(this->parent_node->id))
+                {
+                  if(out_way.is_node_first(this->parent_node->id))
+                  {
+                    if(k<in_way.get_num_forward_lanes() && l<out_way.get_num_forward_lanes())
+                      this->create_FF_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_forward_lanes(),out_way.get_num_forward_lanes(),this->connections[i][j]);
+                  }
+                  else
+                  {
+                    if(k<in_way.get_num_forward_lanes() && l>=out_way.get_num_forward_lanes())
+                      this->create_FB_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_forward_lanes(),out_way.get_num_backward_lanes(),out_way.get_num_lanes(),this->connections[i][j]);
+                  }
+                }
+                else
+                {
+                  if(out_way.is_node_first(this->parent_node->id))
+                  {
+                    if(k>=in_way.get_num_forward_lanes() && l<out_way.get_num_forward_lanes())
+                      this->create_BF_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_backward_lanes(),in_way.get_num_lanes(),out_way.get_num_forward_lanes(),this->connections[i][j]);
+                  }
+                  else
+                  {
+                    if(k>=in_way.get_num_forward_lanes() && l>=out_way.get_num_forward_lanes())
+                      this->create_BB_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_backward_lanes(),in_way.get_num_lanes(),out_way.get_num_backward_lanes(),out_way.get_num_lanes(),this->connections[i][j]);
+                  }
+                }
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+
+}
+
+void COSMJunction::apply_node_restrictions(void)
+{
+  COSMNode *check_node;
+
+  for(unsigned int k=0;k<this->parent_node->get_num_restrictions();k++)
+  {
+    COSMRestriction &res=this->parent_node->get_restriction(k);
+    for(unsigned int i=0;i<this->in_roads.size();i++)
+    {
+      COSMWay &in_way=this->in_roads[i]->get_parent_way();
+      for(unsigned int j=0;j<this->out_roads.size();j++)
+      {
+        COSMWay &out_way=this->out_roads[j]->get_parent_way();
+        switch(res.get_action())
+        {
+          case RESTRICTION_NO:
+            if(out_way.is_node_first(this->parent_node->id))
+              check_node=&out_way.get_next_node_by_id(this->parent_node->id);
+            else 
+              check_node=&out_way.get_prev_node_by_id(this->parent_node->id);  
+            switch(res.get_type())
+            {
+              case RESTRICTION_LEFT_TURN:
+                if(res.get_from_way().get_id()==in_way.get_id() && res.get_to_way().get_id()==out_way.get_id())
+                {
+                  if(in_way.is_node_first(this->parent_node->id))
+                  {
+                    if(k>=in_way.get_num_forward_lanes())
+                    {
+                      if(in_way.is_node_left(*check_node,true))
+                        this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                    }
+                    else
+                    {
+                      if(in_way.is_node_left(*check_node,false))
+                        this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                    }
+                  }
+                  else
+                  {
+                    if(k>=in_way.get_num_forward_lanes())
+                    {
+                      if(in_way.is_node_left(*check_node,true))
+                        this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                    }
+                    else
+                    {
+                      if(in_way.is_node_left(*check_node,false))
+                        this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                    }
+                  }
+                }
+                break;
+              case RESTRICTION_RIGHT_TURN:
+                if(res.get_from_way().get_id()==in_way.get_id() && res.get_to_way().get_id()==out_way.get_id())
+                {
+                  if(in_way.is_node_first(this->parent_node->id))
+                  {
+                    if(k>=in_way.get_num_forward_lanes())
+                    {
+                      if(in_way.is_node_right(*check_node,true))
+                        this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                    }
+                    else
+                    {
+                      if(in_way.is_node_right(*check_node,false))
+                        this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                    }
+                  }
+                  else
+                  {
+                    if(k>=in_way.get_num_forward_lanes())
+                    {
+                      if(in_way.is_node_right(*check_node,true))
+                        this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                    }
+                    else
+                    {
+                      if(in_way.is_node_right(*check_node,false))
+                        this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                    }
+                  }
+                }
+                break;
+              case RESTRICTION_U_TURN:
+                if(res.get_from_way().get_id()==in_way.get_id() && res.get_to_way().get_id()==out_way.get_id())
+                {
+                  this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                }
+                break;
+              default:
+                break;
+            }
+            break;
+          case RESTRICTION_ONLY:
+            if(out_way.is_node_first(this->parent_node->id))
+              check_node=&out_way.get_next_node_by_id(this->parent_node->id);
+            else
+              check_node=&out_way.get_prev_node_by_id(this->parent_node->id);
+            switch(res.get_type())
+            {
+              case RESTRICTION_LEFT_TURN:
+                if(res.get_from_way().get_id()==in_way.get_id())
+                {
+                  if(in_way.is_one_way())
+                  {
+                    if(out_way.is_one_way())
+                    {
+                      if(!in_way.is_node_left(*check_node,true))
+                        this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                    }
+                    else
+                    {
+                      if(!in_way.is_node_left(*check_node,true))
+                        this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                    }
+                  }
+                  else
+                  {
+                    if(in_way.is_node_first(this->parent_node->id))
+                    {
+                      if(k>=in_way.get_num_forward_lanes())
+                      {
+                        if(!in_way.is_node_left(*check_node,true))
+                          this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                      }
+                      else
+                      {
+                        if(!in_way.is_node_left(*check_node,false))
+                          this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                      }
+                    }
+                    else
+                    {
+                      if(k>=in_way.get_num_forward_lanes())
+                      {
+                        if(!in_way.is_node_left(*check_node,true))
+                          this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                      }
+                      else
+                      {
+                        if(!in_way.is_node_left(*check_node,false))
+                          this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                      }
+                    }
+                  }
+                }
+                break;
+              case RESTRICTION_RIGHT_TURN:
+                if(res.get_from_way().get_id()==in_way.get_id())
+                {
+                  if(in_way.is_one_way())
+                  {
+                    if(out_way.is_one_way())
+                    {
+                      if(!in_way.is_node_right(*check_node,true))
+                        this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                    }
+                    else
+                    {
+                      if(!in_way.is_node_right(*check_node,true))
+                        this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                    }
+                  }
+                  else
+                  {
+                    if(in_way.is_node_first(this->parent_node->id))
+                    {
+                      if(k>=in_way.get_num_forward_lanes())
+                      {
+                        if(!in_way.is_node_right(*check_node,true))
+                          this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                      }
+                      else
+                      {
+                        if(!in_way.is_node_right(*check_node,false))
+                          this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                      }
+                    }
+                    else
+                    {
+                      if(k>=in_way.get_num_forward_lanes())
+                      {
+                        if(!in_way.is_node_right(*check_node,true))
+                          this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                      }
+                      else
+                      {
+                        if(!in_way.is_node_right(*check_node,false))
+                          this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                      }
+                    }
+                  }
+                }
+                break;
+              case RESTRICTION_STRAIGHT_ON:
+                if(res.get_from_way().get_id()==in_way.get_id())
+                {
+                  if(in_way.is_one_way())
+                  {
+                    if(out_way.is_one_way())
+                    {
+                      if(in_way.is_node_right(*check_node,true) || in_way.is_node_left(*check_node,true))
+                        this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                    }
+                    else
+                    {
+                      if(in_way.is_node_right(*check_node,true) || in_way.is_node_left(*check_node,true))
+                        this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                    }
+                  }
+                  else
+                  {                   
+                    if(in_way.is_node_first(this->parent_node->id))
+                    {
+                      if(in_way.is_node_right(*check_node,false) || in_way.is_node_left(*check_node,false))
+                        this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                    }
+                    else
+                    {
+                      if(in_way.is_node_right(*check_node,true) || in_way.is_node_left(*check_node,true))
+                        this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+                    }
+                  }
+                }
+                break;
+              default:
+                break;
+            }
+            break;
+          default:
+            break;
+        }
+      }
+    }
+  }
+}
+
+void COSMJunction::apply_way_restrictions(void)
+{
+  lane_restructions_t restriction;
+  COSMNode *check_node;
+  bool left,right;
+
+  for(unsigned int i=0;i<this->in_roads.size();i++)
+  {
+    COSMWay &in_way=this->in_roads[i]->get_parent_way();
+    for(unsigned int j=0;j<this->out_roads.size();j++)
+    {
+      COSMWay &out_way=this->out_roads[j]->get_parent_way();
+      if(out_way.is_node_first(this->parent_node->id))
+        check_node=&out_way.get_next_node_by_id(this->parent_node->id);
+      else
+        check_node=&out_way.get_prev_node_by_id(this->parent_node->id);
+      for(unsigned int k=0;k<in_way.get_num_lanes();k++)
+      {
+        if(in_way.is_node_first(this->parent_node->id))
+        {
+          if(k<in_way.get_num_forward_lanes())
+          {
+            left=in_way.is_node_left(*check_node,false,0.2);
+            right=in_way.is_node_right(*check_node,false,0.2);
+          }
+          else
+          {
+            left=in_way.is_node_left(*check_node,true,0.2);
+            right=in_way.is_node_right(*check_node,true,0.2);
+          }
+        }
+        else
+        {
+          if(k<in_way.get_num_forward_lanes())
+          {
+            left=in_way.is_node_left(*check_node,true,0.2);
+            right=in_way.is_node_right(*check_node,true,0.2);
+          }
+          else
+          {
+            left=in_way.is_node_left(*check_node,false,0.2);
+            right=in_way.is_node_right(*check_node,false,0.2);
+          }          
+        }
+        restriction=in_way.get_lane_restriction(k);
+        if(restriction==NO_RESTRICTION)
+          continue;
+        if(!(restriction&RESTRICTION_THROUGH))//can't go through
+        {
+          if(!left && !right)
+          {
+            for(unsigned int l=0;l<out_way.get_num_lanes();l++)
+              this->connections[i][j](k,l)=0;
+          }
+        }
+        if(!(restriction&RESTRICTION_LEFT))//can't go through
+        {
+          if(left)
+          {
+            for(unsigned int l=0;l<out_way.get_num_lanes();l++)
+              this->connections[i][j](k,l)=0;
+          }
+        }
+        if(!(restriction&RESTRICTION_RIGHT))//can't go through
+        {
+          if(right)
+          {
+            for(unsigned int l=0;l<out_way.get_num_lanes();l++)
+              this->connections[i][j](k,l)=0;
+          }
+        }
+      }
+    }
+  }
+}
+
+void COSMJunction::create_links(void)
+{
+  TOSMLink new_link;
+  COSMNode *node1,*node3;
+  double length1,length2,angle,in_radius,out_radius,in_dist,out_dist;
+  bool left=false,right=false;
+
+  for(unsigned int i=0;i<this->connections.size();i++)
+  {
+    COSMWay &in_way=this->in_roads[i]->get_parent_way();
+    for(unsigned int j=0;j<this->connections[i].size();j++)
+    {
+      COSMWay &out_way=this->out_roads[j]->get_parent_way();
+      for(unsigned int k=0;k<in_way.get_num_lanes();k++)
+      {
+        for(unsigned int l=0;l<out_way.get_num_lanes();l++)
+        {
+          if(this->connections[i][j](k,l)==1)// link exists
+          {
+            new_link.in_way=&in_way;
+            new_link.out_way=&out_way;
+            new_link.in_lane_id=k;
+            new_link.out_lane_id=l;
+            if(out_way.is_node_first(this->parent_node->get_id()))
+              node3=&out_way.get_segment_end_node(FIRST_SEGMENT);
+            else
+              node3=&out_way.get_segment_start_node(LAST_SEGMENT);
+            in_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0)*in_way.get_lane_width();
+            if(in_way.is_node_first(this->parent_node->get_id()))
+            {
+              node1=&in_way.get_segment_end_node(FIRST_SEGMENT);
+              left=in_way.is_node_left(*node3,false,0.2);
+              right=in_way.is_node_right(*node3,false,0.2);
+            }
+            else
+            {
+              node1=&in_way.get_segment_start_node(LAST_SEGMENT);
+              left=in_way.is_node_left(*node3,true,0.2);
+              right=in_way.is_node_right(*node3,true,0.2);
+            }
+            /*
+            if(in_way.is_node_first(this->parent_node->get_id()))
+            {
+              node1=&in_way.get_segment_end_node(FIRST_SEGMENT);
+              if((left=in_way.is_node_left(*node3,false,0.2))==true)
+                in_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0-k)*in_way.get_lane_width();
+              else if((right=in_way.is_node_right(*node3,false,0.2))==true)
+                in_radius=DEFAULT_MIN_RADIUS+(k+1-in_way.get_num_lanes()/2.0)*in_way.get_lane_width();
+            }
+            else
+            {
+              node1=&in_way.get_segment_start_node(LAST_SEGMENT);
+              if((left=in_way.is_node_left(*node3,true,0.2))==true)
+                in_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0)*in_way.get_lane_width();
+              else if((right=in_way.is_node_right(*node3,true,0.2))==true)
+                in_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0-k)*in_way.get_lane_width();
+            }
+            */
+            out_radius=DEFAULT_MIN_RADIUS+(out_way.get_num_lanes()/2.0)*out_way.get_lane_width();
+            /*
+            if(out_way.is_node_first(this->parent_node->get_id()))
+            {
+              if(left)
+                out_radius=DEFAULT_MIN_RADIUS+(l+1-in_way.get_num_lanes()/2.0)*in_way.get_lane_width();
+              else if(right)
+                out_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0-l)*in_way.get_lane_width();
+            }
+            else
+            {
+              if(left)
+                out_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0-l)*in_way.get_lane_width();
+              else if(right)
+                out_radius=DEFAULT_MIN_RADIUS+(l+1-in_way.get_num_lanes()/2.0)*in_way.get_lane_width();
+            }
+            */
+//          std::cout << "in way: " << in_way.id << " lane " << k << " out way: " << out_way.id << " lane " << l << " in radius " << in_radius << " out radius " << out_radius << " num lanes in: " << in_way.get_num_lanes() << " num fwd lanes in: " << in_way.get_num_forward_lanes() << " num lanes out: " << out_way.get_num_lanes() <<  " num fwd lanes out: " << out_way.get_num_forward_lanes()  << std::endl;
+            angle=this->parent_node->compute_angle(*node1,*node3);
+            if(!left && !right)
+            {
+              length1=this->parent_node->compute_distance(*node1);
+              length2=this->parent_node->compute_distance(*node3);
+              in_dist=std::min(length1,length2)/2.0;
+              if(in_dist>DEFAULT_MIN_RADIUS)
+                in_dist=DEFAULT_MIN_RADIUS;
+              else if(in_dist<MIN_ROAD_LENGTH/2.0)
+                in_dist=MIN_ROAD_LENGTH/2.0;
+              out_dist=in_dist;
+            }
+            else
+            {
+              in_dist=(out_radius+in_radius*cos(angle))/fabs(sin(angle));
+              if(in_dist<MIN_ROAD_LENGTH/2.0)
+                in_dist=MIN_ROAD_LENGTH/2.0;
+              out_dist=(in_radius+out_radius*cos(angle))/fabs(sin(angle));
+              if(out_dist<MIN_ROAD_LENGTH/2.0)
+                out_dist=MIN_ROAD_LENGTH;
+            }
+            if(this->add_link(new_link))
+            {
+              if(in_way.is_node_first(this->parent_node->get_id()))
+                in_way.road_segment->set_start_distance(in_dist);
+              else
+                in_way.road_segment->set_end_distance(in_dist);
+              if(out_way.is_node_first(this->parent_node->get_id()))
+                out_way.road_segment->set_start_distance(out_dist);
+              else
+                out_way.road_segment->set_end_distance(out_dist);
+            }
+          }
+        }
+      }
+    }
+  }
+}
+
+bool COSMJunction::add_link(TOSMLink &new_link)
+{
+  for(unsigned int i=0;i<this->links.size();i++)
+  {
+    if(new_link.in_way->get_id()==this->links[i].in_way->get_id() && new_link.in_lane_id==this->links[i].in_lane_id && new_link.out_way->get_id()==this->links[i].out_way->get_id() && new_link.out_lane_id==this->links[i].out_lane_id)
+      return false;
+  }
+  this->links.push_back(new_link);
+
+  return true;
+}
+
+void COSMJunction::convert(CJunction **junction,osm_road_map_t &road_map)
+{
+  CRoad *in=NULL,*out=NULL;
+  unsigned int from_index,to_index;
+
+  (*junction)=new CJunction();
+
+  // add incomming and outgoing roads
+  for(unsigned int i=0;i<this->links.size();i++)
+  {
+    for(osm_road_map_t::iterator it=road_map.begin();it!=road_map.end();it++)
+    {
+      if(it->first->parent_way->get_id()==this->links[i].in_way->get_id())
+      {
+        if(it->first->parent_way->is_node_first(this->parent_node->get_id()))
+        {
+          if((it->second).size()>1)
+          {
+            in=(it->second)[1];
+            from_index=this->links[i].in_lane_id-it->first->parent_way->get_num_forward_lanes();
+            if(!(*junction)->has_incomming_road(in->get_id()))
+              (*junction)->add_incomming_road(in);
+          }
+        }
+        else if(it->first->parent_way->is_node_last(this->parent_node->get_id()))
+        {
+          if((it->second).size()>0)
+          {
+            in=(it->second)[0];
+            from_index=it->first->parent_way->get_num_forward_lanes()-this->links[i].in_lane_id-1;
+            if(!(*junction)->has_incomming_road(in->get_id()))
+              (*junction)->add_incomming_road(in);
+          }
+        }
+        else
+          continue;
+      }
+      else if(it->first->parent_way->get_id()==this->links[i].out_way->get_id())
+      {
+        if(it->first->parent_way->is_node_first(this->parent_node->get_id()))
+        {
+          if((it->second).size()>0)
+          {
+            out=(it->second)[0];
+            to_index=it->first->parent_way->get_num_forward_lanes()-this->links[i].out_lane_id-1;
+            if(!(*junction)->has_outgoing_road(out->get_id()))
+              (*junction)->add_outgoing_road(out);
+          }
+        }
+        else if(it->first->parent_way->is_node_last(this->parent_node->get_id()))
+        {
+          if((it->second).size()>1)
+          {
+            out=(it->second)[1];
+            to_index=this->links[i].out_lane_id-it->first->parent_way->get_num_forward_lanes();
+            if(!(*junction)->has_outgoing_road(out->get_id()))
+              (*junction)->add_outgoing_road(out);
+          }
+        }
+      }
+      else
+        continue;
+    }
+    if(in!=NULL && out!=NULL)
+    {
+      if(!(*junction)->are_roads_linked_by_id(in->get_id(),out->get_id()))
+        (*junction)->link_roads_by_id(in->get_id(),out->get_id());
+      (*junction)->link_lanes_by_id(in->get_id(),from_index,out->get_id(),to_index);
+    }
+  }
+}
+
+COSMNode &COSMJunction::get_parent_node(void)
+{
+  return *this->parent_node;
+}
+
+std::ostream& operator<<(std::ostream& out, COSMJunction &junction)
+{
+  out << "  ID: " << junction.parent_node->get_id() << std::endl;
+  out << "  number of in roads: " << junction.in_roads.size() << std::endl;
+  for(unsigned int i=0;i<junction.in_roads.size();i++)
+    out << "    Road ID: " << junction.in_roads[i]->get_parent_way().get_id() << std::endl;
+  out << "  number of out roads: " << junction.out_roads.size() << std::endl;
+  for(unsigned int i=0;i<junction.out_roads.size();i++)
+    out << "    Road ID: " << junction.out_roads[i]->get_parent_way().get_id() << std::endl;
+
+  return out;
+}
+
+COSMJunction::~COSMJunction()
+{
+  this->parent_node=NULL;
+  this->in_roads.clear();
+  this->out_roads.clear();
+  this->links.clear();
+}
diff --git a/src/osm/osm_map.cpp b/src/osm/osm_map.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c9d5967e86a8f832da9010e995a427fca13a52b6
--- /dev/null
+++ b/src/osm/osm_map.cpp
@@ -0,0 +1,382 @@
+#include "osm/osm_map.h"
+#include "exceptions.h"
+
+#include <iostream>
+
+#include <osmium/io/xml_input.hpp>
+#include <osmium/visitor.hpp>
+#include <osmium/tags/taglist.hpp>
+#include <GeographicLib/UTMUPS.hpp>
+
+#include "road.h"
+
+const std::vector<std::string> highway_tags={"motorway","trunk","primary","secondary","tertiary","motor_way_link","trunk_link","primary_link","secondary_link","tertiary_link","minor","residential","living","service","living_street"};
+
+const std::vector<std::string> relation_type_tags={"restriction"};
+
+COSMMap::COSMMap() :
+  way_filter{false}
+{
+  for(unsigned int i=0;i<highway_tags.size();i++)
+    this->way_filter.add_rule(true, "highway", highway_tags[i]);
+  for(unsigned int i=0;i<relation_type_tags.size();i++)
+    this->relation_filter.add_rule(true, "type", relation_type_tags[i]);
+  this->max_id=-std::numeric_limits<long int>::max();
+  this->utm_zone=31;
+}
+
+void COSMMap::free(void)
+{
+  for(unsigned int i=0;i<this->segments.size();i++)
+  {
+    delete this->segments[i];
+    this->segments[i]=NULL;
+  }
+  this->segments.clear();
+  for(unsigned int i=0;i<this->junctions.size();i++)
+  {
+    delete this->junctions[i];
+    this->junctions[i]=NULL;
+  }
+  this->junctions.clear();
+  for(unsigned int i=0;i<this->nodes.size();i++)
+  {
+    delete this->nodes[i];
+    this->nodes[i]=NULL;
+  }
+  this->nodes.clear();
+  for(unsigned int i=0;i<this->ways.size();i++)
+  {
+    delete this->ways[i];
+    this->ways[i]=NULL;
+  }
+  this->ways.clear();
+  for(unsigned int i=0;i<this->restrictions.size();i++)
+  {
+    delete this->restrictions[i];
+    this->restrictions[i]=NULL;
+  }
+  this->ways.clear();
+}
+
+void COSMMap::process_map(void)
+{
+  std::vector<COSMNode *>::iterator node_it;
+  std::vector<COSMWay *>::iterator way_it;
+
+  // remove all nodes that do not belong to any road
+  for(node_it=this->nodes.begin();node_it!=this->nodes.end();)
+  {
+    if((*node_it)->get_num_ways()==0)
+      node_it=this->nodes.erase(node_it);
+    else
+      node_it++;
+  }
+  // merge and split ways as necessary
+  for(unsigned int i=0;i<this->nodes.size();i++)
+  {
+    if(!this->nodes[i]->merge_ways())
+      this->nodes[i]->split_ways();
+  }
+  // remove unconnected ways
+  for(way_it=this->ways.begin();way_it!=this->ways.end();)
+  {
+    if((*way_it)->is_not_connected())
+      way_it=this->ways.erase(way_it);
+    else
+      way_it++;
+  }
+  // remove colinear nodes in ways
+  for(unsigned int i=0;i<this->ways.size();i++)
+    this->ways[i]->remove_straight_nodes();
+  // remove nodes inside other ways width
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    this->nodes[i]->remove_in_junction_nodes();
+}
+
+void COSMMap::create_junctions(void)
+{
+  COSMJunction *new_junction;
+
+  for(unsigned int i=0;i<this->nodes.size();i++)
+  {
+    if(this->nodes[i]->get_num_ways()>1)
+    {
+      new_junction=new COSMJunction(this->nodes[i]);
+      this->junctions.push_back(new_junction);
+    }
+  }
+}
+
+void COSMMap::create_road_segments(void)
+{
+  COSMRoadSegment *new_segment;
+
+  for(unsigned int i=0;i<this->ways.size();i++)
+  {
+    new_segment=new COSMRoadSegment(this->ways[i]);
+    this->segments.push_back(new_segment);
+  }
+}
+
+void COSMMap::delete_way(COSMWay *way)
+{
+  std::vector<COSMWay *>::iterator it;
+
+  for(it=this->ways.begin();it!=this->ways.end();++it)
+    if(*it==way)
+    {
+      this->ways.erase(it);
+      delete way;
+      break;
+    }
+}
+
+void COSMMap::add_way(COSMWay *way)
+{
+  if(way->id==-1)
+  {
+    this->max_id++;
+    way->id=this->max_id;
+  }
+  else
+  {
+    if(way->id>this->max_id)
+      this->max_id=way->id;
+    for(unsigned int i=0;i<this->ways.size();i++)
+      if(this->ways[i]->id==way->id)
+        return;
+  }
+  this->ways.push_back(way);
+}
+
+COSMWay *COSMMap::get_way_by_id(long int id)
+{
+  for(unsigned int i=0;i<this->ways.size();i++)
+    if(this->ways[i]->get_id()==id)
+      return this->ways[i];
+
+  return NULL;
+}
+
+void COSMMap::delete_node(COSMNode *node)
+{
+  std::vector<COSMNode *>::iterator it;
+
+  for(it=this->nodes.begin();it!=this->nodes.end();++it)
+    if(*it==node)
+    {
+      this->nodes.erase(it);
+      delete node;
+      break;
+    }
+}
+
+void COSMMap::add_node(COSMNode *node)
+{
+  if(node->id==-1)
+  {
+    this->max_id++;
+    node->id=this->max_id;
+  }
+  else
+  {
+    if(node->id>this->max_id)
+      this->max_id=node->id;
+    for(unsigned int i=0;i<this->nodes.size();i++)
+      if(this->nodes[i]->id==node->id)
+        return;
+  }
+  this->nodes.push_back(node);
+}
+
+COSMNode *COSMMap::get_node_by_id(long int id)
+{
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    if(this->nodes[i]->get_id()==id)
+      return this->nodes[i];
+
+  return NULL;
+}
+
+void COSMMap::add_restriction(COSMRestriction *restriction)
+{
+  if(restriction->id==-1)
+  {
+    this->max_id++;
+    restriction->id=this->max_id;
+  }
+  else
+  {
+    if(restriction->id>this->max_id)
+      this->max_id=restriction->id;
+    for(unsigned int i=0;i<this->restrictions.size();i++)
+      if(this->restrictions[i]->get_id()==restriction->id)
+        return;
+  }
+  this->restrictions.push_back(restriction);
+}
+
+void COSMMap::normalize_locations(std::vector<osmium::Box> &boxes)
+{
+  double max_x,min_x,max_y,min_y,center_x,center_y,gamma,k;
+  int zone;
+  bool northp;
+
+  if(boxes.size()!=1)
+  {
+    this->free();
+    throw CException(_HERE_,"Invalid number of bounding boxes. Only one supported");
+  }
+  osmium::Location top_right=boxes[0].top_right();
+  osmium::Location bottom_left=boxes[0].bottom_left();
+  GeographicLib::UTMUPS::Forward(top_right.lat(),top_right.lon(),zone,northp,max_x,max_y,gamma,k,this->utm_zone);
+  GeographicLib::UTMUPS::Forward(bottom_left.lat(),bottom_left.lon(),zone,northp,min_x,min_y,gamma,k,this->utm_zone);
+  center_x=(max_x+min_x)/2.0;
+  center_y=(max_y+min_y)/2.0;
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    this->nodes[i]->normalize_location(center_x,center_y);
+}
+
+void COSMMap::set_utm_zone(int zone)
+{
+  this->utm_zone=zone;
+}
+
+int COSMMap::get_utm_zone(void)
+{
+  return this->utm_zone;
+}
+
+void COSMMap::load(const std::string &filename)
+{
+  osmium::osm_entity_bits::type read_types;
+
+  this->free();
+  // read the nodes
+  read_types = osmium::osm_entity_bits::node;
+  osmium::io::Reader node_reader{filename, read_types};
+  osmium::apply(node_reader,*this);
+  node_reader.close();
+
+  // read the ways
+  read_types = osmium::osm_entity_bits::way;
+  osmium::io::Reader way_reader{filename, read_types};
+  osmium::apply(way_reader,*this);
+  way_reader.close();
+
+  // read relations
+  read_types = osmium::osm_entity_bits::relation;
+  osmium::io::Reader relation_reader{filename, read_types};
+  osmium::apply(relation_reader,*this);
+  relation_reader.close();
+
+  // read the bounds of the map
+  std::vector<osmium::Box> boxes=node_reader.header().boxes();
+  this->normalize_locations(boxes);
+
+  this->process_map();
+  this->create_road_segments();
+  this->create_junctions();
+}
+
+void COSMMap::node(const osmium::Node& node)
+{
+  COSMNode *new_node;
+
+  // create and add the new node
+  new_node = new COSMNode(node,this);
+  this->add_node(new_node);
+}
+
+void COSMMap::way(const osmium::Way& way)
+{
+  COSMWay *new_way=NULL;
+
+  if(osmium::tags::match_any_of(way.tags(),this->way_filter))
+  {
+    try{
+      new_way=new COSMWay(way,this);
+      this->add_way(new_way);
+    }catch(CException &e){
+      if(new_way!=NULL)
+        delete new_way;
+    }
+  }
+}
+
+void COSMMap::relation(const osmium::Relation& relation)
+{
+  COSMRestriction *new_restriction=NULL;
+
+  if(osmium::tags::match_any_of(relation.tags(),this->relation_filter))
+  {
+    try{
+      new_restriction=new COSMRestriction(relation,this);
+      this->add_restriction(new_restriction);
+    }catch(CException &e){
+      if(new_restriction!=NULL)
+        delete new_restriction;
+    }
+  }
+}
+
+void COSMMap::convert(CRoadMap &road)
+{
+  CRoad *left_road,*right_road;
+  CJunction *junction;
+  osm_road_map_t road_map;
+  osm_map_pair_t map_pair;
+  std::vector<CRoad *> segment_roads;
+
+  for(unsigned int i=0;i<this->segments.size();i++)
+  {
+    this->segments[i]->convert(&left_road,&right_road,road.get_resolution());
+    segment_roads.clear();
+    if(right_road!=NULL)
+    {
+      road.add_road(right_road);
+      segment_roads.push_back(right_road);
+    }
+    if(left_road!=NULL)
+    {
+      road.add_road(left_road);
+      segment_roads.push_back(left_road);
+    }
+    if(right_road!=NULL || left_road!=NULL)
+    {
+      map_pair=std::make_pair(this->segments[i],segment_roads);
+      road_map.push_back(map_pair);    
+    }
+  }
+  for(unsigned int i=0;i<this->junctions.size();i++)
+  {
+    this->junctions[i]->convert(&junction,road_map);
+    if(junction!=NULL)
+      road.add_junction(junction);
+  }
+} 
+
+std::ostream& operator<<(std::ostream& out, COSMMap &map)
+{
+  out << "Number of nodes: " << map.nodes.size() << std::endl;
+  for(unsigned int i=0;i<map.nodes.size();i++)
+    out << *map.nodes[i] << std::endl;
+  out << "Number of junctions: " << map.junctions.size() << std::endl;
+  for(unsigned int i=0;i<map.junctions.size();i++)
+    out << *map.junctions[i] << std::endl;
+  out << "Number of ways: " << map.ways.size() << std::endl;
+  for(unsigned int i=0;i<map.ways.size();i++)
+    out << *map.ways[i] << std::endl;
+  out << "Number of roads: " << map.segments.size() << std::endl;
+  for(unsigned int i=0;i<map.segments.size();i++)
+    out << *map.segments[i] << std::endl;
+
+  return out;
+}
+
+COSMMap::~COSMMap()
+{
+  this->free();
+}
+
diff --git a/src/osm/osm_node.cpp b/src/osm/osm_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fdc1808d74b58fcf4de37057c1066c12fc8a5e31
--- /dev/null
+++ b/src/osm/osm_node.cpp
@@ -0,0 +1,366 @@
+#include "osm/osm_node.h"
+#include "exceptions.h"
+
+#include <iostream>
+#include <sstream>
+
+#include <math.h>
+
+#include <osmium/geom/coordinates.hpp>
+#include <osmium/geom/mercator_projection.hpp>
+#include <GeographicLib/UTMUPS.hpp>
+
+COSMNode::COSMNode(const osmium::Node &node,COSMMap *parent)
+{
+  int zone;
+  bool northp;
+  double gamma,k;
+
+  this->id=node.id();
+  GeographicLib::UTMUPS::Forward(node.location().lat(),node.location().lon(),zone,northp,this->location.x,this->location.y,gamma,k,parent->get_utm_zone());
+  this->ways.clear();
+  this->junction=NULL;
+  this->restrictions.clear();
+  this->parent=parent;
+}
+
+COSMNode::COSMNode(const COSMNode &object)
+{
+  this->id=object.id;
+  this->ways=object.ways;
+  this->location.x=object.location.x;
+  this->location.y=object.location.y;
+  this->junction=object.junction;
+  this->restrictions=object.restrictions;
+  this->parent=object.parent;
+}
+
+void COSMNode::set_parent(COSMMap *parent)
+{
+  this->parent=parent;
+}
+
+void COSMNode::add_way(COSMWay *new_way)
+{
+  for(unsigned int i=0;i<this->ways.size();i++)
+    if(this->ways[i]->id==new_way->id)
+      return;
+  this->ways.push_back(new_way);
+}
+
+void COSMNode::delete_way(COSMWay *way)
+{
+  std::vector<COSMWay *>::iterator it;
+
+  for(it=this->ways.begin();it!=this->ways.end();)
+  {
+    if((*it)==way)
+      it=this->ways.erase(it);
+    else
+      it++;
+  }
+}
+
+void COSMNode::update_way(COSMWay *old_way,COSMWay *new_way)
+{
+  std::vector<COSMWay *>::iterator it;
+
+  for(it=this->ways.begin();it!=this->ways.end();++it)
+  {
+    if((*it)==old_way)
+      (*it)=new_way;
+  }
+}
+
+bool COSMNode::merge_ways(void)
+{
+  COSMWay *way1,*way2;
+
+  if(this->ways.size()==2)
+  {
+    way1=*this->ways.begin();
+    way2=*this->ways.rbegin();
+
+    if((way1->is_node_first(this->id) || way1->is_node_last(this->id)) &&
+       (way2->is_node_first(this->id) || way2->is_node_last(this->id)))
+    {
+      if(way1->get_num_forward_lanes()==way2->get_num_forward_lanes() &&
+         way1->get_num_backward_lanes()==way2->get_num_backward_lanes())
+      {
+        // merge both ways into one
+        if(way1->is_node_last(this->id) && way2->is_node_first(this->id))
+        {
+          way1->merge(way2);
+          this->parent->delete_way(way2);
+          return true;
+        }
+        else if(way1->is_node_first(this->id) && way2->is_node_last(this->id))
+        {
+          way2->merge(way1);
+          this->parent->delete_way(way1);
+          return true;
+        }
+        else
+          return false;
+      }
+      else
+        return false;
+    }
+    else
+      return false;
+  }
+  else
+    return false;
+}
+
+void COSMNode::split_ways(void)
+{
+  std::vector<COSMWay *>::iterator it;
+  std::vector<COSMWay *> new_ways;
+  COSMWay *new_way;
+
+  new_ways.clear();
+  if(this->ways.size()>1)
+  {
+    for(it=this->ways.begin();it!=this->ways.end();++it)
+    {
+      if(!(*it)->is_node_first(id) && !(*it)->is_node_last(id))
+      {
+        new_way=(*it)->split(this->id);
+        if(new_way!=NULL)
+          new_ways.push_back(new_way);
+      }
+    } 
+    for(unsigned int i=0;i<new_ways.size();i++)
+      this->add_way(new_ways[i]);
+  }
+  else
+  {
+    if(this->ways[0]->get_node_multiplicity(this->id)>1)
+    {
+
+    }
+  }
+}
+
+void COSMNode::remove_in_junction_nodes(void)
+{
+  double width,width2,dist,x,y,heading,length,start_x,start_y,end_x,end_y,cross,angle;
+  COSMNode *check_node,*node1,*node2;
+
+  if(this->ways.size()>1)
+  {
+    for(unsigned int i=0;i<this->ways.size();i++)
+    {
+      width=this->ways[i]->get_num_lanes()*this->ways[i]->get_lane_width()/2.0;
+      if(this->ways[i]->is_node_first(this->id))
+      {
+        node1=(COSMNode *)&this->ways[i]->get_segment_end_node(FIRST_SEGMENT);
+        node2=(COSMNode *)&this->ways[i]->get_segment_start_node(FIRST_SEGMENT);
+      }
+      else
+      {
+        node2=(COSMNode *)&this->ways[i]->get_segment_end_node(LAST_SEGMENT);
+        node1=(COSMNode *)&this->ways[i]->get_segment_start_node(LAST_SEGMENT);
+      }
+      node1->get_location(start_x,start_y);
+      node2->get_location(end_x,end_y);
+      length=node1->compute_distance(*node2);
+      for(unsigned int j=0;j<this->ways.size();j++)
+      {
+        if(j!=i)
+        {
+          if(this->ways[j]->is_node_first(this->id))
+            check_node=(COSMNode *)&this->ways[j]->get_segment_end_node(FIRST_SEGMENT);
+          else
+            check_node=(COSMNode *)&this->ways[j]->get_segment_start_node(LAST_SEGMENT);
+          angle=this->compute_angle(*node1,*check_node);
+          if(fabs(angle)<1.5707)
+          {
+            width2=this->ways[j]->get_num_lanes()*this->ways[j]->get_lane_width()/2.0;
+            heading=this->compute_heading(*check_node);
+            check_node->get_location(x,y);
+            x=x-width2*sin(heading);
+            y=y+width2*cos(heading);
+            cross=(x-start_x)*(end_y-start_y)-(y-start_y)*(end_x-start_x);
+            dist=fabs(cross)/length;
+            if(dist<width)
+            {
+              this->ways[j]->delete_node(check_node);
+              continue;
+            }
+            check_node->get_location(x,y);
+            x=x+width2*sin(heading);
+            y=y-width2*cos(heading);
+            cross=(x-start_x)*(end_y-start_y)-(y-start_y)*(end_x-start_x);
+            dist=fabs(cross)/length;
+            if(dist<width)
+              this->ways[j]->delete_node(check_node);
+          }
+        }
+      }
+    }
+  }
+}
+
+void COSMNode::add_restriction(COSMRestriction *new_restriction)
+{
+  for(unsigned int i=0;i<this->restrictions.size();i++)
+    if(this->restrictions[i]->id==new_restriction->id)
+      return;
+  this->restrictions.push_back(new_restriction);
+}
+
+void COSMNode::delete_restriction(COSMRestriction *restriction)
+{
+  std::vector<COSMRestriction *>::iterator it;
+
+  for(it=this->restrictions.begin();it!=this->restrictions.end();it++)
+  {
+    if((*it)==restriction)
+    {
+      this->restrictions.erase(it);
+      break;
+    }
+  }
+}
+
+void COSMNode::clear_restrictions(void)
+{
+  this->restrictions.clear();
+}
+
+void COSMNode::normalize_location(double center_x,double center_y)
+{
+  this->location.x-=center_x;
+  this->location.y-=center_y;
+}
+
+long int COSMNode::get_id(void)
+{
+  return this->id;
+}
+
+unsigned int COSMNode::get_num_ways(void)
+{
+  return this->ways.size();
+}
+
+COSMWay &COSMNode::get_way_by_index(unsigned int index)
+{
+  unsigned int i;
+  std::stringstream text;
+  std::vector<COSMWay *>::iterator it;
+
+  for(it=this->ways.begin(),i=0;it!=this->ways.end();++it,i++)
+    if(i==index)
+      return **it;
+  text << "Node with id " << this->id << " does not belong to so many ways";
+  throw CException(_HERE_,text.str());
+}
+
+COSMWay &COSMNode::get_way_by_id(long int id)
+{
+  std::stringstream text;
+
+  for(unsigned int i=0;i<this->ways.size();i++)
+    if(this->ways[i]->id==id)
+      return *this->ways[i];
+  text << "Node with id " << this-> id << " does not belong to way with id " << id;
+  throw CException(_HERE_,text.str());
+}
+
+void COSMNode::get_location(double &x, double &y)
+{
+  x=this->location.x;
+  y=this->location.y;
+}
+
+double COSMNode::compute_distance(COSMNode &node)
+{
+  double distance;
+
+  distance=sqrt(pow(this->location.x-node.location.x,2.0)+pow(this->location.y-node.location.y,2.0));
+
+  return distance;
+}
+
+double COSMNode::compute_heading(COSMNode &node)
+{
+  double heading;
+
+  heading=atan2(node.location.y-this->location.y,node.location.x-this->location.x);
+  if(heading<0.0)
+    heading+=2.0*3.14159;
+
+  return heading;
+}
+
+double COSMNode::compute_angle(COSMNode &node1,COSMNode &node2)
+{
+  double angle,cross;
+  double dist01,dist02,dist12;
+  double start_x,start_y,end_x,end_y;
+
+  dist01=this->compute_distance(node1);
+  node1.get_location(start_x,start_y);
+  dist02=this->compute_distance(node2);
+  node2.get_location(end_x,end_y);
+  dist12=node1.compute_distance(node2);
+
+  cross=(this->location.x-start_x)*(end_y-start_y)-(this->location.y-start_y)*(end_x-start_x);  
+  angle=acos((pow(dist01,2.0)+pow(dist02,2.0)-pow(dist12,2.0))/(2.0*dist01*dist02));
+  if(cross>=0.0)
+    angle=-angle;
+
+  return angle;
+}
+
+unsigned int COSMNode::get_num_restrictions(void)
+{
+  return this->restrictions.size();
+}
+
+COSMRestriction &COSMNode::get_restriction(unsigned int index)
+{
+  unsigned int i;
+  std::stringstream text;
+  std::vector<COSMRestriction *>::iterator it;
+
+  for(it=this->restrictions.begin(),i=0;it!=this->restrictions.end();++it,i++)
+    if(i==index)
+      return **it;
+  text << "Node with id " << this->id << " does not have so many restrictions";
+  throw CException(_HERE_,text.str());
+}
+
+COSMMap &COSMNode::get_parent(void)
+{
+  return *this->parent;
+}
+
+std::ostream& operator<<(std::ostream& out,COSMNode &node)
+{
+  double x,y;
+
+  out << "  id: " << node.get_id() << std::endl;
+  node.get_location(x,y);
+  out << "  location: x: " << x << " y: " << y << std::endl;
+  out << "  number of ways: " << node.get_num_ways() << std::endl;
+  for(unsigned int i=0;i<node.get_num_ways();i++)
+    out << "    way id: " << node.get_way_by_index(i).get_id() << std::endl;
+
+  return out;
+}
+
+COSMNode::~COSMNode()
+{
+  this->location.x=0.0;
+  this->location.y=0.0;
+  this->id=-1;
+  this->ways.clear();
+  this->junction=NULL;
+  this->restrictions.clear();
+  this->parent=NULL;
+}
+
diff --git a/src/osm/osm_restriction.cpp b/src/osm/osm_restriction.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ded5c665388e499d551c534038e600fb0b71e9d8
--- /dev/null
+++ b/src/osm/osm_restriction.cpp
@@ -0,0 +1,178 @@
+#include "osm/osm_restriction.h"
+#include "exceptions.h"
+
+#include <iostream>
+
+COSMRestriction::COSMRestriction(COSMWay *from,COSMNode *via,COSMWay *to,restriction_action_t action, restriction_type_t type,COSMMap *parent)
+{
+  this->parent=parent;
+  this->id=-1;
+  this->from=from;
+  this->via=via;
+  this->to=to;
+  this->action=action;
+  this->type=type;
+}
+
+COSMRestriction::COSMRestriction(const osmium::Relation &relation,COSMMap *parent)
+{
+  COSMNode *node;
+  COSMWay *way;
+  const char *value;
+  osmium::RelationMemberList::const_iterator it;
+  const osmium::RelationMemberList &members=relation.members(); 
+  bool from_missing=true,to_missing=true,via_missing=true;
+
+  for(it=members.begin();it!=members.end();++it)
+  {
+    std::string role=std::string(it->role());
+    if(role.compare("from")==0)
+    {
+      way=parent->get_way_by_id(it->ref());
+      if(way!=NULL)
+      {
+        this->from=way;
+        from_missing=false;
+      }
+      else
+        throw CException(_HERE_,"From way does not exist");
+    }
+    else if(role.compare("to")==0)
+    {
+      way=parent->get_way_by_id(it->ref());
+      if(way!=NULL)
+      {
+        this->to=way;
+        to_missing=false;
+      }
+      else
+        throw CException(_HERE_,"To way does not exist");
+    }
+    else if(role.compare("via")==0)
+    {
+      node=parent->get_node_by_id(it->ref());
+      if(node!=NULL)
+      {
+        this->via=node;
+        via_missing=false;
+      }
+      else
+        throw CException(_HERE_,"Via node does not exist");
+    }
+    else
+      throw CException(_HERE_,"Invalid restriction role");
+  }
+  if(via_missing || from_missing || to_missing)
+    throw CException(_HERE_,"Incomplete restriction from->via->to information");
+  value=relation.get_value_by_key("restriction");
+  if(value!=NULL)
+  {
+    std::string restriction_text(value);
+    std::string action=restriction_text.substr(0,restriction_text.find_first_of("_"));
+    if(action.compare("no")==0)
+      this->action=RESTRICTION_NO;
+    else if(action.compare("only")==0)
+      this->action=RESTRICTION_ONLY;
+    else
+      throw CException(_HERE_,"Unsupported restriction action");
+    std::string restriction=restriction_text.substr(restriction_text.find_first_of("_")+1);
+    if(restriction.compare("left_turn")==0)
+      this->type=RESTRICTION_LEFT_TURN;
+    else if(restriction.compare("right_turn")==0)
+      this->type=RESTRICTION_RIGHT_TURN;
+    else if(restriction.compare("u_turn")==0)
+      this->type=RESTRICTION_U_TURN;
+    else if(restriction.compare("straight_on")==0)
+      this->type=RESTRICTION_STRAIGHT_ON;
+    else
+      throw CException(_HERE_,"Unsupported restriction type");
+  }
+  else
+    throw CException(_HERE_,"Missing restriction data");
+  this->parent=parent;
+  this->id=relation.id();
+  this->from->add_restriction(this);
+  this->via->add_restriction(this);
+  this->to->add_restriction(this);
+}
+
+COSMRestriction::COSMRestriction(const COSMRestriction &object)
+{
+  this->id=object.id;
+  this->from=object.from;
+  this->to=object.to;
+  this->via=object.via;
+  this->action=object.action;
+  this->type=object.type;
+  this->parent=object.parent;
+}
+
+COSMRestriction::COSMRestriction()
+{
+  this->id=-1;
+  this->from=NULL;
+  this->to=NULL;
+  this->via=NULL;
+  this->action=RESTRICTION_NONE;
+  this->parent=NULL;
+}
+
+void COSMRestriction::set_parent(COSMMap *parent)
+{
+  this->parent=parent;
+}
+
+void COSMRestriction::update_to_way(COSMWay *old_way,COSMWay *new_way)
+{
+  if(this->to->get_id()==old_way->get_id())
+    this->to=new_way;
+}
+
+void COSMRestriction::update_from_way(COSMWay *old_way,COSMWay *new_way)
+{
+  if(this->from->get_id()==old_way->get_id())
+    this->from=new_way;
+}
+
+long int COSMRestriction::get_id(void)
+{
+  return this->id;
+}
+
+COSMWay &COSMRestriction::get_from_way(void)
+{
+  return *this->from;
+}
+
+COSMWay &COSMRestriction::get_to_way(void)
+{
+  return *this->to;
+}
+
+COSMNode &COSMRestriction::get_via_node(void)
+{
+  return *this->via;
+}
+
+restriction_action_t COSMRestriction::get_action(void)
+{
+  return this->action;
+}
+
+restriction_type_t COSMRestriction::get_type(void)
+{
+  return this->type;
+}
+
+COSMMap &COSMRestriction::get_parent(void)
+{
+  return *this->parent;
+}
+
+COSMRestriction::~COSMRestriction()
+{
+  this->from=NULL;
+  this->to=NULL;
+  this->via=NULL;
+  this->parent=NULL;
+}
diff --git a/src/osm/osm_road_segment.cpp b/src/osm/osm_road_segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7ed8da2e771be666316ba716f513c68d18d02870
--- /dev/null
+++ b/src/osm/osm_road_segment.cpp
@@ -0,0 +1,419 @@
+#include "osm/osm_road_segment.h"
+#include "exceptions.h"
+
+COSMRoadSegment::COSMRoadSegment(COSMWay *way)
+{
+  this->parent_way=way;
+  way->road_segment=this;
+  this->start_junction=NULL;
+  this->end_junction=NULL;
+  this->start_distance=0.0;
+  this->original_start_distance=0.0;
+  this->end_distance=0.0;
+  this->original_end_distance=0.0;
+}
+
+void COSMRoadSegment::set_start_distance(double dist)
+{
+  COSMNode *node1,*node2;
+  double length,start_end_distance;
+
+  if(dist>this->original_start_distance)
+    this->original_start_distance=dist;
+  if(dist>this->start_distance)
+  {
+    node1=&this->parent_way->get_segment_start_node(FIRST_SEGMENT);
+    node2=&this->parent_way->get_segment_end_node(FIRST_SEGMENT);
+    length=node1->compute_distance(*node2);
+    if(this->parent_way->get_num_nodes()==2)
+    {
+      if((length-this->end_distance-dist)<MIN_ROAD_LENGTH)
+      {
+        if(this->original_end_distance==0.0)
+          this->start_distance=length-MIN_ROAD_LENGTH;
+        else
+        {
+          start_end_distance=this->original_end_distance+dist;
+          this->end_distance=this->original_end_distance*length/start_end_distance;
+          this->end_distance-=MIN_ROAD_LENGTH/2.0;
+          this->start_distance=dist*length/start_end_distance;
+          this->start_distance-=MIN_ROAD_LENGTH/2.0;
+        }
+      }
+      else
+        this->start_distance=dist;
+    }
+    else
+    {
+      if((length-dist)<MIN_ROAD_LENGTH)
+        this->start_distance=length-MIN_ROAD_LENGTH;
+      else
+        this->start_distance=dist;
+    }
+  }
+}
+
+void COSMRoadSegment::set_end_distance(double dist)
+{ 
+  COSMNode *node1,*node2;
+  double length,start_end_distance;
+
+  if(dist>this->original_end_distance)
+    this->original_end_distance=dist;
+  if(dist>this->end_distance)
+  {
+    node1=&this->parent_way->get_segment_start_node(LAST_SEGMENT);
+    node2=&this->parent_way->get_segment_end_node(LAST_SEGMENT);
+    length=node1->compute_distance(*node2);
+    if(this->parent_way->get_num_nodes()==2)
+    {
+      if((length-this->start_distance-dist)<MIN_ROAD_LENGTH)
+      {
+        if(this->original_start_distance==0.0)
+          this->end_distance=length-MIN_ROAD_LENGTH;
+        else
+        {
+          start_end_distance=dist+this->original_start_distance;
+          this->end_distance=dist*length/start_end_distance;
+          this->end_distance-=MIN_ROAD_LENGTH/2.0;
+          this->start_distance=this->original_start_distance*length/start_end_distance;
+          this->start_distance-=MIN_ROAD_LENGTH/2.0;
+        }
+      }
+      else
+        this->end_distance=dist;
+    }
+    else
+    {
+      if((length-dist)<MIN_ROAD_LENGTH)
+        this->end_distance=length-MIN_ROAD_LENGTH;
+      else
+        this->end_distance=dist;
+    }
+  }
+}
+
+bool COSMRoadSegment::generate_fwd_geometry(CRoad *road,double resolution)
+{
+  double heading1,heading2,x,y,angle,length1,dist,prev_dist,y_offset;
+//  double angle2,angle3,prev_heading;
+  TPoint start_point,end_point;
+  COSMNode *node1,*node2,*node3;
+
+  y_offset=((double)this->parent_way->get_num_forward_lanes()-((double)this->parent_way->get_num_lanes()/2.0))*this->parent_way->get_lane_width();
+  if(this->parent_way->get_num_nodes()<2)
+    throw CException(_HERE_,"Road segment does not have enough nodes to generate a geometry");
+  prev_dist=this->start_distance;
+  for(unsigned int i=0;i<=this->parent_way->get_num_nodes()-2;i++)
+  {
+    node1=&this->parent_way->get_node_by_index(i);
+    node2=&this->parent_way->get_node_by_index(i+1);
+    length1=node1->compute_distance(*node2);
+    heading1=node1->compute_heading(*node2);
+    node1->get_location(x,y);
+    if(i==0)
+    {
+      start_point.x=x+prev_dist*cos(heading1)-y_offset*sin(heading1);
+      start_point.y=y+prev_dist*sin(heading1)+y_offset*cos(heading1);
+      start_point.heading=heading1;
+      start_point.curvature=0.0;
+      road->set_start_point(start_point);
+//      prev_heading=heading1;
+    }
+    if((i+1)==(this->parent_way->get_num_nodes()-1))// straight line
+    {
+//      angle2=atan2(y_offset,length1-this->end_distance);
+//      angle3=diff_angle(heading1,prev_heading)+angle2;
+      if((length1-this->end_distance-prev_dist)>resolution)// && fabs(angle3)<1.5707)
+      {
+        end_point.x=x+(length1-this->end_distance)*cos(heading1)-y_offset*sin(heading1);
+        end_point.y=y+(length1-this->end_distance)*sin(heading1)+y_offset*cos(heading1);    
+        end_point.heading=heading1;
+        end_point.curvature=0.0;
+        road->add_segment(end_point);
+      }
+    }
+    else 
+    {
+      node3=&this->parent_way->get_node_by_index(i+2);
+      heading2=node2->compute_heading(*node3);
+      angle=node2->compute_angle(*node1,*node3);
+      dist=(DEFAULT_MIN_RADIUS+y_offset)/fabs(tan(angle/2.0));
+      if(dist>(length1-prev_dist))
+      {
+        if(i==0)
+        {
+          node2->get_location(x,y);
+          start_point.x=x-y_offset*sin(heading2);
+          start_point.y=y+y_offset*cos(heading2);
+          start_point.heading=heading2;
+          start_point.curvature=0.0;
+          road->set_start_point(start_point);
+          prev_dist=0.0;
+//          prev_heading=heading2;
+        }
+        else if((i+2)==(this->parent_way->get_num_nodes()-1))
+        {
+          prev_dist=dist;
+//          prev_heading=heading2;
+        }
+        else
+        {
+          prev_dist=dist;
+//          prev_heading=heading1;
+        }
+        continue;
+      }
+      else if(dist<MIN_ROAD_LENGTH/2.0)
+        dist=MIN_ROAD_LENGTH/2.0;
+      if(length1-prev_dist-dist>resolution)// straight segment
+      {
+        end_point.x=x+(length1-dist)*cos(heading1)-y_offset*sin(heading1);
+        end_point.y=y+(length1-dist)*sin(heading1)+y_offset*cos(heading1);
+        end_point.heading=heading1;
+        end_point.curvature=0.0;
+        road->add_segment(end_point);
+      }
+      // arc between the two segments
+      node2->get_location(x,y);
+      end_point.x=x+dist*cos(heading2)-y_offset*sin(heading2);
+      end_point.y=y+dist*sin(heading2)+y_offset*cos(heading2);
+      end_point.heading=heading2;
+      end_point.curvature=0.0;
+      road->add_segment(end_point);
+      prev_dist=dist;
+//      prev_heading=heading1;
+    } 
+  }
+  if(road->get_num_segments()==0)
+    return false;
+  else
+    return true;
+}
+
+bool COSMRoadSegment::generate_bwd_geometry(CRoad *road,double resolution)
+{
+  double heading1,heading2,x,y,angle,length1,dist,prev_dist,y_offset;
+//  double angle2,angle3,prev_heading;
+  TPoint start_point,end_point;
+  COSMNode *node1,*node2,*node3;
+
+  y_offset=-((double)this->parent_way->get_num_forward_lanes()-((double)this->parent_way->get_num_lanes()/2.0))*this->parent_way->get_lane_width();
+  if(this->parent_way->get_num_nodes()<2)
+    throw CException(_HERE_,"Road segment does not have enough nodes to generate a geometry");
+  prev_dist=this->end_distance;
+  for(unsigned int i=this->parent_way->get_num_nodes()-1;i>=1;i--)
+  {
+    node1=&this->parent_way->get_node_by_index(i);
+    node2=&this->parent_way->get_node_by_index(i-1);
+    length1=node1->compute_distance(*node2);
+    heading1=node1->compute_heading(*node2);
+    node1->get_location(x,y);
+    if(i==(this->parent_way->get_num_nodes()-1))
+    {
+      start_point.x=x+prev_dist*cos(heading1)-y_offset*sin(heading1);
+      start_point.y=y+prev_dist*sin(heading1)+y_offset*cos(heading1);
+      start_point.heading=heading1;
+      start_point.curvature=0.0;
+      road->set_start_point(start_point);
+//      prev_heading=heading1;
+    }
+    if((i-1)==0)// straight line
+    {
+//      angle2=atan2(y_offset,length1-this->end_distance);
+//      angle3=diff_angle(heading1,prev_heading)+angle2;
+//      if(fabs(angle3)<1.5707)
+      if((length1-this->start_distance-prev_dist)>resolution)// && fabs(angle3)<1.5707)
+      {
+        end_point.x=x+(length1-this->start_distance)*cos(heading1)-y_offset*sin(heading1);
+        end_point.y=y+(length1-this->start_distance)*sin(heading1)+y_offset*cos(heading1);
+        end_point.heading=heading1;
+        end_point.curvature=0.0;
+        road->add_segment(end_point);
+      }
+    }
+    else 
+    {
+      node3=&this->parent_way->get_node_by_index(i-2);
+      heading2=node2->compute_heading(*node3);
+      angle=node2->compute_angle(*node1,*node3);
+      dist=(DEFAULT_MIN_RADIUS+y_offset)/fabs(tan(angle/2.0));
+      if(dist>(length1-prev_dist))
+      {
+        if(i==(this->parent_way->get_num_nodes()-1))
+        {
+          node2->get_location(x,y);
+          start_point.x=x-y_offset*sin(heading2);
+          start_point.y=y+y_offset*cos(heading2);
+          start_point.heading=heading2;
+          start_point.curvature=0.0;
+          road->set_start_point(start_point);
+          prev_dist=0.0;
+//          prev_heading=heading2;
+        }
+        else if((i-2)==0)
+        {
+          prev_dist=dist;
+//          prev_heading=heading2;
+        }
+        else
+        {
+          prev_dist=dist;
+//          prev_heading=heading1;
+        }
+        continue;
+      }
+      else if(dist<MIN_ROAD_LENGTH/2.0)
+        dist=MIN_ROAD_LENGTH/2.0;
+      if(length1-prev_dist-dist>resolution)// straight segment
+      {
+        end_point.x=x+(length1-dist)*cos(heading1)-y_offset*sin(heading1);
+        end_point.y=y+(length1-dist)*sin(heading1)+y_offset*cos(heading1);
+        end_point.heading=heading1;
+        end_point.curvature=0.0;
+        road->add_segment(end_point);
+      }
+      // arc between the two segments
+      node2->get_location(x,y);
+      end_point.x=x+dist*cos(heading2)-y_offset*sin(heading2);
+      end_point.y=y+dist*sin(heading2)+y_offset*cos(heading2);
+      end_point.heading=heading2;
+      end_point.curvature=0.0;
+      road->add_segment(end_point);
+      prev_dist=dist;
+//      prev_heading=heading1;
+    } 
+  }
+  if(road->get_num_segments()==0)
+    return false;
+  else
+    return true;
+}
+
+void COSMRoadSegment::convert(CRoad **left_road,CRoad **right_road,double resolution)
+{
+  double lane_width;
+  lane_restructions_t restrictions;
+  CRoadSegment *segment;
+  unsigned int num_lanes;
+
+  num_lanes=this->get_parent_way().get_num_forward_lanes();
+  lane_width=this->get_parent_way().get_lane_width();
+  if(num_lanes>0)
+  {
+    (*right_road)=new CRoad();
+    (*right_road)->set_num_lanes(num_lanes);
+    (*right_road)->set_lane_width(lane_width);
+    // generate geometry
+    if(this->generate_fwd_geometry(*right_road,resolution))
+    {
+      // generate connectivity
+      for(unsigned int i=0;i<(*right_road)->get_num_segments();i++)
+      {
+        segment=(*right_road)->get_segment_by_index(i);
+        for(unsigned int j=0;j<num_lanes;j++)
+        {
+          restrictions=this->get_parent_way().get_lane_restriction(this->get_parent_way().get_num_forward_lanes()-j-1);
+          if(restrictions&RESTRICTION_RIGHT)
+          {
+            for(unsigned int k=j+1;k<num_lanes;k++)
+              segment->unlink_lanes(j,k);
+          }
+          if(restrictions&RESTRICTION_LEFT)
+          {
+            for(unsigned int k=0;k<j;k++)
+              segment->unlink_lanes(j,k);
+          }   
+//          if(restrictions&RESTRICTION_THROUGH)
+//            segment->unlink_lanes(j,j);
+        }
+      }
+    }
+    else
+    {
+      delete (*right_road);
+      (*right_road)=NULL;
+    }
+  }
+  else
+    (*right_road)=NULL;
+  num_lanes=this->get_parent_way().get_num_backward_lanes();
+  if(num_lanes>0)
+  {
+    (*left_road)=new CRoad();
+    (*left_road)->set_num_lanes(num_lanes);
+    (*left_road)->set_lane_width(lane_width);
+    // generate geometry
+    if(this->generate_bwd_geometry(*left_road,resolution))
+    {
+      // generate connectivity
+      for(unsigned int i=0;i<(*left_road)->get_num_segments();i++)
+      {
+        segment=(*left_road)->get_segment_by_index(i);
+        for(unsigned int j=0;j<num_lanes;j++)
+        {
+          restrictions=this->get_parent_way().get_lane_restriction(j+this->get_parent_way().get_num_forward_lanes());
+          if(restrictions&RESTRICTION_RIGHT)
+          {
+            for(unsigned int k=j+1;k<num_lanes;k++)
+              segment->unlink_lanes(j,k);
+          }
+          if(restrictions&RESTRICTION_LEFT)
+          {
+            for(unsigned int k=0;k<j;k++)
+              segment->unlink_lanes(j,k);
+          }  
+//        if(restrictions&RESTRICTION_THROUGH)
+//          segment->unlink_lanes(j,j);
+        }
+      }
+    }
+    else
+    {
+      delete (*left_road);
+      (*left_road)=NULL;
+    }
+  }
+  else
+    (*left_road)=NULL;
+  if((*right_road)!=NULL && (*left_road)!=NULL)
+  {
+    (*left_road)->set_opposite_direction_road((*right_road));
+    (*right_road)->set_opposite_direction_road((*left_road));
+  }  
+}
+
+COSMWay &COSMRoadSegment::get_parent_way(void)
+{
+  return *this->parent_way;
+}
+
+double COSMRoadSegment::get_start_distance(void)
+{
+  return this->start_distance;
+}
+
+double COSMRoadSegment::get_end_distance(void)
+{
+  return this->end_distance;
+}
+
+std::ostream& operator<<(std::ostream& out, COSMRoadSegment &segment)
+{
+  out << "  ID: " << segment.parent_way->get_id() << std::endl;
+  if(segment.start_junction!=NULL)
+    out << "  start junction ID: " << segment.start_junction->get_parent_node().get_id() << std::endl;
+  else
+    out << "  NO start junction" << std::endl;
+  if(segment.end_junction!=NULL)
+    out << "  end junction ID: " << segment.end_junction->get_parent_node().get_id() << std::endl;
+  else
+    out << "  No end junction" << std::endl;
+
+  return out;
+}
+
+COSMRoadSegment::~COSMRoadSegment()
+{
+}
+
diff --git a/src/osm/osm_way.cpp b/src/osm/osm_way.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e07adfe28af8245eccd017b0eb4b6fe958dc25d8
--- /dev/null
+++ b/src/osm/osm_way.cpp
@@ -0,0 +1,803 @@
+#include "osm/osm_way.h"
+#include "exceptions.h"
+
+#include <iostream>
+#include <sstream>
+
+COSMWay::COSMWay(const osmium::Way &way,COSMMap *parent)
+{
+  static unsigned int road_id=0;
+  std::stringstream new_name;
+  osmium::NodeRefList::const_iterator it;
+  COSMNode *node;
+  const char *value;
+  unsigned int i=0;
+
+  this->id=way.id();
+  value=way.get_value_by_key("name");
+  if(value!=NULL)
+    this->name=value;
+  else
+  {
+    new_name << "road" << road_id;
+    this->name=new_name.str();
+    road_id++;
+  }
+  // get the number of lanes
+  value=way.get_value_by_key("lanes:forward");
+  if(value==NULL)
+  {
+    value=way.get_value_by_key("lanes");
+    if(value==NULL)
+    {
+      value=way.get_value_by_key("oneway");
+      if(value!=NULL)
+      {
+        if(std::string(value).compare("no")==0)
+        {
+          this->num_forward_lanes=1;
+          this->num_backward_lanes=1;
+        }
+        else
+        {
+          this->num_forward_lanes=1;
+          this->num_backward_lanes=0;
+        }
+      }
+      else
+      {
+        this->num_forward_lanes=1;
+        this->num_backward_lanes=0;
+      }
+    }
+    else
+    {
+      this->num_forward_lanes=std::stoi(std::string(value));
+      if(this->num_forward_lanes>1)
+      {
+        value=way.get_value_by_key("oneway");
+        if(value!=NULL)
+        {
+          if(std::string(value).compare("no")==0)
+          {
+            this->num_forward_lanes--;
+            this->num_backward_lanes=1;
+          }
+          else
+            this->num_backward_lanes=0;
+        }
+        else
+          this->num_backward_lanes=0;
+      }
+      else
+        this->num_backward_lanes=0;
+
+    }
+  }
+  else
+  {
+    this->num_forward_lanes=std::stoi(std::string(value));
+    value=way.get_value_by_key("lanes:backward");
+    if(value==NULL)
+      this->num_backward_lanes=0;
+    else
+      this->num_backward_lanes=std::stoi(std::string(value));
+  }
+  // get the road width
+//  value=way.get_value_by_key("width");
+//  if(value==NULL)
+//    this->lane_width=-1.0;
+//  else
+//    this->lane_width=std::stof(std::string(value))/(this->num_forward_lanes+this->num_backward_lanes);
+  this->lane_width=DEFAULT_LANE_WIDTH;
+  // get the lane constraints
+  this->load_lane_restrictions(way);
+  // create and add the way
+  const osmium::WayNodeList &nodes=way.nodes();
+  for(it=nodes.begin(),i=0;it!=nodes.end();++it,i++)
+  {
+    node=parent->get_node_by_id(it->ref());
+    if(node!=NULL)
+    {
+      this->nodes.push_back(node);
+      node->add_way(this);
+    }
+  }
+  this->road_segment=NULL;
+  this->parent=parent;
+  this->restrictions.clear();
+}
+
+COSMWay::COSMWay(const COSMWay &object)
+{
+  this->id=object.id;
+  this->name=object.name;
+  this->nodes=object.nodes;
+  this->num_forward_lanes=object.num_forward_lanes;
+  this->forward_lanes=object.forward_lanes;
+  this->num_backward_lanes=object.num_backward_lanes;
+  this->backward_lanes=object.backward_lanes;
+  this->lane_width=object.lane_width;
+  this->road_segment=object.road_segment;
+  this->parent=object.parent;
+  this->restrictions=object.restrictions;
+}
+
+bool COSMWay::load_turn_lane_tag(const osmium::Way &way,const std::string &tag,std::vector<lane_restructions_t> &restrictions,unsigned int max_lanes)
+{
+  const char *value;
+  std::size_t prev_pos=0,pos;
+  std::vector<std::string> way_restrictions;
+  std::vector<std::string> lane_restrictions;
+
+  value=way.get_value_by_key(tag.c_str());
+  if(value!=NULL)
+  {
+    restrictions.clear();
+    std::string restriction_text(value);
+    while((pos=restriction_text.find_first_of("|",prev_pos))!=std::string::npos)
+    {
+      way_restrictions.push_back(restriction_text.substr(prev_pos,pos-prev_pos));
+      prev_pos=pos+1;
+    }
+    way_restrictions.push_back(restriction_text.substr(prev_pos));
+    if(max_lanes<way_restrictions.size())
+      restrictions.resize(max_lanes,(lane_restructions_t)0);
+    else
+      restrictions.resize(way_restrictions.size(),(lane_restructions_t)0);
+    for(unsigned int i=0;i<way_restrictions.size() && i<max_lanes;i++)
+    {
+      lane_restrictions.clear();
+      prev_pos=0;
+      while((pos=way_restrictions[i].find_first_of(";",prev_pos))!=std::string::npos)
+      {
+        lane_restrictions.push_back(way_restrictions[i].substr(prev_pos,pos-prev_pos));
+        prev_pos=pos+1;
+      }
+      lane_restrictions.push_back(way_restrictions[i].substr(prev_pos));
+      for(unsigned int j=0;j<lane_restrictions.size();j++)
+      {
+        if(lane_restrictions[j].compare("through")==0)
+          restrictions[max_lanes-i-1]=(lane_restructions_t)(restrictions[max_lanes-i-1]|RESTRICTION_THROUGH);
+        else if(lane_restrictions[j].compare("left")==0)
+          restrictions[max_lanes-i-1]=(lane_restructions_t)(restrictions[max_lanes-i-1]|RESTRICTION_LEFT);
+        else if(lane_restrictions[j].compare("right")==0)
+          restrictions[max_lanes-i-1]=(lane_restructions_t)(restrictions[max_lanes-i-1]|RESTRICTION_RIGHT);
+        else if(lane_restrictions[j].compare("")==0)
+          restrictions[max_lanes-i-1]=NO_RESTRICTION;
+        else
+          throw CException(_HERE_,"Unknown lane restriction");
+      }
+    }
+    return true;
+  }
+  else
+    return false;
+}
+
+void COSMWay::load_lane_restrictions(const osmium::Way &way)
+{
+  std::vector<lane_restructions_t> restrictions;
+
+  this->forward_lanes.resize(this->num_forward_lanes,(lane_restructions_t)0);
+  this->backward_lanes.resize(this->num_backward_lanes,(lane_restructions_t)0);
+
+  if(this->load_turn_lane_tag(way,"turn:lanes",restrictions,this->num_forward_lanes))
+    this->forward_lanes=restrictions;
+  else
+  {
+    if(this->load_turn_lane_tag(way,"turn:lanes:forward",restrictions,this->num_forward_lanes))
+      this->forward_lanes=restrictions;
+    if(this->load_turn_lane_tag(way,"turn:lanes:backward",restrictions,this->num_backward_lanes))
+      this->backward_lanes=restrictions;
+  }
+}
+
+void COSMWay::set_parent(COSMMap *parent)
+{
+  this->parent=parent;
+}
+
+void COSMWay::add_node(COSMNode *new_node)
+{
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    if(this->nodes[i]->id==new_node->id)
+      return;
+  this->nodes.push_back(new_node);
+  new_node->add_way(this);
+}
+
+void COSMWay::delete_node(COSMNode *node)
+{
+  std::vector<COSMNode *>::iterator it;
+
+  if(!this->is_node_first(node->get_id()) && !this->is_node_last(node->get_id()))
+  {
+    for(it=this->nodes.begin();it!=nodes.end();it++)
+    {
+      if((*it)==node)
+      {
+        this->nodes.erase(it);
+        this->parent->delete_node(node);
+        break;
+      }
+    }
+  }
+}
+
+void COSMWay::update_node(COSMNode *old_node,COSMNode *new_node)
+{
+  std::vector<COSMNode *>::iterator it;
+
+  for(it=this->nodes.begin();it!=this->nodes.end();++it)
+  {
+    if((*it)==old_node)
+      (*it)=new_node;
+  }
+}
+
+bool COSMWay::is_node_first(long int id)
+{
+  if(this->nodes[0]->id==id)
+    return true;
+  else
+    return false;
+}
+
+bool COSMWay::is_node_last(long int id)
+{
+  if(this->nodes[this->nodes.size()-1]->id==id)
+    return true;
+  else
+    return false;
+}
+
+bool COSMWay::is_node_left(COSMNode &node,bool forward,double tol)
+{
+  COSMNode *start_node,*end_node;
+  unsigned int closest_seg;
+  double angle;
+
+  closest_seg=this->get_closest_segment(node);
+  if(forward)
+  {
+    start_node=&this->get_segment_start_node(closest_seg);
+    end_node=&this->get_segment_end_node(closest_seg);
+    angle=end_node->compute_angle(*start_node,node);
+    if(angle<0.0 && fabs(angle)<(3.14159-tol) && fabs(angle)>tol)
+      return true;
+    else
+      return false;
+  }
+  else
+  {
+    start_node=&this->get_segment_start_node(closest_seg);
+    end_node=&this->get_segment_end_node(closest_seg);
+    angle=start_node->compute_angle(*end_node,node);
+    if(angle<0.0 && fabs(angle)<(3.14159-tol) && fabs(angle)>tol)
+      return true;
+    else
+      return false;
+  }
+}
+
+bool COSMWay::is_node_right(COSMNode &node,bool forward,double tol)
+{
+  COSMNode *start_node,*end_node;
+  unsigned int closest_seg;
+  double angle;
+ 
+  closest_seg=this->get_closest_segment(node);
+  if(forward)
+  {
+    start_node=&this->get_segment_start_node(closest_seg);
+    end_node=&this->get_segment_end_node(closest_seg);
+    angle=end_node->compute_angle(*start_node,node);
+    if(angle>0.0 && fabs(angle)<(3.14159-tol) && fabs(angle)>tol)
+      return true;
+    else
+      return false;
+  }
+  else
+  {
+    start_node=&this->get_segment_start_node(closest_seg);
+    end_node=&this->get_segment_end_node(closest_seg);
+    angle=start_node->compute_angle(*end_node,node);
+    if(angle>0.0 && fabs(angle)<(3.14159-tol) && fabs(angle)>tol)
+      return true;
+    else
+      return false;
+  }
+}
+
+void COSMWay::remove_straight_nodes(void)
+{
+  std::vector<COSMNode *>::iterator it;
+  unsigned int i;
+  double angle;
+
+  if(this->nodes.size()>2)
+  {
+    it=this->nodes.begin();
+    it++;
+    for(i=0;i<this->nodes.size()-2;)
+    {
+      if(this->get_node_multiplicity(this->nodes[i+1]->id)==1)
+      {
+        angle=this->nodes[i+1]->compute_angle(*this->nodes[i],*this->nodes[i+2]);
+        if(fabs(angle)>=(3.14159-0.01))// straight line
+        {
+//          std::cout << "erasing node (ID " << (*it)->id << " of way ID " << this->id << " because it is aligned with the previuous and next ones" << std::endl;
+          this->parent->delete_node(*it);
+          it=this->nodes.erase(it);
+        }
+        else
+        {
+          it++;
+          i++;
+        }
+      }
+      else
+      {
+        it++;
+        i++;
+      }
+    }
+  }
+}
+
+void COSMWay::add_restriction(COSMRestriction *new_restriction)
+{
+  for(unsigned int i=0;i<this->restrictions.size();i++)
+    if(this->restrictions[i]->id==new_restriction->id)
+      return;
+  this->restrictions.push_back(new_restriction);
+}
+
+void COSMWay::delete_restriction(COSMRestriction *restriction)
+{
+  std::vector<COSMRestriction *>::iterator it;
+
+  for(it=this->restrictions.begin();it!=this->restrictions.end();it++)
+  {
+    if((*it)==restriction)
+    {
+      this->restrictions.erase(it);
+      break;
+    }
+  }
+}
+
+void COSMWay::delete_restriction(long int id)
+{
+  std::vector<COSMRestriction *>::iterator it;
+
+  for(it=this->restrictions.begin();it!=this->restrictions.end();)
+  {
+    if((*it)->get_via_node().get_id()==id)
+      it=this->restrictions.erase(it);
+    else
+      it++;
+  }
+}
+
+void COSMWay::update_restriction(COSMWay *old_way,COSMWay *new_way)
+{
+  std::vector<COSMRestriction *>::const_iterator it;
+  COSMRestriction *new_restriction=NULL;
+  bool no_u_turn=true;
+
+  for(it=this->restrictions.begin();it!=this->restrictions.end();)
+  {
+    if(!this->has_node((*it)->get_via_node().get_id()))
+      it=this->restrictions.erase(it);
+    else
+      it++;
+  }
+  for(it=this->restrictions.begin();it!=this->restrictions.end();it++)
+  {
+    if(this->id==old_way->get_id())
+    {
+      if(!((*it)->get_action()==RESTRICTION_NO && (*it)->get_type()==RESTRICTION_U_TURN))
+      {
+        if((*it)->get_to_way().get_id()==old_way->get_id())
+          (*it)->update_to_way(old_way,new_way);
+      }
+      else
+        no_u_turn=false;
+    }
+    else if(this->id==new_way->get_id())
+    {
+      if((*it)->get_action()==RESTRICTION_NO && (*it)->get_type()==RESTRICTION_U_TURN)
+      {
+        (*it)->update_from_way(old_way,new_way);
+        (*it)->update_to_way(old_way,new_way);
+        no_u_turn=false;
+      }
+      else
+      {
+        if((*it)->get_from_way().get_id()==old_way->get_id())
+          (*it)->update_from_way(old_way,new_way);
+      }
+    }
+  }
+  /*
+  if(!no_u_turn)
+  {
+    new_restriction= new COSMRestriction(this,,this,RESTRICTION_NO,RESTRICTION_U_TURN,this->parent);
+  }
+  if(new_restriction!=NULL)
+  {
+    this->parent->add_restriction(new_restriction);
+    new_way->add_restriction(new_restriction);
+    new_restriction->via->add_restriction(new_restriction);
+  }
+  */
+}
+
+void COSMWay::clear_restrictions(void)
+{
+  this->restrictions.clear();
+}
+
+COSMWay *COSMWay::split(long int node_id)
+{
+  std::vector<COSMNode *>::iterator node_it,it;
+  COSMWay *new_way;
+
+  for(node_it=this->nodes.begin();node_it!=this->nodes.end();++node_it)
+  {
+    if((*node_it)->id==node_id)
+    {
+      // create a new way
+      new_way=new COSMWay(*this);
+      new_way->id=-1;
+      this->parent->add_way(new_way);
+      // update the current way
+      node_it++;
+      this->nodes.erase(node_it,this->nodes.end());
+      this->update_restriction(this,new_way);
+      // update the new way
+      for(it=new_way->nodes.begin();it!=new_way->nodes.end();++it)
+        if((*it)->id==node_id)
+        {
+          it=new_way->nodes.erase(new_way->nodes.begin(),it);
+          break;
+        }
+      for(it=new_way->nodes.begin()+1;it!=new_way->nodes.end();++it)
+        (*it)->update_way(this,new_way);
+      new_way->update_restriction(this,new_way);
+      return new_way;
+    }  
+  }
+  return NULL;
+}
+
+void COSMWay::merge(COSMWay *way)
+{
+  std::vector<COSMNode *>::iterator it;
+
+  for(it=way->nodes.begin();it!=way->nodes.end();++it)
+  {
+    if(it==way->nodes.begin())// remove the reference to the second way
+      (*it)->delete_way(way);
+    else
+      (*it)->update_way(way,this);
+  }
+  this->nodes.insert(this->nodes.end(),way->nodes.begin()+1,way->nodes.end());
+  for(unsigned int i=0;i<way->restrictions.size();i++)
+    this->add_restriction(way->restrictions[i]);
+  for(unsigned int i=0;i<this->restrictions.size();i++)
+  {
+    this->restrictions[i]->update_to_way(way,this);
+    this->restrictions[i]->update_from_way(way,this);
+  }
+}
+
+bool COSMWay::is_not_connected(void)
+{
+  for(unsigned int i=0;i<this->nodes.size();i++)
+  {
+    if(this->nodes[i]->get_num_ways()>1)
+      return false;
+  }
+  return true;
+}
+
+long int COSMWay::get_id(void)
+{
+  return this->id;
+}
+
+std::string COSMWay::get_name(void)
+{
+  return this->name;
+}
+
+unsigned int COSMWay::get_num_nodes(void)
+{
+  return this->nodes.size();
+}
+
+COSMNode &COSMWay::get_node_by_index(unsigned int index)
+{ 
+  unsigned int i;
+  std::stringstream text;
+  std::vector<COSMNode *>::iterator it;
+  
+  for(it=this->nodes.begin(),i=0;it!=this->nodes.end();++it,i++)
+    if(i==index)
+      return **it;
+  text << "Way with id " << this->id << " does not have to so many nodes";
+  throw CException(_HERE_,text.str());
+}
+
+COSMNode &COSMWay::get_next_node_by_index(unsigned int index)
+{
+  unsigned int i;
+  std::stringstream text;
+  std::vector<COSMNode *>::iterator it;
+
+  for(it=this->nodes.begin(),i=0;it!=this->nodes.end();++it,i++)
+    if(i==index)
+    {
+      if(i<this->nodes.size()-1)
+      {
+        it++;
+        return **it;
+      }
+      text << "Node at index " << index << " is the last one";
+      throw CException(_HERE_,text.str());
+    }
+  text << "Way with id " << this->id << " does not have to so many nodes";
+  throw CException(_HERE_,text.str());
+}
+
+COSMNode &COSMWay::get_prev_node_by_index(unsigned int index)
+{
+  unsigned int i;
+  std::stringstream text;
+  std::vector<COSMNode *>::iterator it;
+  
+  for(it=this->nodes.begin(),i=0;it!=this->nodes.end();++it,i++)
+    if(i==index)
+    {
+      if(i>0)
+      {
+        it--;
+        return **it;
+      }
+      text << "Node at index " << index << " is the first one";
+      throw CException(_HERE_,text.str());
+    }
+  text << "Way with id " << this->id << " does not have to so many nodes";
+  throw CException(_HERE_,text.str());
+}
+
+COSMNode &COSMWay::get_node_by_id(long int id)
+{
+  std::stringstream text;
+
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    if(this->nodes[i]->id==id)
+      return *this->nodes[i];
+  text << "Way with id " << this-> id << " does not include node with id " << id;
+  throw CException(_HERE_,text.str());
+}
+
+COSMNode &COSMWay::get_next_node_by_id(long int id)
+{
+  std::stringstream text;
+
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    if(this->nodes[i]->id==id)
+    {
+      if(i<this->nodes.size()-1)
+        return *this->nodes[i+1];
+      text << "Node with id " << id << " is the last one";
+      throw CException(_HERE_,text.str());
+    }
+  text << "Way with id " << this-> id << " does not include node with id " << id;
+  throw CException(_HERE_,text.str());
+}
+
+COSMNode &COSMWay::get_prev_node_by_id(long int id)
+{
+  std::stringstream text;
+
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    if(this->nodes[i]->id==id)
+    {
+      if(i>0)
+        return *this->nodes[i-1];
+      text << "Node with id " << id << " is the first one";
+      throw CException(_HERE_,text.str());
+    }
+  text << "Way with id " << this-> id << " does not include node with id " << id;
+  throw CException(_HERE_,text.str());
+}
+
+COSMNode &COSMWay::get_closest_node(COSMNode &node)
+{
+  unsigned int i=0,min_index=-1;
+  std::vector<COSMNode *>::iterator it;
+  double min_dist=std::numeric_limits<double>::max(),dist;
+
+  if(this->nodes.size()>0)
+  {
+    for(it=this->nodes.begin(),i=0;it!=this->nodes.end();it++,i++)
+    {
+      dist=(*it)->compute_distance(node);
+      if(dist<min_dist)
+      {
+        min_dist=dist;
+        min_index=i;
+      }
+    }
+    return *this->nodes[min_index];
+  }
+  else
+    throw CException(_HERE_,"The way has no nodes");
+}
+
+unsigned int COSMWay::get_node_multiplicity(long int id)
+{
+  unsigned int num=0;
+
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    if(this->nodes[i]->id==id)
+      num++;
+
+  return num;
+}
+
+bool COSMWay::has_node(long int id)
+{
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    if(this->nodes[i]->get_id()==id)
+      return true;
+  
+  return false;
+}
+
+unsigned int COSMWay::get_num_lanes(void)
+{
+  return this->num_forward_lanes+this->num_backward_lanes;
+}
+
+unsigned int COSMWay::get_num_forward_lanes(void)
+{
+  return this->num_forward_lanes;
+}
+
+unsigned int COSMWay::get_num_backward_lanes(void)
+{
+  return this->num_backward_lanes;
+}
+
+bool COSMWay::is_one_way(void)
+{
+  if(this->num_backward_lanes==0)
+    return true;
+  else
+    return false;
+}
+
+unsigned int COSMWay::get_num_segments(void)
+{
+  return this->nodes.size()-1;
+}
+
+COSMNode &COSMWay::get_segment_start_node(unsigned int index)
+{
+  std::stringstream text;
+
+  if(index>this->nodes.size()-2 && index!=(unsigned int)-1)
+  {
+    text << "Way with id " << this->id << " does not have to so many segments";
+    throw CException(_HERE_,text.str());
+  }
+  if(index==(unsigned int)-1)
+    return *this->nodes[this->nodes.size()-2];
+  else
+    return *this->nodes[index];
+}
+
+COSMNode &COSMWay::get_segment_end_node(unsigned int index)
+{
+  std::stringstream text;
+
+  if(index>this->nodes.size()-2 && index!=(unsigned int)-1)
+  { 
+    text << "Way with id " << this->id << " does not have to so many segments";
+    throw CException(_HERE_,text.str());
+  }
+  if(index==(unsigned int)-1)
+    return *this->nodes[this->nodes.size()-1];
+  else
+    return *this->nodes[index+1];
+}
+
+unsigned int COSMWay::get_closest_segment(COSMNode &node)
+{
+  unsigned int i=0,min_index=-1;
+  std::vector<COSMNode *>::iterator it;
+  double min_dist=std::numeric_limits<double>::max(),dist,dist_next,dist_prev;
+
+  if(this->nodes.size()>0)
+  {
+    for(it=this->nodes.begin(),i=0;it!=this->nodes.end();it++,i++)
+    {
+      dist=(*it)->compute_distance(node);
+      if(dist<min_dist)
+      {
+        min_dist=dist;
+        min_index=i;
+      }
+    }
+    if(min_index==0)
+      return 0;
+    else if(min_index==this->nodes.size()-1)
+      return this->nodes.size()-2;
+    else
+    {
+      dist_next=this->nodes[min_index+1]->compute_distance(node);
+      dist_prev=this->nodes[min_index-1]->compute_distance(node);
+      if(dist_prev<dist_next)
+        return min_index-1;
+      else
+        return min_index;
+    }
+  }
+  else
+    throw CException(_HERE_,"The way has no nodes");
+}
+
+lane_restructions_t COSMWay::get_lane_restriction(unsigned int index)
+{
+  if(index>this->get_num_lanes()-1)
+    throw CException(_HERE_,"The was does not have so many lanes");
+  if(index<this->num_forward_lanes)
+    return this->forward_lanes[index];
+  else
+    return this->backward_lanes[index-this->num_forward_lanes];
+}
+
+double COSMWay::get_lane_width(void)
+{
+  if(this->lane_width==-1.0)
+    return DEFAULT_LANE_WIDTH;
+  else
+    return this->lane_width;
+}
+
+COSMRoadSegment &COSMWay::get_road_segment(void)
+{
+  return *this->road_segment;
+}
+
+COSMMap &COSMWay::get_parent(void)
+{
+  return *this->parent;
+}
+
+std::ostream& operator<<(std::ostream& out, COSMWay &way)
+{
+  out << "  id: " << way.get_id() << std::endl;
+  out << "  num. forward lanes: " << way.get_num_forward_lanes() << std::endl;
+  out << "  num. backward lanes: " << way.get_num_backward_lanes() << std::endl;
+  out << "  lane width: " << way.get_lane_width() << std::endl;
+  out << "  number of nodes: " << way.get_num_nodes() << std::endl;
+  for(unsigned int i=0;i<way.get_num_nodes();i++)
+    out << "    node id: " << way.get_node_by_index(i).get_id() << std::endl;
+
+  return out;
+}
+
+COSMWay::~COSMWay()
+{
+  this->nodes.clear();
+  this->num_forward_lanes=0;
+  this->num_backward_lanes=0;
+}
+
diff --git a/src/road.cpp b/src/road.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c7c1f090c90e352be6962be07a7a1ae616db399a
--- /dev/null
+++ b/src/road.cpp
@@ -0,0 +1,455 @@
+#include "road.h"
+#include "exceptions.h"
+
+#include <limits>
+
+CRoad::CRoad()
+{
+  this->id=-1;
+  this->resolution=0.1;
+  this->parent_roadmap=NULL;
+  this->prev_junction=NULL;
+  this->next_junction=NULL;
+  this->opposite_direction_road=NULL;
+  this->segments.clear();
+  this->num_lanes=0;
+  this->lane_width=0.0;
+  this->lane_speed=0.0;
+  this->start_point.x=std::numeric_limits<double>::max();
+}
+
+CRoad::CRoad(unsigned int id)
+{
+  this->set_id(id);
+  this->resolution=0.1;
+  this->parent_roadmap=NULL;
+  this->prev_junction=NULL;
+  this->next_junction=NULL;
+  this->opposite_direction_road=NULL;
+  this->segments.clear();
+  this->num_lanes=0;
+  this->lane_width=0.0;
+  this->lane_speed=0.0;
+  this->start_point.x=std::numeric_limits<double>::max();
+}
+
+void CRoad::set_id(unsigned int id)
+{
+  this->id=id;
+}
+
+unsigned int CRoad::get_index_by_id(const std::vector<CRoad *> &roads,unsigned int id)
+{
+  std::vector<CRoad *>::const_iterator it;
+  unsigned int index;
+
+  for(it=roads.begin(),index=0;it!=roads.end();it++,index++)
+    if((*it)->id==id)
+      return index;
+
+  return -1;
+}
+
+unsigned int CRoad::get_id(void)
+{
+  return this->id;
+}
+
+CRoad::~CRoad()
+{
+  this->id=-1;
+  this->parent_roadmap=NULL;
+  this->prev_junction=NULL;
+  this->next_junction=NULL;
+  this->opposite_direction_road=NULL;
+  for(unsigned int i=0;i<this->segments.size();i++)
+  {
+    if(this->segments[i]!=NULL)
+    {
+      delete this->segments[i];
+      this->segments[i]=NULL;
+    }
+  }
+  this->segments.clear();
+}
+
+/* connectivity */
+void CRoad::set_parent_roadmap(CRoadMap *roadmap)
+{
+  if(roadmap==NULL)
+    throw CException(_HERE_,"Invalid roadmap reference");
+  this->parent_roadmap=roadmap;
+}
+
+void CRoad::set_prev_junction(CJunction *junction)
+{
+  this->prev_junction=junction;
+}
+
+void CRoad::set_next_junction(CJunction *junction)
+{
+  this->next_junction=junction;
+}
+
+void CRoad::set_opposite_direction_road(CRoad *road)
+{
+  if(road==NULL)
+    throw CException(_HERE_,"Invalid opposite direction road reference");
+  this->opposite_direction_road=road;
+}
+
+/* geometry */
+CRoadSegment *CRoad::get_first_segment(void)
+{
+  if(this->segments.size()==0)
+    throw CException(_HERE_,"Road has no segments");
+  return this->segments[0];
+}
+
+CRoadSegment *CRoad::get_last_segment(void)
+{
+  if(this->segments.size()==0)
+    throw CException(_HERE_,"Road has no segments");
+  return this->segments[this->segments.size()-1];
+}
+
+void CRoad::set_resolution(double resolution)
+{
+  this->resolution=resolution;
+  for(unsigned int i=0;i<this->segments.size();i++)
+    this->segments[i]->set_resolution(resolution);
+}
+
+double CRoad::get_resolution(void)
+{
+  return this->resolution;
+}
+
+CRoadMap &CRoad::get_parent_roadmap(void)
+{
+  if(this->parent_roadmap==NULL)
+    throw CException(_HERE_,"No parent road map has been defined");
+  return *this->parent_roadmap;
+}
+
+bool CRoad::exist_prev_junction(void)
+{
+  if(this->prev_junction!=NULL)
+    return true;
+  else
+    return false;
+}
+
+CJunction &CRoad::get_prev_junction(void)
+{
+  if(this->prev_junction==NULL)
+    throw CException(_HERE_,"No previous junction has been defined");
+  return *this->prev_junction;
+}
+
+bool CRoad::exist_next_junction(void)
+{
+  if(this->next_junction!=NULL)
+    return true;
+  else
+    return false;
+}
+
+CJunction &CRoad::get_next_junction(void)
+{
+  if(this->next_junction==NULL)
+    throw CException(_HERE_,"No next junction has been defined");
+  return *this->next_junction;
+}
+
+bool CRoad::has_opposite_direction_road(void)
+{
+  if(this->opposite_direction_road!=NULL)
+    return true;
+  else
+    return false;
+}
+
+CRoad &CRoad::get_opposite_direction_road(void)
+{
+  if(this->opposite_direction_road==NULL)
+    throw CException(_HERE_,"No opposite direction road has been defined");
+  return *this->opposite_direction_road;
+}
+
+/* geometry */
+void CRoad::set_num_lanes(unsigned int num)
+{
+  this->num_lanes=num;
+}
+
+unsigned int CRoad::get_num_lanes(void)
+{
+  return this->num_lanes;
+}
+
+void CRoad::set_lane_width(double width)
+{
+  if(width<0.0)
+    throw CException(_HERE_,"Lane width can not be negative");
+  this->lane_width=width;
+}
+
+double CRoad::get_lane_width(void)
+{
+  return this->lane_width;
+}
+
+void CRoad::set_lane_speed(double speed)
+{
+  if(speed<0.0)
+    throw CException(_HERE_,"Lane speed can not be negative");
+  this->lane_speed=speed;
+}
+
+double CRoad::get_lane_speed(void)
+{
+  return this->lane_speed;
+}
+
+
+unsigned int CRoad::get_num_segments(void)
+{
+  return this->segments.size();
+}
+
+CRoadSegment *CRoad::get_segment_by_index(unsigned int index)
+{
+  if(index<0 || index >= this->segments.size())
+    throw CException(_HERE_,"Invalid segment index");
+  return this->segments[index];
+}
+
+CRoadSegment &CRoad::get_segment_by_id(unsigned int id)
+{
+  unsigned int index;
+  
+  index=CRoadSegment::get_index_by_id(this->segments,id);
+  return *this->get_segment_by_index(index);
+}
+
+void CRoad::set_start_point(TPoint &start_point)
+{
+  TPoint end_point;
+
+  this->start_point=start_point;
+  if(this->segments.size()>0)
+  {
+    this->segments[0]->get_end_point(end_point);
+    this->segments[0]->set_geometry(this->start_point,end_point);
+  }
+}
+
+void CRoad::set_start_point(double x,double y,double heading, double curvature)
+{
+  TPoint point;
+
+  point.x=x;
+  point.y=y;
+  point.heading=heading;
+  point.curvature=curvature;
+  this->set_start_point(point);
+}
+
+void CRoad::add_segment(TPoint &end_point)
+{
+  CRoadSegment *new_segment;
+  TPoint start_point;
+
+  if(this->start_point.x==std::numeric_limits<double>::max())
+    throw CException(_HERE_,"The start position has not been defined");
+  if(this->num_lanes<1)
+    throw CException(_HERE_,"Road has no lanes");
+  if(this->segments.size()==0)
+    start_point=this->start_point;
+  else 
+    this->segments[this->segments.size()-1]->get_end_point(start_point);
+  new_segment=new CRoadSegment(this->num_lanes,this->num_lanes);
+  new_segment->set_resolution(this->resolution);
+  new_segment->set_geometry(start_point,end_point);
+  new_segment->set_parent_road(this);
+  if(this->segments.size()>0)
+  {
+    this->segments[this->segments.size()-1]->add_next_segment(new_segment);
+    new_segment->add_prev_segment(this->segments[this->segments.size()-1]);
+    if(this->parent_roadmap!=NULL)
+      new_segment->id=this->parent_roadmap->get_next_segment_id();
+  }
+  new_segment->set_full_connectivity();
+  this->segments.push_back(new_segment);
+}
+
+void CRoad::add_segment(double x,double y,double heading, double curvature)
+{
+  TPoint point;
+
+  point.x=x;
+  point.y=y;
+  point.heading=heading;
+  point.curvature=curvature;
+  this->add_segment(point);
+}
+
+void CRoad::remove_segment_by_index(unsigned int index)
+{
+  std::vector<CRoadSegment *>::iterator it;
+  TPoint end_point,start_point;
+  unsigned int i;
+
+  if(index<0 || index >= this->segments.size())
+    throw CException(_HERE_,"Invalid segment index");
+  if(this->segments.size()==1)
+    throw CException(_HERE_,"Thr road will have no geometry after removing the segment");
+  for(it=this->segments.begin(),i=0;it!=this->segments.end();it++,i++)
+  {
+    if(index==i)
+    {
+      (*it)->get_end_point(end_point);
+      for(unsigned int i=0;i<(*it)->get_num_prev_segments();i++)
+      {
+        (*it)->prev_segments[i]->get_start_point(start_point);
+        (*it)->prev_segments[i]->set_geometry(start_point,end_point);
+        (*it)->prev_segments[i]->next_segments=(*it)->next_segments;
+      }
+      for(unsigned int i=0;i<(*it)->get_num_next_segments();i++)
+        (*it)->next_segments[i]->prev_segments=(*it)->prev_segments;
+      delete *it;
+      this->segments.erase(it);
+      break;
+    }
+  }
+}
+
+void CRoad::merge(CRoad *road)
+{
+  TPoint end_point;
+
+  if(road==NULL)
+    throw CException(_HERE_,"Invalid road reference");
+  for(unsigned int i=0;i<road->get_num_segments();i++)
+  {
+    road->segments[i]->get_end_point(end_point);
+    this->add_segment(end_point);
+    this->get_last_segment()->connectivity=road->segments[i]->connectivity;
+  } 
+}
+
+bool CRoad::get_point_at(double distance, double lateral_offset,TPoint &point)
+{
+  for(unsigned int i=0;i<this->segments.size();i++)
+  {
+    if(this->segments[i]->get_point_at(distance,lateral_offset,point))
+      return true;
+    else
+      distance-=this->segments[i]->get_length();
+  }
+  return false;
+}
+
+bool CRoad::get_closest_point(TPoint &target_point,TPoint &closest_point,double &distance,double &lateral_offset,double angle_tol)
+{
+  double min_offset=std::numeric_limits<double>::max(),tmp_offset,tmp_distance;
+  TPoint tmp_point;
+
+  for(unsigned int i=0;i<this->segments.size();i++)
+  {
+    if(this->segments[i]->get_closest_point(target_point,tmp_point,tmp_distance,tmp_offset,angle_tol))
+    {
+      if(fabs(tmp_offset)<min_offset)
+      {
+        min_offset=fabs(tmp_offset);
+        closest_point=tmp_point;
+        distance=tmp_distance;
+        lateral_offset=tmp_offset;
+      }
+    } 
+  }
+  if(min_offset!=std::numeric_limits<double>::max())
+    return true;
+  else
+    return false;
+}
+
+bool CRoad::get_closest_segment_id(TPoint &point,unsigned int &segment_id,double &distance,double &lateral_offset,double angle_tol)
+{
+  double min_offset=std::numeric_limits<double>::max(),tmp_offset,tmp_distance;
+  TPoint tmp_point;
+
+  for(unsigned int i=0;i<this->segments.size();i++)
+  { 
+    if(this->segments[i]->get_closest_point(point,tmp_point,tmp_distance,tmp_offset,angle_tol))
+    {
+      if(fabs(tmp_offset)<min_offset)
+      {
+        min_offset=fabs(tmp_offset);
+        distance=tmp_distance;
+        lateral_offset=tmp_offset;
+        segment_id=this->segments[i]->get_id();
+      }
+    }
+  }
+  if(min_offset!=std::numeric_limits<double>::max())
+    return true;
+  else
+    return false;
+}
+
+bool CRoad::get_segment_id_at(double distance,unsigned int &segment_id)
+{
+  for(unsigned int i=0;i<this->segments.size();i++)
+  {
+    if(distance<this->segments[i]->get_length())
+    {
+      segment_id=this->segments[i]->get_length();
+      return true;
+    }
+    else
+      distance-=this->segments[i]->get_length();
+  }
+  return false;
+}
+
+void CRoad::get_segment_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw)
+{
+  std::vector<double> partial_x,partial_y,partial_heading;
+
+  x.clear();
+  y.clear();
+  yaw.clear();
+  for(unsigned int i=0;i<this->segments.size();i++)
+  {
+    this->segments[i]->get_geometry(partial_x,partial_y,partial_heading);
+    x.insert(x.end(),partial_x.begin(),partial_x.end());
+    y.insert(y.end(),partial_y.begin(),partial_y.end());
+    yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end());
+  }
+}
+
+void CRoad::get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw)
+{
+  std::vector<double> partial_x,partial_y,partial_heading;
+  double lateral_offset;
+
+  x.clear();
+  y.clear();
+  yaw.clear();
+  for(unsigned int i=0;i<this->segments.size();i++)
+  {
+    lateral_offset=-this->lane_width/2.0;
+    for(unsigned int j=0;j<this->num_lanes;j++)
+    {
+      this->segments[i]->get_geometry(partial_x,partial_y,partial_heading,lateral_offset);
+      x.insert(x.end(),partial_x.begin(),partial_x.end());
+      y.insert(y.end(),partial_y.begin(),partial_y.end());
+      yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end());
+      lateral_offset-=this->lane_width;
+    }
+  }
+}
+
+
diff --git a/src/road_map.cpp b/src/road_map.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dd9e4bb5e419032536bfee23ac818834ba085844
--- /dev/null
+++ b/src/road_map.cpp
@@ -0,0 +1,893 @@
+#include "road_map.h"
+#include "exceptions.h"
+
+#include "opendrive/opendrive_road_segment.h"
+#include "opendrive/opendrive_junction.h"
+
+#include "osm/osm_map.h"
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <chrono>
+#include <fstream>
+
+CRoadMap::CRoadMap()
+{
+  this->resolution=0.1;
+  this->next_segment_id=0;
+  this->next_road_id=0;
+  this->next_junction_id=0;
+}
+
+void CRoadMap::free(void)
+{
+  for(unsigned int i=0;i<this->roads.size();i++)
+    if(this->roads[i]!=NULL)
+    {
+      delete this->roads[i];
+      this->roads[i]=NULL;
+    }
+  this->roads.clear();
+  for(unsigned int i=0;i<this->junctions.size();i++)
+    if(this->junctions[i]!=NULL)
+    {
+      delete this->junctions[i];
+      this->junctions[i]=NULL;
+    }
+  this->junctions.clear();
+  this->next_segment_id=0;
+  this->next_road_id=0;
+  this->next_junction_id=0;
+}
+
+unsigned int CRoadMap::get_next_segment_id(void)
+{
+  this->next_segment_id++;
+
+  return this->next_segment_id-1;
+}
+
+unsigned int CRoadMap::get_next_road_id(void)
+{
+  this->next_road_id++;
+
+  return this->next_road_id-1;
+}
+
+unsigned int CRoadMap::get_next_junction_id(void)
+{
+  this->next_junction_id++;
+
+  return this->next_junction_id-1;
+}
+
+CRoad *CRoadMap::get_road_by_index(unsigned int index)
+{
+  if(index<0 || index >= this->roads.size())
+    throw CException(_HERE_,"Invalid road index");
+  return this->roads[index];
+}
+
+CJunction *CRoadMap::get_junction_by_index(unsigned int index)
+{
+  if(index<0 || index >= this->junctions.size())
+    throw CException(_HERE_,"Invalid junctions index");
+  return this->junctions[index];
+}
+
+void CRoadMap::get_segment_by_id(unsigned int id,CRoadSegment **segment) 
+{
+  for(unsigned int i=0;i<this->roads.size();i++)
+    for(unsigned int j=0;j<this->roads[i]->get_num_segments();j++)
+      if(this->roads[i]->segments[j]->get_id()==id)
+      {
+        (*segment)=this->roads[i]->segments[j];
+        return;
+      }
+  for(unsigned int i=0;i<this->junctions.size();i++)
+    for(unsigned int j=0;j<this->junctions[i]->get_num_segments();j++)
+      if(this->junctions[i]->segments[j]->get_id()==id)
+      {
+        (*segment)=this->junctions[i]->segments[j];
+        return;
+      }
+  throw CException(_HERE_,"No segment with the given id"); 
+}
+
+void CRoadMap::remove_road_by_index(unsigned int index)
+{
+  std::vector<CRoad *>::iterator it;
+  unsigned int i;
+
+  if(index<0 || index >= this->roads.size())
+    throw CException(_HERE_,"Invalid road index");
+  for(it=this->roads.begin(),i=0;it!=this->roads.end();it++,i++)
+  {
+    if(i==index)
+    {
+      /* update connectivity information */
+      if((*it)->exist_prev_junction())
+        (*it)->prev_junction->remove_outgoing_road_by_id((*it)->id);
+      if((*it)->exist_next_junction())
+        (*it)->next_junction->remove_incomming_road_by_id((*it)->id);
+      delete *it;
+      this->roads.erase(it);
+      break;
+    }
+  }
+}
+
+void CRoadMap::set_opposite_direction_roads_by_index(unsigned int right_road,unsigned int left_road)
+{
+  CRoad *left,*right;
+  CRoadSegment *left_segment,*right_segment;
+  TPoint left_point,right_point;
+
+  if(right_road<0 || right_road >= this->roads.size())
+    throw CException(_HERE_,"Invalid right road index");
+  if(left_road<0 || left_road >= this->roads.size())
+    throw CException(_HERE_,"Invalid left road index");
+
+  left=this->get_road_by_index(left_road);
+  right=this->get_road_by_index(right_road);
+  // check geometry
+  if(left->get_num_segments()!=right->get_num_segments())
+    throw CException(_HERE_,"The number of segments in both roads do not match");
+  for(unsigned int i=0;i<right->get_num_segments();i++)
+  {
+    left_segment=left->get_segment_by_index(left->get_num_segments()-1-i);
+    left_segment->get_start_point(left_point);
+    right_segment=right->get_segment_by_index(i);
+    right_segment->get_end_point(right_point);
+    if(sqrt(pow(right_point.x-left_point.x,2.0)+pow(right_point.y-left_point.y,2.0))>this->resolution)
+      throw CException(_HERE_,"Intermediate geometry points do not match");
+  }
+  left->set_opposite_direction_road(right);
+  right->set_opposite_direction_road(left);
+}
+
+void CRoadMap::remove_junction_by_index(unsigned int index)
+{
+  std::vector<CJunction *>::iterator it;
+  unsigned int i;
+
+  if(index<0 || index >= this->junctions.size())
+    throw CException(_HERE_,"Invalid junction index");
+  for(it=this->junctions.begin(),i=0;it!=this->junctions.end();it++,i++)
+  { 
+    if(i==index)
+    { 
+      /* update connectivity information */
+      for(unsigned int i=0;i<(*it)->get_num_incomming_roads();i++)
+        (*it)->get_incomming_road_by_index(i)->set_next_junction(NULL);  
+      for(unsigned int i=0;i<(*it)->get_num_outgoing_roads();i++)
+        (*it)->get_outgoing_road_by_index(i)->set_prev_junction(NULL);  
+      delete *it;
+      this->junctions.erase(it);
+      break;
+    }
+  }
+}
+
+void CRoadMap::merge_roads(opendrive_road_map_t &road_map)
+{
+  CRoad *del_road;
+
+  for(opendrive_road_map_t::iterator it=road_map.begin();it!=road_map.end();it++)
+  {
+    if(it->first->has_successor())
+    {
+      if(it->first->is_successor_road())
+      {
+        for(opendrive_road_map_t::iterator it2=road_map.begin();it2!=road_map.end();it2++)
+        {
+          if(it2->first->get_id()==it->first->get_successor_id())
+          {
+            if(it->second.size()>0 && it2->second.size()>0)
+            {
+              (it->second)[0]->merge((it2->second)[0]);
+              del_road=(it2->second)[0];
+              delete (it2->second)[0];
+              for(opendrive_road_map_t::iterator it3=road_map.begin();it3!=road_map.end();it3++)
+                if((it3->second)[0]==del_road)
+                  (it3->second)[0]=(it->second)[0];
+            }
+            if(it->second.size()>1 && it2->second.size()>1)
+            {
+              (it2->second)[1]->merge((it->second)[1]);
+              del_road=(it->second)[1];
+              delete (it->second)[1];
+              for(opendrive_road_map_t::iterator it3=road_map.begin();it3!=road_map.end();it3++)
+                if((it3->second)[1]==del_road)
+                  (it3->second)[1]=(it2->second)[1];
+              (it2->second)[1]->set_opposite_direction_road((it->second)[0]);
+              (it2->second)[0]->set_opposite_direction_road((it->second)[1]);
+            }
+            break;
+          }
+        }
+      }
+    }
+  }
+}
+
+void CRoadMap::load_opendrive(const std::string &filename)
+{
+  struct stat buffer;
+  COpendriveRoadSegment *segment;
+  COpendriveJunction *junction;
+  CRoad *left_road,*right_road;
+  CJunction *new_junction;
+  opendrive_road_map_t road_map;
+  opendrive_map_pair_t map_pair;
+  std::vector<CRoad *> segment_roads;
+
+  this->free();
+  if(stat(filename.c_str(),&buffer)==0)
+  {
+    try{
+      std::unique_ptr<OpenDRIVE> open_drive(OpenDRIVE_(filename.c_str(), xml_schema::flags::dont_validate));
+      /* load roads */
+      for (OpenDRIVE::road_iterator road_it(open_drive->road().begin()); road_it != open_drive->road().end(); ++road_it)
+      {
+        try{
+          segment=new COpendriveRoadSegment();
+          segment->load(*road_it);
+          if(!segment->is_junction())
+          {
+            segment->convert(&left_road,&right_road,this->resolution);
+            segment_roads.clear();
+            if(right_road!=NULL)
+              segment_roads.push_back(right_road);
+            if(left_road!=NULL)
+              segment_roads.push_back(left_road);
+            map_pair=std::make_pair(segment,segment_roads);
+            road_map.push_back(map_pair);
+          }
+        }catch(CException &e){
+          for(opendrive_road_map_t::iterator it=road_map.begin();it!=road_map.end();it++)
+          {
+            delete it->first;
+            for(unsigned int i=0;i<it->second.size();i++)
+              delete it->second[i];
+          }
+          std::cout << e.what() << std::endl;
+        }
+      }
+      /* merge consecutive roads */
+      this->merge_roads(road_map);
+      /* add all roads */
+      for(opendrive_road_map_t::iterator it=road_map.begin();it!=road_map.end();it++)
+      {
+        for(unsigned int i=0;i<it->second.size();i++)
+        {
+          try{
+            this->add_road((it->second)[i]);
+          }catch(CException &e){
+          }
+        }
+      }
+      /* load junctions */
+      for (OpenDRIVE::junction_iterator junction_it(open_drive->junction().begin()); junction_it != open_drive->junction().end(); ++junction_it)
+      {
+        try{
+          junction=new COpendriveJunction();
+          junction->load(*junction_it);
+          junction->convert(&new_junction,road_map);
+          if(new_junction!=NULL)
+            this->add_junction(new_junction);
+          delete junction;
+        }catch(CException &e){
+          std::cout << e.what() << std::endl;
+        }
+      }
+      /* free all opendrive segments */
+      for(unsigned int i=0;i<road_map.size();i++)
+        delete road_map[i].first;
+    }catch (const xml_schema::exception& e){
+      this->free();
+      std::ostringstream os;
+      os << e;
+      /* handle exceptions */
+      throw CException(_HERE_,os.str());
+    }
+  }
+  else
+    throw CException(_HERE_,"The .xodr file does not exist");
+}
+
+void CRoadMap::save_opendrive(const std::string &filename)
+{
+  std::vector<CRoad *> processed_roads;
+  opendrive_inverse_map_pair_t map_pair;
+  opendrive_road_inverse_map_t road_map;
+  CRoad *opposite_road;
+  std::vector<COpendriveRoadSegment *> junction_segments,segments;
+  COpendriveJunction *junction;
+  xml_schema::namespace_infomap map;
+  ::header opendrive_header;
+  ::OpenDRIVE *opendrive_data;
+  OpenDRIVE::road_type *new_road;
+  unsigned int road_id=0;
+
+  try{
+    std::ofstream output_file(filename.c_str(),std::ios_base::out);
+    opendrive_header.revMajor(1.0);
+    opendrive_header.revMinor(1.0);
+    opendrive_header.name("iri");
+    opendrive_header.version(1.0);
+    opendrive_header.north(0.0);
+    opendrive_header.south(0.0);
+    opendrive_header.east(0.0);
+    opendrive_header.west(0.0);
+    std::time_t current_time = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
+    opendrive_header.date(std::ctime(&current_time));
+    opendrive_data=new ::OpenDRIVE(opendrive_header);
+    for(unsigned int i=0;i<this->roads.size();i++)
+    {
+      if(std::find(processed_roads.begin(),processed_roads.end(),this->roads[i])==processed_roads.end())
+      {
+        map_pair.first.clear();
+        map_pair.second.clear();
+        processed_roads.push_back(this->roads[i]);
+        map_pair.first.push_back(this->roads[i]);
+        if(this->roads[i]->has_opposite_direction_road())
+        {
+          opposite_road=&this->roads[i]->get_opposite_direction_road();
+          processed_roads.push_back(opposite_road);
+          map_pair.first.push_back(opposite_road);
+        }
+        else
+          opposite_road=NULL;
+        COpendriveRoadSegment::convert(opposite_road,this->roads[i],road_id,segments);
+        for(unsigned j=0;j<segments.size();j++)
+        {
+          segments[j]->save(&new_road);
+          opendrive_data->road().push_back(*new_road);
+          delete new_road;
+          map_pair.second.push_back(segments[j]);
+        }
+        road_map.push_back(map_pair);
+      }
+    }
+    for(unsigned int i=0;i<this->junctions.size();i++)
+    {
+      junction=new COpendriveJunction();
+      junction->convert(this->junctions[i],road_id,junction,junction_segments,road_map);
+      OpenDRIVE::junction_type new_junction;
+      junction->save(new_junction);
+      opendrive_data->junction().push_back(new_junction);
+      for(unsigned int j=0;j<junction_segments.size();j++)
+      {
+        junction_segments[j]->save(&new_road);
+        opendrive_data->road().push_back(*new_road);
+        delete new_road;
+        delete junction_segments[j];
+      }
+      delete junction;
+    }
+    for(unsigned int i=0;i<road_map.size();i++)
+      for(unsigned int j=0;j<road_map[i].second.size();j++)
+        delete road_map[i].second[j];
+    map[""].name="";
+    map[""].schema="OpenDRIVE_1.4H.xsd";
+    OpenDRIVE_(output_file,*opendrive_data,map);
+  }catch (const xml_schema::exception& e){
+    std::ostringstream os;
+    os << e;
+    /* handle exceptions */
+    throw CException(_HERE_,os.str());
+  }
+}
+
+void CRoadMap::load_osm(const std::string &filename)
+{
+  COSMMap road_map;
+
+  this->free();
+  road_map.load(filename);
+  road_map.convert(*this);
+}
+
+void CRoadMap::set_resolution(double resolution)
+{
+  this->resolution=resolution;
+  for(unsigned int i=0;i<this->roads.size();i++)
+    this->roads[i]->set_resolution(resolution);
+  for(unsigned int i=0;i<this->junctions.size();i++)
+    this->junctions[i]->set_resolution(resolution);
+}
+
+double CRoadMap::get_resolution(void)
+{
+  return this->resolution;
+}
+
+std::vector<unsigned int> CRoadMap::get_path_sub_roadmap(std::vector<unsigned int> &segment_ids,CRoadMap &new_road_map)
+{
+  std::map<CRoad *,CRoad *> road_ref_update;
+  std::map<CRoadSegment *,CRoadSegment *> segment_ref_update;
+  std::vector<CRoadSegment *> junction_segments;
+  CRoad *new_road=NULL,*old_parent_road;
+  CJunction *new_junction;
+  CRoadSegment *new_segment,*old_segment;
+  TPoint start_point,end_point;
+  unsigned int next_id,prev_id,num_lanes_in,num_lanes_out;
+  std::vector<unsigned int> new_path;
+
+  new_road_map.free();
+  for(unsigned int i=0;i<segment_ids.size();i++) 
+  {
+    old_segment=&this->get_segment_by_id(segment_ids[i]);
+    if(old_segment->has_parent_road())// parent is a road
+    {
+      if(new_road==NULL)
+      {
+        new_road=new CRoad();
+        old_parent_road=&old_segment->get_parent_road();
+        road_ref_update[old_parent_road]=new_road;
+        new_road->set_resolution(old_parent_road->get_resolution());
+        new_road->set_num_lanes(old_parent_road->get_num_lanes());
+        new_road->set_lane_width(old_parent_road->get_lane_width());
+        new_road->set_lane_speed(old_parent_road->get_lane_speed());
+        old_segment->get_start_point(start_point);
+        new_road->set_start_point(start_point);
+        old_segment->get_end_point(end_point);
+        new_road->add_segment(end_point);
+      }
+      else
+      {
+        old_segment->get_end_point(end_point);
+        new_road->add_segment(end_point);
+      }
+      new_segment=new_road->get_last_segment();
+      new_segment->connectivity=old_segment->connectivity;
+      segment_ref_update[old_segment]=new_segment;
+    }
+    else // parent is a junction
+    {
+      if(new_road!=NULL)
+      {
+        new_road_map.add_road(new_road);
+        new_road=NULL;
+      }
+      junction_segments.push_back(old_segment);
+    }
+  }
+  if(new_road!=NULL)
+  {
+    new_road_map.add_road(new_road);
+    new_road=NULL;
+  }
+  //handle the junctions
+  for(unsigned int i=0;i<junction_segments.size();i++)
+  {
+    if(junction_segments[i]->get_num_prev_segments()==1)
+    {
+      if(junction_segments[i]->get_prev_segment_by_index(0).has_parent_road())
+      {
+        if(road_ref_update.find(&junction_segments[i]->get_prev_segment_by_index(0).get_parent_road())!=road_ref_update.end())
+          prev_id=road_ref_update[&junction_segments[i]->get_prev_segment_by_index(0).get_parent_road()]->get_id();
+        else
+        {
+          prev_id=-1;
+          junction_segments[i]->get_prev_segment_by_index(0).get_end_point(start_point);
+          num_lanes_in=junction_segments[i]->get_prev_segment_by_index(0).get_num_lanes_out();
+        }
+      }
+      else
+        continue;
+    }
+    else
+      continue;
+    if(junction_segments[i]->get_num_next_segments()==1)
+    {
+      if(junction_segments[i]->get_next_segment_by_index(0).has_parent_road())
+      {
+        if(road_ref_update.find(&junction_segments[i]->get_next_segment_by_index(0).get_parent_road())!=road_ref_update.end())
+          next_id=road_ref_update[&junction_segments[i]->get_next_segment_by_index(0).get_parent_road()]->get_id();
+        else
+        {
+          next_id=-1;
+          junction_segments[i]->get_next_segment_by_index(0).get_start_point(end_point);
+          num_lanes_out=junction_segments[i]->get_next_segment_by_index(0).get_num_lanes_in();
+        }
+      }
+      else
+        continue;
+    }
+    else
+      continue;
+    new_junction=new CJunction();
+    if(prev_id!=(unsigned int)-1)
+      new_junction->add_incomming_road(&new_road_map.get_road_by_id(prev_id));
+    if(next_id!=(unsigned int)-1)
+      new_junction->add_outgoing_road(&new_road_map.get_road_by_id(next_id));
+    if(prev_id==(unsigned int)-1)
+    {
+      if(next_id==(unsigned int)-1)
+        new_segment=new_junction->link_point_to_point(start_point,num_lanes_in,end_point,num_lanes_out);
+      else
+        new_segment=new_junction->link_point_to_road(start_point,num_lanes_in,next_id);
+    }
+    else
+    {
+      if(next_id==(unsigned int)-1)
+        new_segment=new_junction->link_road_to_point(prev_id,end_point,num_lanes_out);
+      else
+      {
+        new_junction->link_roads_by_id(prev_id,next_id);
+        new_segment=&new_junction->get_segment_between_by_id(prev_id,next_id);
+      }
+    }
+    new_segment->connectivity=junction_segments[i]->connectivity;
+    segment_ref_update[junction_segments[i]]=new_segment;
+    new_road_map.add_junction(new_junction);
+  }
+  for(unsigned int i=0;i<segment_ids.size();i++)
+  {
+    for(std::map<CRoadSegment *,CRoadSegment *>::iterator it=segment_ref_update.begin();it!=segment_ref_update.end();it++)
+    {
+      if(it->first->get_id()==segment_ids[i])
+      {
+        new_path.push_back(it->second->get_id());
+        break;
+      }
+    }
+  }
+
+  return new_path;
+}
+
+double CRoadMap::get_path_length(std::vector<unsigned int> &segment_ids)
+{
+  CRoadSegment *segment;
+  double length=0.0;
+
+  for(unsigned int i=0;i<segment_ids.size();i++)
+  {
+    segment=&this->get_segment_by_id(segment_ids[i]);
+    length+=segment->get_length();
+  }
+
+  return length;
+}
+
+unsigned int CRoadMap::get_num_roads(void)
+{
+  return this->roads.size();
+}
+
+CRoad &CRoadMap::get_road_by_id(unsigned int id)
+{
+  unsigned int index;
+
+  index=CRoad::get_index_by_id(this->roads,id);
+  return *this->get_road_by_index(index);
+}
+
+unsigned int CRoadMap::get_num_junctions(void)
+{
+  return this->junctions.size();
+}
+
+CJunction &CRoadMap::get_junction_by_id(unsigned int id)
+{
+  unsigned int index;
+
+  index=CJunction::get_index_by_id(this->junctions,id);
+  return *this->get_junction_by_index(index);
+}
+
+unsigned int CRoadMap::get_num_segments(void)
+{
+  unsigned int num_segments=0;
+
+  for(unsigned int i=0;i<this->roads.size();i++)
+    num_segments+=this->roads[i]->get_num_segments();
+  for(unsigned int i=0;i<this->junctions.size();i++)
+    num_segments+=this->junctions[i]->get_num_segments();
+  return num_segments;
+}
+
+CRoadSegment &CRoadMap::get_segment_by_id(unsigned int id)
+{
+  for(unsigned int i=0;i<this->roads.size();i++)
+    for(unsigned int j=0;j<this->roads[i]->get_num_segments();j++)
+      if(this->roads[i]->segments[j]->get_id()==id)
+        return *this->roads[i]->segments[j];
+  for(unsigned int i=0;i<this->junctions.size();i++)
+    for(unsigned int j=0;j<this->junctions[i]->get_num_segments();j++)
+      if(this->junctions[i]->segments[j]->get_id()==id)
+        return *this->junctions[i]->segments[j];
+  throw CException(_HERE_,"No segment with the given id");
+}
+
+void CRoadMap::add_road(CRoad *road)
+{
+  if(road==NULL)
+    throw CException(_HERE_,"Invalid road reference");
+  if(road->get_id()==(unsigned int)-1)
+    road->set_id(this->get_next_road_id());
+  if(this->has_road(road->get_id()))
+    throw CException(_HERE_,"Road already present");
+  road->set_resolution(this->resolution);
+  for(unsigned int i=0;i<road->get_num_segments();i++)
+    if(road->segments[i]->id==(unsigned int)-1)
+      road->segments[i]->id=this->get_next_segment_id();
+  this->roads.push_back(road);
+  road->set_parent_roadmap(this);
+}
+
+bool CRoadMap::has_road(unsigned int id)
+{
+  unsigned index;
+
+  index=CRoad::get_index_by_id(this->roads,id);
+  if(index==(unsigned int)-1)
+    return false;
+  else
+    return true;
+}
+
+void CRoadMap::remove_road_by_id(unsigned int id)
+{
+  unsigned int index;
+
+  index=CRoad::get_index_by_id(this->roads,id);
+  this->remove_road_by_index(index);
+}
+
+void CRoadMap::set_opposite_direction_roads_by_id(unsigned int right_road,unsigned int left_road)
+{
+  unsigned int left_index,right_index;
+
+  right_index=CRoad::get_index_by_id(this->roads,right_road);
+  left_index=CRoad::get_index_by_id(this->roads,left_road);
+  this->set_opposite_direction_roads_by_index(right_index,left_index);
+}
+
+void CRoadMap::add_junction(CJunction *junction)
+{
+  if(junction==NULL)
+    throw CException(_HERE_,"Invalid junction reference");
+  if(junction->get_id()==(unsigned int)-1)
+    junction->set_id(this->get_next_junction_id());
+  if(this->has_junction(junction->get_id()))
+    throw CException(_HERE_,"Junction already present");
+  junction->set_resolution(this->resolution);
+  for(unsigned int i=0;i<junction->get_num_segments();i++)
+    if(junction->segments[i]->id==(unsigned int)-1)
+      junction->segments[i]->id=this->get_next_segment_id();
+  this->junctions.push_back(junction);
+  junction->set_parent_roadmap(this);
+}
+
+bool CRoadMap::has_junction(unsigned int id)
+{
+  unsigned int index;
+ 
+  index=CJunction::get_index_by_id(this->junctions,id);
+  if(index==(unsigned int)-1)
+    return false;
+  else
+    return true;
+}
+
+void CRoadMap::remove_junction_by_id(unsigned int id)
+{
+  unsigned int index;
+
+  index=CRoad::get_index_by_id(this->roads,id);
+  this->remove_junction_by_index(index);
+}
+
+void CRoadMap::get_road_connectivity(Eigen::MatrixXi &connectivity,std::vector<unsigned int> &id_map)
+{
+  connectivity=Eigen::MatrixXi::Zero(this->roads.size(),this->roads.size());
+
+  id_map.clear();
+  for(unsigned int i=0;i<this->roads.size();i++)
+  {
+    id_map.push_back(this->roads[i]->get_id());
+    for(unsigned int j=0;j<this->roads.size();j++)
+    {
+      for(unsigned int k=0;k<this->junctions.size();k++)
+      {
+        try{
+          if(this->junctions[k]->are_roads_linked_by_id(this->roads[i]->get_id(),this->roads[j]->get_id()))
+          {
+            connectivity(i,j)=1;
+          }
+        }catch(CException &e){
+          /* roads i and j are not connected throw junction k */
+        }
+      }
+    }
+  }
+}
+
+void CRoadMap::get_segment_connectivity(Eigen::MatrixXi &connectivity,std::vector<unsigned int> &id_map)
+{
+  unsigned int num_segments=0,col_index;
+  std::vector<CRoadSegment *> segments;
+
+  id_map.clear();
+  for(unsigned int i=0;i<this->roads.size();i++)
+  {
+    num_segments+=this->roads[i]->get_num_segments();
+    for(unsigned int j=0;j<this->roads[i]->get_num_segments();j++)
+    {
+      id_map.push_back(this->roads[i]->segments[j]->get_id());
+      segments.push_back(this->roads[i]->segments[j]);
+    }
+  }
+  for(unsigned int i=0;i<this->junctions.size();i++)
+  {
+    num_segments+=this->junctions[i]->get_num_segments();
+    for(unsigned int j=0;j<this->junctions[i]->get_num_segments();j++)
+    {
+      id_map.push_back(this->junctions[i]->segments[j]->get_id());
+      segments.push_back(this->junctions[i]->segments[j]);
+    }
+  }
+  connectivity=Eigen::MatrixXi::Zero(num_segments,num_segments);
+  for(unsigned int i=0;i<segments.size();i++)
+  {
+    for(unsigned int j=0;j<segments[i]->get_num_next_segments();j++)
+    {
+      col_index=CRoadSegment::get_index_by_id(segments,segments[i]->next_segments[j]->get_id());
+      connectivity(i,col_index)=1;
+    }
+  }
+}
+
+void CRoadMap::get_lane_connectivity(Eigen::MatrixXi &connectivity,std::vector<TLaneID> &id_map)
+{
+
+}
+
+unsigned int CRoadMap::get_max_num_lanes(void)
+{
+  unsigned max_num_lanes=0,num_lanes;
+
+  for(unsigned int i=0;i<this->roads.size();i++)
+  {
+    num_lanes=this->roads[i]->get_num_lanes();
+    if(num_lanes>max_num_lanes)
+      max_num_lanes=num_lanes;
+  }
+
+  return max_num_lanes;
+}
+
+bool CRoadMap::get_closest_segment_id(TPoint &point,unsigned int &segment_id,double &distance,double &lateral_offset,double angle_tol)
+{
+  double min_offset=std::numeric_limits<double>::max(),tmp_offset,tmp_distance;
+  unsigned int tmp_id;
+
+  for(unsigned int i=0;i<this->roads.size();i++)
+  {
+    if(this->roads[i]->get_closest_segment_id(point,tmp_id,tmp_distance,tmp_offset,angle_tol))
+    {
+      if(fabs(tmp_offset)<min_offset)
+      {
+        min_offset=fabs(tmp_offset);
+        distance=tmp_distance;
+        lateral_offset=tmp_offset;
+        segment_id=tmp_id;
+      }
+    }
+  }
+  for(unsigned int i=0;i<this->junctions.size();i++)
+  {
+    if(this->junctions[i]->get_closest_segment_id(point,tmp_id,tmp_distance,tmp_offset,angle_tol))
+    {
+      if(fabs(tmp_offset)<min_offset)
+      {
+        min_offset=fabs(tmp_offset);
+        distance=tmp_distance;
+        lateral_offset=tmp_offset;
+        segment_id=tmp_id;
+      }
+    }
+  }
+
+  if(min_offset!=std::numeric_limits<double>::max())
+    return true;
+  else
+    return false;
+}
+
+bool CRoadMap::get_closest_segment_ids(TPoint &point,std::vector<unsigned int> &segment_ids,std::vector<double >&distances,std::vector<double >&lateral_offsets,double angle_tol,double offset_tol)
+{
+  std::vector<unsigned int> tmp_ids;
+  std::vector<double> tmp_distances,tmp_offsets;
+  double tmp_offset,tmp_distance;
+  unsigned int tmp_id;
+
+  segment_ids.clear();
+  distances.clear();
+  lateral_offsets.clear();
+  for(unsigned int i=0;i<this->roads.size();i++)
+  {
+    if(this->roads[i]->get_closest_segment_id(point,tmp_id,tmp_distance,tmp_offset,angle_tol))
+    {
+      if(fabs(tmp_offset)<offset_tol)
+      {
+        distances.push_back(tmp_distance);
+        lateral_offsets.push_back(tmp_offset);
+        segment_ids.push_back(tmp_id);
+      }
+    }
+  }
+  for(unsigned int i=0;i<this->junctions.size();i++)
+  {
+    if(this->junctions[i]->get_closest_segment_ids(point,tmp_ids,tmp_distances,tmp_offsets,angle_tol,offset_tol))
+    {
+      for(unsigned int j=0;j<tmp_ids.size();j++)
+      {
+        distances.push_back(tmp_distances[j]);
+        lateral_offsets.push_back(tmp_offsets[j]);
+        segment_ids.push_back(tmp_ids[j]);
+      }
+    }
+  }
+
+  if(segment_ids.size()>0)
+    return true;
+  else
+    return false;
+}
+
+void CRoadMap::get_segment_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw)
+{
+  std::vector<double> partial_x,partial_y,partial_heading;
+
+  x.clear();
+  y.clear();
+  yaw.clear();
+  for(unsigned int i=0;i<this->roads.size();i++)
+  {
+    this->roads[i]->get_segment_geometry(partial_x,partial_y,partial_heading);
+    x.insert(x.end(),partial_x.begin(),partial_x.end());
+    y.insert(y.end(),partial_y.begin(),partial_y.end());
+    yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end());
+  }
+  for(unsigned int i=0;i<this->junctions.size();i++)
+  {
+    this->junctions[i]->get_segment_geometry(partial_x,partial_y,partial_heading);
+    x.insert(x.end(),partial_x.begin(),partial_x.end());
+    y.insert(y.end(),partial_y.begin(),partial_y.end());
+    yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end());
+  }
+}
+
+void CRoadMap::get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw)
+{
+  std::vector<double> partial_x,partial_y,partial_heading;
+
+  x.clear();
+  y.clear();
+  yaw.clear(); 
+  for(unsigned int i=0;i<this->roads.size();i++)
+  {
+    this->roads[i]->get_lane_geometry(partial_x,partial_y,partial_heading);
+    x.insert(x.end(),partial_x.begin(),partial_x.end());
+    y.insert(y.end(),partial_y.begin(),partial_y.end());
+    yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end());
+  }
+  for(unsigned int i=0;i<this->junctions.size();i++)
+  {
+    this->junctions[i]->get_lane_geometry(partial_x,partial_y,partial_heading);
+    x.insert(x.end(),partial_x.begin(),partial_x.end());
+    y.insert(y.end(),partial_y.begin(),partial_y.end());
+    yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end());
+  }
+}
+
+CRoadMap::~CRoadMap()
+{
+  this->free();
+}
+
diff --git a/src/road_segment.cpp b/src/road_segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3dca6c5f1c16f667809a29b0091f1e797d5d84fd
--- /dev/null
+++ b/src/road_segment.cpp
@@ -0,0 +1,470 @@
+#include "road_segment.h"
+#include "exceptions.h"
+#include "common.h"
+
+CRoadSegment::CRoadSegment(unsigned int lanes_in,unsigned int lanes_out)
+{
+  this->id=-1;
+  this->resolution=0.1;
+  this->parent_road=NULL;
+  this->parent_junction=NULL;
+  this->prev_segments.clear();
+  this->next_segments.clear();
+  this->connectivity=Eigen::MatrixXd::Zero(lanes_in,lanes_out);
+  this->start_point.x=std::numeric_limits<double>::max();
+}
+
+unsigned int CRoadSegment::get_index_by_id(const std::vector<CRoadSegment *> &segments,unsigned int id)
+{
+  std::vector<CRoadSegment *>::const_iterator it;
+  unsigned int index;
+
+  for(it=segments.begin(),index=0;it!=segments.end();it++,index++)
+    if((*it)->id==id)
+      return index;
+
+  return -1;
+}
+
+void CRoadSegment::set_id(unsigned int id)
+{
+  this->id=id;
+}
+
+void CRoadSegment::set_parent_road(CRoad *road)
+{
+  if(road==NULL)
+    throw CException(_HERE_,"Invalid road reference");
+  this->parent_road=road;
+}
+
+void CRoadSegment::set_parent_junction(CJunction *junction)
+{
+  if(junction==NULL)
+    throw CException(_HERE_,"Invalid junction reference");
+  this->parent_junction=junction;
+}
+
+void CRoadSegment::add_prev_segment(CRoadSegment *segment)
+{
+  if(segment==NULL)
+    throw CException(_HERE_,"Invalid road segment reference");
+  if(this->has_prev_segment(segment))
+    throw CException(_HERE_,"Previous road segment already present");
+  this->prev_segments.push_back(segment);
+}
+
+bool CRoadSegment::has_prev_segment(CRoadSegment *segment)
+{
+  if(segment==NULL)
+    throw CException(_HERE_,"Invalid road segment reference");
+  for(unsigned int i=0;i<this->prev_segments.size();i++)
+    if(this->prev_segments[i]==segment)
+      return true;
+
+  return false;
+}
+
+void CRoadSegment::remove_prev_segment(CRoadSegment *segment)
+{
+  std::vector<CRoadSegment *>::iterator it;
+
+  if(segment==NULL)
+    throw CException(_HERE_,"Invalid road segment reference");
+  if(!this->has_prev_segment(segment))
+    throw CException(_HERE_,"Previous road segment not present");
+  for(it=this->prev_segments.begin();it!=this->prev_segments.end();it++)
+  {
+    if((*it)==segment)
+    {
+      this->prev_segments.erase(it);
+      break;
+    }
+  }
+}
+
+void CRoadSegment::add_next_segment(CRoadSegment *segment)
+{
+  if(segment==NULL)
+    throw CException(_HERE_,"Invalid road segment reference");
+  if(this->has_next_segment(segment))
+    throw CException(_HERE_,"Next road segment already present");
+  this->next_segments.push_back(segment);
+}
+
+bool CRoadSegment::has_next_segment(CRoadSegment *segment)
+{
+  if(segment==NULL)
+    throw CException(_HERE_,"Invalid road segment reference");
+  for(unsigned int i=0;i<this->next_segments.size();i++)
+    if(this->next_segments[i]==segment)
+      return true;
+
+  return false;
+}
+
+void CRoadSegment::remove_next_segment(CRoadSegment *segment)
+{
+  std::vector<CRoadSegment *>::iterator it;
+
+  if(segment==NULL)
+    throw CException(_HERE_,"Invalid road segment reference");
+  if(!this->has_next_segment(segment))
+    throw CException(_HERE_,"Next road segment not present");
+  for(it=this->next_segments.begin();it!=this->next_segments.end();it++)
+  {
+    if((*it)==segment)
+    {
+      this->next_segments.erase(it);
+      break;
+    }
+  }
+}
+
+CRoad &CRoadSegment::get_parent_road(void)
+{
+  if(this->parent_road==NULL)
+    throw CException(_HERE_,"Inavalid parent road reference");
+  return *this->parent_road;
+}
+
+unsigned int CRoadSegment::get_id(void)
+{
+  return this->id;
+}
+
+void CRoadSegment::set_resolution(double resolution)
+{
+  this->resolution=resolution;
+  if(this->start_point.x!=std::numeric_limits<double>::max())
+    this->spline.generate(this->resolution,(unsigned int)10);
+}
+
+double CRoadSegment::get_resolution(void)
+{
+  return this->resolution;
+}
+
+bool CRoadSegment::has_parent_road(void)
+{
+  if(this->parent_road==NULL)
+    return false;
+  else
+    return true;
+}
+
+CJunction &CRoadSegment::get_parent_junction(void)
+{
+  if(this->parent_junction==NULL)
+    throw CException(_HERE_,"Inavalid parent junction reference");
+  return *this->parent_junction;
+}
+
+bool CRoadSegment::has_parent_junction(void)
+{
+  if(this->parent_junction==NULL)
+    return false;
+  else
+    return true;
+}
+
+unsigned int CRoadSegment::get_num_prev_segments(void)
+{
+  return this->prev_segments.size();
+}
+
+CRoadSegment &CRoadSegment::get_prev_segment_by_index(unsigned int index)
+{
+  if(index<0 || index >= this->prev_segments.size())
+    throw CException(_HERE_,"Invalid previous segment index");
+  return *this->prev_segments[index];
+}
+
+unsigned int CRoadSegment::get_num_next_segments(void)
+{
+  return this->next_segments.size();
+}
+
+CRoadSegment &CRoadSegment::get_next_segment_by_index(unsigned int index)
+{
+  if(index<0 || index >= this->next_segments.size())
+    throw CException(_HERE_,"Invalid next segment index");
+  return *this->next_segments[index];
+}
+
+unsigned int CRoadSegment::get_num_lanes_in(void)
+{
+  if(this->parent_road!=NULL)
+    return this->parent_road->get_num_lanes();
+  else
+    return this->connectivity.rows();
+}
+
+unsigned int CRoadSegment::get_num_lanes_out(void)
+{
+  if(this->parent_road!=NULL)
+    return this->parent_road->get_num_lanes();
+  else
+    return this->connectivity.cols();
+}
+
+double CRoadSegment::get_lane_width(void)
+{
+  double width;
+
+  if(this->parent_road!=NULL)
+    return this->parent_road->get_lane_width();
+  else
+  {
+    if(this->get_num_prev_segments()==1)
+    {
+      CRoadSegment &prev_segment=this->get_prev_segment_by_index(0);
+      if(prev_segment.has_parent_road())
+        width=prev_segment.get_parent_road().get_lane_width();
+      else
+        width=4.0;
+    }
+    else if(this->get_num_next_segments()==1)
+    {
+      CRoadSegment &next_segment=this->get_next_segment_by_index(0);
+      if(next_segment.has_parent_road())
+        width=next_segment.get_parent_road().get_lane_width();
+      else
+        width=4.0;
+    }
+    else
+      width=0.0;
+  }
+
+  return width;
+}
+
+double CRoadSegment::get_lane_speed(void)
+{
+  double speed;
+
+  if(this->parent_road!=NULL)
+    return this->parent_road->get_lane_width();
+  else
+  {
+    if(this->get_num_prev_segments()==1)
+    {
+      CRoadSegment &prev_segment=this->get_prev_segment_by_index(0);
+      if(prev_segment.has_parent_road())
+        speed=prev_segment.get_parent_road().get_lane_speed();
+      else
+        speed=50.0;
+    }
+    else if(this->get_num_next_segments()==1)
+    {
+      CRoadSegment &next_segment=this->get_next_segment_by_index(0);
+      if(next_segment.has_parent_road())
+        speed=next_segment.get_parent_road().get_lane_speed();
+      else
+        speed=50.0;
+    }
+    else
+      speed=0.0;
+  }
+
+  return speed;
+}
+
+unsigned int CRoadSegment::get_lane(double lateral_offset)
+{
+  unsigned int num_lanes;
+  double lane_width;
+
+  if(lateral_offset>0.0)
+    return 0;
+  num_lanes=std::max(this->get_num_lanes_in(),this->get_num_lanes_out());
+  lane_width=this->get_lane_width();
+  for(unsigned int i=0;i<num_lanes;i++)
+  {
+    if(fabs(lateral_offset)<lane_width)
+      return i;
+    else
+      lateral_offset+=lane_width;
+  }
+  return num_lanes-1;
+}
+
+void CRoadSegment::set_geometry(TPoint &start_point,TPoint &end_point)
+{
+  this->start_point=start_point;
+  this->end_point=end_point;
+  this->spline.set_start_point(start_point);
+  this->spline.set_end_point(end_point);
+  this->spline.generate(this->resolution,(unsigned int)10);
+}
+
+void CRoadSegment::get_start_point(TPoint &point)
+{
+  point=this->start_point;
+}
+
+void CRoadSegment::get_end_point(TPoint &point)
+{
+  point=this->end_point;
+}
+
+void CRoadSegment::set_full_connectivity(void)
+{
+  for(unsigned int i=0;i<this->connectivity.rows();i++)
+    for(unsigned int j=0;j<this->connectivity.cols();j++)
+      this->connectivity(i,j)=1.0;
+}
+
+void CRoadSegment::link_lanes(unsigned int lane1,unsigned int lane2)
+{
+  if(lane1>=this->connectivity.rows())
+    throw CException(_HERE_,"Invalid lane1 index");
+  if(lane2>=this->connectivity.cols())
+    throw CException(_HERE_,"Invalid lane2 index");
+  this->connectivity(lane1,lane2)=1.0;
+}
+
+void CRoadSegment::unlink_lanes(unsigned int lane1,unsigned int lane2)
+{
+  if(lane1>=this->connectivity.rows())
+    throw CException(_HERE_,"Invalid lane1 index");
+  if(lane2>=this->connectivity.cols())
+    throw CException(_HERE_,"Invalid lane2 index");
+  this->connectivity(lane1,lane2)=0.0;
+}   
+
+bool CRoadSegment::are_lanes_linked(unsigned int lane1,unsigned int lane2)
+{
+  if(lane1>=this->connectivity.rows())
+    throw CException(_HERE_,"Invalid lane1 index");
+  if(lane2>=this->connectivity.cols())
+    throw CException(_HERE_,"Invalid lane2 index");
+  if(this->connectivity(lane1,lane2)==1.0)
+     return true;
+  else
+     return false;
+}   
+
+void CRoadSegment::get_connectivity_matrix(Eigen::MatrixXd &connectivity)
+{
+  connectivity=this->connectivity;
+}
+
+bool CRoadSegment::is_input_lane_connected(unsigned int lane)
+{
+  unsigned int num=0;
+
+  if(lane>=this->connectivity.rows())
+    throw CException(_HERE_,"Invalid lane index");
+  for(unsigned int i=0;i<this->connectivity.cols();i++)
+    if(this->connectivity(lane,i)==1.0)
+      num++;
+  if(num>0)
+    return true;
+  else
+    return false;
+}
+
+double CRoadSegment::get_length(void)
+{
+  return this->spline.get_length();
+}
+
+bool CRoadSegment::get_point_at(double distance, double lateral_offset,TPoint &point)
+{
+  TPoint segment_point;
+
+  if(this->spline.evaluate(distance,segment_point))
+  {
+    point.x=segment_point.x-lateral_offset*sin(segment_point.heading);
+    point.y=segment_point.y+lateral_offset*cos(segment_point.heading);
+    point.heading=normalize_angle(segment_point.heading);
+    if(segment_point.curvature>0.0)
+      point.curvature=1.0/((1.0/segment_point.curvature)-lateral_offset);
+    else
+      point.curvature=1.0/((1.0/segment_point.curvature)+lateral_offset);
+
+    return true;
+  }
+  else
+    return false;
+}
+
+bool CRoadSegment::get_closest_point(TPoint &target_point,TPoint &closest_point,double &distance,double &lateral_offset,double angle_tol)
+{
+  double cross;
+
+  distance=this->spline.find_closest_point(target_point.x,target_point.y,closest_point);
+  if(distance==std::numeric_limits<double>::max())
+    return false;
+  else
+  {
+    if(fabs(diff_angle(target_point.heading,closest_point.heading))<angle_tol)
+    { 
+      lateral_offset=sqrt(pow(target_point.x-closest_point.x,2.0)+pow(target_point.y-closest_point.y,2.0));
+      /* check the side */
+      cross=(this->end_point.x-this->start_point.x)*(target_point.y-this->start_point.y)-(this->end_point.y-this->start_point.y)*(target_point.x-this->start_point.x);
+      if(cross<0.0)
+        lateral_offset*=-1.0;
+      return true;
+    }
+    else
+      return false;
+  }
+}
+
+void CRoadSegment::get_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double lateral_offset,double start_length, double end_length)
+{
+  std::vector<double> curvature;
+  CG2Spline *new_spline=NULL,*partial_spline;
+  TPoint start_point,end_point;
+
+  if(lateral_offset!=0.0)
+  {
+    if(!get_point_at(0.0,lateral_offset,start_point))
+      throw CException(_HERE_,"Impossible to compute the new start point");
+    if(!get_point_at(this->get_length(),lateral_offset,end_point))
+      throw CException(_HERE_,"Impossible to compute the new start point");
+    new_spline=new CG2Spline(start_point,end_point);
+    if(start_length!=0.0 || end_length!=std::numeric_limits<double>::max())// get partial spline
+    {
+      partial_spline=new CG2Spline;
+      if(!new_spline->get_part(partial_spline,start_length,end_length))
+      {
+        delete new_spline;
+        delete partial_spline;
+        throw CException(_HERE_,"Impossible to generate partial spline");
+      }
+      partial_spline->evaluate_all(x,y,curvature,yaw);
+      delete partial_spline;
+    }
+    else
+      new_spline->evaluate_all(x,y,curvature,yaw);
+    delete new_spline;
+  }
+  else
+  {
+    if(start_length!=0.0 || end_length!=std::numeric_limits<double>::max())// get partial spline
+    {
+      partial_spline=new CG2Spline;
+      if(!this->spline.get_part(partial_spline,start_length,end_length))
+      {
+        delete partial_spline;
+        throw CException(_HERE_,"Impossible to generate partial spline");
+      }
+      partial_spline->evaluate_all(x,y,curvature,yaw);
+      delete partial_spline;
+    }
+    else
+      this->spline.evaluate_all(x,y,curvature,yaw);
+  }
+}
+
+CRoadSegment::~CRoadSegment()
+{
+  this->parent_road=NULL;
+  this->parent_junction=NULL;
+  this->prev_segments.clear();
+  this->next_segments.clear();
+}
+
diff --git a/src/vel_profile.cpp b/src/vel_profile.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4b1be562573a07357c6513dee394f623c253c49e
--- /dev/null
+++ b/src/vel_profile.cpp
@@ -0,0 +1,131 @@
+#include "vel_profile.h"
+#include "exceptions.h"
+
+CVelProfile::CVelProfile()
+{
+  this->start_vel=0.0;
+  this->end_vel=0.0;
+  this->start_acc=0.0;
+  this->end_acc=0.0;
+  this->time=0.0;
+  this->length=0.0;
+  this->max_acc=0.0;
+  this->generated=false;
+}
+
+CVelProfile::CVelProfile(double start_vel,double end_vel,double max_vel,double start_acc,double end_acc,double max_acc)
+{
+  this->set_start_vel(start_vel);
+  this->set_end_vel(end_vel);
+  this->set_max_vel(max_vel);
+  this->set_start_acc(start_acc);
+  this->set_end_acc(end_acc);
+  this->set_max_acc(max_acc);
+}
+
+void CVelProfile::set_start_vel(double vel)
+{
+  if(vel<0.0)
+    throw CException(_HERE_,"Invalid velocity value");  
+  else
+    this->start_vel=vel;
+}
+
+double CVelProfile::get_start_vel(void)
+{
+  return this->start_vel; 
+}
+
+void CVelProfile::set_end_vel(double vel)
+{
+  if(vel<0.0)
+    throw CException(_HERE_,"Invalid velocity value");  
+  else
+    this->end_vel=vel;
+}
+
+double CVelProfile::get_end_vel(void)
+{
+  return this->end_vel;
+}
+
+void CVelProfile::set_max_vel(double vel)
+{
+  if(vel<0.0)
+    throw CException(_HERE_,"Invalid velocity value");  
+  else
+    this->max_vel=vel;
+}
+
+double CVelProfile::get_max_vel(void)
+{
+  return this->max_vel;
+}
+
+void CVelProfile::set_start_acc(double acc)
+{
+//  if(acc<0.0)
+//    throw CException(_HERE_,"Invalid acceleration value");  
+//  else
+    this->start_acc=acc;
+}
+
+double CVelProfile::get_start_acc(void)
+{
+  return this->start_acc;
+}
+
+void CVelProfile::set_end_acc(double acc)
+{
+//  if(acc<0.0)
+//    throw CException(_HERE_,"Invalid acceleration value");  
+//  else
+    this->end_acc=acc;
+}
+
+double CVelProfile::get_end_acc(void)
+{
+  return this->end_acc;
+}
+
+void CVelProfile::set_max_acc(double acc)
+{
+  if(acc<0.0)
+    throw CException(_HERE_,"Invalid acceleration value");  
+  else
+    this->max_acc=acc;
+}
+
+double CVelProfile::get_max_acc(void)
+{
+  return this->max_acc;
+}
+
+double CVelProfile::get_time(void)
+{
+  return this->time;
+}
+
+double CVelProfile::get_length(void)
+{
+  return this->length;
+}
+
+std::ostream& operator<< (std::ostream& out, const CVelProfile &profile)
+{
+  out << "Velocity profile:" << std::endl;
+  out << "  Start velocity: " << profile.start_vel << " m/s, start acceleration: " << profile.start_acc << " m/s2" << std::endl;
+  out << "  End velocity: " << profile.end_vel << "m/s, end acceleration: " << profile.end_acc << " m/s2" << std::endl;
+  out << "  Maximum velocity: " << profile.max_vel << " m/s" << std::endl;
+  out << "  Maximum acceleration: " << profile.max_acc << " m/s2" << std::endl;
+  out << "  Length: " << profile.length << " m" << std::endl;
+  out << "  Time: " << profile.time << " s" << std::endl;
+
+  return out;
+}
+
+CVelProfile::~CVelProfile()
+{
+ 
+}
+
diff --git a/src/vel_spline.cpp b/src/vel_spline.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..76d833c78afa5a2a8b5cb62b7ba8b544aab78531
--- /dev/null
+++ b/src/vel_spline.cpp
@@ -0,0 +1,387 @@
+#include "vel_spline.h"
+#include "exceptions.h"
+#include <math.h>
+#include <boost/bind.hpp>
+
+CVelSpline::CVelSpline()
+{
+  this->coeff[0]=0.0; 
+  this->coeff[1]=0.0; 
+  this->coeff[2]=0.0; 
+  this->coeff[3]=0.0; 
+  // initialize optimization objects
+  this->time_grad.set_function(boost::bind(&CVelSpline::length_pow2,this,_1));
+  this->time_grad.set_function_der(boost::bind(&CVelSpline::length_pow2_der,this,_1));
+  this->time_grad.set_max_coord_inc(1.0);
+}
+
+CVelSpline::CVelSpline(double start_vel,double end_vel,double max_vel,double start_acc,double end_acc,double max_acc) : CVelProfile(start_vel,end_vel,max_vel,start_acc,end_acc,max_acc)
+{
+  this->coeff[0]=0.0; 
+  this->coeff[1]=0.0; 
+  this->coeff[2]=0.0; 
+  this->coeff[3]=0.0; 
+  // initialize optimization objects
+  this->time_grad.set_function(boost::bind(&CVelSpline::length_pow2,this,_1));
+  this->time_grad.set_function_der(boost::bind(&CVelSpline::length_pow2_der,this,_1));
+}
+
+double CVelSpline::length_pow2(double time)
+{
+  double pow_t,length;
+
+  length=-this->target_length;
+  pow_t=time;
+  length+=this->coeff[3]*pow_t;
+  pow_t*=time;
+  length+=(this->coeff[2]*pow_t)/2.0;
+  pow_t*=time;
+  length+=(this->coeff[1]*pow_t)/3.0;
+  pow_t*=time;
+  length+=(this->coeff[0]*pow_t)/4.0;
+  
+  return pow(length,2.0);
+}
+
+double CVelSpline::length_pow2_der(double time)
+{
+  double pow_t,grad,func;
+
+  func=-this->target_length;
+  grad=this->coeff[3];
+  pow_t=time;
+  func+=this->coeff[3]*pow_t;
+  grad+=this->coeff[2]*pow_t;
+  pow_t*=time;
+  func+=(this->coeff[2]*pow_t)/2.0;
+  grad+=this->coeff[1]*pow_t;
+  pow_t*=time;
+  func+=(this->coeff[1]*pow_t)/3.0;
+  grad+=this->coeff[0]*pow_t;
+  pow_t*=time;
+  func+=(this->coeff[0]*pow_t)/4.0;
+
+  return 2.0*grad*func;
+}
+
+void CVelSpline::set_target_length(double length)
+{
+  if(length<0.0)
+    throw CException(_HERE_,"Invalid length value");
+  else
+    this->target_length=length;
+}
+
+void CVelSpline::compute_max_vel(void)
+{
+  double time1,time2,vel1,vel2;
+
+  if(4.0*pow(this->coeff[1],2.0)>12*this->coeff[1]*this->coeff[2])
+  {
+    time1=(-2.0*this->coeff[1]+sqrt(4.0*pow(this->coeff[1],2.0)-12*this->coeff[1]*this->coeff[2]))/(6.0*this->coeff[0]);
+    vel1=this->coeff[3]+this->coeff[2]*time1+this->coeff[1]*pow(time1,2.0)+this->coeff[0]*pow(time1,3.0);
+    time2=(-2.0*this->coeff[1]-sqrt(4.0*pow(this->coeff[1],2.0)-12*this->coeff[1]*this->coeff[2]))/(6.0*this->coeff[0]);
+    vel2=this->coeff[3]+this->coeff[2]*time2+this->coeff[1]*pow(time2,2.0)+this->coeff[0]*pow(time2,3.0);
+    if(time1>=0.0 && time1<=this->time)
+    {
+      if(time2>=0.0 && time2<=this->time)
+        this->max_vel=(vel1+vel2)/2.0;
+      else
+        this->max_vel=vel1;
+    }
+    else
+    {
+      if(time2>=0.0 && time2<=this->time)
+        this->max_vel=vel2;
+      else
+        this->max_vel=this->start_vel;
+    }
+  }
+  else
+    this->max_vel=0.0;
+}
+
+void CVelSpline::generate_max_acc(double max_acc)
+{
+  double t1,t2,t3,T;
+
+  this->coeff[2]=this->start_acc;
+  this->coeff[3]=this->start_vel;  
+  if(this->start_vel==this->end_vel)// acceleration 0
+  {
+    if(this->start_acc!=0.0 || this->start_acc!=this->end_acc)
+    {
+      /* no solution */
+      throw CException(_HERE_,"Impossible to create spline");  
+    }
+    else
+    {
+      this->coeff[0]=0.0;
+      this->coeff[1]=0.0;
+      T=0.0;
+    }  
+  }
+  else if(this->start_vel>this->end_vel)//deceleration
+  {
+    t1=sqrt((max_acc+this->end_acc)/(max_acc+this->start_acc));
+    t2=max_acc+this->start_acc;
+    t3=pow(t2,2.0);
+    if(this->start_acc==-max_acc)
+    {
+      this->coeff[1]=0.0;
+      this->coeff[0]=-((max_acc-this->end_acc)*pow(2.0*max_acc+this->end_acc,2.0))/(27.0*pow(this->end_vel-this->start_vel,2.0));
+      T=sqrt(3.0*(this->end_acc+max_acc))/(3.0*sqrt(this->coeff[0]));
+    }
+    else
+    {
+      this->coeff[1]=-(-pow(t1+1,2.0)*t3+pow(t1+1,3.0)*t3/3.0+this->start_acc*(t1+1)*t2)/(this->end_vel-this->start_vel);
+      this->coeff[0]=pow(this->coeff[1],2.0)/(3.0*t2);
+      T=(t1+1)*t2/this->coeff[1];
+    }
+  }
+  else// acceleration
+  {
+    t1=sqrt((max_acc-this->end_acc)/(max_acc-this->start_acc));
+    t2=max_acc-this->start_acc;
+    t3=pow(t2,2.0);
+    if(this->start_acc==max_acc)
+    {
+      this->coeff[1]=0.0;
+      this->coeff[0]=-((max_acc-this->end_acc)*pow(2.0*max_acc+this->end_acc,2.0))/(27.0*pow(this->end_vel-this->start_vel,2.0));
+      T=-sqrt(3.0*(this->end_acc-max_acc))/(3.0*sqrt(this->coeff[0]));
+    }
+    else
+    {
+      this->coeff[1]=(pow(t1+1,2.0)*t3-pow(t1+1,3.0)*t3/3.0+this->start_acc*(t1+1)*t2)/(this->end_vel-this->start_vel);
+      this->coeff[0]=-pow(this->coeff[1],2.0)/(3.0*t2);
+      T=(t1+1)*t2/this->coeff[1];
+    }
+  }
+  this->time=T;
+  this->length=this->start_vel*T;
+  T*=this->time;
+  this->length+=(this->start_acc/2.0)*T;
+  T*=this->time;
+  this->length+=(this->coeff[1]/3.0)*T;
+  T*=this->time;
+  this->length+=(this->coeff[0]/4.0)*T;
+  this->max_acc=max_acc;
+  this->compute_max_vel();
+  this->generated=true;
+  this->time_grad.set_coord_constraints(this->time,0.0);
+}
+
+bool CVelSpline::generate_max_length(double &max_len)
+{
+  double t,b,a;
+  bool status=true;
+
+  this->coeff[2]=this->start_acc;
+  this->coeff[3]=this->start_vel;
+  if(fabs(this->start_acc-this->end_acc)<0.001)
+  {
+    if(fabs(this->end_vel-this->start_vel)<0.001)
+    {
+      this->generate_constant_vel(this->start_vel,max_len);
+      return true;
+    }
+    else
+    {
+      this->time=(2.0*max_len)/(this->end_vel+this->start_vel);
+      this->coeff[1]=(3.0*(this->end_vel-this->start_vel))/pow(this->time,2.0);
+      this->coeff[0]=-(2.0*this->coeff[1])/(3.0*this->time);
+      this->max_acc=this->start_acc-pow(this->coeff[1],2.0)/(3.0*this->coeff[0]);
+    }
+  } 
+  else
+  {
+    if((this->end_acc-this->start_acc)<((3.0*pow(this->start_vel+this->end_vel,2.0))/(4.0*max_len)))
+    {
+      t=(3.0*this->end_vel+3.0*this->start_vel-sqrt(9.0*pow(this->end_vel,2.0)+18.0*this->end_vel*this->start_vel+9.0*pow(this->start_vel,2.0)-12.0*max_len*this->end_acc+12.0*max_len*this->start_acc))/(this->end_acc-this->start_acc);
+      b=-(3.0*(this->start_vel-this->end_vel)+(this->end_acc+2.0*this->start_acc)*t)/pow(t,2.0);
+      a=-(this->start_acc-this->end_acc+2.0*b*t)/(3.0*pow(t,2.0));
+    }
+    else
+    {
+      t=(3.0*this->end_vel+3.0*this->start_vel)/(this->end_acc-this->start_acc);
+      b=-(3.0*(this->start_vel-this->end_vel)+(this->end_acc+2.0*this->start_acc)*t)/pow(t,2.0);
+      a=-(this->start_acc-this->end_acc+2.0*b*t)/(3.0*pow(t,2.0));
+      max_len=(3.0*pow(this->start_vel+this->end_vel,2.0))/(4.0*(this->end_acc-this->start_acc));
+      status=false;
+    }
+    this->max_acc=this->start_acc-pow(b,2.0)/(3.0*a);
+    this->time=t;
+    this->coeff[1]=b;
+    this->coeff[0]=a;
+  }
+  this->length=max_len;
+  this->compute_max_vel();
+  this->generated=true;
+  this->time_grad.set_coord_constraints(this->time,0.0);
+
+  return status;
+}
+
+void CVelSpline::generate_max_acc(double max_acc,double start_vel,double start_acc,double end_vel,double end_acc)
+{
+  this->set_start_vel(start_vel);
+  this->set_start_acc(start_acc);
+  this->set_end_vel(end_vel);
+  this->set_end_acc(end_acc);
+  this->generate_max_acc(max_acc);
+}
+
+bool CVelSpline::generate_max_length(double &max_len,double start_vel,double start_acc,double end_vel,double end_acc)
+{
+  this->set_start_vel(start_vel);
+  this->set_start_acc(start_acc);
+  this->set_end_vel(end_vel);
+  this->set_end_acc(end_acc);
+  return this->generate_max_length(max_len);
+}
+
+void CVelSpline::generate_constant_vel(double vel, double length)
+{
+  this->coeff[0]=0.0;
+  this->coeff[1]=0.0;
+  this->coeff[2]=0.0;
+  this->coeff[3]=vel;
+  this->start_vel=vel;
+  this->end_vel=vel;
+  this->start_acc=0.0;
+  this->end_acc=0.0;
+  this->length=length;
+  if(vel==0.0)
+    this->time=1.0;
+  else
+    this->time=length/vel;
+  this->max_acc=0.0;
+  this->max_vel=vel;
+  this->generated=true;
+}
+
+void CVelSpline::update_max_start_vel(double max_acc,double length,double max_end_vel)
+{
+  double T,vel;
+  double t1;
+
+  this->set_end_vel(max_end_vel);
+  if(this->start_vel>max_end_vel)// decelerate
+  {
+    t1=2.0*max_acc*sqrt((max_acc+this->end_acc-this->start_acc)/max_acc);
+    T=-(6.0*this->end_vel-6.0*sqrt(pow(this->end_vel,2.0)+2.0*length*max_acc/3.0-length*(this->end_acc+this->start_acc)+length*t1/3.0))/(-3.0*(this->end_acc+this->start_acc)+2*max_acc+t1);
+    vel=this->end_vel+(max_acc*T)/3.0-(this->end_acc*T)/3.0-(2.0*this->start_acc*T)/3.0+(T*t1)/6.0;
+    this->set_start_vel(vel);
+    this->coeff[1]=-(t1/2.0+max_acc)/T;
+    this->coeff[0]=pow(this->coeff[1],2.0)/(3.0*max_acc);
+    this->time=T;
+  }
+  else//accelerate
+  {
+//    T1=(6.0*this->end_vel+6.0*sqrt(pow(this->end_vel,2.0)-2.0*length*max_acc/3.0-length*(this->end_acc+this->start_acc)-length*t1/3.0))/(3.0*(this->end_acc+this->start_acc)+2*max_acc+t1);
+//    T2=(6.0*this->end_vel-6.0*sqrt(pow(this->end_vel,2.0)-2.0*length*max_acc/3.0-length*(this->end_acc+this->start_acc)-length*t1/3.0))/(3.0*(this->end_acc+this->start_acc)+2*max_acc+t1);
+//    T3=(6.0*this->end_vel+6.0*sqrt(pow(this->end_vel,2.0)-2.0*length*max_acc/3.0-length*(this->end_acc+this->start_acc)+length*t1/3.0))/(3.0*(this->end_acc+this->start_acc)+2*max_acc-t1);
+//    T4=(6.0*this->end_vel-6.0*sqrt(pow(this->end_vel,2.0)-2.0*length*max_acc/3.0-length*(this->end_acc+this->start_acc)+length*t1/3.0))/(3.0*(this->end_acc+this->start_acc)+2*max_acc-t1);
+//    vel=this->end_vel+(max_acc*T4)/3.0-(this->end_acc*T4)/3.0-(2.0*this->start_acc*T4)/3.0+(max_acc*T4*t1)/3.0;
+//    this->set_start_vel(vel);
+//    this->coeff[1]=-(max_acc*(t1+1.0))/T4;
+//    this->coeff[0]=-pow(this->coeff[1],2.0)/(3.0*max_acc);
+//    this->time=T4;
+  }
+  this->max_acc=max_acc;
+  this->length=length;
+}
+
+void CVelSpline::evaluate_at_time(double time,double &vel,double &acc,double &len,double start_len)
+{
+  double pow_t;
+
+  if(!this->generated)
+    this->generate_max_acc(DEFAULT_MAX_ACC);
+  if(time>this->time)
+    throw CException(_HERE_,"Spline not defined at the desired time");
+  else
+  {
+    vel=this->coeff[3];
+    acc=this->coeff[2];
+    len=start_len;
+    pow_t=time;
+    len+=this->coeff[3]*pow_t;
+    vel+=this->coeff[2]*pow_t;
+    acc+=2.0*this->coeff[1]*pow_t;
+    pow_t*=time;
+    len+=this->coeff[2]*pow_t/2.0;
+    vel+=this->coeff[1]*pow_t;
+    acc+=3.0*this->coeff[0]*pow_t;
+    pow_t*=time;
+    len+=this->coeff[1]*pow_t/3.0;
+    vel+=this->coeff[0]*pow_t;
+    pow_t*=time;
+    len+=this->coeff[0]*pow_t/4.0;
+  }  
+}
+
+void CVelSpline::evaluate_at_length(double length,double &vel,double &acc,double &time)
+{
+  double start_point,pow_t;
+  bool beyond_limits;
+
+  start_point=this->time/2.0;
+  this->set_target_length(length);
+  if(!this->time_grad.compute(0.1,100,start_point,beyond_limits,false))
+    throw CException(_HERE_,"No solution found for the spline");
+  else
+  {
+    time=this->time_grad.get_coordinate();
+    vel=this->coeff[3];
+    acc=this->coeff[2];
+    pow_t=time;
+    vel+=this->coeff[2]*pow_t;
+    acc+=2.0*this->coeff[1]*pow_t;
+    pow_t*=time;
+    vel+=this->coeff[1]*pow_t;
+    acc+=3.0*this->coeff[0]*pow_t;
+    pow_t*=time;
+    vel+=this->coeff[0]*pow_t;
+  }
+}
+
+void CVelSpline::evaluate_all(double resolution,std::vector<double> &vel, std::vector<double> &acc,std::vector<double> &len,double start_len)
+{
+  unsigned int i=0,num_points;
+  double pow_t,t;
+
+  if(resolution<0.0)
+    throw CException(_HERE_,"Invalid time resolution");
+  if(!this->generated)
+    this->generate_max_acc(DEFAULT_MAX_ACC);
+  num_points=ceil(this->time/resolution);
+  vel.resize(num_points);
+  acc.resize(num_points);
+  len.resize(num_points);
+  for(i=0,t=0;i<num_points;i++,t+=this->time/(num_points-1))
+  {
+    vel[i]=this->coeff[3];
+    acc[i]=this->coeff[2];
+    len[i]=start_len;
+    pow_t=t;
+    len[i]+=this->coeff[3]*pow_t;
+    vel[i]+=this->coeff[2]*pow_t;
+    acc[i]+=2.0*this->coeff[1]*pow_t;
+    pow_t*=t;
+    len[i]+=this->coeff[2]*pow_t/2.0;
+    vel[i]+=this->coeff[1]*pow_t;
+    acc[i]+=3.0*this->coeff[0]*pow_t;
+    pow_t*=t;
+    len[i]+=this->coeff[1]*pow_t/3.0;
+    vel[i]+=this->coeff[0]*pow_t;
+    pow_t*=t;
+    len[i]+=this->coeff[0]*pow_t/4.0;
+  }
+}
+
+CVelSpline::~CVelSpline()
+{
+ 
+}
+
diff --git a/src/vel_trapezoid.cpp b/src/vel_trapezoid.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ebb3d76e81f69588b98d6344150b49005d687096
--- /dev/null
+++ b/src/vel_trapezoid.cpp
@@ -0,0 +1,136 @@
+#include "vel_trapezoid.h"
+#include "exceptions.h"
+#include <math.h>
+
+CVelTrapezoid::CVelTrapezoid()
+{
+  this->acc_time=0.0;
+  this->dec_time=0.0;
+}
+
+CVelTrapezoid::CVelTrapezoid(double start_vel,double end_vel,double max_vel,double start_acc,double end_acc,double max_acc) : CVelProfile(start_vel,end_vel,max_vel,start_acc,end_acc,max_acc)
+{
+  this->acc_time=0.0;
+  this->dec_time=0.0;
+}
+
+void CVelTrapezoid::generate(double start_vel,double end_vel,double max_vel, double max_acc,double length)
+{
+  this->set_start_vel(start_vel);
+  this->set_end_vel(end_vel);
+  this->set_max_vel(max_vel);
+  this->set_max_acc(max_acc);
+  if(start_vel==max_vel)
+    this->acc_time=0.0;
+  else
+    this->acc_time=fabs(max_vel-start_vel)/max_acc;
+  if(end_vel==max_vel)
+    this->dec_time=0.0;
+  else
+    this->dec_time=fabs(end_vel-max_vel)/max_acc;
+  this->acc_length=std::min(start_vel,max_vel)*this->acc_time+fabs(max_vel-start_vel)*this->acc_time/2.0;
+  this->dec_length=std::min(end_vel,max_vel)*this->dec_time+fabs(end_vel-max_vel)*this->dec_time/2.0;
+  this->length=length;
+  this->generated=true;
+}
+
+bool CVelTrapezoid::generate(double start_vel,double end_vel,double max_vel, double max_acc,double length,double &new_start_vel,double &new_max_vel)
+{
+  /*
+  if(start_vel>max_vel)
+    throw CException(_HERE_,"Start velocity must be smaller than the maximum velocity");
+  if(end_vel>max_vel)
+    throw CException(_HERE_,"End velocity must be smaller than the maximum velocity");
+  if(max_acc<=0.0)
+    throw CException(_HERE_,"Acceleration must be positive");
+  */
+  if(start_vel>max_vel)
+    start_vel=max_vel;
+  if(end_vel>max_vel)
+    end_vel=max_vel;
+  if(max_acc<=0.0)
+    max_acc=0.0;
+  this->generate(start_vel,end_vel,max_vel,max_acc,length);
+  if((length-this->acc_length-this->dec_length)>0.0)
+  {
+    this->time=(length-this->acc_length-this->dec_length)/this->max_vel+this->acc_time+this->dec_time;
+    new_start_vel=this->start_vel;
+    new_max_vel=this->max_vel;
+    return true;
+  }
+  else
+  {
+    new_max_vel=sqrt(length*this->max_acc+(pow(start_vel,2.0)+pow(end_vel,2.0))/2.0);
+    if(new_max_vel>=start_vel)
+    {
+      this->generate(this->start_vel,this->end_vel,new_max_vel,this->max_acc,this->length);
+      this->time=(length-this->acc_length-this->dec_length)/new_max_vel+this->acc_time+this->dec_time;
+      new_start_vel=this->start_vel;
+      return true;
+    }
+    else
+    {
+      new_max_vel=sqrt(pow(end_vel,2.0)+2*length*this->max_acc);
+      this->generate(this->start_vel,this->end_vel,new_max_vel,this->max_acc,this->length);
+      this->time=(length-this->acc_length-this->dec_length)/new_max_vel+this->acc_time+this->dec_time;
+      new_start_vel=this->start_vel;
+      return true;
+    }
+  }
+}
+
+void CVelTrapezoid::evaluate_at_time(double time,double &vel,double &acc,double &len,double start_len)
+{
+  if(time>this->time)
+    throw CException(_HERE_,"Trapezoid not defined at the desired time");
+  if(!this->generated)
+    throw CException(_HERE_,"Trapezoid not generated");
+  if(time<=this->acc_time)
+  {
+    vel=this->max_acc*time+this->start_vel;
+    acc=this->max_acc;
+    len=start_len+this->max_acc*pow(time,2.0)/2.0+this->start_vel*time;
+  }
+  else if(time>=this->time-this->dec_time)
+  {
+    vel=this->max_vel-this->max_acc*(time-this->time+this->dec_time);
+    acc=-this->max_acc;
+    len=start_len+this->max_acc*pow(this->acc_time,2.0)/2.0+this->start_vel*this->acc_time;
+    len+=this->max_vel*(this->time-this->acc_time-this->dec_time);
+    len+=-this->max_acc*pow(time-this->time+this->dec_time,2.0)/2.0+this->max_vel*(time-this->time+this->dec_time);
+  }
+  else
+  {
+    vel=this->max_vel;
+    acc=0.0;
+    len=start_len+this->max_acc*pow(this->acc_time,2.0)/2.0+this->start_vel*this->acc_time;
+    len+=this->max_vel*(time-this->acc_time);
+  }
+}
+
+void CVelTrapezoid::evaluate_at_length(double length,double &vel,double &acc,double &time)
+{
+}
+
+void CVelTrapezoid::evaluate_all(double resolution,std::vector<double> &vel, std::vector<double> &acc,std::vector<double> &len,double start_len)
+{
+  unsigned int i=0,num_points;
+  double t;
+
+  if(time>this->time)
+    throw CException(_HERE_,"Trapezoid not defined at the desired time");
+  if(!this->generated)
+    throw CException(_HERE_,"Trapezoid not generated");
+  num_points=ceil(this->time/resolution);
+  vel.resize(num_points);
+  acc.resize(num_points);
+  len.resize(num_points);
+  for(i=0,t=0;i<num_points;i++,t+=resolution)
+    this->evaluate_at_time(t,vel[i],acc[i],len[i],start_len);
+}
+
+CVelTrapezoid::~CVelTrapezoid()
+{
+ 
+}
+
diff --git a/src/xml/add.xodr b/src/xml/add.xodr
new file mode 100644
index 0000000000000000000000000000000000000000..3a1f52624f118d2b1351a9bd44df25b2806a688e
--- /dev/null
+++ b/src/xml/add.xodr
@@ -0,0 +1,2498 @@
+<?xml version="1.0" ?>
+<OpenDRIVE>
+    <header revMajor="1" revMinor="1" name="Testfile" version="1" date="Thu Feb 8 14:24:06 2007" north="2.0000000000000000e+03" south="-2.0000000000000000e+03" east="2.0000000000000000e+03" west="-2.0000000000000000e+03" />
+    <road name="road0" length="1.3119999999999999e+01" id="0" junction="-1">
+        <link>
+            <predecessor elementType="junction" elementId="1" contactPoint="end" />
+            <successor elementType="road" elementId="1" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="8.3379999999999995e+01" y="1.7204999999999998e+01" hdg="0.0000000000000000e+00" length="1.3119999999999999e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects>
+            <object s="1.0500000000000000e+01" t="1.4910000000000000e+01" id="2005" name="building5" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="1.4779999999999999e+01" width="9.8000000000000007e+00" height="2.0000000000000000e+00" />
+        </objects>
+        <signals>
+            <signal s="1.0500000000000000e+01" t="7.5199999999999996e+00" id="101" name="locsign1" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="1.2800000000000001e+01" t="-1.6120000000000001e+01" id="102" name="locsign2" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="1.0500000000000000e+01" t="2.2300000000000001e+01" id="106" name="locsign6" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+        </signals>
+    </road>
+    <road name="road1" length="2.0232056689000000e+01" id="1" junction="-1">
+        <link>
+            <predecessor elementType="road" elementId="0" contactPoint="end" />
+            <successor elementType="road" elementId="2" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="9.6500000000000000e+01" y="1.7204999999999998e+01" hdg="0.0000000000000000e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="7.7639752000000006e-02" />
+            </geometry>
+            <geometry s="1.0000000000000000e-04" x="9.6500100000000003e+01" y="1.7205000000129399e+01" hdg="3.8819876000000010e-06" length="2.0231856689000001e+01">
+                <arc curvature="7.7639752000000006e-02" />
+            </geometry>
+            <geometry s="2.0231956689000000e+01" x="1.0938004992571371e+02" y="3.0085050042260775e+01" hdg="1.5708002178211014e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="7.7639752000000006e-02" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="road2" length="4.1550000000000002e+00" id="2" junction="-1">
+        <link>
+            <predecessor elementType="road" elementId="1" contactPoint="end" />
+            <successor elementType="road" elementId="3" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="1.0938000000000000e+02" y="3.0085000000000001e+01" hdg="1.5707000000000000e+00" length="4.1550000000000002e+00">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals>
+            <signal s="2.1250000000000000e+00" t="1.0480000000000000e+01" id="103" name="locsign3" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="2.1250000000000000e+00" t="2.0280000000000001e+01" id="107" name="locsign7" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="2.1250000000000000e+00" t="-1.1119999999999999e+01" id="104" name="locsign4" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+        </signals>
+    </road>
+    <road name="road3" length="2.0232056689000000e+01" id="3" junction="-1">
+        <link>
+            <predecessor elementType="road" elementId="2" contactPoint="end" />
+            <successor elementType="road" elementId="25" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="1.0938000000000000e+02" y="3.4240000000000002e+01" hdg="1.5707000000000000e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="7.7639752000000006e-02" />
+            </geometry>
+            <geometry s="1.0000000000000000e-04" x="1.0938000000950328e+02" y="3.4240099999999551e+01" hdg="1.5707038819876000e+00" length="2.0231856689000001e+01">
+                <arc curvature="7.7639752000000006e-02" />
+            </geometry>
+            <geometry s="2.0231956689000000e+01" x="9.6501190711420790e+01" y="4.7121290559894419e+01" hdg="3.1415002178211013e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="7.7639752000000006e-02" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="road4" length="4.0526745231000007e+01" id="4" junction="-1">
+        <link>
+            <predecessor elementType="road" elementId="25" contactPoint="end" />
+            <successor elementType="road" elementId="26" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="9.6280000000000001e+01" y="4.7119999999999997e+01" hdg="3.1415899999999999e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="-7.7519378999999999e-02" />
+            </geometry>
+            <geometry s="1.0000000000000000e-04" x="9.6279899999999998e+01" y="4.7120000000394555e+01" hdg="3.1415861240310501e+00" length="4.0526545231000000e+01">
+                <arc curvature="-7.7519378999999999e-02" />
+            </geometry>
+            <geometry s="4.0526645231000003e+01" x="9.6280068020569701e+01" y="7.2920000281067445e+01" hdg="-6.4952914806681861e-06" length="1.0000000000000000e-04">
+                <spiral curvStart="-7.7519378999999999e-02" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects>
+            <object s="2.0231900000000000e+01" t="-3.7880000000000003e+01" id="3000" name="fence0" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="barrier" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="2.0000000000000000e+00" width="1.2200000000000000e+02" height="2.0000000000000000e+00" />
+        </objects>
+        <signals>
+            <signal s="2.0231900000000000e+01" t="-3.6880000000000003e+01" id="105" name="locsign5" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="2.0231900000000000e+01" t="1.5960000000000001e+01" id="121" name="locsign21" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="2.0231900000000000e+01" t="2.9960000000000001e+01" id="122" name="locsign22" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+        </signals>
+    </road>
+    <road name="road5" length="2.0232056689000000e+01" id="5" junction="-1">
+        <link>
+            <predecessor elementType="road" elementId="26" contactPoint="end" />
+            <successor elementType="road" elementId="6" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="9.6680000000000007e+01" y="7.2920000000000002e+01" hdg="0.0000000000000000e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="7.7639752000000006e-02" />
+            </geometry>
+            <geometry s="1.0000000000000000e-04" x="9.6680100000000010e+01" y="7.2920000000129406e+01" hdg="3.8819876000000010e-06" length="2.0231856689000001e+01">
+                <arc curvature="7.7639752000000006e-02" />
+            </geometry>
+            <geometry s="2.0231956689000000e+01" x="1.0956004992571371e+02" y="8.5800050042260779e+01" hdg="1.5708002178211014e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="7.7639752000000006e-02" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="road6" length="4.2800000000000002e+00" id="6" junction="-1">
+        <link>
+            <predecessor elementType="road" elementId="5" contactPoint="end" />
+            <successor elementType="road" elementId="7" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="1.0956000000000000e+02" y="8.5799999999999997e+01" hdg="1.5707000000000000e+00" length="4.2800000000000002e+00">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals>
+            <signal s="2.1250000000000000e+00" t="-1.1119999999999999e+01" id="108" name="locsign8" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="2.1250000000000000e+00" t="1.1880000000000001e+01" id="109" name="locsign9" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="2.1250000000000000e+00" t="2.3879999999999999e+01" id="110" name="locsign10" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="2.1250000000000000e+00" t="4.1719999999999999e+01" id="119" name="locsign19" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="2.1250000000000000e+00" t="5.5719999999999999e+01" id="120" name="locsign20" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+        </signals>
+    </road>
+    <road name="road7" length="2.0232056689000000e+01" id="7" junction="-1">
+        <link>
+            <predecessor elementType="road" elementId="6" contactPoint="end" />
+            <successor elementType="road" elementId="8" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="1.0956000000000000e+02" y="9.0079999999999998e+01" hdg="1.5707000000000000e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="7.7639752000000006e-02" />
+            </geometry>
+            <geometry s="1.0000000000000000e-04" x="1.0956000000950328e+02" y="9.0080099999999547e+01" hdg="1.5707038819876000e+00" length="2.0231856689000001e+01">
+                <arc curvature="7.7639752000000006e-02" />
+            </geometry>
+            <geometry s="2.0231956689000000e+01" x="9.6681190711420797e+01" y="1.0296129055989442e+02" hdg="3.1415002178211013e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="7.7639752000000006e-02" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="road8" length="1.3240000000000000e+01" id="8" junction="-1">
+        <link>
+            <predecessor elementType="road" elementId="7" contactPoint="end" />
+            <successor elementType="junction" elementId="2" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="9.6680000000000007e+01" y="1.0295999999999999e+02" hdg="3.1415899999999999e+00" length="1.3240000000000000e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects>
+            <object s="5.0000000000000000e+00" t="1.5000000000000000e+01" id="2007" name="building7" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" radius="6.0000000000000000e+00" height="2.0000000000000000e+00" />
+        </objects>
+        <signals>
+            <signal s="5.0000000000000000e+00" t="9.0000000000000000e+00" id="111" name="locsign11" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="5.0000000000000000e+00" t="2.1000000000000000e+01" id="112" name="locsign12" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="5.0000000000000000e+00" t="-1.6120000000000001e+01" id="113" name="locsign13" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+        </signals>
+    </road>
+    <road name="road9" length="1.5050000000000001e+01" id="9" junction="-1">
+        <link>
+            <predecessor elementType="junction" elementId="2" contactPoint="end" />
+            <successor elementType="junction" elementId="3" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="6.7640000000000001e+01" y="1.0295999999999999e+02" hdg="3.1415899999999999e+00" length="1.5050000000000001e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects>
+            <object s="7.2400000000000002e+00" t="1.5480000000000000e+01" id="2002" name="building2" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="1.4000000000000000e+01" width="1.4000000000000000e+01" height="2.0000000000000000e+00" />
+            <object s="7.2400000000000002e+00" t="4.3659999999999997e+01" id="2003" name="building3" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="1.4000000000000000e+01" width="1.4000000000000000e+01" height="2.0000000000000000e+00" />
+            <object s="7.4000000000000004e+00" t="-1.7120000000000001e+01" id="3003" name="fence3" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="2.0000000000000000e+00" width="1.2000000000000000e+02" height="2.0000000000000000e+00" />
+            <object s="7.5250000000000004e+00" t="-6.5999999999999996e+00" id="4006" name="parking6" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="parkingSpace" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="8.6999999999999993e+00" width="4.7000000000000002e+00" height="2.0000000000000000e+00">
+                <parkingSpace access="all" restrictions="none">
+                    <marking side="front" type="solid" width="1.4999999999999999e-01" color="standard" />
+                    <marking side="rear" type="solid" width="1.4999999999999999e-01" color="standard" />
+                    <marking side="right" type="solid" width="1.4999999999999999e-01" color="standard" />
+                </parkingSpace>
+            </object>
+        </objects>
+        <signals>
+            <signal s="7.2400000000000002e+00" t="-1.6120000000000001e+01" id="114" name="locsign14" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="7.2400000000000002e+00" t="8.4800000000000004e+00" id="115" name="locsign15" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="7.2400000000000002e+00" t="2.2480000000000000e+01" id="116" name="locsign16" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="7.2400000000000002e+00" t="3.6659999999999997e+01" id="117" name="locsign17" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="7.2400000000000002e+00" t="5.0659999999999997e+01" id="118" name="locsign18" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+        </signals>
+    </road>
+    <road name="road10" length="1.3119999999999999e+01" id="10" junction="-1">
+        <link>
+            <predecessor elementType="junction" elementId="3" contactPoint="end" />
+            <successor elementType="road" elementId="11" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="3.6789999999999999e+01" y="1.0295999999999999e+02" hdg="3.1415899999999999e+00" length="1.3119999999999999e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals>
+            <signal s="8.3750000000000000e+00" t="-1.6120000000000001e+01" id="123" name="locsign23" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="8.3750000000000000e+00" t="8.4499999999999993e+00" id="124" name="locsign24" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="8.3750000000000000e+00" t="2.2230000000000000e+01" id="125" name="locsign25" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="7.8399999999999999e+00" t="3.5969999999999999e+01" id="132" name="locsign32" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="7.8399999999999999e+00" t="4.9969999999999999e+01" id="133" name="locsign33" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+        </signals>
+    </road>
+    <road name="road11" length="2.0232056689000000e+01" id="11" junction="-1">
+        <link>
+            <predecessor elementType="road" elementId="10" contactPoint="end" />
+            <successor elementType="road" elementId="12" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="2.3859999999999999e+01" y="1.0295999999999999e+02" hdg="3.1415899999999999e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="7.7639752000000006e-02" />
+            </geometry>
+            <geometry s="1.0000000000000000e-04" x="2.3859900000000000e+01" y="1.0296000000013595e+02" hdg="3.1415938819875997e+00" length="2.0231856689000001e+01">
+                <arc curvature="7.7639752000000006e-02" />
+            </geometry>
+            <geometry s="2.0231956689000000e+01" x="1.0979915895962318e+01" y="9.0079984136153584e+01" hdg="4.7123902178211008e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="7.7639752000000006e-02" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="road12" length="7.2100000000000000e+00" id="12" junction="-1">
+        <link>
+            <predecessor elementType="road" elementId="11" contactPoint="end" />
+            <successor elementType="junction" elementId="4" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="1.0980000000000000e+01" y="9.0079999999999998e+01" hdg="4.7123889800000001e+00" length="7.2100000000000000e+00">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects>
+            <object s="2.4600000000000000e+00" t="1.7504999999999999e+01" id="2000" name="building0" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="1.1570000000000000e+01" width="1.3779999999999999e+01" height="2.0000000000000000e+00" />
+        </objects>
+        <signals>
+            <signal s="2.4600000000000000e+00" t="1.1720000000000001e+01" id="126" name="locsign26" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="2.4600000000000000e+00" t="2.3289999999999999e+01" id="127" name="locsign27" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="2.4600000000000000e+00" t="-1.1119999999999999e+01" id="128" name="locsign28" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+        </signals>
+    </road>
+    <road name="road13" length="1.4170000000000000e+01" id="13" junction="-1">
+        <link>
+            <predecessor elementType="junction" elementId="4" contactPoint="end" />
+            <successor elementType="junction" elementId="5" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="1.0980000000000000e+01" y="6.7069999999999993e+01" hdg="4.7123889800000001e+00" length="1.4170000000000000e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects>
+            <object s="7.2000000000000002e+00" t="1.8039999999999999e+01" id="2001" name="building1" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="1.4000000000000000e+01" width="1.4000000000000000e+01" height="2.0000000000000000e+00" />
+            <object s="7.2000000000000002e+00" t="-1.2119999999999999e+01" id="3001" name="fence1" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="2.0000000000000000e+00" width="1.2200000000000000e+02" height="2.0000000000000000e+00" />
+        </objects>
+        <signals>
+            <signal s="7.2000000000000002e+00" t="-1.1119999999999999e+01" id="129" name="locsign29" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="7.2000000000000002e+00" t="1.1039999999999999e+01" id="130" name="locsign30" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="7.2000000000000002e+00" t="2.5039999999999999e+01" id="131" name="locsign31" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+        </signals>
+    </road>
+    <road name="road14" length="7.0149999999999997e+00" id="14" junction="-1">
+        <link>
+            <predecessor elementType="junction" elementId="5" contactPoint="end" />
+            <successor elementType="road" elementId="15" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="1.0980000000000000e+01" y="3.7100000000000001e+01" hdg="4.7123889800000001e+00" length="7.0149999999999997e+00">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals>
+            <signal s="4.2999999999999998e+00" t="-1.1119999999999999e+01" id="134" name="locsign34" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="4.2999999999999998e+00" t="1.0840000000000000e+01" id="135" name="locsign35" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="4.2999999999999998e+00" t="2.4840000000000000e+01" id="136" name="locsign36" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="5.4299999999999997e+00" t="4.1430000000000000e+01" id="143" name="locsign43" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="5.4299999999999997e+00" t="5.5979999999999997e+01" id="144" name="locsign44" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+        </signals>
+    </road>
+    <road name="road15" length="2.0232056689000000e+01" id="15" junction="-1">
+        <link>
+            <predecessor elementType="road" elementId="14" contactPoint="end" />
+            <successor elementType="road" elementId="16" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="1.0980000000000000e+01" y="3.0085000000000001e+01" hdg="4.7123889800000001e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="7.7639752000000006e-02" />
+            </geometry>
+            <geometry s="1.0000000000000000e-04" x="1.0980000000129360e+01" y="3.0084900000000001e+01" hdg="4.7123928619875999e+00" length="2.0231856689000001e+01">
+                <arc curvature="7.7639752000000006e-02" />
+            </geometry>
+            <geometry s="2.0231956689000000e+01" x="2.3860050037305950e+01" y="1.7204950069331481e+01" hdg="6.2831891978211010e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="7.7639752000000006e-02" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="road16" length="1.2960000000000001e+01" id="16" junction="-1">
+        <link>
+            <predecessor elementType="road" elementId="15" contactPoint="end" />
+            <successor elementType="junction" elementId="0" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="2.3859999999999999e+01" y="1.7204999999999998e+01" hdg="0.0000000000000000e+00" length="1.2960000000000001e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects>
+            <object s="4.9600000000000000e+00" t="1.5779999999999999e+01" id="2004" name="building4" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="1.0000000000000000e+01" width="1.4000000000000000e+01" height="2.0000000000000000e+00" />
+        </objects>
+        <signals>
+            <signal s="4.9600000000000000e+00" t="1.0779999999999999e+01" id="137" name="locsign37" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="4.9600000000000000e+00" t="2.0780000000000001e+01" id="138" name="locsign38" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="4.9600000000000000e+00" t="-1.6120000000000001e+01" id="139" name="locsign39" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+        </signals>
+    </road>
+    <road name="road17" length="1.4960000000000001e+01" id="17" junction="-1">
+        <link>
+            <predecessor elementType="junction" elementId="0" contactPoint="end" />
+            <successor elementType="junction" elementId="1" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="unknown" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="5.2619999999999997e+01" y="1.7204999999999998e+01" hdg="0.0000000000000000e+00" length="1.4960000000000001e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects>
+            <object s="7.2249999999999996e+00" t="1.4650000000000000e+01" id="2006" name="building6" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="1.3080000000000000e+01" width="1.4550000000000001e+01" height="2.0000000000000000e+00" />
+            <object s="7.4000000000000004e+00" t="-1.7120000000000001e+01" id="3002" name="fence2" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="2.0000000000000000e+00" width="1.2000000000000000e+02" height="2.0000000000000000e+00" />
+            <object s="7.4800000000000004e+00" t="-6.5999999999999996e+00" id="4001" name="parking1" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="parkingSpace" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="8.6999999999999993e+00" width="4.7000000000000002e+00" height="2.0000000000000000e+00">
+                <parkingSpace access="all" restrictions="none">
+                    <marking side="front" type="solid" width="1.4999999999999999e-01" color="standard" />
+                    <marking side="rear" type="solid" width="1.4999999999999999e-01" color="standard" />
+                    <marking side="right" type="solid" width="1.4999999999999999e-01" color="standard" />
+                </parkingSpace>
+            </object>
+        </objects>
+        <signals>
+            <signal s="7.2249999999999996e+00" t="-1.6120000000000001e+01" id="140" name="locsign40" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="7.2249999999999996e+00" t="8.1099999999999994e+00" id="141" name="locsign41" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+            <signal s="7.2249999999999996e+00" t="2.1190000000000001e+01" id="142" name="locsign42" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" />
+        </signals>
+    </road>
+    <road name="road18" length="1.5740000000000000e+01" id="18" junction="-1">
+        <link>
+            <predecessor elementType="junction" elementId="7" contactPoint="end" />
+            <successor elementType="junction" elementId="5" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="3.6820000000000000e+01" y="4.2799999999999997e+01" hdg="3.1415899999999999e+00" length="1.5740000000000000e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="road19" length="9.7949999999999999e+00" id="19" junction="-1">
+        <link>
+            <predecessor elementType="junction" elementId="7" contactPoint="end" />
+            <successor elementType="junction" elementId="0" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="4.6920000000000002e+01" y="3.7100000000000001e+01" hdg="4.7123889800000001e+00" length="9.7949999999999999e+00">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="road20" length="3.9499313349999994e+01" id="20" junction="-1">
+        <link>
+            <predecessor elementType="junction" elementId="7" contactPoint="end" />
+            <successor elementType="junction" elementId="1" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="unknown" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="5.2619999999999997e+01" y="4.7200000000000003e+01" hdg="0.0000000000000000e+00" length="1.2359999999999999e+01">
+                <line />
+            </geometry>
+            <geometry s="1.2359999999999999e+01" x="6.4979999999999990e+01" y="4.7200000000000003e+01" hdg="0.0000000000000000e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="-7.8740157000000005e-02" />
+            </geometry>
+            <geometry s="1.2360099999999999e+01" x="6.4980099999999993e+01" y="4.7199999999868773e+01" hdg="-3.9370078499816491e-06" length="1.9949113350000001e+01">
+                <arc curvature="-7.8740157000000005e-02" />
+            </geometry>
+            <geometry s="3.2309213350000000e+01" x="7.7680050077372044e+01" y="3.4499950044481970e+01" hdg="-1.5708002541976460e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="-7.8740157000000005e-02" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+            <geometry s="3.2309313350000004e+01" x="7.7680050076716839e+01" y="3.4499850044481967e+01" hdg="-1.5708041912054962e+00" length="7.1900000000000004e+00">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects>
+            <object s="3.4249099999999999e+01" t="4.4000000000000004e+00" id="4003" name="parking3" dynamic="no" orientation="none" zOffset="0.0000000000000000e+00" validLength="0.0000000000000000e+00" type="parkingSpace" hdg="1.5707000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="8.6999999999999993e+00" width="4.7000000000000002e+00" height="2.0000000000000000e+00">
+                <parkingSpace access="all" restrictions="none">
+                    <marking side="front" type="solid" width="1.4999999999999999e-01" color="standard" />
+                    <marking side="left" type="solid" width="1.4999999999999999e-01" color="standard" />
+                    <marking side="right" type="solid" width="1.4999999999999999e-01" color="standard" />
+                </parkingSpace>
+            </object>
+            <object s="3.8949100000000001e+01" t="4.4000000000000004e+00" id="4004" name="parking4" dynamic="no" orientation="none" zOffset="0.0000000000000000e+00" validLength="0.0000000000000000e+00" type="parkingSpace" hdg="1.5707000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="8.6999999999999993e+00" width="4.7000000000000002e+00" height="2.0000000000000000e+00">
+                <parkingSpace access="all" restrictions="none">
+                    <marking side="front" type="solid" width="1.4999999999999999e-01" color="standard" />
+                    <marking side="left" type="solid" width="1.4999999999999999e-01" color="standard" />
+                    <marking side="right" type="solid" width="1.4999999999999999e-01" color="standard" />
+                </parkingSpace>
+            </object>
+        </objects>
+        <signals />
+    </road>
+    <road name="road21" length="1.4170000000000000e+01" id="21" junction="-1">
+        <link>
+            <predecessor elementType="junction" elementId="6" contactPoint="end" />
+            <successor elementType="junction" elementId="7" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="4.6920000000000002e+01" y="6.7069999999999993e+01" hdg="4.7123889800000001e+00" length="1.4170000000000000e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="road22" length="1.5740000000000000e+01" id="22" junction="-1">
+        <link>
+            <predecessor elementType="junction" elementId="4" contactPoint="end" />
+            <successor elementType="junction" elementId="6" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="2.1079999999999998e+01" y="7.7170000000000002e+01" hdg="0.0000000000000000e+00" length="1.5740000000000000e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="road23" length="1.0015000000000001e+01" id="23" junction="-1">
+        <link>
+            <predecessor elementType="junction" elementId="6" contactPoint="end" />
+            <successor elementType="junction" elementId="3" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="4.2520000000000003e+01" y="8.2870000000000005e+01" hdg="1.5707000000000000e+00" length="1.0015000000000001e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="road24" length="3.2847809632999997e+01" id="24" junction="-1">
+        <link>
+            <predecessor elementType="junction" elementId="6" contactPoint="end" />
+            <successor elementType="junction" elementId="2" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="5.2619999999999997e+01" y="7.7170000000000002e+01" hdg="0.0000000000000000e+00" length="1.2420000000000000e+01">
+                <line />
+            </geometry>
+            <geometry s="1.2420000000000000e+01" x="6.5039999999999992e+01" y="7.7170000000000002e+01" hdg="0.0000000000000000e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="1.2048192700000000e-01" />
+            </geometry>
+            <geometry s="1.2420100000000000e+01" x="6.5040099999999995e+01" y="7.7170000000200801e+01" hdg="6.0240963499719210e-06" length="1.3037609633000001e+01">
+                <arc curvature="1.2048192700000000e-01" />
+            </geometry>
+            <geometry s="2.5457709633000000e+01" x="7.3340050048819123e+01" y="8.5470050092700660e+01" hdg="1.5708023561539528e+00" length="1.0000000000000000e-04">
+                <spiral curvStart="1.2048192700000000e-01" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+            <geometry s="2.5457809633000000e+01" x="7.3340050047814572e+01" y="8.5470150092700649e+01" hdg="1.5708083802503028e+00" length="7.3899999999999997e+00">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="road25" length="2.0000000000000001e-01" id="25" junction="-1">
+        <link>
+            <predecessor elementType="road" elementId="3" contactPoint="end" />
+            <successor elementType="road" elementId="4" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="9.6480000000000004e+01" y="4.7119999999999997e+01" hdg="3.1415899999999999e+00" length="2.0000000000000001e-01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="road26" length="4.0000000000000002e-01" id="26" junction="-1">
+        <link>
+            <predecessor elementType="road" elementId="4" contactPoint="end" />
+            <successor elementType="road" elementId="5" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="9.6280000000000001e+01" y="7.2920000000000002e+01" hdg="0.0000000000000000e+00" length="4.0000000000000002e-01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_16_-1_17_-1" length="1.5799999999999997e+01" id="1000" junction="0">
+        <link>
+            <predecessor elementType="road" elementId="16" contactPoint="end" />
+            <successor elementType="road" elementId="17" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="3.6820000000000000e+01" y="1.7204999999999998e+01" hdg="0.0000000000000000e+00" length="1.5799999999999997e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects>
+            <object s="1.4580000000000000e+01" t="-6.7500000000000000e+00" id="4000" name="parking0" dynamic="yes" orientation="none" zOffset="0.0000000000000000e+00" validLength="0.0000000000000000e+00" type="parkingSpace" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="8.6999999999999993e+00" width="4.7000000000000002e+00" height="2.0000000000000000e+00">
+                <parkingSpace access="all" restrictions="none">
+                    <marking side="front" type="solid" width="1.4999999999999999e-01" color="standard" />
+                    <marking side="right" type="solid" width="1.4999999999999999e-01" color="standard" />
+                    <marking side="rear" type="solid" width="1.4999999999999999e-01" color="standard" />
+                </parkingSpace>
+            </object>
+        </objects>
+        <signals />
+    </road>
+    <road name="junction_17_1_16_1" length="1.5799999999999997e+01" id="1001" junction="0">
+        <link>
+            <predecessor elementType="road" elementId="17" contactPoint="start" />
+            <successor elementType="road" elementId="16" contactPoint="end" />
+        </link>
+        <type s="0.0000000000000000e+00" type="unknown" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="5.2619999999999997e+01" y="1.7204999999999998e+01" hdg="3.1415799999999998e+00" length="1.5799999999999997e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_19_-1_16_1" length="1.5867055652294562e+01" id="1002" junction="0">
+        <link>
+            <predecessor elementType="road" elementId="19" contactPoint="end" />
+            <successor elementType="road" elementId="16" contactPoint="end" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="4.6919999996231965e+01" y="2.7305000000000000e+01" hdg="4.7123889800000001e+00" length="1.2780810170376355e-04">
+                <line />
+            </geometry>
+            <geometry s="1.2780810170376355e-04" x="4.6919999996231915e+01" y="2.7304872191898298e+01" hdg="4.7123889800000001e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="-9.9011416593046367e-02" />
+            </geometry>
+            <geometry s="1.1278081017037636e-03" x="4.6919999979729631e+01" y="2.7303872191898542e+01" hdg="4.7123394742917037e+00" length="1.5864927844192859e+01">
+                <arc curvature="-9.9011416593046367e-02" />
+            </geometry>
+            <geometry s="1.5866055652294563e+01" x="3.6819526797096088e+01" y="1.7204526819585467e+01" hdg="3.1415304942917039e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="-9.9011416593046367e-02" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_19_-1_17_-1" length="1.3355597643755036e+01" id="1003" junction="0">
+        <link>
+            <predecessor elementType="road" elementId="19" contactPoint="end" />
+            <successor elementType="road" elementId="17" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="4.6919999996231965e+01" y="2.7305000000000000e+01" hdg="4.7123889800000001e+00" length="4.3999999923466024e+00">
+                <line />
+            </geometry>
+            <geometry s="4.3999999923466024e+00" x="4.6919999994539332e+01" y="2.2905000007653399e+01" hdg="4.7123889800000001e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="1.7543462205417634e-01" />
+            </geometry>
+            <geometry s="4.4009999923466028e+00" x="4.6920000023778051e+01" y="2.2904000007654169e+01" hdg="4.7124766973110273e+00" length="8.9535976514084350e+00">
+                <arc curvature="1.7543462205417634e-01" />
+            </geometry>
+            <geometry s="1.3354597643755039e+01" x="5.2620484880042277e+01" y="1.7204370884176605e+01" hdg="6.2832477173110277e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="1.7543462205417634e-01" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_17_-1_0_-1" length="1.5799999999999997e+01" id="1004" junction="1">
+        <link>
+            <predecessor elementType="road" elementId="17" contactPoint="end" />
+            <successor elementType="road" elementId="0" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="unknown" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="6.7579999999999998e+01" y="1.7204999999999998e+01" hdg="0.0000000000000000e+00" length="1.5799999999999997e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects>
+            <object s="1.2200000000000000e+00" t="-6.7500000000000000e+00" id="4002" name="parking2" dynamic="no" orientation="none" zOffset="0.0000000000000000e+00" validLength="0.0000000000000000e+00" type="parkingSpace" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="8.6999999999999993e+00" width="4.7000000000000002e+00" height="2.0000000000000000e+00">
+                <parkingSpace access="all" restrictions="none">
+                    <marking side="right" type="solid" width="1.4999999999999999e-01" color="standard" />
+                    <marking side="front" type="solid" width="1.4999999999999999e-01" color="standard" />
+                    <marking side="rear" type="solid" width="1.4999999999999999e-01" color="standard" />
+                </parkingSpace>
+            </object>
+        </objects>
+        <signals>
+            <signal s="6.0000000000000000e+00" t="-9.0000000000000000e+00" id="10000" name="global_loc" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1001" subtype="-1" value="0.0000000000000000e+00" />
+        </signals>
+    </road>
+    <road name="junction_0_1_17_1" length="1.5799999999999997e+01" id="1005" junction="1">
+        <link>
+            <predecessor elementType="road" elementId="0" contactPoint="start" />
+            <successor elementType="road" elementId="17" contactPoint="end" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="8.3379999999999995e+01" y="1.7204999999999998e+01" hdg="3.1415799999999998e+00" length="1.5799999999999997e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_20_-1_17_1" length="1.5871901969384437e+01" id="1006" junction="1">
+        <link>
+            <predecessor elementType="road" elementId="20" contactPoint="end" />
+            <successor elementType="road" elementId="17" contactPoint="end" />
+        </link>
+        <type s="0.0000000000000000e+00" type="unknown" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="7.7679993531604623e+01" y="2.7309850044704316e+01" hdg="4.7123758087945031e+00" length="5.1174112856067211e-03">
+                <line />
+            </geometry>
+            <geometry s="5.1174112856067211e-03" x="7.7679993464200180e+01" y="2.7304732633419153e+01" hdg="4.7123758087945031e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="-9.9011480618728345e-02" />
+            </geometry>
+            <geometry s="6.1174112856067211e-03" x="7.7679993434526679e+01" y="2.7303732633419703e+01" hdg="4.7123263030541942e+00" length="1.5864784558098831e+01">
+                <arc curvature="-9.9011480618728345e-02" />
+            </geometry>
+            <geometry s="1.5870901969384438e+01" x="6.7579526790493134e+01" y="1.7204526819215236e+01" hdg="3.1415304942596904e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="-9.9011480618728345e-02" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_20_-1_0_-1" length="1.3360484496420572e+01" id="1007" junction="1">
+        <link>
+            <predecessor elementType="road" elementId="20" contactPoint="end" />
+            <successor elementType="road" elementId="0" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="unknown" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="7.7679993531604623e+01" y="2.7309850044704316e+01" hdg="4.7123758087945031e+00" length="4.4047104802418007e+00">
+                <line />
+            </geometry>
+            <geometry s="4.4047104802418007e+00" x="7.7679935514563283e+01" y="2.2905139564844603e+01" hdg="4.7123758087945031e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="1.7543263749645815e-01" />
+            </geometry>
+            <geometry s="4.4057104802418010e+00" x="7.7679935530630473e+01" y="2.2904139564845075e+01" hdg="4.7124635251132512e+00" length="8.9537740161787713e+00">
+                <arc curvature="1.7543263749645815e-01" />
+            </geometry>
+            <geometry s="1.3359484496420572e+01" x="8.3380484873285440e+01" y="1.7204370881014821e+01" hdg="6.2832477163187477e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="1.7543263749645815e-01" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_8_-1_9_-1" length="1.5800000000085685e+01" id="1008" junction="2">
+        <link>
+            <predecessor elementType="road" elementId="8" contactPoint="end" />
+            <successor elementType="road" elementId="9" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="8.3440000000046624e+01" y="1.0296003513352886e+02" hdg="3.1415899999999999e+00" length="1.5800000000085685e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects>
+            <object s="1.4625000000000000e+01" t="-6.8499999999999996e+00" id="4005" name="parking5" dynamic="no" orientation="none" zOffset="0.0000000000000000e+00" validLength="0.0000000000000000e+00" type="parkingSpace" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="8.6999999999999993e+00" width="4.7000000000000002e+00" height="2.0000000000000000e+00">
+                <parkingSpace access="all" restrictions="none">
+                    <marking side="right" type="solid" width="1.4999999999999999e-01" color="standard" />
+                    <marking side="rear" type="solid" width="1.4999999999999999e-01" color="standard" />
+                    <marking side="front" type="solid" width="1.4999999999999999e-01" color="standard" />
+                </parkingSpace>
+            </object>
+        </objects>
+        <signals />
+    </road>
+    <road name="junction_9_1_8_1" length="1.5800000000085685e+01" id="1009" junction="2">
+        <link>
+            <predecessor elementType="road" elementId="9" contactPoint="start" />
+            <successor elementType="road" elementId="8" contactPoint="end" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="6.7640000000000001e+01" y="1.0295999999999999e+02" hdg="6.2831700000000001e+00" length="1.5800000000085685e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_24_-1_8_1" length="1.5867172362152607e+01" id="1010" junction="2">
+        <link>
+            <predecessor elementType="road" elementId="24" contactPoint="end" />
+            <successor elementType="road" elementId="8" contactPoint="end" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="7.3339960972779124e+01" y="9.2860150092163821e+01" hdg="1.5708083802503028e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="-9.9009978376276273e-02" />
+            </geometry>
+            <geometry s="1.0000000000000000e-03" x="7.3339960977227321e+01" y="9.2861150092163697e+01" hdg="1.5707588752611146e+00" length="1.5865051240397811e+01">
+                <arc curvature="-9.9009978376276273e-02" />
+            </geometry>
+            <geometry s="1.5866051240397811e+01" x="8.3440352076065167e+01" y="1.0296076392992921e+02" hdg="-3.9504989187477690e-05" length="1.0000000000000000e-03">
+                <spiral curvStart="-9.9009978376276273e-02" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+            <geometry s="1.5867051240397810e+01" x="8.3441352076062429e+01" y="1.0296076385742090e+02" hdg="-8.9009978375560961e-05" length="1.2112175479650489e-04">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_24_-1_9_-1" length="1.3355306172018555e+01" id="1011" junction="2">
+        <link>
+            <predecessor elementType="road" elementId="24" contactPoint="end" />
+            <successor elementType="road" elementId="9" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="7.3339960972779124e+01" y="9.2860150092163821e+01" hdg="1.5708083802503028e+00" length="4.3999955486436164e+00">
+                <line />
+            </geometry>
+            <geometry s="4.3999955486436164e+00" x="7.3339907937628993e+01" y="9.7260145640487806e+01" hdg="1.5708083802503028e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="1.7544143008383561e-01" />
+            </geometry>
+            <geometry s="4.4009955486436168e+00" x="7.3339907896335291e+01" y="9.7261145640486603e+01" hdg="1.5708961009653448e+00" length="8.9533106233749393e+00">
+                <arc curvature="1.7544143008383561e-01" />
+            </geometry>
+            <geometry s="1.3354306172018557e+01" x="6.7639515111861243e+01" y="1.0296048485434106e+02" hdg="3.1416777207150419e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="1.7544143008383561e-01" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_12_-1_13_-1" length="1.5800000000000011e+01" id="1012" junction="4">
+        <link>
+            <predecessor elementType="road" elementId="12" contactPoint="end" />
+            <successor elementType="road" elementId="13" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="1.0979999997226386e+01" y="8.2870000000000005e+01" hdg="4.7123889800000001e+00" length="1.5800000000000011e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_13_1_12_1" length="1.5800000000000011e+01" id="1013" junction="4">
+        <link>
+            <predecessor elementType="road" elementId="13" contactPoint="start" />
+            <successor elementType="road" elementId="12" contactPoint="end" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="1.0980000000000000e+01" y="6.7069999999999993e+01" hdg="1.5707889799999997e+00" length="1.5800000000000011e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_12_-1_22_-1" length="1.3355597644352770e+01" id="1014" junction="4">
+        <link>
+            <predecessor elementType="road" elementId="12" contactPoint="end" />
+            <successor elementType="road" elementId="22" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="1.0979999997226386e+01" y="8.2870000000000005e+01" hdg="4.7123889800000001e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="1.7543462228973258e-01" />
+            </geometry>
+            <geometry s="1.0000000000000000e-03" x="1.0980000026465104e+01" y="8.2869000000000767e+01" hdg="4.7124766973111445e+00" length="8.9535976393864303e+00">
+                <arc curvature="1.7543462228973258e-01" />
+            </geometry>
+            <geometry s="8.9545976393864297e+00" x="1.6680484875075951e+01" y="7.7169370884176772e+01" hdg="6.2832477173111441e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="1.7543462228973258e-01" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+            <geometry s="8.9555976393864292e+00" x="1.6681484875068303e+01" y="7.7169371005065116e+01" hdg="6.2833354346222885e+00" length="4.4000000049663406e+00">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_13_1_22_-1" length="1.5867000801409723e+01" id="1015" junction="4">
+        <link>
+            <predecessor elementType="road" elementId="13" contactPoint="start" />
+            <successor elementType="road" elementId="22" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="1.0980000000000000e+01" y="6.7069999999999993e+01" hdg="1.5707889799999997e+00" length="7.4202901048536773e-05">
+                <line />
+            </geometry>
+            <geometry s="7.4202901048536773e-05" x="1.0980000000545154e+01" y="6.7070074202901040e+01" hdg="1.5707889799999997e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="-9.9010163724782427e-02" />
+            </geometry>
+            <geometry s="1.0742029010485368e-03" x="1.0980000024393643e+01" y="6.7071074202900647e+01" hdg="1.5707394749181374e+00" length="1.5864926598508676e+01">
+                <arc curvature="-9.9010163724782427e-02" />
+            </geometry>
+            <geometry s="1.5866000801409724e+01" x="2.1080473206577260e+01" y="7.7170473186599196e+01" hdg="-4.9505081862299960e-05" length="1.0000000000000000e-03">
+                <spiral curvStart="-9.9010163724782427e-02" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_9_-1_10_-1" length="1.5800000000103461e+01" id="1016" junction="3">
+        <link>
+            <predecessor elementType="road" elementId="9" contactPoint="end" />
+            <successor elementType="road" elementId="10" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="5.2590000000052989e+01" y="1.0296003993652639e+02" hdg="3.1415899999999999e+00" length="1.5800000000103461e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects>
+            <object s="4.2499999999999999e-01" t="-6.8499999999999996e+00" id="4007" name="parking7" dynamic="no" orientation="none" zOffset="0.0000000000000000e+00" validLength="0.0000000000000000e+00" type="parkingSpace" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="8.6999999999999993e+00" width="4.7000000000000002e+00" height="2.0000000000000000e+00">
+                <parkingSpace access="all" restrictions="none">
+                    <marking side="rear" type="solid" width="1.4999999999999999e-01" color="standard" />
+                    <marking side="front" type="solid" width="1.4999999999999999e-01" color="standard" />
+                    <marking side="right" type="solid" width="1.4999999999999999e-01" color="standard" />
+                </parkingSpace>
+            </object>
+        </objects>
+        <signals />
+    </road>
+    <road name="junction_10_1_9_1" length="1.5800000000103461e+01" id="1017" junction="3">
+        <link>
+            <predecessor elementType="road" elementId="10" contactPoint="start" />
+            <successor elementType="road" elementId="9" contactPoint="end" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="3.6789999999999999e+01" y="1.0295999999999999e+02" hdg="6.2831700000000001e+00" length="1.5800000000103461e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_23_-1_9_1" length="1.5824577584342926e+01" id="1018" junction="3">
+        <link>
+            <predecessor elementType="road" elementId="23" contactPoint="end" />
+            <successor elementType="road" elementId="9" contactPoint="end" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="4.2520964712849398e+01" y="9.2884999953536152e+01" hdg="1.5707000000000000e+00" length="7.1293661841558276e-03">
+                <line />
+            </geometry>
+            <geometry s="7.1293661841558276e-03" x="4.2520965399598389e+01" y="9.2892129319687228e+01" hdg="1.5707000000000000e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="-9.9313657022795335e-02" />
+            </geometry>
+            <geometry s="8.1293661841558285e-03" x="4.2520965512477460e+01" y="9.2893129319680753e+01" hdg="1.5706503431714887e+00" length="1.5815448218158771e+01">
+                <arc curvature="-9.9313657022795335e-02" />
+            </geometry>
+            <geometry s="1.5823577584342926e+01" x="5.2590473333978316e+01" y="1.0296076800668715e+02" hdg="-3.9656828511258624e-05" length="1.0000000000000000e-03">
+                <spiral curvStart="-9.9313657022795335e-02" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_23_-1_10_-1" length="1.3348422063760626e+01" id="1019" junction="3">
+        <link>
+            <predecessor elementType="road" elementId="23" contactPoint="end" />
+            <successor elementType="road" elementId="10" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="4.2520964712849398e+01" y="9.2884999953536152e+01" hdg="1.5707000000000000e+00" length="4.3430496791307425e+00">
+                <line />
+            </geometry>
+            <geometry s="4.3430496791307425e+00" x="4.2521383064904420e+01" y="9.7228049612517637e+01" hdg="1.5707000000000000e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="1.7447795480299652e-01" />
+            </geometry>
+            <geometry s="4.3440496791307428e+00" x="4.2521383132151556e+01" y="9.7229049612515041e+01" hdg="1.5707872389774016e+00" length="9.0033723846298841e+00">
+                <arc curvature="1.7447795480299652e-01" />
+            </geometry>
+            <geometry s="1.3347422063760627e+01" x="3.6789515249613551e+01" y="1.0296048476929037e+02" hdg="3.1416772389774015e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="1.7447795480299652e-01" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_13_-1_14_-1" length="1.5799999999999990e+01" id="1024" junction="5">
+        <link>
+            <predecessor elementType="road" elementId="13" contactPoint="end" />
+            <successor elementType="road" elementId="14" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="1.0979999994548946e+01" y="5.2899999999999991e+01" hdg="4.7123889800000001e+00" length="1.5799999999999990e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_14_1_13_1" length="1.5799999999999990e+01" id="1025" junction="5">
+        <link>
+            <predecessor elementType="road" elementId="14" contactPoint="start" />
+            <successor elementType="road" elementId="13" contactPoint="end" />
+        </link>
+        <type s="0.0000000000000000e+00" type="motorway" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="1.0980000000000000e+01" y="3.7100000000000001e+01" hdg="1.5707889799999997e+00" length="1.5799999999999990e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_18_-1_13_1" length="1.5867008814246613e+01" id="1026" junction="5">
+        <link>
+            <predecessor elementType="road" elementId="18" contactPoint="end" />
+            <successor elementType="road" elementId="13" contactPoint="end" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="2.1080000000055417e+01" y="4.2800041767503345e+01" hdg="3.1415899999999999e+00" length="1.4277635133908007e-04">
+                <line />
+            </geometry>
+            <geometry s="1.4277635133908007e-04" x="2.1079857223704078e+01" y="4.2800041767882213e+01" hdg="3.1415899999999999e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="-9.9011300583814563e-02" />
+            </geometry>
+            <geometry s="1.1427763513390801e-03" x="2.1078857223704368e+01" y="4.2800041787037692e+01" hdg="3.1415404943497078e+00" length="1.5864866037895275e+01">
+                <arc curvature="-9.9011300583814563e-02" />
+            </geometry>
+            <geometry s="1.5866008814246614e+01" x="1.0979526811441305e+01" y="5.2900473204341438e+01" hdg="1.5707394743497076e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="-9.9011300583814563e-02" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_18_-1_14_-1" length="1.3355545807755462e+01" id="1027" junction="5">
+        <link>
+            <predecessor elementType="road" elementId="18" contactPoint="end" />
+            <successor elementType="road" elementId="14" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="2.1080000000055417e+01" y="4.2800041767503345e+01" hdg="3.1415899999999999e+00" length="4.3999314291379683e+00">
+                <line />
+            </geometry>
+            <geometry s="4.3999314291379683e+00" x="1.6680068570932939e+01" y="4.2800053443116475e+01" hdg="3.1415899999999999e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="1.7543741706715577e-01" />
+            </geometry>
+            <geometry s="4.4009314291379686e+00" x="1.6679068570933634e+01" y="4.2800053416530496e+01" hdg="3.1416777187085336e+00" length="8.9536143786174947e+00">
+                <arc curvature="1.7543741706715577e-01" />
+            </geometry>
+            <geometry s="1.3354545807755464e+01" x="1.0979515147553306e+01" y="3.7099515119601108e+01" hdg="4.7124766987085334e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="1.7543741706715577e-01" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_22_-1_21_-1" length="1.5867146698236754e+01" id="1028" junction="6">
+        <link>
+            <predecessor elementType="road" elementId="22" contactPoint="end" />
+            <successor elementType="road" elementId="21" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="3.6820000000000000e+01" y="7.7170000000000002e+01" hdg="0.0000000000000000e+00" length="3.8853595896171100e-09">
+                <line />
+            </geometry>
+            <geometry s="3.8853595896171100e-09" x="3.6820000003885362e+01" y="7.7170000000000002e+01" hdg="0.0000000000000000e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="-9.9007658123908465e-02" />
+            </geometry>
+            <geometry s="1.0000038853595896e-03" x="3.6821000003885118e+01" y="7.7169999983498727e+01" hdg="-4.9503829061954224e-05" length="1.5865146694351395e+01">
+                <arc curvature="-9.9007658123908465e-02" />
+            </geometry>
+            <geometry s="1.5866146698236754e+01" x="4.6920728800891588e+01" y="6.7069526800330266e+01" hdg="-1.5708205238290616e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="-9.9007658123908465e-02" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_22_-1_23_-1" length="8.9555152901052466e+00" id="1029" junction="6">
+        <link>
+            <predecessor elementType="road" elementId="22" contactPoint="end" />
+            <successor elementType="road" elementId="23" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="3.6820000000000000e+01" y="7.7170000000000002e+01" hdg="0.0000000000000000e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="1.7543906284793165e-01" />
+            </geometry>
+            <geometry s="1.0000000000000000e-03" x="3.6820999999999231e+01" y="7.7170000029239844e+01" hdg="8.7719531423965832e-05" length="8.9529662009279125e+00">
+                <arc curvature="1.7543906284793165e-01" />
+            </geometry>
+            <geometry s="8.9539662009279120e+00" x="4.2520484847899709e+01" y="8.2869935794149555e+01" hdg="1.5707877195314239e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="1.7543906284793165e-01" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+            <geometry s="8.9549662009279114e+00" x="4.2520484798027283e+01" y="8.2870935794147968e+01" hdg="1.5708754390628479e+00" length="5.4908917733520468e-04">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_22_-1_24_-1" length="1.5799999999999997e+01" id="1030" junction="6">
+        <link>
+            <predecessor elementType="road" elementId="22" contactPoint="end" />
+            <successor elementType="road" elementId="24" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="3.6820000000000000e+01" y="7.7170000000000002e+01" hdg="0.0000000000000000e+00" length="1.5799999999999997e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_21_-1_18_-1" length="1.5867012301500518e+01" id="1034" junction="7">
+        <link>
+            <predecessor elementType="road" elementId="21" contactPoint="end" />
+            <successor elementType="road" elementId="18" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="4.6919999994548945e+01" y="5.2899999999999991e+01" hdg="4.7123889800000001e+00" length="2.6810557756107301e-05">
+                <line />
+            </geometry>
+            <geometry s="2.6810557756107301e-05" x="4.6919999994548938e+01" y="5.2899973189442235e+01" hdg="4.7123889800000001e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="-9.9010426507907062e-02" />
+            </geometry>
+            <geometry s="1.0268105577561073e-03" x="4.6919999978046818e+01" y="5.2898973189442479e+01" hdg="4.7123394747867460e+00" length="1.5864985490942763e+01">
+                <arc curvature="-9.9010426507907062e-02" />
+            </geometry>
+            <geometry s="1.5866012301500518e+01" x="3.6819526797095953e+01" y="4.2799526814853344e+01" hdg="3.1415404947867458e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="-9.9010426507907062e-02" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_21_-1_19_-1" length="1.5799999999999990e+01" id="1035" junction="7">
+        <link>
+            <predecessor elementType="road" elementId="21" contactPoint="end" />
+            <successor elementType="road" elementId="19" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="4.6919999994548945e+01" y="5.2899999999999991e+01" hdg="4.7123889800000001e+00" length="1.5799999999999990e+01">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <road name="junction_21_-1_20_-1" length="8.9555976470302028e+00" id="1036" junction="7">
+        <link>
+            <predecessor elementType="road" elementId="21" contactPoint="end" />
+            <successor elementType="road" elementId="20" contactPoint="start" />
+        </link>
+        <type s="0.0000000000000000e+00" type="town" />
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="4.6919999994548945e+01" y="5.2899999999999991e+01" hdg="4.7123889800000001e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="0.0000000000000000e+00" curvEnd="1.7543462228973303e-01" />
+            </geometry>
+            <geometry s="1.0000000000000000e-03" x="4.6920000023787665e+01" y="5.2899000000000761e+01" hdg="4.7124766973111445e+00" length="8.9535976393864072e+00">
+                <arc curvature="1.7543462228973303e-01" />
+            </geometry>
+            <geometry s="8.9545976393864066e+00" x="5.2620484872398499e+01" y="4.7199370884176780e+01" hdg="6.2832477173111441e+00" length="1.0000000000000000e-03">
+                <spiral curvStart="1.7543462228973303e-01" curvEnd="0.0000000000000000e+00" />
+            </geometry>
+            <geometry s="8.9555976393864061e+00" x="5.2621484872390852e+01" y="4.7199371005065117e+01" hdg="6.2833354346222885e+00" length="7.6437958185238131e-09">
+                <line />
+            </geometry>
+        </planView>
+        <elevationProfile />
+        <lateralProfile />
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <center>
+                    <lane id="0" type="driving" level="false">
+                        <link />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="false">
+                        <link />
+                        <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" />
+                        <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects />
+        <signals />
+    </road>
+    <junction name="junction0" id="0">
+        <connection id="0" incomingRoad="16" connectingRoad="17" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+        <connection id="1" incomingRoad="17" connectingRoad="16" contactPoint="start">
+            <laneLink from="1" to="1" />
+        </connection>
+        <connection id="2" incomingRoad="19" connectingRoad="16" contactPoint="end">
+            <laneLink from="-1" to="1" />
+        </connection>
+        <connection id="3" incomingRoad="19" connectingRoad="17" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+    </junction>
+    <junction name="junction1" id="1">
+        <connection id="0" incomingRoad="17" connectingRoad="0" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+        <connection id="1" incomingRoad="0" connectingRoad="17" contactPoint="start">
+            <laneLink from="1" to="1" />
+        </connection>
+        <connection id="2" incomingRoad="20" connectingRoad="17" contactPoint="end">
+            <laneLink from="-1" to="1" />
+        </connection>
+        <connection id="3" incomingRoad="20" connectingRoad="0" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+    </junction>
+    <junction name="junction2" id="2">
+        <connection id="0" incomingRoad="8" connectingRoad="9" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+        <connection id="1" incomingRoad="9" connectingRoad="8" contactPoint="start">
+            <laneLink from="1" to="1" />
+        </connection>
+        <connection id="2" incomingRoad="24" connectingRoad="8" contactPoint="end">
+            <laneLink from="-1" to="1" />
+        </connection>
+        <connection id="3" incomingRoad="24" connectingRoad="9" contactPoint="start">
+            <laneLink from="-1" to="-1" />
+        </connection>
+    </junction>
+    <junction name="junction3" id="3">
+        <connection id="0" incomingRoad="9" connectingRoad="10" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+        <connection id="1" incomingRoad="10" connectingRoad="9" contactPoint="start">
+            <laneLink from="1" to="1" />
+        </connection>
+        <connection id="2" incomingRoad="23" connectingRoad="9" contactPoint="end">
+            <laneLink from="-1" to="1" />
+        </connection>
+        <connection id="3" incomingRoad="23" connectingRoad="10" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+    </junction>
+    <junction name="junction4" id="4">
+        <connection id="0" incomingRoad="12" connectingRoad="13" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+        <connection id="1" incomingRoad="13" connectingRoad="12" contactPoint="start">
+            <laneLink from="1" to="1" />
+        </connection>
+        <connection id="2" incomingRoad="12" connectingRoad="22" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+        <connection id="3" incomingRoad="13" connectingRoad="22" contactPoint="start">
+            <laneLink from="1" to="-1" />
+        </connection>
+    </junction>
+    <junction name="junction5" id="5">
+        <connection id="0" incomingRoad="13" connectingRoad="14" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+        <connection id="1" incomingRoad="14" connectingRoad="13" contactPoint="start">
+            <laneLink from="1" to="1" />
+        </connection>
+        <connection id="2" incomingRoad="18" connectingRoad="13" contactPoint="end">
+            <laneLink from="-1" to="1" />
+        </connection>
+        <connection id="3" incomingRoad="18" connectingRoad="14" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+    </junction>
+    <junction name="junction6" id="6">
+        <connection id="0" incomingRoad="22" connectingRoad="21" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+        <connection id="1" incomingRoad="22" connectingRoad="23" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+        <connection id="2" incomingRoad="22" connectingRoad="24" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+    </junction>
+    <junction name="junction7" id="7">
+        <connection id="0" incomingRoad="21" connectingRoad="18" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+        <connection id="1" incomingRoad="21" connectingRoad="19" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+        <connection id="2" incomingRoad="21" connectingRoad="20" contactPoint="end">
+            <laneLink from="-1" to="-1" />
+        </connection>
+    </junction>
+</OpenDRIVE>