Commit 7bd8274a authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added some functions:

* to get the pose at a given distance in the link.
* to get the curvature at a given distance in the link.
* to get the closest node, node index, lane, lane index and pose to a given point.
* to get the next segment in the road.
Added the link reference update to the clone() and get_sub_segment() functions.
parent 17b30b19
......@@ -71,8 +71,9 @@ class COpendriveRoadSegment
void link_segment(COpendriveRoadSegment *segment,int from,bool from_start, int to,bool to_start);
bool connects_to(COpendriveRoadSegment *segment);
bool connects_segments(COpendriveRoadSegment *segment1,COpendriveRoadSegment *segment2);
COpendriveRoadSegment *get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,int node_side,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL);
COpendriveRoadSegment *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref);
COpendriveRoadSegment *get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL);
COpendriveRoadSegment *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref);
COpendriveRoadSegment *get_next_segment(int &side);
public:
std::string get_name(void) const;
unsigned int get_id(void) const;
......@@ -89,8 +90,15 @@ class COpendriveRoadSegment
unsigned int get_num_connecting(void) const;
const COpendriveRoadSegment &get_connecting(unsigned int index) const;
double get_length(void);
TOpendriveWorldPose get_pose_at(double length);
double get_curvature_at(double length);
TOpendriveLocalPose transform_to_local_pose(const TOpendriveTrackPose &track);
TOpendriveWorldPose transform_to_world_pose(const TOpendriveTrackPose &track);
const COpendriveRoadNode &get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
const COpendriveLane &get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1);
int get_closest_lane_id(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment);
~COpendriveRoadSegment();
};
......
......@@ -94,6 +94,7 @@ void COpendriveRoadSegment::set_resolution(double res)
void COpendriveRoadSegment::set_scale_factor(double scale)
{
std::map<int,COpendriveLane *>::const_iterator lane_it;
TPoint spline_start,spline_end;
this->scale_factor=scale;
......@@ -105,7 +106,20 @@ void COpendriveRoadSegment::set_scale_factor(double scale)
this->objects[i]->set_scale_factor(scale);
for(unsigned int i=0;i<this->geometries.size();i++)
{
this->geometries[i].opendrive->set_scale_factor(scale);
this->geometries[i].spline->get_start_point(spline_start);
spline_start.x*=this->scale_factor/scale;
spline_start.y*=this->scale_factor/scale;
spline_start.curvature*=scale/this->scale_factor;
this->geometries[i].spline->set_start_point(spline_start);
this->geometries[i].spline->get_end_point(spline_end);
spline_end.x*=this->scale_factor/scale;
spline_end.y*=this->scale_factor/scale;
spline_end.curvature*=scale/this->scale_factor;
this->geometries[i].spline->set_end_point(spline_end);
this->geometries[i].spline->generate(this->resolution);
}
}
void COpendriveRoadSegment::set_min_road_length(double length)
......@@ -396,7 +410,7 @@ void COpendriveRoadSegment::add_nodes(void)
void COpendriveRoadSegment::add_node(TOpendriveRoadSegmentGeometry &geometry,COpendriveLane *lane)
{
TOpendriveTrackPose track_pose;
double start_curvature,end_curvature,length;
double start_curvature,end_curvature;
COpendriveRoadNode *new_node;
TOpendriveWorldPose node_pose;
unsigned int node_index;
......@@ -440,11 +454,10 @@ void COpendriveRoadSegment::add_node(TOpendriveRoadSegmentGeometry &geometry,COp
end_curvature=1.0/((1.0/end_curvature)-lane->get_center_offset());
else
end_curvature=1.0/((1.0/end_curvature)+lane->get_center_offset());
length=geometry.opendrive->get_length();
if(lane->get_id()<0)
new_node->add_parent(lane,this,length,start_curvature,end_curvature);
new_node->add_parent(lane,this,start_curvature,end_curvature);
else
new_node->add_parent(lane,this,length,-end_curvature,-start_curvature);
new_node->add_parent(lane,this,-end_curvature,-start_curvature);
for(unsigned int i=0;i<this->nodes.size();i++)
if(this->nodes[i]==new_node)
{
......@@ -591,7 +604,7 @@ void COpendriveRoadSegment::link_neightbour_lanes(void)
if(this->lanes.find(i-1)!=this->lanes.end())// if the lane exists
this->lanes[i]->link_neightbour_lane(this->lanes[i-1]);
else
this->lanes[i]->set_right_lane(NULL,this->center_mark);
this->lanes[i]->set_left_lane(NULL,this->center_mark);
}
}
......@@ -609,11 +622,11 @@ void COpendriveRoadSegment::link_segment(COpendriveRoadSegment *segment)
if(segment->lanes.find(j)!=segment->lanes.end())
{
if(j==i-1)
this->lanes[i]->link_lane(segment->lanes[j],this->lanes[i]->right_mark,false,true);
this->lanes[i]->link_lane(segment->lanes[j],this->lanes[i]->right_mark,false,true,false);
else if(j==i)
this->lanes[i]->link_lane(segment->lanes[j],OD_MARK_NONE,false,true);
this->lanes[i]->link_lane(segment->lanes[j],OD_MARK_NONE,false,true,true);
else
this->lanes[i]->link_lane(segment->lanes[j],this->lanes[i]->left_mark,false,true);
this->lanes[i]->link_lane(segment->lanes[j],this->lanes[i]->left_mark,false,true,false);
}
}
}
......@@ -624,11 +637,11 @@ void COpendriveRoadSegment::link_segment(COpendriveRoadSegment *segment)
if(this->lanes.find(j)!=this->lanes.end())
{
if(j==i-1)
segment->lanes[i]->link_lane(this->lanes[j],segment->lanes[i]->right_mark,false,true);
segment->lanes[i]->link_lane(this->lanes[j],segment->lanes[i]->right_mark,false,true,false);
else if(j==i)
segment->lanes[i]->link_lane(this->lanes[j],OD_MARK_NONE,false,true);
segment->lanes[i]->link_lane(this->lanes[j],OD_MARK_NONE,false,true,true);
else
segment->lanes[i]->link_lane(this->lanes[j],segment->lanes[i]->left_mark,false,true);
segment->lanes[i]->link_lane(this->lanes[j],segment->lanes[i]->left_mark,false,true,false);
}
}
}
......@@ -644,19 +657,19 @@ void COpendriveRoadSegment::link_segment(COpendriveRoadSegment *segment,int from
if(segment->lanes.find(to)!=segment->lanes.end())
{
if(this->lanes.find(from-1)!=this->lanes.end())
this->lanes[from-1]->link_lane(segment->lanes[to],this->lanes[from-1]->right_mark,from_start,to_start);
this->lanes[from-1]->link_lane(segment->lanes[to],this->lanes[from-1]->right_mark,from_start,to_start,false);
if(this->lanes.find(from)!=this->lanes.end())
this->lanes[from]->link_lane(segment->lanes[to],OD_MARK_NONE,from_start,to_start);
this->lanes[from]->link_lane(segment->lanes[to],OD_MARK_NONE,from_start,to_start,true);
if(this->lanes.find(from+1)!=this->lanes.end())
this->lanes[from+1]->link_lane(segment->lanes[to],this->lanes[from+1]->left_mark,from_start,to_start);
this->lanes[from+1]->link_lane(segment->lanes[to],this->lanes[from+1]->left_mark,from_start,to_start,false);
}
/*
if(segment.lanes.find(to-1)!=segment.lanes.end())
if(this->lanes.find(from)!=this->lanes.end())
this->lanes[from]->link_lane(segment.lanes[to-1],this->lanes[from]->right_mark,from_start,to_start);
this->lanes[from]->link_lane(segment.lanes[to-1],this->lanes[from]->right_mark,from_start,to_start,false);
if(segment.lanes.find(to+1)!=segment.lanes.end())
if(this->lanes.find(from)!=this->lanes.end())
this->lanes[from]->link_lane(segment.lanes[to+1],this->lanes[from]->left_mark,from_start,to_start);
this->lanes[from]->link_lane(segment.lanes[to+1],this->lanes[from]->left_mark,from_start,to_start,false);
*/
}
......@@ -677,8 +690,9 @@ bool COpendriveRoadSegment::connects_segments(COpendriveRoadSegment *segment1,CO
return false;
}
COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,int node_side,TOpendriveWorldPose *start,TOpendriveWorldPose *end)
COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start,TOpendriveWorldPose *end)
{
TOpendriveWorldPose *start_pose,*end_pose;
std::map<int,COpendriveLane *>::iterator lane_it;
lane_up_ref_t::iterator lane_ref_it;
COpendriveLane *new_lane;
......@@ -692,48 +706,60 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
double length;
if(start==NULL && end==NULL)
return this->clone(new_node_ref,new_lane_ref);
return this->clone(new_node_ref,new_lane_ref,new_link_ref);
new_segment=new COpendriveRoadSegment(*this);
new_segment_ref[this]=new_segment;
if(node_side<0)
{
start_pose=start;
end_pose=end;
}
else
{
start_pose=end;
end_pose=start;
}
/* get the sublanes */
for(lane_it=new_segment->lanes.begin();lane_it!=new_segment->lanes.end();lane_it++)
{
if(node_side<0)
new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,start,end);
else
new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,end,start);
new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,new_link_ref,start_pose,end_pose);
new_lane_ref[lane_it->second]=new_lane;
}
new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);
// update geometry
for(geom_it=this->geometries.begin();geom_it!=this->geometries.end();)
if(start_pose!=NULL)
{
if(start!=NULL)
for(geom_it=new_segment->geometries.begin();geom_it!=new_segment->geometries.end();)
{
length=geom_it->spline->find_closest_point(start->x,start->y,new_point);
if(length>geom_it->opendrive->get_length())
geom_it=this->geometries.erase(geom_it);
length=geom_it->spline->find_closest_point(start_pose->x,start_pose->y,new_point);
if(fabs(length-geom_it->spline->get_length())<this->resolution)
geom_it=new_segment->geometries.erase(geom_it);
else
{
geom_it->spline->set_start_point(new_point);
geom_it->spline->generate(this->resolution);
geom_it->opendrive->set_start_pose(*start);
geom_it->opendrive->set_start_pose(*start_pose);
geom_it->opendrive->set_max_s(geom_it->opendrive->get_max_s()-length);
break;
}
}
for(unsigned int i=1;i<new_segment->geometries.size();i++)
{
new_segment->geometries[i].opendrive->set_min_s(new_segment->geometries[i].opendrive->get_min_s()-length);
new_segment->geometries[i].opendrive->set_max_s(new_segment->geometries[i].opendrive->get_max_s()-length);
}
}
for(geom_it=this->geometries.begin();geom_it!=this->geometries.end();)
if(end_pose!=NULL)
{
if(end!=NULL)
for(geom_it=new_segment->geometries.begin();geom_it!=new_segment->geometries.end();)
{
length=geom_it->spline->find_closest_point(end->x,end->y,new_point);
if(length<geom_it->opendrive->get_length())
length=geom_it->spline->find_closest_point(end_pose->x,end_pose->y,new_point);
if(length<geom_it->spline->get_length())
{
geom_it->spline->set_end_point(new_point);
geom_it->spline->generate(this->resolution);
geom_it->opendrive->set_max_s(length);;
geom_it=this->geometries.erase(geom_it+1,this->geometries.end());
geom_it->opendrive->set_max_s(geom_it->opendrive->get_min_s()+length);;
geom_it=new_segment->geometries.erase(++geom_it,new_segment->geometries.end());
break;
}
else
......@@ -744,7 +770,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
return new_segment;
}
COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref)
COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref)
{
std::map<int,COpendriveLane *>::iterator lane_it;
lane_up_ref_t::iterator lane_ref_it;
......@@ -759,7 +785,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,
/* get the sublanes */
for(lane_it=new_segment->lanes.begin();lane_it!=new_segment->lanes.end();lane_it++)
{
new_lane=lane_it->second->clone(new_node_ref,new_lane_ref);
new_lane=lane_it->second->clone(new_node_ref,new_lane_ref,new_link_ref);
new_lane_ref[lane_it->second]=new_lane;
}
new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);
......@@ -767,6 +793,71 @@ COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,
return new_segment;
}
COpendriveRoadSegment *COpendriveRoadSegment::get_next_segment(int &side)
{
std::vector<COpendriveRoadSegment *> segment_candidates;
std::vector<int> side_candidates;
bool already_present;
if(side<0)
{
for(unsigned int i=1;i<=this->get_num_right_lanes();i++)
{
const COpendriveLane &lane=this->get_lane(-i);
if(lane.get_num_next_lanes()>1)
throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
if(lane.get_num_next_lanes()==1)
{
already_present=false;
for(unsigned int k=0;k<segment_candidates.size();k++)
if(segment_candidates[k]==lane.get_next_lane(0).segment)
{
already_present=true;
break;
}
if(!already_present)
{
segment_candidates.push_back(lane.get_next_lane(0).segment);
side_candidates.push_back(lane.get_next_lane(0).get_id());
}
}
}
}
else
{
for(unsigned int i=1;i<=this->get_num_left_lanes();i++)
{
const COpendriveLane &lane=this->get_lane(i);
if(lane.get_num_next_lanes()>1)
throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
if(lane.get_num_next_lanes()==1)
{
already_present=false;
for(unsigned int k=0;k<segment_candidates.size();k++)
if(segment_candidates[k]==lane.get_next_lane(0).segment)
{
already_present=true;
break;
}
if(!already_present)
{
segment_candidates.push_back(lane.get_next_lane(0).segment);
side_candidates.push_back(lane.get_next_lane(0).get_id());
}
}
}
}
if(segment_candidates.size()==0)
return NULL;
else if(segment_candidates.size()>1)
throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
else
{
side=side_candidates[0];
return segment_candidates[0];
}
}
void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
{
std::map<int,COpendriveLane *>::const_iterator lane_it;
......@@ -953,6 +1044,48 @@ double COpendriveRoadSegment::get_length(void)
return length;
}
TOpendriveWorldPose COpendriveRoadSegment::get_pose_at(double length)
{
TOpendriveWorldPose world_pose{0};
TPoint spline_pose;
for(unsigned int i=0;i<this->geometries.size();i++)
{
if((length-this->geometries[i].spline->get_length())<0.0)
{
spline_pose=this->geometries[i].spline->evaluate(length);
world_pose.x=spline_pose.x;
world_pose.y=spline_pose.y;
world_pose.heading=spline_pose.heading;
return world_pose;
}
else
length-=this->geometries[i].spline->get_length();
}
return world_pose;
}
double COpendriveRoadSegment::get_curvature_at(double length)
{
double curvature;
TPoint spline_pose;
for(unsigned int i=0;i<this->geometries.size();i++)
{
if((length-this->geometries[i].spline->get_length())<0.0)
{
spline_pose=this->geometries[i].spline->evaluate(length);
curvature=spline_pose.curvature;
return curvature;
}
else
length-=this->geometries[i].spline->get_length();
}
return 0.0;
}
TOpendriveLocalPose COpendriveRoadSegment::transform_to_local_pose(const TOpendriveTrackPose &track)
{
TOpendriveTrackPose pose;
......@@ -1015,6 +1148,96 @@ TOpendriveWorldPose COpendriveRoadSegment::transform_to_world_pose(const TOpendr
throw CException(_HERE_,error.str());
}
unsigned int COpendriveRoadSegment::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
double dist,min_dist=std::numeric_limits<double>::max();
TOpendriveWorldPose closest_pose;
unsigned int closest_index=-1;
for(unsigned int i=0;i<this->nodes.size();i++)
{
this->nodes[i]->get_closest_pose(pose,closest_pose,false,angle_tol);
dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
if(dist<min_dist)
{
min_dist=dist;
closest_index=i;
}
}
return closest_index;
}
const COpendriveRoadNode &COpendriveRoadSegment::get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
unsigned int closest_index;
closest_index=this->get_closest_node_index(pose,distance,angle_tol);
if(closest_index==(unsigned int)-1)
throw CException(_HERE_,"Impossible to find the closest node");
return *this->nodes[closest_index];
}
int COpendriveRoadSegment::get_closest_lane_id(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
double dist,min_dist=std::numeric_limits<double>::max();
TOpendriveWorldPose closest_pose;
int closest_id=0;
std::map<int,COpendriveLane *>::const_iterator lane_it;
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
{
lane_it->second->get_closest_pose(pose,closest_pose,angle_tol);
dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
if(dist<min_dist)
{
min_dist=dist;
closest_id=lane_it->first;
}
}
return closest_id;
}
const COpendriveLane &COpendriveRoadSegment::get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol)
{
int closest_id=this->get_closest_lane_id(pose,distance,angle_tol);
if(closest_id==0)
throw CException(_HERE_,"Impossible to find the closest lane");
return *this->lanes[closest_id];
}
double COpendriveRoadSegment::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
{
double dist,min_dist=std::numeric_limits<double>::max(),distance;
unsigned int closest_index=-1;
TPoint closest_spline_point;
closest_pose.x=std::numeric_limits<double>::max();
closest_pose.y=std::numeric_limits<double>::max();
closest_pose.heading=std::numeric_limits<double>::max();
for(unsigned int i=0;i<this->geometries.size();i++)
{
this->geometries[i].spline->find_closest_point(pose.x,pose.y,closest_spline_point);
dist=sqrt(pow(closest_spline_point.x-pose.x,2.0)+pow(closest_spline_point.y-pose.y,2.0));
if(dist<min_dist)
{
min_dist=dist;
closest_index=i;
closest_pose.x=closest_spline_point.x;
closest_pose.y=closest_spline_point.y;
closest_pose.heading=normalize_angle(closest_spline_point.heading);
}
}
distance=0.0;
for(unsigned int i=0;i<closest_index;i++)
distance+=this->geometries[i].spline->get_length();
distance+=min_dist;
return distance;
}
std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment)
{
out << "Road " << segment.get_name() << " (" << segment.get_id() << ")" << std::endl;
......
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