diff --git a/include/opendrive_road.h b/include/opendrive_road.h
index 44cfea726379b38d3a354316dedfe01404a0ca00..5896723aa29fe89c8aa23162b22c401e27a587c7 100644
--- a/include/opendrive_road.h
+++ b/include/opendrive_road.h
@@ -28,16 +28,18 @@ class COpendriveRoad
     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,std::vector<unsigned int> &old_path,std::vector<unsigned int> &new_path);
+    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);
-    bool node_exists_at(TOpendriveWorldPose &pose);
-    COpendriveRoadNode* get_node_at(TOpendriveWorldPose &pose);
   public:
     COpendriveRoad();
     COpendriveRoad(const COpendriveRoad& object);
@@ -50,14 +52,21 @@ class COpendriveRoad
     double get_min_road_length(void) const;
     unsigned int get_num_segments(void) const;
     const COpendriveRoadSegment& get_segment(unsigned int index) const;
+    unsigned int get_num_lanes(void) const;
+    const COpendriveLane &get_lane(unsigned int index) const;
     unsigned int get_num_nodes(void) const;
     const COpendriveRoadNode& get_node(unsigned int index) const;
-    const COpendriveRoadSegment& operator[](std::size_t index);
-    unsigned int get_closest_node(TOpendriveWorldPose &pose,double angle_tol=0.1);
-    double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1);
-    void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1);
+    const COpendriveRoadSegment &operator[](std::size_t index);
+    const COpendriveRoadNode &get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
+    unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
+    const COpendriveLane &get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
+    unsigned int get_closest_lane_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
+    const COpendriveRoadSegment &get_closest_segment(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
+    unsigned int get_closest_segment_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
+    double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
+    void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1) const;
     std::vector<unsigned int> get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPose &start_pose,TOpendriveWorldPose &end_pose,COpendriveRoad &new_road);
-    void get_sub_road(TOpendriveWorldPose &start_pose,double length,COpendriveRoad &new_road);
+    void get_sub_road(TOpendriveWorldPose &start_pose,double &length,COpendriveRoad &new_road);
     void operator=(const COpendriveRoad& object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
     ~COpendriveRoad();
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index 9b300bdfd7a521b4fbc33362adddf8ed22049686..f26267a49ffd4fbff5286e72aeb48f07482500f0 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -316,7 +316,7 @@ bool COpendriveRoad::has_segment(COpendriveRoadSegment *segment)
   return false;
 }
 
-void COpendriveRoad::add_segment(COpendriveRoadSegment *segment,std::vector<unsigned int> &old_path,std::vector<unsigned int> &new_path)
+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
@@ -325,28 +325,36 @@ void COpendriveRoad::add_segment(COpendriveRoadSegment *segment,std::vector<unsi
   this->segments.push_back(segment);
   // add the lanes
   for(unsigned int i=1;i<=segment->get_num_right_lanes();i++)
-  {
-    segment->lanes[-i]->set_index(this->lanes.size());
     this->lanes.push_back(segment->lanes[-i]);
-  }
   for(unsigned int i=1;i<=segment->get_num_left_lanes();i++)
-  {
-    segment->lanes[i]->set_index(this->lanes.size());
     this->lanes.push_back(segment->lanes[i]);
-  }
   // add the nodes
   for(unsigned int i=0;i<segment->get_num_nodes();i++)
-  {
-    for(unsigned int j=0;j<old_path.size();j++)
-      if(old_path[j]==segment->nodes[i]->index)
-        new_path[j]=this->nodes.size();
-    segment->nodes[i]->set_index(this->nodes.size());
     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;
@@ -403,6 +411,16 @@ void COpendriveRoad::clean_references(segment_up_ref_t &segment_refs,lane_up_ref
   }
 }
 
+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;
@@ -466,17 +484,6 @@ void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
     else
       lane_it++;
   }
-  // re-index all nodes and the path
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  {
-    for(unsigned int j=0;j<path_nodes.size();j++)
-    {
-      if(this->nodes[i]->get_index()==path_nodes[j])
-        path_nodes[j]=i;
-    }
-    this->nodes[i]->set_index(i);
-  }
-
   // remove links to non-consecutive nodes
   for(unsigned int i=0;i<this->nodes.size();i++)
   {
@@ -516,7 +523,6 @@ COpendriveRoadNode* COpendriveRoad::get_node_at(TOpendriveWorldPose &pose)
   return NULL;
 }
 
-
 void COpendriveRoad::load(const std::string &filename)
 {
   struct stat buffer;
@@ -614,6 +620,24 @@ const COpendriveRoadSegment& COpendriveRoad::get_segment(unsigned int index) con
   }
 }
 
+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();
@@ -645,15 +669,45 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
   }
 }
 
-unsigned int COpendriveRoad::get_closest_node(TOpendriveWorldPose &pose,double angle_tol)
+unsigned int COpendriveRoad::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
 {
   double dist,min_dist=std::numeric_limits<double>::max();
   TOpendriveWorldPose closest_pose;
-  unsigned int closest_index;
+  unsigned int closest_index=-1;
 
   for(unsigned int i=0;i<this->nodes.size();i++)
+  {
+    this->nodes[i]->get_closest_pose(pose,closest_pose,false,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;
+      closest_index=i;
+    }
+  }
+
+  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();
+  TOpendriveWorldPose closest_pose;
+  unsigned int closest_index=-1;
+
+  for(unsigned int i=0;i<this->lanes.size();i++)
   { 
-    this->nodes[i]->get_closest_pose(pose,closest_pose,angle_tol);
+    this->lanes[i]->get_closest_pose(pose,closest_pose,angle_tol);
     dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
     if(dist<min_dist)
     { 
@@ -665,7 +719,47 @@ unsigned int COpendriveRoad::get_closest_node(TOpendriveWorldPose &pose,double a
   return closest_index;
 }
 
-double COpendriveRoad::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol)
+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();
+  TOpendriveWorldPose closest_pose;
+  unsigned int closest_index=-1;
+
+  for(unsigned int i=0;i<this->segments.size();i++)
+  { 
+    this->segments[i]->get_closest_pose(pose,closest_pose,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;
+      closest_index=i;
+    }
+  }
+
+  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;
@@ -673,7 +767,7 @@ double COpendriveRoad::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorl
 
   for(unsigned int i=0;i<this->nodes.size();i++)
   {
-    length=this->nodes[i]->get_closest_pose(pose,pose_found,angle_tol);
+    length=this->nodes[i]->get_closest_pose(pose,pose_found,false,angle_tol);
     dist=sqrt(pow(pose_found.x-pose.x,2.0)+pow(pose_found.y-pose.y,2.0));
     if(dist<min_dist)
     {
@@ -686,7 +780,7 @@ double COpendriveRoad::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorl
   return closest_length;
 }
 
-void COpendriveRoad::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol)
+void COpendriveRoad::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol) const
 {
   std::vector<TOpendriveWorldPose> poses;
   std::vector<double> lengths;
@@ -695,7 +789,7 @@ void COpendriveRoad::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOp
   closest_lengths.clear();
   for(unsigned int i=0;i<this->nodes.size();i++)
   {
-    this->nodes[i]->get_closest_poses(pose,poses,lengths,dist_tol,angle_tol);
+    this->nodes[i]->get_closest_poses(pose,poses,lengths,dist_tol,false,angle_tol);
     for(unsigned int j=0;j<poses.size();i++)
     {
       closest_poses.push_back(poses[i]);
@@ -709,6 +803,7 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
   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;
@@ -733,22 +828,22 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
     segment=link->segment;
     if(new_segment_ref.find(segment)==new_segment_ref.end())
     {
-      new_segment=segment->clone(new_node_ref,new_lane_ref);
-      new_road.add_segment(new_segment,path_nodes,new_path_nodes);
+      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(end_pose,3.14159);
+  link_index=node->get_closest_link(end_pose);
   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_road.add_segment(new_segment,path_nodes,new_path_nodes);
+    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;
   }
 
@@ -766,8 +861,8 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
         {
           if(!new_road.has_segment(new_segment_ref[this->segments[i]]))
           {
-            new_segment=this->segment[k]->clone(new_node_ref,new_lane_ref);
-            new_road.add_segment(new_segment,path_nodes,new_path_nodes);
+            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;
           }
         }
@@ -778,34 +873,79 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
   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(new_path_nodes);
+  new_road.prune(path_nodes);
+  new_road.reindex();
   new_road.complete_open_lanes();
 
-  return new_path_nodes;
+  return new_road.update_path(new_node_ref,path_nodes);
 }
 
-void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double length,COpendriveRoad &road)
+void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double &length,COpendriveRoad &new_road)
 {
   segment_up_ref_t new_segment_ref;
   lane_up_ref_t new_lane_ref;
   node_up_ref_t new_node_ref;
-  COpendriveRoadSegment *segment,*new_segment;
+  link_up_ref_t new_link_ref;
+  unsigned int segment_index,node_index;
+  TOpendriveWorldPose end_pose;
+  COpendriveRoadSegment *segment,*new_segment,*next_segment;
   COpendriveRoadNode *node;
-  unsigned int node_index;
-  int node_size;
+  double rem_length=length,distance,start_length;
+  int node_side;
 
-  // get the first segment
-/*
-  node_index=this->get_closest_nodes(start_pose,3.14159);
+  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);
+  node_index=this->get_closest_node_index(start_pose,distance);
   if(node_index==(unsigned int)-1)
-    throw CException(_HERE_,"Start position does not beloang to the road");
+    throw CException(_HERE_,"Start position does not belang to the road");
   node=this->nodes[node_index];
-  if(node->get_num_parents()>1)
-    throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
-  segment=node->parents[0].segment;
+  segment_index=this->get_closest_segment_index(start_pose,distance);
+  if(segment_index==(unsigned int)-1)
+    throw CException(_HERE_,"Start position does not belang to the road");
+  segment=this->segments[segment_index];
   node_side=segment->get_node_side(node);
-  new_segment=gegment->get_sub_sugment(new_node_ref,new_lane_ref,node_side,&start_pose,NULL);
-*/
+  new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,&start_pose,NULL);
+  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);
+    segment=new_segment;
+    new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,NULL,&end_pose);
+    delete segment;
+  }
+  rem_length-=new_segment->get_length();
+  new_road.add_segment(new_segment);
+  new_segment_ref[segment]=new_segment;
+  while(rem_length>0)
+  {
+    next_segment=segment->get_next_segment(node_side);
+    if(next_segment==NULL)
+      break;
+    if((rem_length-next_segment->get_length())<0.0)
+    {
+      if(node_side<0)
+        end_pose=next_segment->get_pose_at(rem_length);
+      else
+        end_pose=next_segment->get_pose_at(next_segment->get_length()-rem_length);
+      new_segment=next_segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,NULL,&end_pose);
+    }
+    else
+      new_segment=next_segment->clone(new_node_ref,new_lane_ref,new_link_ref);
+    rem_length-=new_segment->get_length();
+    new_road.add_segment(new_segment);
+    new_segment_ref[next_segment]=new_segment;
+    segment=next_segment;
+  }
+  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)