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

Taken into account when the segments can repeat in the path.

Added a function to add additional node in unconnected lanes.
Solved some bugs.
parent 3dddbc16
......@@ -54,8 +54,8 @@ class COpendriveLane
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 update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *start=NULL);
COpendriveRoadNode *update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *end=NULL);
COpendriveLane *get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,COpendriveRoadNode **new_node,TOpendriveWorldPoint *start=NULL,TOpendriveWorldPoint *end=NULL);
void update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *end=NULL);
COpendriveLane *get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *start=NULL,TOpendriveWorldPoint *end=NULL);
COpendriveLane *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref);
public:
unsigned int get_num_nodes(void) const;
......
......@@ -30,6 +30,7 @@ class COpendriveRoad
void remove_node(COpendriveRoadNode *node);
unsigned int add_lane(COpendriveLane *lane);
void 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 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);
......@@ -54,7 +55,7 @@ class COpendriveRoad
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);
std::vector<unsigned int> 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);
......
......@@ -39,7 +39,7 @@ class COpendriveRoadNode
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 update_start_pose(COpendriveLane *lane,TOpendriveWorldPoint *start=NULL);
COpendriveRoadNode *update_end_pose(COpendriveLane *lane,TOpendriveWorldPoint *end=NULL);
void update_end_pose(COpendriveLane *lane,TOpendriveWorldPoint *end=NULL);
COpendriveLink *get_link_with(COpendriveRoadNode *end);
public:
double get_resolution(void) const;
......@@ -56,6 +56,7 @@ class COpendriveRoadNode
unsigned int get_num_geometries(void) const;
const COpendriveGeometry &get_geometry(unsigned int index) const;
unsigned int get_closest_link(TOpendriveWorldPoint &point,double angle_tol=0.1) const;
double get_closest_distance(TOpendriveWorldPoint &point,double angle_tol=0.1) const;
double get_closest_point(TOpendriveWorldPoint &point,TOpendriveWorldPoint &closest_point,double angle_tol=0.1) const;
void get_closest_points(TOpendriveWorldPoint &point,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1) const;
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
......
......@@ -129,7 +129,6 @@ void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool f
}
start->add_link(end,mark,this->segment,this);
// link lane
// link laness
this->add_next_lane(lane);
lane->add_prev_lane(this);
}
......@@ -374,7 +373,7 @@ void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t
this->nodes[0]->update_start_pose(this,start);
}
COpendriveRoadNode *COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *end)
void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *end)
{
std::vector<COpendriveRoadNode *>::iterator it;
segment_up_ref_t new_segment_ref;
......@@ -384,7 +383,7 @@ COpendriveRoadNode *COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,
bool exists;
if(end==NULL)
return NULL;
return;
end_node_index=this->get_closest_node(*end,3.14159);
// eliminate all the node before the start one
for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
......@@ -411,18 +410,10 @@ COpendriveRoadNode *COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,
}
this->next.clear();// it is no longer connected to any next lane
this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
new_node=this->nodes[this->nodes.size()-1]->update_end_pose(this,end);
if(new_node!=NULL)
{
this->add_node(new_node);
new_node_ref[new_node]=new_node;
this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
}
return new_node;
this->nodes[this->nodes.size()-1]->update_end_pose(this,end);
}
COpendriveLane *COpendriveLane::get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,COpendriveRoadNode **new_node,TOpendriveWorldPoint *start,TOpendriveWorldPoint *end)
COpendriveLane *COpendriveLane::get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *start,TOpendriveWorldPoint *end)
{
COpendriveLane *new_lane;
......@@ -433,12 +424,12 @@ COpendriveLane *COpendriveLane::get_sub_lane(node_up_ref_t &new_node_ref,lane_up
if(this->id<0)
{
new_lane->update_start_node(new_node_ref,new_lane_ref,start);
(*new_node)=new_lane->update_end_node(new_node_ref,new_lane_ref,end);
new_lane->update_end_node(new_node_ref,new_lane_ref,end);
}
else
{
new_lane->update_start_node(new_node_ref,new_lane_ref,end);
(*new_node)=new_lane->update_end_node(new_node_ref,new_lane_ref,start);
new_lane->update_end_node(new_node_ref,new_lane_ref,start);
}
return new_lane;
......
......@@ -240,6 +240,41 @@ void COpendriveRoad::remove_lane(COpendriveLane *lane)
}
}
void COpendriveRoad::complete_open_lanes(void)
{
std::vector<COpendriveLane *>::iterator lane_it;
COpendriveRoadNode *new_node;
TOpendriveWorldPoint end_point;
unsigned int new_node_index;
// remove all nodes and lanes not present in the path
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
{
if((*lane_it)->next.empty())// lane is not connected
{
if((*lane_it)->get_id()<0)
end_point=(*lane_it)->get_end_point();
else
end_point=(*lane_it)->get_start_point();
if(!this->node_exists_at(end_point))// there is no node at the end point
{
new_node=new COpendriveRoadNode();
new_node->set_resolution(this->resolution);
new_node->set_scale_factor(this->scale_factor);
new_node->set_start_point(end_point);
new_node_index=this->add_node(new_node);
new_node->set_index(new_node_index);
(*lane_it)->add_node(new_node);
(*lane_it)->segment->add_node(new_node);
}
else
lane_it++;
}
else
lane_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++)
......@@ -263,7 +298,7 @@ void COpendriveRoad::add_segment(COpendriveRoadSegment *segment,std::vector<unsi
{
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());
new_path[j]=this->nodes.size();
segment->nodes[i]->set_index(this->nodes.size());
this->nodes.push_back(segment->nodes[i]);
}
......@@ -405,6 +440,16 @@ 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++)
......@@ -627,17 +672,19 @@ void COpendriveRoad::get_closest_points(TOpendriveWorldPoint &point,std::vector<
}
}
void COpendriveRoad::get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPoint &start_point,TOpendriveWorldPoint &end_point,COpendriveRoad &new_road)
std::vector<unsigned int> 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;
COpendriveRoadNode *node,*next_node,*repeat_node,*repeat_next_node;
COpendriveRoadSegment *segment,*new_segment,*repeat_segment;
std::vector<unsigned int> new_path_nodes;
unsigned int link_index;
bool added,repeated;
int node_side;
bool added;
new_path_nodes.resize(path_nodes.size());
new_road.free();
new_road.set_resolution(this->resolution);
......@@ -668,7 +715,30 @@ void COpendriveRoad::get_sub_road(std::vector<unsigned int> &path_nodes,TOpendri
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);
repeated=false;
for(unsigned int j=1;j<path_nodes.size();j++)
{
repeat_node=this->nodes[path_nodes[j]];
if(j==path_nodes.size()-1)
{
link_index=node->get_closest_link(end_point,3.14159);
repeat_segment=node->links[link_index]->segment;
}
else
{
repeat_next_node=this->nodes[path_nodes[j+1]];
repeat_segment=repeat_node->get_link_with(repeat_next_node)->segment;
}
if(segment==repeat_segment)
{
repeated=true;
break;
}
}
if(repeated)
new_segment=segment->clone(new_node_ref,new_lane_ref);
else
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);
......@@ -680,10 +750,25 @@ void COpendriveRoad::get_sub_road(std::vector<unsigned int> &path_nodes,TOpendri
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;
repeated=false;
for(unsigned int j=0;j<path_nodes.size()-1;j++)
{
repeat_node=this->nodes[path_nodes[j]];
repeat_next_node=this->nodes[path_nodes[j+1]];
repeat_segment=repeat_node->get_link_with(repeat_next_node)->segment;
if(segment==repeat_segment)
{
repeated=true;
break;
}
}
if(!repeated)
{
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
......@@ -712,6 +797,9 @@ void COpendriveRoad::get_sub_road(std::vector<unsigned int> &path_nodes,TOpendri
// remove unconnected elements
new_road.prune(new_path_nodes);
new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref);
new_road.complete_open_lanes();
return new_path_nodes;
}
void COpendriveRoad::get_sub_road(TOpendriveWorldPoint &start_point,double length,COpendriveRoad &road)
......
......@@ -354,17 +354,16 @@ void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldP
}
}
COpendriveRoadNode *COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPoint *end)
void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPoint *end)
{
std::vector<COpendriveLane *>::iterator lane_it;
std::vector<COpendriveLink *>::iterator link_it;
COpendriveRoadNode *new_node=NULL;
TOpendriveWorldPoint end_point;
unsigned int i;
double length;
if(end==NULL)
return NULL;
return;
// remove the references to all lanes and segments except for lane
for(i=0,lane_it=this->lanes.begin();lane_it!=this->lanes.end();i++)
{
......@@ -382,34 +381,19 @@ COpendriveRoadNode *COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOp
for(unsigned int i=0;i<this->geometries.size();i++)
this->geometries[i]->max_s=length*this->scale_factor;
// update the links
for(i=0,link_it=this->links.begin();link_it!=this->links.end();i++)
for(link_it=this->links.begin();link_it!=this->links.end();)
{
if((*link_it)->prev==this)
{
if((*link_it)->lane==lane)
{
// create a dummy node
new_node=new COpendriveRoadNode();
new_node->set_resolution(this->resolution);
new_node->set_scale_factor(this->scale_factor);
new_node->set_start_point(end_point);
(*link_it)->set_next(new_node);
new_node->add_link(*link_it);
// update the end position of the link
(*link_it)->update_end_pose(end);
link_it++;
}
else
{
delete *link_it;
link_it=this->links.erase(link_it);
}
delete *link_it;
link_it=this->links.erase(link_it);
}
else
{
(*link_it)->update_end_pose(end);
link_it++;
}
}
return new_node;
}
COpendriveLink *COpendriveRoadNode::get_link_with(COpendriveRoadNode *end)
......@@ -534,6 +518,30 @@ unsigned int COpendriveRoadNode::get_closest_link(TOpendriveWorldPoint &point,do
return closest_index;
}
double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPoint &point,double angle_tol)const
{
double dist,min_dist=std::numeric_limits<double>::max();
double angle;
TPoint spline_point;
for(unsigned int i=0;i<this->links.size();i++)
{
if(&this->links[i]->get_prev()==this)// links starting at this node
{
this->links[i]->find_closest_world_point(point,spline_point);
angle=diff_angle(normalize_angle(point.heading),spline_point.heading);
if(fabs(angle)<angle_tol)
{
dist=sqrt(pow(spline_point.x-point.x,2.0)+pow(spline_point.y-point.y,2.0));
if(dist<min_dist)
min_dist=dist;
}
}
}
return min_dist;
}
double COpendriveRoadNode::get_closest_point(TOpendriveWorldPoint &point,TOpendriveWorldPoint &closest_point,double angle_tol) const
{
double dist,min_dist=std::numeric_limits<double>::max();
......
......@@ -603,7 +603,6 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
COpendriveRoadSegment *new_segment;
std::vector<COpendriveRoadNode *>::iterator node_it;
node_up_ref_t::iterator node_ref_it;
COpendriveRoadNode *new_node;
if(start==NULL && end==NULL)
return this->clone(new_node_ref,new_lane_ref);
......@@ -613,12 +612,10 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
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,&new_node,start,end);
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,&new_node,end,start);
new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,end,start);
new_lane_ref[lane_it->second]=new_lane;
if(new_node!=NULL)
new_segment->add_node(new_node);
}
new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);
// update signals and objects
......
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