Commit 166b4a95 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added functions to get a part of a lane and also clone it.

Added functions to create a copy of the lane and update its references.
Added a function to clean any invalid reference.
Added a function to get the closest point on the lane to a given point.
parent 4e0fb284
......@@ -5,16 +5,20 @@
#include "xml/OpenDRIVE_1.4H.hxx"
#endif
#include "opendrive_common.h"
#include "opendrive_road_node.h"
#include "opendrive_road_segment.h"
class COpendriveRoadSegment;
#include "opendrive_lane.h"
class COpendriveLane
{
friend class COpendriveRoadSegment;
friend class COpendriveRoadNode;
friend class COpendriveRoad;
private:
std::vector<COpendriveRoadNode *> nodes;
std::vector<COpendriveLane *> next;
std::vector<COpendriveLane *> prev;
COpendriveLane *left_lane;
opendrive_mark_t left_mark;
COpendriveLane *right_lane;
......@@ -26,30 +30,52 @@ class COpendriveLane
double speed;
double offset;
int id;
unsigned int index;
protected:
COpendriveLane();
COpendriveLane(const COpendriveLane &object,std::map<COpendriveRoadNode *,COpendriveRoadNode *> &node_refs,COpendriveRoadSegment *segment_ref);
COpendriveLane(const COpendriveLane &object);
void load(const right::lane_type &lane_info,COpendriveRoadSegment *segment);
void link_neightbour_lane(COpendriveLane *lane);
void link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start,bool to_start);
void add_next_lane(COpendriveLane *lane);
void add_prev_lane(COpendriveLane *lane);
void add_node(COpendriveRoadNode *node);
bool has_node(COpendriveRoadNode *node);
bool has_node(unsigned int index);
void set_parent_segment(COpendriveRoadSegment *segment);
void set_resolution(double res);
void set_scale_factor(double scale);
void set_width(double width);
void set_speed(double speed);
void set_offset(double offset);
void update_references(std::map<COpendriveLane *,COpendriveLane *> &lane_refs);
void set_id(int id);
void set_index(unsigned int index);
bool updated(lane_up_ref_t &refs);
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);
COpendriveLane *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref);
public:
unsigned int get_num_nodes(void) const;
const COpendriveRoadNode &get_node(unsigned int index) const;
const COpendriveRoadSegment &get_segment(void) const;
unsigned int get_num_next_lanes(void) const;
const COpendriveLane &get_next_lane(unsigned int index) const;
unsigned int get_num_prev_lanes(void) const;
const COpendriveLane &get_prev_lane(unsigned int index) const;
double get_width(void) const;
double get_speed(void) const;
double get_offset(void) const;
unsigned int get_index(void) const;
int get_id(void) const;
TOpendriveWorldPoint get_start_point(void) const;
TOpendriveWorldPoint get_end_point(void) const;
double get_length(void) const;
TOpendriveLocalPoint transform_to_local_point(TOpendriveTrackPoint &track);
TOpendriveWorldPoint transform_to_world_point(TOpendriveTrackPoint &track);
unsigned int get_closest_node(TOpendriveWorldPoint &point,double angle_tol=0.1);
friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
~COpendriveLane();
};
......
#include "opendrive_lane.h"
#include "exceptions.h"
#include <math.h>
COpendriveLane::COpendriveLane()
{
this->nodes.clear();
this->next.clear();
this->prev.clear();
this->left_lane=NULL;
this->left_mark=OD_MARK_NONE;
this->right_lane=NULL;
......@@ -15,24 +18,26 @@ COpendriveLane::COpendriveLane()
this->speed=0.0;
this->offset=0.0;
this->id=0;
this->index=-1;
}
COpendriveLane::COpendriveLane(const COpendriveLane &object,std::map<COpendriveRoadNode *,COpendriveRoadNode *> &node_refs,COpendriveRoadSegment *segment_ref)
COpendriveLane::COpendriveLane(const COpendriveLane &object)
{
this->nodes.resize(object.nodes.size());
for(unsigned int i=0;i<object.nodes.size();i++)
this->nodes[i]=node_refs[object.nodes[i]];
this->nodes=object.nodes;
this->next=object.next;
this->prev=object.prev;
this->left_lane=object.left_lane;
this->left_mark=object.left_mark;
this->right_lane=object.right_lane;
this->right_mark=object.right_mark;
this->segment=segment_ref;
this->segment=object.segment;
this->resolution=object.resolution;
this->scale_factor=object.scale_factor;
this->width=object.width;
this->speed=object.speed;
this->offset=object.offset;
this->id=object.id;
this->index=object.index;
}
void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
......@@ -47,8 +52,8 @@ void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
{
for(unsigned int i=0;i<this->get_num_nodes();i++)
{
this->nodes[i]->add_link(lane->nodes[lane->get_num_nodes()-i-1],this->left_mark);
lane->nodes[lane->get_num_nodes()-i-1]->add_link(this->nodes[i],this->left_mark);
this->nodes[i]->add_link(lane->nodes[lane->get_num_nodes()-i-1],this->left_mark,this->segment,this);
lane->nodes[lane->get_num_nodes()-i-1]->add_link(this->nodes[i],this->left_mark,this->segment,this);
lane->right_mark=this->left_mark;
}
}
......@@ -56,8 +61,8 @@ void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
{
for(unsigned int i=0;i<this->get_num_nodes()-1;i++)
{
this->nodes[i]->add_link(lane->nodes[i+1],this->left_mark);
lane->nodes[i]->add_link(this->nodes[i+1],this->left_mark);
this->nodes[i]->add_link(lane->nodes[i+1],this->left_mark,this->segment,this);
lane->nodes[i]->add_link(this->nodes[i+1],this->left_mark,this->segment,this);
if(this->id>0)// left lanes
this->right_mark=lane->left_mark;
else// right lanes
......@@ -122,25 +127,71 @@ void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool f
end=lane->nodes[0];
}
}
start->add_link(end,mark);
start->add_link(end,mark,this->segment,this);
// link lane
// link laness
this->add_next_lane(lane);
lane->add_prev_lane(this);
}
else
throw CException(_HERE_,"One of the lanes to link has no nodes");
}
}
void COpendriveLane::add_node(COpendriveRoadNode *node)
void COpendriveLane::add_next_lane(COpendriveLane *lane)
{
for(unsigned int i=0;i<this->nodes.size();i++)
if(this->nodes[i]==node)
for(unsigned int i=0;i<this->next.size();i++)
if(this->next[i]==lane)// lane is already linked
return;
// add the lane
this->next.push_back(lane);
}
void COpendriveLane::add_prev_lane(COpendriveLane *lane)
{
for(unsigned int i=0;i<this->prev.size();i++)
if(this->prev[i]==lane)// lane is already linked
return;
// add the lane
this->prev.push_back(lane);
}
void COpendriveLane::add_node(COpendriveRoadNode *node)
{
if(this->has_node(node))
return;
// link the new node with the previous one in the current lane, if any
if(this->nodes.size()>0)
this->nodes[this->nodes.size()-1]->add_link(node,OD_MARK_NONE);
this->nodes[this->nodes.size()-1]->add_link(node,OD_MARK_NONE,this->segment,this);
// add the new node
this->nodes.push_back(node);
}
bool COpendriveLane::has_node(COpendriveRoadNode *node)
{
for(unsigned int i=0;i<this->nodes.size();i++)
if(this->nodes[i]==node)
return true;
return false;
}
bool COpendriveLane::has_node(unsigned int index)
{
for(unsigned int i=0;i<this->nodes.size();i++)
if(this->nodes[i]->get_index()==index)
return true;
return false;
}
void COpendriveLane::set_parent_segment(COpendriveRoadSegment *segment)
{
this->segment=segment;
}
void COpendriveLane::set_resolution(double res)
{
this->resolution=res;
......@@ -157,18 +208,259 @@ void COpendriveLane::set_scale_factor(double scale)
this->nodes[i]->set_scale_factor(scale);
}
void COpendriveLane::set_width(double width)
{
this->width=width;
}
void COpendriveLane::set_speed(double speed)
{
this->speed=speed;
}
void COpendriveLane::set_offset(double offset)
{
this->offset=offset;
}
void COpendriveLane::update_references(std::map<COpendriveLane *,COpendriveLane *> &lane_refs)
void COpendriveLane::set_id(int id)
{
this->left_lane=lane_refs[this->left_lane];
this->right_lane=lane_refs[this->right_lane];
this->id=id;
}
for(unsigned int i=0;i<this->nodes.size();i++)
this->nodes[i]->update_references(lane_refs);
void COpendriveLane::set_index(unsigned int index)
{
this->index=index;
}
bool COpendriveLane::updated(lane_up_ref_t &refs)
{
lane_up_ref_t::iterator updated_it;
for(updated_it=refs.begin();updated_it!=refs.end();updated_it++)
if(updated_it->second==this)
return true;
return false;
}
void COpendriveLane::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
{
std::vector<COpendriveLane *>::iterator lane_it;
std::vector<COpendriveRoadNode *>::iterator node_it;
if(this->updated(lane_refs))
{
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];
(*node_it)->update_references(segment_refs,lane_refs,node_refs);
}
else if((*node_it)->updated(node_refs))
(*node_it)->update_references(segment_refs,lane_refs,node_refs);
}
for(lane_it=this->next.begin();lane_it!=this->next.end();lane_it++)
if(lane_refs.find(*lane_it)!=lane_refs.end())
(*lane_it)=lane_refs[*lane_it];
for(lane_it=this->prev.begin();lane_it!=this->prev.end();lane_it++)
if(lane_refs.find(*lane_it)!=lane_refs.end())
(*lane_it)=lane_refs[*lane_it];
if(lane_refs.find(this->left_lane)!=lane_refs.end())
this->left_lane=lane_refs[this->left_lane];
if(lane_refs.find(this->right_lane)!=lane_refs.end())
this->right_lane=lane_refs[this->right_lane];
if(segment_refs.find(this->segment)!=segment_refs.end())
this->segment=segment_refs[this->segment];
}
}
void COpendriveLane::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
{
std::vector<COpendriveLane *>::iterator lane_it;
std::vector<COpendriveRoadNode *>::iterator node_it;
if(this->updated(lane_refs))
{
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)->clean_references(segment_refs,lane_refs,node_refs);
node_it++;
}
else if((*node_it)->updated(node_refs))
{
(*node_it)->clean_references(segment_refs,lane_refs,node_refs);
node_it++;
}
else
node_it=this->nodes.erase(node_it);
}
for(lane_it=this->next.begin();lane_it!=this->next.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->next.erase(lane_it);
else
lane_it++;
}
for(lane_it=this->prev.begin();lane_it!=this->prev.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->prev.erase(lane_it);
else
lane_it++;
}
if(lane_refs.find(this->left_lane)!=lane_refs.end())
this->left_lane=lane_refs[this->left_lane];
else if(!this->left_lane->updated(lane_refs))
this->left_lane=NULL;
if(lane_refs.find(this->right_lane)!=lane_refs.end())
this->right_lane=lane_refs[this->right_lane];
else if(!this->right_lane->updated(lane_refs))
this->right_lane=NULL;
}
}
void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *start)
{
std::vector<COpendriveRoadNode *>::iterator it;
segment_up_ref_t new_segment_ref;
node_up_ref_t::iterator exist_it;
unsigned int start_node_index,i;
COpendriveRoadNode *new_node;
bool exists;
if(start==NULL)
return;
start_node_index=this->get_closest_node(*start,3.14159);
// eliminate all the node before the start one
for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
{
if(i<start_node_index)
it=this->nodes.erase(it);
else
{
exists=false;
// avoid creating the node for a second time
for(exist_it=new_node_ref.begin();exist_it!=new_node_ref.end();exist_it++)
if((*it)==exist_it->second)
{
exists=true;
break;
}
if(!exists)
{
new_node=new COpendriveRoadNode(**it);
new_node_ref[*it]=new_node;
}
it++;
}
}
this->prev.clear();// it is no longer connected to any previous lane
this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
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)
{
std::vector<COpendriveRoadNode *>::iterator it;
segment_up_ref_t new_segment_ref;
node_up_ref_t::iterator exist_it;
unsigned int end_node_index,i;
COpendriveRoadNode *new_node;
bool exists;
if(end==NULL)
return NULL;
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++)
{
if(i>end_node_index)
it=this->nodes.erase(it);
else
{
exists=false;
// avoid creating the node for a second time
for(exist_it=new_node_ref.begin();exist_it!=new_node_ref.end();exist_it++)
if((*it)==exist_it->second)
{
exists=true;
break;
}
if(!exists)
{
new_node=new COpendriveRoadNode(**it);
new_node_ref[*it]=new_node;
}
it++;
}
}
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;
}
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 *new_lane;
if(start==NULL && end==NULL)
return this->clone(new_node_ref,new_lane_ref);
new_lane=new COpendriveLane(*this);
new_lane_ref[this]=new_lane;
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);
}
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);
}
return new_lane;
}
COpendriveLane *COpendriveLane::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref)
{
COpendriveLane *new_lane;
std::vector<COpendriveRoadNode *>::iterator it;
COpendriveRoadNode *new_node;
segment_up_ref_t new_segment_ref;
new_lane=new COpendriveLane(*this);
new_lane_ref[this]=new_lane;
for(it=new_lane->nodes.begin();it!=new_lane->nodes.end();it++)
{
new_node=new COpendriveRoadNode(**it);
new_node_ref[*it]=new_node;
}
this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
return new_lane;
}
void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegment *segment)
......@@ -186,21 +478,21 @@ void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegmen
this->left_mark=OD_MARK_NONE;
this->right_lane=NULL;
this->right_mark=OD_MARK_NONE;
this->id=lane_info.id().get();
this->set_id(lane_info.id().get());
// import lane width
if(lane_info.width().size()!=1)
{
error << "Only one width record supported for lane " << this->id;
throw CException(_HERE_,error.str());
}
this->width=lane_info.width().begin()->a().get();
this->set_width(lane_info.width().begin()->a().get());
// import lane speed
if(lane_info.speed().size()!=1)
{
error << "Only one speed record supported for lane " << this->id;
throw CException(_HERE_,error.str());
}
this->speed=lane_info.speed().begin()->max().get();
this->set_speed(lane_info.speed().begin()->max().get());
//lane mark
if(lane_info.roadMark().size()==0)
mark=OD_MARK_NONE;
......@@ -238,7 +530,7 @@ void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegmen
else
this->left_mark=mark;
this->segment=segment;
this->set_parent_segment(segment);
}
unsigned int COpendriveLane::get_num_nodes(void) const
......@@ -254,6 +546,32 @@ const COpendriveRoadNode &COpendriveLane::get_node(unsigned int index) const
throw CException(_HERE_,"Invalid node index");
}
unsigned int COpendriveLane::get_num_next_lanes(void) const
{
return this->next.size();
}
const COpendriveLane &COpendriveLane::get_next_lane(unsigned int index) const
{
if(index>=0 && index<this->next.size())
return *this->next[index];
else
throw CException(_HERE_,"Invalid next lane index");
}
unsigned int COpendriveLane::get_num_prev_lanes(void) const
{
return this->prev.size();
}
const COpendriveLane &COpendriveLane::get_prev_lane(unsigned int index) const
{
if(index>=0 && index<this->prev.size())
return *this->prev[index];
else
throw CException(_HERE_,"Invalid previous lane index");
}
const COpendriveRoadSegment &COpendriveLane::get_segment(void) const
{
return *this->segment;
......@@ -274,6 +592,11 @@ double COpendriveLane::get_offset(void) const
return this->offset/this->scale_factor;
}
unsigned int COpendriveLane::get_index(void) const
{
return this->index;
}
int COpendriveLane::get_id(void) const
{
return this->id;
......@@ -409,6 +732,26 @@ TOpendriveWorldPoint COpendriveLane::transform_to_world_point(TOpendriveTrackPoi
throw CException(_HERE_,"Track point does not belong to lane");
}
unsigned int COpendriveLane::get_closest_node(TOpendriveWorldPoint &point,double angle_tol)
{
double dist,min_dist=std::numeric_limits<double>::max();
TOpendriveWorldPoint found_point;
unsigned int closest_index;
for(unsigned int i=0;i<this->nodes.size();i++)
{
this->nodes[i]->get_closest_point(point,found_point,angle_tol);
dist=sqrt(pow(found_point.x-point.x,2.0)+pow(found_point.y-point.y,2.0));
if(dist<min_dist)
{
min_dist=dist;
closest_index=i;
}
}
return closest_index;
}
std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
{
out << " Lane: " << lane.id << " (offset: " << lane.offset << ")" << std::endl;
......@@ -484,6 +827,8 @@ std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
COpendriveLane::~COpendriveLane()
{
this->nodes.clear();
this->prev.clear();
this->next.clear();
this->left_lane=NULL;
this->left_mark=OD_MARK_NONE;
this->right_lane=NULL;
......@@ -494,4 +839,5 @@ COpendriveLane::~COpendriveLane()
this->width=0.0;