diff --git a/src/osm/osm_road_segment.cpp b/src/osm/osm_road_segment.cpp index 9c1f9151b7fab57f3a5ebf27468c13c4c6519a69..b63cff43d523787ba98b77400f6e15c69c4300b1 100644 --- a/src/osm/osm_road_segment.cpp +++ b/src/osm/osm_road_segment.cpp @@ -271,6 +271,7 @@ void COSMRoadSegment::generate_opendrive_geometry(void) COpendriveGeometry *new_geometry; TOpendriveWorldPose start_pose; double y_offset,heading1,length1,length2,heading2,x,y,angle,radius,prev_dist,dist,lane_offset,diff; + double arc_length; const COSMNode *node1,*node2,*node3; this->opendrive_geometries.clear(); @@ -302,10 +303,8 @@ void COSMRoadSegment::generate_opendrive_geometry(void) length2=node2->compute_distance(*node3); heading2=node2->compute_heading(*node3); angle=node2->compute_angle(*node1,*node3); - //lane_offset=((double)this->parent_way->get_num_lanes())*this->parent_way->get_lane_width()/2.0; -// lane_offset=((double)this->parent_way->get_num_lanes())*2.0; -// radius=DEFAULT_MIN_RADIUS+lane_offset; - radius=DEFAULT_MIN_RADIUS+y_offset; + lane_offset=((double)this->parent_way->get_num_lanes())*this->parent_way->get_lane_width()/2.0; + radius=DEFAULT_MIN_RADIUS+lane_offset; dist=radius*cos(angle/2.0); if(length1<length2) { @@ -347,6 +346,7 @@ void COSMRoadSegment::generate_opendrive_geometry(void) } if(dist<MIN_ROAD_LENGTH) dist=MIN_ROAD_LENGTH; +// std::cout << "way " << this->parent_way->id << " segment " << i << " radius " << radius << std::endl; if((i+1)==(this->parent_way->get_num_segments()-1)) { if((length2-this->end_distance-dist)<MIN_ROAD_LENGTH) @@ -402,10 +402,17 @@ void COSMRoadSegment::generate_opendrive_geometry(void) start_pose.x=x+(length1-dist)*cos(heading1)-y_offset*sin(heading1); start_pose.y=y+(length1-dist)*sin(heading1)+y_offset*cos(heading1); start_pose.heading=heading1; + // compute the new laength and radius if(angle>0.0) - new_geometry=new COpendriveArc(start_pose,2.0*dist,-1.0/radius); + { + arc_length=2.0*dist*(radius+y_offset)/radius; + new_geometry=new COpendriveArc(start_pose,arc_length,-1.0/(radius+y_offset)); + } else - new_geometry=new COpendriveArc(start_pose,2.0*dist,1.0/radius); + { + arc_length=2.0*dist*(radius-y_offset)/radius; + new_geometry=new COpendriveArc(start_pose,arc_length,1.0/(radius-y_offset)); + } this->opendrive_geometries.push_back(new_geometry); if((i+1)==(this->parent_way->get_num_segments()-1)) {