Commit 863db51e authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added funcitons to get a part of a segment and also clone it.

Added functions to create a copy of the segment and update its references.
Added a function to clean any invalid reference.
parent 166b4a95
......@@ -5,21 +5,21 @@
#include "xml/OpenDRIVE_1.4H.hxx"
#endif
#include "opendrive_common.h"
#include "opendrive_lane.h"
#include "opendrive_signal.h"
#include "opendrive_object.h"
#include "opendrive_road.h"
class COpendriveLane;
class COpendriveRoad;
class COpendriveSignal;
class COpendriveObject;
#include "opendrive_road_node.h"
class COpendriveRoadSegment
{
friend class COpendriveRoad;
friend class COpendriveRoadNode;
friend class COpendriveLane;
private:
std::map<int,COpendriveLane *> lanes;
std::vector<COpendriveRoadNode *> nodes;
COpendriveRoad *parent_road;
double resolution;
double scale_factor;
......@@ -34,25 +34,41 @@ class COpendriveRoadSegment
opendrive_mark_t center_mark;
protected:
COpendriveRoadSegment();
COpendriveRoadSegment(const COpendriveRoadSegment &object,std::map<COpendriveRoadNode *,COpendriveRoadNode *> &node_refs,COpendriveRoad *road_ref);
COpendriveRoadSegment(const COpendriveRoadSegment &object);
void load(OpenDRIVE::road_type &road_info);
void free(void);
void set_resolution(double res);
void set_scale_factor(double scale);
void set_min_road_length(double length);
void set_parent_road(COpendriveRoad *parent);
void update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> &segment_refs);
void set_name(std::string &name);
void set_id(unsigned int id);
void set_center_mark(opendrive_mark_t mark);
bool updated(segment_up_ref_t &segment_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 add_lanes(lanes::laneSection_type &lane_section);
void remove_lane(COpendriveLane *lane);
void add_nodes(OpenDRIVE::road_type &road_info);
void add_node(COpendriveRoadNode *node);
void remove_node(COpendriveRoadNode *node);
bool has_node(COpendriveRoadNode *node);
int get_node_side(COpendriveRoadNode *node);
void link_neightbour_lanes(lanes::laneSection_type &lane_section);
void link_segment(COpendriveRoadSegment &segment);
void link_segment(COpendriveRoadSegment &segment,int from,bool from_start, int to,bool to_start);
bool connects_to(COpendriveRoadSegment *segment);
bool connects_nodes(COpendriveRoadNode *node1,COpendriveRoadNode *node2);
COpendriveRoadSegment *get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,int node_side,TOpendriveWorldPoint *start=NULL,TOpendriveWorldPoint *end=NULL);
COpendriveRoadSegment *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref);
public:
std::string get_name(void) const;
unsigned int get_id(void) const;
unsigned int get_num_right_lanes(void) const;
unsigned int get_num_left_lanes(void) const;
const COpendriveLane &get_lane(int index) const;
unsigned int get_num_nodes(void) const;
const COpendriveRoadNode &get_node(unsigned int index) const;
const COpendriveRoad &get_parent_road(void) const;
unsigned int get_num_signals(void) const;
const COpendriveSignal &get_signal(unsigned int index) const;
......
#include "opendrive_road_segment.h"
#include "exceptions.h"
#include <math.h>
COpendriveRoadSegment::COpendriveRoadSegment()
{
......@@ -8,6 +9,7 @@ COpendriveRoadSegment::COpendriveRoadSegment()
this->lanes.clear();
this->num_left_lanes=0;
this->num_right_lanes=0;
this->nodes.clear();
this->signals.clear();
this->objects.clear();
this->connecting.clear();
......@@ -16,27 +18,18 @@ COpendriveRoadSegment::COpendriveRoadSegment()
this->scale_factor=DEFAULT_SCALE_FACTOR;
}
COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object,std::map<COpendriveRoadNode *,COpendriveRoadNode *> &node_refs,COpendriveRoad *road_ref)
COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object)
{
COpendriveLane *new_lane;
COpendriveSignal *new_signal;
COpendriveObject *new_object;
std::map<int,COpendriveLane *>::const_iterator lane_it;
std::map<COpendriveLane *,COpendriveLane *> new_refs;
this->name=object.name;
this->id=object.id;
this->lanes.clear();
for(lane_it=object.lanes.begin();lane_it!=object.lanes.end();++lane_it)
{
new_lane=new COpendriveLane(*lane_it->second,node_refs,this);
this->lanes[lane_it->first]=new_lane;
new_refs[lane_it->second]=new_lane;
}
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
lane_it->second->update_references(new_refs);
this->parent_road=road_ref;
this->lanes=object.lanes;
this->num_right_lanes=object.num_right_lanes;
this->num_left_lanes=object.num_left_lanes;
this->nodes=object.nodes;
this->parent_road=object.parent_road;
this->signals.clear();
for(unsigned int i=0;i<object.signals.size();i++)
{
......@@ -49,21 +42,15 @@ COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object
new_object=new COpendriveObject(*object.objects[i]);
this->objects.push_back(new_object);
}
this->connecting.resize(object.connecting.size());
for(unsigned int i=0;i<object.connecting.size();i++)
this->connecting[i]=object.connecting[i];
this->connecting=object.connecting;
this->resolution=object.resolution;
this->scale_factor=object.scale_factor;
this->min_road_length=object.min_road_length;
this->center_mark=object.center_mark;
}
void COpendriveRoadSegment::free(void)
{
for(int i=-this->num_right_lanes;i<=this->num_left_lanes && i!=0;i++)
{
delete this->lanes[i];
this->lanes[i]=NULL;
}
this->lanes.clear();
this->num_left_lanes=0;
this->num_right_lanes=0;
......@@ -72,6 +59,7 @@ void COpendriveRoadSegment::free(void)
delete this->signals[i];
this->signals[i]=NULL;
}
this->nodes.clear();
this->signals.clear();
for(unsigned int i=0;i<this->objects.size();i++)
{
......@@ -116,14 +104,112 @@ void COpendriveRoadSegment::set_parent_road(COpendriveRoad *parent)
this->parent_road=parent;
}
void COpendriveRoadSegment::update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> &segment_refs)
void COpendriveRoadSegment::set_name(std::string &name)
{
for(unsigned int i=0;i<this->connecting.size();i++)
this->connecting[i]=segment_refs[this->connecting[i]];
for(unsigned int i=0;i<this->signals.size();i++)
this->signals[i]->update_references(segment_refs);
for(unsigned int i=0;i<this->objects.size();i++)
this->objects[i]->update_references(segment_refs);
this->name=name;
}
void COpendriveRoadSegment::set_id(unsigned int id)
{
this->id=id;
}
void COpendriveRoadSegment::set_center_mark(opendrive_mark_t mark)
{
this->center_mark=mark;
}
bool COpendriveRoadSegment::updated(segment_up_ref_t &refs)
{
segment_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 COpendriveRoadSegment::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::map<int,COpendriveLane *>::iterator lane_it;
std::vector<COpendriveRoadNode *>::iterator node_it;
if(this->updated(segment_refs))
{
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
{
if(lane_refs.find((*lane_it).second)!=lane_refs.end())
{
lane_it->second=lane_refs[lane_it->second];
lane_it->second->update_references(segment_refs,lane_refs,node_refs);
}
else if(lane_it->second->updated(lane_refs))
lane_it->second->update_references(segment_refs,lane_refs,node_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];
for(seg_it=this->connecting.begin();seg_it!=this->connecting.end();seg_it++)
if(segment_refs.find(*seg_it)!=segment_refs.end())
(*seg_it)=segment_refs[*seg_it];
for(unsigned int i=0;i<this->signals.size();i++)
this->signals[i]->update_references(segment_refs);
for(unsigned int i=0;i<this->objects.size();i++)
this->objects[i]->update_references(segment_refs);
}
}
void COpendriveRoadSegment::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::map<int,COpendriveLane *>::iterator lane_it;
std::vector<COpendriveRoadNode *>::iterator node_it;
if(this->updated(segment_refs))
{
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
{
if(lane_refs.find((*lane_it).second)!=lane_refs.end())
{
lane_it->second=lane_refs[lane_it->second];
lane_it->second->clean_references(segment_refs,lane_refs,node_refs);
lane_it++;
}
else if(lane_it->second->updated(lane_refs))
{
lane_it->second->clean_references(segment_refs,lane_refs,node_refs);
lane_it++;
}
else
lane_it=this->lanes.erase(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++;
}
for(seg_it=this->connecting.begin();seg_it!=this->connecting.end();)
{
if(segment_refs.find(*seg_it)!=segment_refs.end())
{
(*seg_it)=segment_refs[*seg_it];
seg_it++;
}
else if(!(*seg_it)->updated(segment_refs))
seg_it=this->connecting.erase(seg_it);
else
seg_it++;
}
}
}
void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
......@@ -132,6 +218,7 @@ void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
left::lane_iterator l_lane_it;
COpendriveLane *new_lane;
std::stringstream text;
unsigned int lane_index;
// right lanes
if(lane_section.right().present())
......@@ -144,6 +231,8 @@ void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
new_lane->set_scale_factor(this->scale_factor);
new_lane->load(*r_lane_it,this);
this->lanes[new_lane->get_id()]=new_lane;
lane_index=parent_road->add_lane(new_lane);
new_lane->set_index(lane_index);
this->num_right_lanes++;
}catch(CException &e){
text << e.what() << " in road " << this->name;
......@@ -162,6 +251,8 @@ void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
new_lane->set_scale_factor(this->scale_factor);
new_lane->load(*l_lane_it,this);
this->lanes[new_lane->get_id()]=new_lane;
lane_index=parent_road->add_lane(new_lane);
new_lane->set_index(lane_index);
this->num_left_lanes++;
}catch(CException &e){
text << e.what() << " in road " << this->name;
......@@ -171,6 +262,53 @@ void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
}
}
void COpendriveRoadSegment::remove_lane(COpendriveLane *lane)
{
std::map<int,COpendriveLane *>::iterator lane_it;
std::vector<COpendriveLane *>::iterator other_lane_it;
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
{
if(lane_it->second==lane)
{
if(lane_it->first>0)
this->num_left_lanes--;
else
this->num_right_lanes--;
// remove associated nodes
for(unsigned int i=0;i<lane->nodes.size();i++)
this->remove_node(lane->nodes[i]);
// remove the reference from neightbour lanes
if(lane_it->second->left_lane!=NULL)
lane_it->second->left_lane->right_lane=NULL;
if(lane_it->second->right_lane!=NULL)
lane_it->second->right_lane->left_lane=NULL;
// remove the reference from next lanes
for(unsigned int i=0;i<lane_it->second->next.size();i++)
for(other_lane_it=lane_it->second->next[i]->prev.begin();other_lane_it!=lane_it->second->next[i]->prev.end();)
{
if((*other_lane_it)==lane)
other_lane_it=lane_it->second->next[i]->prev.erase(other_lane_it);
else
other_lane_it++;
}
// remove the reference from prev lanes
for(unsigned int i=0;i<lane_it->second->prev.size();i++)
for(other_lane_it=lane_it->second->prev[i]->next.begin();other_lane_it!=lane_it->second->prev[i]->next.end();)
{
if((*other_lane_it)==lane)
other_lane_it=lane_it->second->prev[i]->next.erase(other_lane_it);
else
other_lane_it++;
}
lane_it=this->lanes.erase(lane_it);
break;
}
else
lane_it++;
}
}
void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
{
std::map<int,COpendriveLane *>::const_iterator lane_it;
......@@ -204,8 +342,9 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
{
index=this->parent_road->add_node(new_node);
new_node->set_index(index);
new_node->set_parent_segment(this);
}
new_node->add_parent_segment(this);
this->add_node(new_node);
this->lanes[i]->add_node(new_node);
}
lane_offset-=this->lanes[i]->width;
......@@ -235,8 +374,9 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
{
index=this->parent_road->add_node(new_node);
new_node->set_index(index);
new_node->set_parent_segment(this);
}
new_node->add_parent_segment(this);
this->add_node(new_node);
this->lanes[i]->add_node(new_node);
}
lane_offset+=this->lanes[i]->width;
......@@ -250,6 +390,56 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
}
}
void COpendriveRoadSegment::add_node(COpendriveRoadNode *node)
{
for(unsigned int i=0;i<this->nodes.size();i++)
if(this->nodes[i]==node)
return;
//add the node
this->nodes.push_back(node);
}
void COpendriveRoadSegment::remove_node(COpendriveRoadNode *node)
{
std::vector<COpendriveRoadNode *>::iterator it;
for(it=this->nodes.begin();it!=this->nodes.end();)
{
if((*it)==node)
{
it=this->nodes.erase(it);
break;
}
else
it++;
}
}
bool COpendriveRoadSegment::has_node(COpendriveRoadNode *node)
{
for(unsigned int i=0;i<this->nodes.size();i++)
if(this->nodes[i]==node)
return true;
return false;
}
int COpendriveRoadSegment::get_node_side(COpendriveRoadNode *node)
{
std::map<int,COpendriveLane *>::iterator it;
if(this->has_node(node))
{
for(it=this->lanes.begin();it!=this->lanes.end();it++)
if(it->second->has_node(node))
return it->first;
return 0;
}
else
throw CException(_HERE_,"Road segment does not include the given node");
}
void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_section)
{
center::lane_type center_lane;
......@@ -260,30 +450,30 @@ void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_
if(center_lane.roadMark().size()>1)
throw CException(_HERE_,"Only one road mark supported for lane");
else if(center_lane.roadMark().size()==0)
this->center_mark=OD_MARK_NONE;
this->set_center_mark(OD_MARK_NONE);
else if(center_lane.roadMark().size()==1)
{
if(center_lane.roadMark().begin()->type1().present())
{
if(center_lane.roadMark().begin()->type1().get()=="none")
this->center_mark=OD_MARK_NONE;
this->set_center_mark(OD_MARK_NONE);
else if(center_lane.roadMark().begin()->type1().get()=="solid")
this->center_mark=OD_MARK_SOLID;
this->set_center_mark(OD_MARK_SOLID);
else if(center_lane.roadMark().begin()->type1().get()=="broken")
this->center_mark=OD_MARK_BROKEN;
this->set_center_mark(OD_MARK_BROKEN);
else if(center_lane.roadMark().begin()->type1().get()=="solid solid")
this->center_mark=OD_MARK_SOLID_SOLID;
this->set_center_mark(OD_MARK_SOLID_SOLID);
else if(center_lane.roadMark().begin()->type1().get()=="solid broken")
this->center_mark=OD_MARK_SOLID_BROKEN;
this->set_center_mark(OD_MARK_SOLID_BROKEN);
else if(center_lane.roadMark().begin()->type1().get()=="broken solid")
this->center_mark=OD_MARK_BROKEN_SOLID;
this->set_center_mark(OD_MARK_BROKEN_SOLID);
else if(center_lane.roadMark().begin()->type1().get()=="broken broken")
this->center_mark=OD_MARK_BROKEN_BROKEN;
this->set_center_mark(OD_MARK_BROKEN_BROKEN);
else
this->center_mark=OD_MARK_NONE;
this->set_center_mark(OD_MARK_NONE);
}
else
this->center_mark=OD_MARK_NONE;
this->set_center_mark(OD_MARK_NONE);
}
}
for(int i=-this->num_right_lanes;i<0;i++)
......@@ -300,14 +490,12 @@ void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_
else
this->lanes[i]->right_mark=OD_MARK_NONE;
}
/*
if(this->lanes.find(1)!=this->lanes.end() && this->lanes.find(-1)!=this->lanes.end())
{
this->lanes[-1]->left_mark=center_mark;
this->lanes[1]->right_mark=center_mark;
this->lanes[-1]->link_neightbour_lane(this->lanes[1]);
}
*/
if(this->lanes.find(1)!=this->lanes.end())
this->lanes[1]->right_mark=this->center_mark;
if(this->lanes.find(-1)!=this->lanes.end())
this->lanes[-1]->left_mark=this->center_mark;
// this->lanes[-1]->link_neightbour_lane(this->lanes[1]);
}
void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment)
......@@ -388,6 +576,79 @@ void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment,int from
}
}
bool COpendriveRoadSegment::connects_to(COpendriveRoadSegment* segment)
{
for(unsigned int i=0;i<this->connecting.size();i++)
if(this->connecting[i]==segment)
return true;
return false;
}
bool COpendriveRoadSegment::connects_nodes(COpendriveRoadNode *node1,COpendriveRoadNode *node2)
{
if(this->has_node(node1) && this->has_node(node2))
return true;
else
return false;
}
COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,int node_side,TOpendriveWorldPoint *start,TOpendriveWorldPoint *end)
{
std::map<int,COpendriveLane *>::iterator lane_it;
lane_up_ref_t::iterator lane_ref_it;
COpendriveLane *new_lane;
std::vector<COpendriveRoadSegment *>::iterator seg_it;
segment_up_ref_t new_segment_ref;
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);
new_segment=new COpendriveRoadSegment(*this);
new_segment_ref[this]=new_segment;
/* 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,&new_node,start,end);
else
new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,&new_node,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
return new_segment;
}
COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref)
{
std::map<int,COpendriveLane *>::iterator lane_it;
lane_up_ref_t::iterator lane_ref_it;
COpendriveLane *new_lane;
segment_up_ref_t new_segment_ref;
COpendriveRoadSegment *new_segment;
std::vector<COpendriveRoadNode *>::iterator node_it;
node_up_ref_t::iterator node_ref_it;
new_segment=new COpendriveRoadSegment(*this);
new_segment_ref[this]=new_segment;
/* 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_ref[lane_it->second]=new_lane;
}
new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);
return new_segment;
}
void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
{
std::map<int,COpendriveLane *>::const_iterator lane_it;
......@@ -398,8 +659,8 @@ void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
COpendriveObject *new_object;
this->free();
this->name=road_info.name().get();
this->id=std::stoi(road_info.id().get());
this->set_name(road_info.name().get());
this->set_id(std::stoi(road_info.id().get()));
// only one lane section is supported
if(road_info.lanes().laneSection().size()<1)
throw CException(_HERE_,"No lane sections defined for road "+road_info.id().get());
......@@ -475,6 +736,19 @@ const COpendriveLane &COpendriveRoadSegment::get_lane(int index) const
throw CException(_HERE_,"invalid lane index");
}
unsigned int COpendriveRoadSegment::get_num_nodes(void) const
{
return this->nodes.size();
}
const COpendriveRoadNode &COpendriveRoadSegment::get_node(unsigned int index) const
{
if(index>=0 && index<this->nodes.size())
return *this->nodes[index];
else
throw CException(_HERE_,"Invalid signal index");
}
const COpendriveRoad &COpendriveRoadSegment::get_parent_road(void) const
{