Improved the get_sub_road() function:

  * Taken into account the direction of the road segment.
  * Added a margin at the beginning and the end of the new road.

include/opendrive_road.h

@@ -66,7 +66,7 @@ class COpendriveRoad
     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,double margin,COpendriveRoad &new_road);
     void operator=(const COpendriveRoad& object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
     ~COpendriveRoad();

src/opendrive_road.cpp

@@ -907,66 +907,154 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
   return new_road.update_path(new_node_ref,path_nodes);
 }
 
-void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double &length,COpendriveRoad &new_road)
+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;
-  unsigned int segment_index,node_index;
-  TOpendriveWorldPose end_pose;
-  COpendriveRoadSegment *segment,*new_segment,*next_segment;
-  COpendriveRoadNode *node;
-  double rem_length=length,distance,start_length;
+  unsigned int segment_index;
+  TOpendriveWorldPose end_pose,int_pose;
+  const COpendriveRoadSegment *segment,*next_segment;
+  COpendriveRoadSegment *new_segment;
+  double rem_length=length+2.0*margin,distance,start_length;
   int node_side;
+  bool valid;
 
   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 belang to the road");
-  node=this->nodes[node_index];
+  // 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 belang to the road");
+    throw CException(_HERE_,"Start position does not belong to the road");
   segment=this->segments[segment_index];
-  node_side=segment->get_node_side(node);
-  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)
+  node_side=segment->get_pose_side(start_pose);
+  if(margin>0.0)
   {
     if(node_side<0)
-      end_pose=new_segment->get_pose_at(rem_length);
+    {
+      rem_length=distance-margin;
+      if(rem_length<0.0)// get the previous segment
+      {
+        next_segment=&segment->get_prev_segment(node_side,valid);
+        if(valid)
+        {
+          segment=next_segment;
+          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(0.0);
+          distance=0.0;
+        }
+      }
+      else
+      {
+        int_pose=segment->get_pose_at(rem_length);
+        distance-=margin;
+      }
+    }
     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=distance+margin-segment->get_length();
+      if(rem_length>0.0)// get the prev segment
+      {
+        next_segment=&segment->get_prev_segment(node_side,valid);
+        if(valid)
+        {
+          segment=next_segment;
+          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(segment->get_length());
+          distance=0.0;
+        }
+      }
+      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);
+  valid=true;
+  if(new_segment->get_length()<this->min_road_length)
+  {
+    next_segment=&segment->get_next_segment(node_side,valid);
+    segment=next_segment;
+    delete new_segment;
+    if(valid)
+    {
+      new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref);
+      distance=0.0;
+    }
   }
-  rem_length-=new_segment->get_length();
-  new_road.add_segment(new_segment);
-  new_segment_ref[segment]=new_segment;
-  while(rem_length>0)
+  if(valid)
   {
-    next_segment=segment->get_next_segment(node_side);
-    if(next_segment==NULL)
-      break;
-    if((rem_length-next_segment->get_length())<0.0)
+    start_length=new_segment->get_length()-distance;
+    if(rem_length<start_length)
     {
       if(node_side<0)
-        end_pose=next_segment->get_pose_at(rem_length);
+        end_pose=new_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);
+        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);
     }
-    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;
+    new_segment_ref[(COpendriveRoadSegment *)segment]=new_segment;
+  }
+  while(rem_length>this->resolution && valid)
+  {
+    next_segment=&segment->get_next_segment(node_side,valid);
     segment=next_segment;
+    if(valid)
+    {
+      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);