Commit db6473e6 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added functions to get a part of the road.

Added functions to update all the references when a new road is created.
parent 863db51e
......@@ -5,28 +5,37 @@
#include "xml/OpenDRIVE_1.4H.hxx"
#endif
#include "opendrive_common.h"
#include "opendrive_road_segment.h"
#include "opendrive_road_node.h"
class COpendriveRoadSegment;
class COpendriveRoadNode;
#include "opendrive_lane.h"
class COpendriveRoad
{
friend class COpendriveRoadSegment;
friend class COpendriveRoadNode;
friend class COpendriveLane;
private:
std::vector<COpendriveRoadSegment *> segments;
std::vector<COpendriveRoadNode *> nodes;
std::vector<COpendriveLane *> lanes;
double resolution;
double scale_factor;
double min_road_length;
protected:
COpendriveRoadSegment &operator[](std::string &key);
void free(void);
void link_segments(OpenDRIVE &open_drive);
unsigned int add_node(COpendriveRoadNode *node);
bool node_exists_at(const TOpendriveWorldPoint &pose);
COpendriveRoadNode* get_node_at(const TOpendriveWorldPoint &pose) const;
void remove_node(COpendriveRoadNode *node);
unsigned int add_lane(COpendriveLane *lane);
void remove_lane(COpendriveLane *lane);
void add_segment(COpendriveRoadSegment *segment,std::vector<unsigned int> &old_path,std::vector<unsigned int> &new_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 prune(std::vector<unsigned int> &path_nodes);
bool node_exists_at(TOpendriveWorldPoint &pose);
COpendriveRoadNode* get_node_at(TOpendriveWorldPoint &pose);
public:
COpendriveRoad();
COpendriveRoad(const COpendriveRoad& object);
......@@ -42,9 +51,11 @@ class COpendriveRoad
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(double x, double y,double heading,double angle_tol=0.1);
double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1);
void get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1);
unsigned int get_closest_node(TOpendriveWorldPoint &point,double angle_tol=0.1);
double get_closest_point(TOpendriveWorldPoint &point,TOpendriveWorldPoint &closest_point,double angle_tol=0.1);
void get_closest_points(TOpendriveWorldPoint &point,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1);
void get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPoint &start_point,TOpendriveWorldPoint &end_point,COpendriveRoad &new_road);
void get_sub_road(TOpendriveWorldPoint &start_point,double length,COpendriveRoad &new_road);
void operator=(const COpendriveRoad& object);
friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
~COpendriveRoad();
......
......@@ -11,41 +11,41 @@ COpendriveRoad::COpendriveRoad()
this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
this->segments.clear();
this->nodes.clear();
this->lanes.clear();
}
COpendriveRoad::COpendriveRoad(const COpendriveRoad& object)
{
COpendriveRoadSegment *segment;
COpendriveRoadNode *node;
std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> new_segment_ref;
std::map<COpendriveRoadNode *,COpendriveRoadNode *> new_node_ref;
COpendriveLane *lane;
segment_up_ref_t new_segment_ref;
node_up_ref_t new_node_ref;
lane_up_ref_t new_lane_ref;
this->free();
this->resolution=object.resolution;
this->scale_factor=object.scale_factor;
this->min_road_length=object.min_road_length;
this->segments.clear();
for(unsigned int i=0;i<object.lanes.size();i++)
{
lane=new COpendriveLane(*object.lanes[i]);
this->lanes.push_back(lane);
new_lane_ref[object.lanes[i]]=lane;
}
for(unsigned int i=0;i<object.nodes.size();i++)
{
node=new COpendriveRoadNode(*object.nodes[i]);
this->nodes.push_back(node);
new_node_ref[object.nodes[i]]=node;
}
// update references
for(unsigned int i=0;i<this->nodes.size();i++)
this->nodes[i]->update_references(new_node_ref);
for(unsigned int i=0;i<object.segments.size();i++)
{
segment=new COpendriveRoadSegment(*object.segments[i],new_node_ref,this);
segment=new COpendriveRoadSegment(*object.segments[i]);
this->segments.push_back(segment);
new_segment_ref[object.segments[i]]=segment;
}
this->nodes.clear();
// update references
for(unsigned int i=0;i<this->segments.size();i++)
this->segments[i]->update_references(new_segment_ref);
for(unsigned int i=0;i<this->nodes.size();i++)
this->nodes[i]->update_references(new_segment_ref);
}
COpendriveRoadSegment &COpendriveRoad::operator[](std::string &key)
......@@ -59,6 +59,28 @@ COpendriveRoadSegment &COpendriveRoad::operator[](std::string &key)
throw CException(_HERE_,"No road segment with the given ID");
}
void COpendriveRoad::free(void)
{
for(unsigned int i=0;i<this->segments.size();i++)
{
delete this->segments[i];
this->segments[i]=NULL;
}
this->segments.clear();
for(unsigned int i=0;i<this->nodes.size();i++)
{
delete this->nodes[i];
this->nodes[i]=NULL;
}
this->nodes.clear();
for(unsigned int i=0;i<this->lanes.size();i++)
{
delete this->lanes[i];
this->lanes[i]=NULL;
}
this->lanes.clear();
}
void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
{
std::string predecessor_id,successor_id;
......@@ -172,7 +194,237 @@ unsigned int COpendriveRoad::add_node(COpendriveRoadNode *node)
return this->nodes.size()-1;
}
bool COpendriveRoad::node_exists_at(const TOpendriveWorldPoint &pose)
void COpendriveRoad::remove_node(COpendriveRoadNode *node)
{
std::vector<COpendriveRoadNode *>::iterator it;
for(it=this->nodes.begin();it!=this->nodes.end();)
{
if((*it)->get_index()==node->get_index())
{
delete *it;
it=this->nodes.erase(it);
break;
}
else
it++;
}
}
unsigned int COpendriveRoad::add_lane(COpendriveLane *lane)
{
for(unsigned int i=0;i<this->lanes.size();i++)
{
if(this->lanes[i]==lane)
throw CException(_HERE_,"Lane already present");
}
this->lanes.push_back(lane);
return this->lanes.size()-1;
}
void COpendriveRoad::remove_lane(COpendriveLane *lane)
{
std::vector<COpendriveLane *>::iterator it;
for(it=this->lanes.begin();it!=this->lanes.end();)
{
if((*it)->get_index()==lane->get_index())
{
delete *it;
it=this->lanes.erase(it);
break;
}
else
it++;
}
}
void COpendriveRoad::add_segment(COpendriveRoadSegment *segment,std::vector<unsigned int> &old_path,std::vector<unsigned int> &new_path)
{
for(unsigned int i=0;i<this->segments.size();i++)
if(this->segments[i]->get_id()==segment->get_id())// segment is already present
return;
// add the new segment
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.push_back(this->nodes.size());
segment->nodes[i]->set_index(this->nodes.size());
this->nodes.push_back(segment->nodes[i]);
}
}
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;
std::vector<COpendriveLane *>::iterator lane_it;
std::vector<COpendriveRoadNode *>::iterator node_it;
for(seg_it=this->segments.begin();seg_it!=this->segments.end();seg_it++)
{
if(segment_refs.find(*seg_it)!=segment_refs.end())
{
(*seg_it)=segment_refs[*seg_it];
(*seg_it)->update_references(segment_refs,lane_refs,node_refs);
}
else if((*seg_it)->updated(segment_refs))
(*seg_it)->update_references(segment_refs,lane_refs,node_refs);
}
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
if(lane_refs.find(*lane_it)!=lane_refs.end())
(*lane_it)=lane_refs[*lane_it];
for(node_it=this->nodes.begin();node_it!=this->nodes.end();node_it++)
if(node_refs.find(*node_it)!=node_refs.end())
(*node_it)=node_refs[*node_it];
}
void COpendriveRoad::clean_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;
std::vector<COpendriveLane *>::iterator lane_it;
std::vector<COpendriveRoadNode *>::iterator node_it;
for(seg_it=this->segments.begin();seg_it!=this->segments.end();)
{
if(segment_refs.find(*seg_it)!=segment_refs.end())
{
(*seg_it)=segment_refs[*seg_it];
(*seg_it)->clean_references(segment_refs,lane_refs,node_refs);
seg_it++;
}
else if((*seg_it)->updated(segment_refs))
{
(*seg_it)->clean_references(segment_refs,lane_refs,node_refs);
seg_it++;
}
else
seg_it=this->segments.erase(seg_it);
}
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
{
if(lane_refs.find(*lane_it)!=lane_refs.end())
{
(*lane_it)=lane_refs[*lane_it];
lane_it++;
}
else if(!(*lane_it)->updated(lane_refs))
lane_it=this->lanes.erase(lane_it);
else
lane_it++;
}
for(node_it=this->nodes.begin();node_it!=this->nodes.end();)
{
if(node_refs.find(*node_it)!=node_refs.end())
{
(*node_it)=node_refs[*node_it];
node_it++;
}
else if(!(*node_it)->updated(node_refs))
node_it=this->nodes.erase(node_it);
else
node_it++;
}
}
void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
{
COpendriveLane *neightbour_lane;
std::vector<COpendriveLane *>::iterator lane_it;
bool present;
// remove all nodes and lanes not present in the path
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
{
present=false;
for(unsigned int i=0;i<path_nodes.size();i++)
{
if((*lane_it)->has_node(path_nodes[i]))
{
present=true;
break;
}
}
if(!present)
{
neightbour_lane=(*lane_it)->left_lane;
while(neightbour_lane!=NULL)
{
for(unsigned int i=0;i<path_nodes.size();i++)
{
if(neightbour_lane->has_node(path_nodes[i]))
{
present=true;
break;
}
}
if(present)
break;
neightbour_lane=neightbour_lane->left_lane;
}
neightbour_lane=(*lane_it)->right_lane;
while(neightbour_lane!=NULL)
{
for(unsigned int i=0;i<path_nodes.size();i++)
{
if(neightbour_lane->has_node(path_nodes[i]))
{
present=true;
break;
}
}
if(present)
break;
neightbour_lane=neightbour_lane->right_lane;
}
if(!present)
{
(*lane_it)->segment->remove_lane(*lane_it);
for(unsigned int i=0;i<(*lane_it)->nodes.size();i++)
this->remove_node((*lane_it)->nodes[i]);
delete *lane_it;
lane_it=this->lanes.erase(lane_it);
}
else
lane_it++;
}
else
lane_it++;
}
// remove links to non-consecutive nodes
for(unsigned int i=0;i<this->nodes.size();i++)
{
for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
{
for(unsigned int k=0;k<path_nodes.size()-1;k++)
{
if(path_nodes[k]==this->nodes[i]->links[j]->prev->index)
if(path_nodes[k+1]!=this->nodes[i]->links[j]->next->index)
{
this->nodes[i]->remove_link(this->nodes[i]->links[j]);
break;
}
}
}
}
}
bool COpendriveRoad::node_exists_at(TOpendriveWorldPoint &pose)
{
TOpendriveWorldPoint node_pose;
......@@ -186,7 +438,7 @@ bool COpendriveRoad::node_exists_at(const TOpendriveWorldPoint &pose)
return false;
}
COpendriveRoadNode* COpendriveRoad::get_node_at(const TOpendriveWorldPoint &pose) const
COpendriveRoadNode* COpendriveRoad::get_node_at(TOpendriveWorldPoint &pose)
{
TOpendriveWorldPoint node_pose;
......@@ -200,6 +452,7 @@ COpendriveRoadNode* COpendriveRoad::get_node_at(const TOpendriveWorldPoint &pose
return NULL;
}
void COpendriveRoad::load(const std::string &filename)
{
struct stat buffer;
......@@ -207,9 +460,7 @@ void COpendriveRoad::load(const std::string &filename)
if(stat(filename.c_str(),&buffer)==0)
{
// delete any previous data
for(unsigned int i=0;i<this->segments.size();i++)
delete this->segments[i];
this->segments.clear();
this->free();
// try to open the specified file
try{
std::unique_ptr<OpenDRIVE> open_drive(OpenDRIVE_(filename.c_str(), xml_schema::flags::dont_validate));
......@@ -317,16 +568,16 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
}
unsigned int COpendriveRoad::get_closest_node(double x, double y,double heading,double angle_tol)
unsigned int COpendriveRoad::get_closest_node(TOpendriveWorldPoint &point,double angle_tol)
{
double dist,min_dist=std::numeric_limits<double>::max();
TOpendriveWorldPoint point;
TOpendriveWorldPoint closest_point;
unsigned int closest_index;
for(unsigned int i=0;i<this->nodes.size();i++)
{
this->nodes[i]->get_closest_point(x,y,heading,point,angle_tol);
dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
this->nodes[i]->get_closest_point(point,closest_point,angle_tol);
dist=sqrt(pow(closest_point.x-point.x,2.0)+pow(closest_point.y-point.y,2.0));
if(dist<min_dist)
{
min_dist=dist;
......@@ -337,20 +588,20 @@ unsigned int COpendriveRoad::get_closest_node(double x, double y,double heading,
return closest_index;
}
double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol)
double COpendriveRoad::get_closest_point(TOpendriveWorldPoint &point,TOpendriveWorldPoint &closest_point,double angle_tol)
{
double dist,min_dist=std::numeric_limits<double>::max();
TOpendriveWorldPoint point;
TOpendriveWorldPoint point_found;
double length,closest_length;
for(unsigned int i=0;i<this->nodes.size();i++)
{
length=this->nodes[i]->get_closest_point(x,y,heading,point,angle_tol);
dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
length=this->nodes[i]->get_closest_point(point,point_found,angle_tol);
dist=sqrt(pow(point_found.x-point.x,2.0)+pow(point_found.y-point.y,2.0));
if(dist<min_dist)
{
min_dist=dist;
closest_point=point;
closest_point=point_found;
closest_length=length;
}
}
......@@ -358,7 +609,7 @@ double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpen
return closest_length;
}
void COpendriveRoad::get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol)
void COpendriveRoad::get_closest_points(TOpendriveWorldPoint &point,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol)
{
std::vector<TOpendriveWorldPoint> points;
std::vector<double> lengths;
......@@ -367,7 +618,7 @@ void COpendriveRoad::get_closest_points(double x, double y,double heading,std::v
closest_lengths.clear();
for(unsigned int i=0;i<this->nodes.size();i++)
{
this->nodes[i]->get_closest_points(x,y,heading,points,lengths,dist_tol,angle_tol);
this->nodes[i]->get_closest_points(point,points,lengths,dist_tol,angle_tol);
for(unsigned int j=0;j<points.size();i++)
{
closest_points.push_back(points[i]);
......@@ -376,40 +627,132 @@ void COpendriveRoad::get_closest_points(double x, double y,double heading,std::v
}
}
void COpendriveRoad::get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPoint &start_point,TOpendriveWorldPoint &end_point,COpendriveRoad &new_road)
{
segment_up_ref_t new_segment_ref;
lane_up_ref_t new_lane_ref;
node_up_ref_t new_node_ref;
COpendriveRoadNode *node,*next_node;
COpendriveRoadSegment *segment,*new_segment;
std::vector<unsigned int> new_path_nodes;
unsigned int link_index;
int node_side;
bool added;
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);
if(path_nodes.size()==1)
{
node=this->nodes[path_nodes[path_nodes.size()-1]];
link_index=node->get_closest_link(end_point);
segment=node->links[link_index]->segment;
node_side=segment->get_node_side(node);
new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,node_side,&start_point,&end_point);
new_road.add_segment(new_segment,path_nodes,new_path_nodes);
new_segment_ref[segment]=new_segment;
}
else
{
for(unsigned int i=0;i<path_nodes.size()-1;i++)
{
node=this->nodes[path_nodes[i]];
next_node=this->nodes[path_nodes[i+1]];
segment=node->get_link_with(next_node)->segment;
if(segment->has_node(next_node))
continue;
else
{
if(i==0)
{
node_side=segment->get_node_side(node);
new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,node_side,&start_point,NULL);
}
else
new_segment=segment->clone(new_node_ref,new_lane_ref);
new_road.add_segment(new_segment,path_nodes,new_path_nodes);
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_point,3.14159);
segment=node->links[link_index]->segment;
node_side=segment->get_node_side(node);
new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,node_side,NULL,&end_point);
new_road.add_segment(new_segment,path_nodes,new_path_nodes);
new_segment_ref[segment]=new_segment;
}
new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);
// add additional nodes not explicitly in the path
/*
for(unsigned int i=0;i<path_nodes.size();i++)
{
added=false;
node=this->nodes[path_nodes[i]];
for(unsigned int j=i+1;j<path_nodes.size();j++)
{
next_node=this->nodes[path_nodes[j]];
for(unsigned int k=0;k<this->segments.size();k++)
if(this->segments[k]->connects_nodes(node,next_node))
{
new_segment=this->segments[k]->clone(new_node_ref,new_lane_ref);
new_road.add_segment(new_segment,path_nodes,new_path_nodes);
new_segment_ref[segment]=new_segment;
added=true;
break;
}
if(added)
break;
}
}
*/
// remove unconnected elements
new_road.prune(new_path_nodes);
new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref);
}
void COpendriveRoad::get_sub_road(TOpendriveWorldPoint &start_point,double length,COpendriveRoad &road)
{
}
void COpendriveRoad::operator=(const COpendriveRoad& object)
{
COpendriveRoadSegment *segment;
COpendriveRoadNode *node;
std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> new_references;
COpendriveLane *lane;
segment_up_ref_t new_segment_ref;
node_up_ref_t new_node_ref;
lane_up_ref_t new_lane_ref;
this->free();
this->resolution=object.resolution;
this->scale_factor=object.scale_factor;
this->min_road_length=object.min_road_length;
for(unsigned int i=0;i<this->segments.size();i++)
for(unsigned int i=0;i<object.lanes.size();i++)
{
delete this->segments[i];
this->segments[i]=NULL;
lane=new COpendriveLane(*object.lanes[i]);
this->lanes.push_back(lane);
new_lane_ref[object.lanes[i]]=lane;
}
this->segments.clear();
for(unsigned int i=0;i<object.segments.size();i++)
{
segment=new COpendriveRoadSegment(*object.segments[i]);
this->segments.push_back(segment);
new_references[object.segments[i]]=segment;
}
for(unsigned int i=0;i<this->nodes.size();i++)