diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp index c247e0a03694fcb6cca8aacb8601d6ba14cfb30c..5d2d396eb1eb2e289535a4e6f1ed31b8a4ad44af 100644 --- a/src/opendrive_road_segment.cpp +++ b/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);