Commit 51fec26a authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

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.
parent 57de0b1b
......@@ -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();
......
......@@ -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);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment