Commit 41858975 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, segment, segment index and pose to a given point.
* to get the number of lane objects and a reference to them.
* to udpate the index of lanes and nodes when creating new roads.
* to update the node index from a path.
parent 7bd8274a
......@@ -28,16 +28,18 @@ class COpendriveRoad
void link_segments(OpenDRIVE &open_drive);
unsigned int add_node(COpendriveRoadNode *node);
bool remove_node(COpendriveRoadNode *node);
COpendriveRoadNode* get_node_at(TOpendriveWorldPose &pose);
bool node_exists_at(TOpendriveWorldPose &pose);
unsigned int add_lane(COpendriveLane *lane);
bool remove_lane(COpendriveLane *lane);
void complete_open_lanes(void);
void add_segment(COpendriveRoadSegment *segment,std::vector<unsigned int> &old_path,std::vector<unsigned int> &new_path);
void add_segment(COpendriveRoadSegment *segment);
bool has_segment(COpendriveRoadSegment *segment);
std::vector<unsigned int> update_path(node_up_ref_t &node_refs,std::vector<unsigned int> &path);
void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
void clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
void reindex(void);
void prune(std::vector<unsigned int> &path_nodes);
bool node_exists_at(TOpendriveWorldPose &pose);
COpendriveRoadNode* get_node_at(TOpendriveWorldPose &pose);
public:
COpendriveRoad();
COpendriveRoad(const COpendriveRoad& object);
......@@ -50,14 +52,21 @@ class COpendriveRoad
double get_min_road_length(void) const;
unsigned int get_num_segments(void) const;
const COpendriveRoadSegment& get_segment(unsigned int index) const;
unsigned int get_num_lanes(void) const;
const COpendriveLane &get_lane(unsigned int index) const;
unsigned int get_num_nodes(void) const;
const COpendriveRoadNode& get_node(unsigned int index) const;
const COpendriveRoadSegment& operator[](std::size_t index);
unsigned int get_closest_node(TOpendriveWorldPose &pose,double angle_tol=0.1);
double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1);
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 COpendriveRoadSegment &operator[](std::size_t index);
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) const;
unsigned int get_closest_lane_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
const COpendriveRoadSegment &get_closest_segment(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
unsigned int get_closest_segment_index(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;
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,COpendriveRoad &new_road);
void operator=(const COpendriveRoad& object);
friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
~COpendriveRoad();
......
......@@ -316,7 +316,7 @@ bool COpendriveRoad::has_segment(COpendriveRoadSegment *segment)
return false;
}
void COpendriveRoad::add_segment(COpendriveRoadSegment *segment,std::vector<unsigned int> &old_path,std::vector<unsigned int> &new_path)
void COpendriveRoad::add_segment(COpendriveRoadSegment *segment)
{
for(unsigned int i=0;i<this->segments.size();i++)
if(this->segments[i]->get_id()==segment->get_id())// segment is already present
......@@ -325,28 +325,36 @@ void COpendriveRoad::add_segment(COpendriveRoadSegment *segment,std::vector<unsi
this->segments.push_back(segment);
// add the lanes
for(unsigned int i=1;i<=segment->get_num_right_lanes();i++)
{
segment->lanes[-i]->set_index(this->lanes.size());
this->lanes.push_back(segment->lanes[-i]);
}
for(unsigned int i=1;i<=segment->get_num_left_lanes();i++)
{
segment->lanes[i]->set_index(this->lanes.size());
this->lanes.push_back(segment->lanes[i]);
}
// add the nodes
for(unsigned int i=0;i<segment->get_num_nodes();i++)
{
for(unsigned int j=0;j<old_path.size();j++)
if(old_path[j]==segment->nodes[i]->index)
new_path[j]=this->nodes.size();
segment->nodes[i]->set_index(this->nodes.size());
this->nodes.push_back(segment->nodes[i]);
}
// update the road reference
segment->set_parent_road(this);
}
std::vector<unsigned int> COpendriveRoad::update_path(node_up_ref_t &node_refs,std::vector<unsigned int> &path)
{
std::vector<unsigned int> new_path;
node_up_ref_t::iterator node_it;
for(unsigned int i=0;i<path.size();i++)
{
for(node_it=node_refs.begin();node_it!=node_refs.end();node_it++)
{
if(node_it->first->get_index()==path[i])
{
new_path.push_back(node_it->second->get_index());
break;
}
}
}
return new_path;
}
void COpendriveRoad::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
{
std::vector<COpendriveRoadSegment *>::iterator seg_it;
......@@ -403,6 +411,16 @@ void COpendriveRoad::clean_references(segment_up_ref_t &segment_refs,lane_up_ref
}
}
void COpendriveRoad::reindex(void)
{
// reindex lanes
for(unsigned int i=0;i<this->get_num_lanes();i++)
this->lanes[i]->set_index(i);
// reindex nodes
for(unsigned int i=0;i<this->get_num_nodes();i++)
this->nodes[i]->set_index(i);
}
void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
{
COpendriveLane *neightbour_lane;
......@@ -466,17 +484,6 @@ void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
else
lane_it++;
}
// re-index all nodes and the path
for(unsigned int i=0;i<this->nodes.size();i++)
{
for(unsigned int j=0;j<path_nodes.size();j++)
{
if(this->nodes[i]->get_index()==path_nodes[j])
path_nodes[j]=i;
}
this->nodes[i]->set_index(i);
}
// remove links to non-consecutive nodes
for(unsigned int i=0;i<this->nodes.size();i++)
{
......@@ -516,7 +523,6 @@ COpendriveRoadNode* COpendriveRoad::get_node_at(TOpendriveWorldPose &pose)
return NULL;
}
void COpendriveRoad::load(const std::string &filename)
{
struct stat buffer;
......@@ -614,6 +620,24 @@ const COpendriveRoadSegment& COpendriveRoad::get_segment(unsigned int index) con
}
}
unsigned int COpendriveRoad::get_num_lanes(void) const
{
return this->lanes.size();
}
const COpendriveLane &COpendriveRoad::get_lane(unsigned int index) const
{
std::stringstream error;
if(index>=0 && index<this->lanes.size())
return *this->lanes[index];
else
{
error << "Invalid lane index " << index;
throw CException(_HERE_,error.str());
}
}
unsigned int COpendriveRoad::get_num_nodes(void) const
{
return this->nodes.size();
......@@ -645,15 +669,45 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
}
}
unsigned int COpendriveRoad::get_closest_node(TOpendriveWorldPose &pose,double angle_tol)
unsigned int COpendriveRoad::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;
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 &COpendriveRoad::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];
}
unsigned int COpendriveRoad::get_closest_lane_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->lanes.size();i++)
{
this->nodes[i]->get_closest_pose(pose,closest_pose,angle_tol);
this->lanes[i]->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)
{
......@@ -665,7 +719,47 @@ unsigned int COpendriveRoad::get_closest_node(TOpendriveWorldPose &pose,double a
return closest_index;
}
double COpendriveRoad::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol)
const COpendriveLane &COpendriveRoad::get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
unsigned int closest_index;
closest_index=this->get_closest_lane_index(pose,distance,angle_tol);
if(closest_index==(unsigned int)-1)
throw CException(_HERE_,"Impossible to find the closest lane");
return *this->lanes[closest_index];
}
unsigned int COpendriveRoad::get_closest_segment_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->segments.size();i++)
{
this->segments[i]->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_index=i;
}
}
return closest_index;
}
const COpendriveRoadSegment &COpendriveRoad::get_closest_segment(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
unsigned int closest_index;
closest_index=this->get_closest_segment_index(pose,distance,angle_tol);
if(closest_index==(unsigned int)-1)
throw CException(_HERE_,"Impossible to find the closest segment");
return *this->segments[closest_index];
}
double COpendriveRoad::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
{
double dist,min_dist=std::numeric_limits<double>::max();
TOpendriveWorldPose pose_found;
......@@ -673,7 +767,7 @@ double COpendriveRoad::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorl
for(unsigned int i=0;i<this->nodes.size();i++)
{
length=this->nodes[i]->get_closest_pose(pose,pose_found,angle_tol);
length=this->nodes[i]->get_closest_pose(pose,pose_found,false,angle_tol);
dist=sqrt(pow(pose_found.x-pose.x,2.0)+pow(pose_found.y-pose.y,2.0));
if(dist<min_dist)
{
......@@ -686,7 +780,7 @@ double COpendriveRoad::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorl
return closest_length;
}
void COpendriveRoad::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol)
void COpendriveRoad::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol) const
{
std::vector<TOpendriveWorldPose> poses;
std::vector<double> lengths;
......@@ -695,7 +789,7 @@ void COpendriveRoad::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOp
closest_lengths.clear();
for(unsigned int i=0;i<this->nodes.size();i++)
{
this->nodes[i]->get_closest_poses(pose,poses,lengths,dist_tol,angle_tol);
this->nodes[i]->get_closest_poses(pose,poses,lengths,dist_tol,false,angle_tol);
for(unsigned int j=0;j<poses.size();i++)
{
closest_poses.push_back(poses[i]);
......@@ -709,6 +803,7 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
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;
COpendriveRoadNode *node,*next_node;
COpendriveRoadSegment *segment,*new_segment;
// CopendriveRoadSegment *original_seg1,*original_seg2;
......@@ -733,22 +828,22 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
segment=link->segment;
if(new_segment_ref.find(segment)==new_segment_ref.end())
{
new_segment=segment->clone(new_node_ref,new_lane_ref);
new_road.add_segment(new_segment,path_nodes,new_path_nodes);
new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref);
new_road.add_segment(new_segment);
new_segment_ref[segment]=new_segment;
}
}
// add the last segment
node=this->nodes[path_nodes[path_nodes.size()-1]];
link_index=node->get_closest_link(end_pose,3.14159);
link_index=node->get_closest_link(end_pose);
if(link_index==(unsigned int)-1)
throw CException(_HERE_,"The provided path has unconnected nodes");
link=node->links[link_index];
segment=link->segment;
if(new_segment_ref.find(segment)==new_segment_ref.end())
{
new_segment=segment->clone(new_node_ref,new_lane_ref);
new_road.add_segment(new_segment,path_nodes,new_path_nodes);
new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref);
new_road.add_segment(new_segment);
new_segment_ref[segment]=new_segment;
}
......@@ -766,8 +861,8 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
{
if(!new_road.has_segment(new_segment_ref[this->segments[i]]))
{
new_segment=this->segment[k]->clone(new_node_ref,new_lane_ref);
new_road.add_segment(new_segment,path_nodes,new_path_nodes);
new_segment=this->segment[k]->clone(new_node_ref,new_lane_ref,new_link_ref);
new_road.add_segment(new_segment);
new_segment_ref[segment]=new_segment;
}
}
......@@ -778,34 +873,79 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);
new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref);
// remove unconnected elements
new_road.prune(new_path_nodes);
new_road.prune(path_nodes);
new_road.reindex();
new_road.complete_open_lanes();
return new_path_nodes;
return new_road.update_path(new_node_ref,path_nodes);
}
void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double length,COpendriveRoad &road)
void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double &length,COpendriveRoad &new_road)
{
segment_up_ref_t new_segment_ref;
lane_up_ref_t new_lane_ref;
node_up_ref_t new_node_ref;
COpendriveRoadSegment *segment,*new_segment;
link_up_ref_t new_link_ref;
unsigned int segment_index,node_index;
TOpendriveWorldPose end_pose;
COpendriveRoadSegment *segment,*new_segment,*next_segment;
COpendriveRoadNode *node;
unsigned int node_index;
int node_size;
double rem_length=length,distance,start_length;
int node_side;
// get the first segment
/*
node_index=this->get_closest_nodes(start_pose,3.14159);
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 beloang to the road");
throw CException(_HERE_,"Start position does not belang to the road");
node=this->nodes[node_index];
if(node->get_num_parents()>1)
throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
segment=node->parents[0].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");
segment=this->segments[segment_index];
node_side=segment->get_node_side(node);
new_segment=gegment->get_sub_sugment(new_node_ref,new_lane_ref,node_side,&start_pose,NULL);
*/
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)
{
if(node_side<0)
end_pose=new_segment->get_pose_at(rem_length);
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-=new_segment->get_length();
new_road.add_segment(new_segment);
new_segment_ref[segment]=new_segment;
while(rem_length>0)
{
next_segment=segment->get_next_segment(node_side);
if(next_segment==NULL)
break;
if((rem_length-next_segment->get_length())<0.0)
{
if(node_side<0)
end_pose=next_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);
}
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;
segment=next_segment;
}
length-=rem_length;
new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);
new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref);
new_road.reindex();
new_road.complete_open_lanes();
}
void COpendriveRoad::operator=(const COpendriveRoad& object)
......
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