Keep the whole last/first geometry when getting a subroad/sublane.

src/opendrive_road_segment.cpp

@@ -971,14 +971,27 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
       length=geom_it->spline->find_closest_point(end_pose->x,end_pose->y,new_point);//,pow(this->resolution,2.0));
       if(length<std::numeric_limits<double>::max())
       {
-//        geom_it->spline->set_end_point(new_point);
-//        geom_it->spline->generate(this->resolution,length);
-//        geom_it->opendrive->set_max_s(geom_it->opendrive->get_min_s()+length);
+/*
+        if(geom_it->spline->get_length()>10.0)
+        {
+          new_pose=this->get_pose_at(removed_length+10.0,-1);
+          new_point.x=new_pose.x;
+          new_point.y=new_pose.y;
+          new_point.heading=new_pose.heading;
+          new_point.curvature=this->get_curvature_at(removed_length+10.0,-1);
+          geom_it->spline->set_end_point(new_point);
+          geom_it->spline->generate(this->resolution,10.0);
+          geom_it->opendrive->set_max_s(geom_it->opendrive->get_min_s()+10.0);
+        }
+*/
         geom_it=new_segment->geometries.erase(++geom_it,new_segment->geometries.end());
         break;
       }
       else
+      {
+        removed_length+=geom_it->spline->get_length();
         geom_it++;
+      }
     }
   }
 
@@ -1319,6 +1332,7 @@ void COpendriveRoadSegment::save(OpenDRIVE::road_type **road_info)
   for(unsigned int i=0;i<this->geometries.size();i++)
   {
     ::geometry new_geometry;
+    /*
     if(this->geometries[i].opendrive->get_name()=="arc")
     {
       double start_curv,end_curv;
@@ -1348,9 +1362,10 @@ void COpendriveRoadSegment::save(OpenDRIVE::road_type **road_info)
     }
     else
     {
+    */
       this->geometries[i].opendrive->save(new_geometry);
       plan_view.geometry().push_back(new_geometry);
-    }
+//    }
   }
   (*road_info)=new OpenDRIVE::road_type(plan_view,lanes);
   (*road_info)->name(this->name);