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

Improved general coding.

Improved Exception report information.
clean_references() functions no longer update references.
parent cf165668
......@@ -25,6 +25,9 @@ class COpendriveGeometry
virtual void load_params(const planView::geometry_type &geometry_info) = 0;
virtual std::string get_name(void)=0;
void set_scale_factor(double scale);
void set_start_point(TOpendriveWorldPoint &point);
void set_max_s(double s);
void set_min_s(double s);
public:
virtual COpendriveGeometry *clone(void) = 0;
bool get_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const;
......@@ -32,6 +35,9 @@ class COpendriveGeometry
bool in_range(double s) const;
double get_length(void) const;
virtual void get_curvature(double &start,double &end)=0;
double get_max_s(void) const;
double get_min_s(void) const;
TOpendriveWorldPoint get_start_point(void) const;
void operator=(const COpendriveGeometry &object);
friend std::ostream& operator<<(std::ostream& out, COpendriveGeometry &geom);
virtual ~COpendriveGeometry();
......
......@@ -34,15 +34,18 @@ class COpendriveLane
protected:
COpendriveLane();
COpendriveLane(const COpendriveLane &object);
void load(const right::lane_type &lane_info,COpendriveRoadSegment *segment);
void load(const ::lane &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 remove_lane(COpendriveLane *lane);
void add_node(COpendriveRoadNode *node);
bool has_node(COpendriveRoadNode *node);
bool has_node(unsigned int index);
bool has_node_with_index(unsigned int index);
void set_parent_segment(COpendriveRoadSegment *segment);
void set_left_lane(COpendriveLane *lane,opendrive_mark_t mark);
void set_right_lane(COpendriveLane *lane,opendrive_mark_t mark);
void set_resolution(double res);
void set_scale_factor(double scale);
void set_width(double width);
......@@ -75,7 +78,7 @@ class COpendriveLane
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);
unsigned int get_closest_node_index(TOpendriveWorldPoint &point,double angle_tol=0.1);
friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
~COpendriveLane();
};
......
......@@ -46,9 +46,7 @@ class COpendriveLink
const COpendriveLane &get_parent_lane(void) const;
double get_resolution(void) const;
double get_scale_factor(void) const;
double find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point);
double get_track_point_world(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world);
double get_track_point_local(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
double find_closest_world_point(TOpendriveWorldPoint &world,TOpendriveWorldPoint &point);
void get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length=0.0, double end_length=-1.0) const;
void get_trajectory(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double start_length=0.0, double end_length=-1.0) const;
double get_length(void) const;
......
......@@ -23,13 +23,13 @@ class COpendriveRoad
double scale_factor;
double min_road_length;
protected:
COpendriveRoadSegment &operator[](std::string &key);
COpendriveRoadSegment *get_segment_by_id(std::string &id);
void free(void);
void link_segments(OpenDRIVE &open_drive);
unsigned int add_node(COpendriveRoadNode *node);
void remove_node(COpendriveRoadNode *node);
bool remove_node(COpendriveRoadNode *node);
unsigned int add_lane(COpendriveLane *lane);
void remove_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);
bool has_segment(COpendriveRoadSegment *segment);
......
......@@ -7,6 +7,13 @@
#include "opendrive_lane.h"
#include "opendrive_road_segment.h"
typedef struct
{
COpendriveLane * lane;
COpendriveGeometry *geometry;
COpendriveRoadSegment *segment;
}TOpendriveRoadNodeParent;
class COpendriveRoadNode
{
friend class COpendriveLane;
......@@ -17,44 +24,46 @@ class COpendriveRoadNode
std::vector<COpendriveLink *> links;
double resolution;
double scale_factor;
TOpendriveWorldPoint start_point;
std::vector<COpendriveLane *> lanes;
std::vector<COpendriveGeometry *> geometries;
std::vector<COpendriveRoadSegment *>parents;
TOpendriveWorldPoint point;
std::vector<TOpendriveRoadNodeParent> parents;
unsigned int index;
protected:
COpendriveRoadNode();
COpendriveRoadNode(const COpendriveRoadNode &object);
void load(const planView::geometry_type &geom_info,COpendriveLane *lane);
void add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane);
void free(void);
void load(const planView::geometry_type &geom_info,COpendriveLane *lane,COpendriveRoadSegment *segment);
bool add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane);
void add_link(COpendriveLink *link);
void remove_link(COpendriveLink *link);
bool has_link(COpendriveLink *link);
bool has_link_with(COpendriveRoadNode *node);
COpendriveLink *get_link_with(COpendriveRoadNode *node);
void set_resolution(double res);
void set_scale_factor(double scale);
void set_index(unsigned int index);
void set_start_point(TOpendriveWorldPoint &start);
void add_parent_segment(COpendriveRoadSegment *segment);
void add_lane(const planView::geometry_type &geom_info,COpendriveLane *lane);
void set_point(TOpendriveWorldPoint &start);
void add_parent(const planView::geometry_type &geom_info,COpendriveLane *lane,COpendriveRoadSegment *segment);
void add_parent(COpendriveGeometry *geometry,COpendriveLane *lane,COpendriveRoadSegment *segment);
TOpendriveRoadNodeParent *get_parent_with_lane(const COpendriveLane *lane);
TOpendriveRoadNodeParent *get_parent_with_segment(const COpendriveRoadSegment *segment);
bool updated(node_up_ref_t &refs);
COpendriveRoadNode *get_original_node(node_up_ref_t &node_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_pose(COpendriveLane *lane,TOpendriveWorldPoint *start=NULL);
void update_end_pose(COpendriveLane *lane,TOpendriveWorldPoint *end=NULL);
COpendriveLink *get_link_with(COpendriveRoadNode *end);
public:
double get_resolution(void) const;
double get_scale_factor(void) const;
unsigned int get_index(void) const;
unsigned int get_num_parent_segments(void) const;
unsigned int get_num_parents(void) const;
const COpendriveRoadSegment& get_parent_segment(unsigned int index) const;
TOpendriveWorldPoint get_start_pose(void) const;
const COpendriveLane &get_parent_lane(unsigned int index) const;
const COpendriveGeometry &get_geometry(unsigned int index) const;
TOpendriveWorldPoint get_point(void) const;
unsigned int get_num_links(void) const;
const COpendriveLink &get_link(unsigned int index) const;
const COpendriveLink &get_link_ending_at(unsigned int node_index) const;
unsigned int get_num_lanes(void) const;
const COpendriveLane &get_lane(unsigned int index) const;
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;
......
......@@ -49,15 +49,18 @@ class COpendriveRoadSegment
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 add_lane(const ::lane &lane_info);
void remove_lane(COpendriveLane *lane);
void add_nodes(OpenDRIVE::road_type &road_info);
void add_node(COpendriveRoadNode *node);
void add_node(const planView::geometry_type &geom_info,COpendriveLane *lane);
void add_empty_node(TOpendriveWorldPoint &point,COpendriveLane *lane);
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);
void link_neightbour_lanes(void);
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_segments(COpendriveRoadSegment *segment1,COpendriveRoadSegment *segment2);
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);
......
......@@ -23,9 +23,11 @@ COpendriveGeometry::COpendriveGeometry(const COpendriveGeometry &object)
void COpendriveGeometry::print(std::ostream& out)
{
TOpendriveWorldPoint tmp_point=this->get_start_point();
out << " Geometry " << this->get_name() << std::endl;
out << " lenght: " << this->get_length() << std::endl;
out << " pose: x = " << this->pose.x/this->scale_factor << ", y = " << this->pose.y/this->scale_factor << ", heading = " << this->pose.heading << std::endl;
out << " pose: x = " << tmp_point.x << ", y = " << tmp_point.y << ", heading = " << tmp_point.heading << std::endl;
}
void COpendriveGeometry::load(const planView::geometry_type &geometry_info)
......@@ -43,6 +45,23 @@ void COpendriveGeometry::set_scale_factor(double scale)
this->scale_factor=scale;
}
void COpendriveGeometry::set_start_point(TOpendriveWorldPoint &point)
{
this->pose.x=point.x*this->scale_factor;
this->pose.y=point.y*this->scale_factor;
this->pose.heading=point.heading;
}
void COpendriveGeometry::set_max_s(double s)
{
this->max_s=s*this->scale_factor;
}
void COpendriveGeometry::set_min_s(double s)
{
this->min_s=s*this->scale_factor;
}
bool COpendriveGeometry::get_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const
{
return this->transform_local_pose(track,local);
......@@ -76,6 +95,27 @@ double COpendriveGeometry::get_length(void) const
return (this->max_s-this->min_s)/this->scale_factor;
}
double COpendriveGeometry::get_max_s(void) const
{
return this->max_s/this->scale_factor;
}
double COpendriveGeometry::get_min_s(void) const
{
return this->min_s/this->scale_factor;
}
TOpendriveWorldPoint COpendriveGeometry::get_start_point(void) const
{
TOpendriveWorldPoint tmp_point;
tmp_point.x=this->pose.x/this->scale_factor;
tmp_point.y=this->pose.y/this->scale_factor;
tmp_point.heading=this->pose.heading;
return tmp_point;
}
void COpendriveGeometry::operator=(const COpendriveGeometry &object)
{
this->min_s = object.min_s;
......
This diff is collapsed.
......@@ -65,12 +65,12 @@ void COpendriveLink::generate(double start_curvature,double end_curvature,double
TOpendriveWorldPoint node_start,node_end;
TPoint start,end;
node_start=this->prev->get_start_pose();
node_start=this->prev->get_point();
start.x=node_start.x;
start.y=node_start.y;
start.heading=node_start.heading;
start.curvature=start_curvature;
node_end=this->next->get_start_pose();
node_end=this->next->get_point();
end.x=node_end.x;
end.y=node_end.y;
end.heading=node_end.heading;
......@@ -107,27 +107,33 @@ bool COpendriveLink::clean_references(node_up_ref_t &refs)
void COpendriveLink::update_start_pose(TOpendriveWorldPoint *start)
{
TPoint spline_start;
TPoint start_point;
double length;
if(start==NULL)
return;
length=this->find_closest_world_point(*start,spline_start);
length=this->spline->get_length()-length;
this->spline->set_start_point(spline_start);
this->spline->generate(this->resolution,length);
if(this->spline!=NULL)
{
length=this->spline->find_closest_point(start->x,start->y,start_point);
length=this->spline->get_length()-length;
this->spline->set_start_point(start_point);
this->spline->generate(this->resolution,length);
}
}
void COpendriveLink::update_end_pose(TOpendriveWorldPoint *end)
{
TPoint spline_end;
TPoint end_point;
double length;
if(end==NULL)
return;
length=this->find_closest_world_point(*end,spline_end);
this->spline->set_end_point(spline_end);
this->spline->generate(this->resolution,length);
if(this->spline!=NULL)
{
length=this->spline->find_closest_point(end->x,end->y,end_point);
this->spline->set_end_point(end_point);
this->spline->generate(this->resolution,length);
}
}
const COpendriveRoadNode &COpendriveLink::get_prev(void) const
......@@ -165,14 +171,17 @@ double COpendriveLink::get_scale_factor(void) const
return this->scale_factor;
}
double COpendriveLink::find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point)
double COpendriveLink::find_closest_world_point(TOpendriveWorldPoint &world,TOpendriveWorldPoint &point)
{
TPoint spline_point;
double length;
if(this->spline!=NULL)
{
length=this->spline->find_closest_point(world.x,world.y,point);
point.heading=normalize_angle(point.heading);
length=this->spline->find_closest_point(world.x,world.y,spline_point);
point.x=spline_point.x;
point.y=spline_point.y;
point.heading=normalize_angle(spline_point.heading);
}
else
length=std::numeric_limits<double>::max();
......@@ -180,16 +189,6 @@ double COpendriveLink::find_closest_world_point(TOpendriveWorldPoint &world,TPoi
return length;
}
double COpendriveLink::get_track_point_world(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world)
{
}
double COpendriveLink::get_track_point_local(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local)
{
}
void COpendriveLink::get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length, double end_length) const
{
std::vector<double> curvature,heading;
......@@ -234,6 +233,7 @@ std::ostream& operator<<(std::ostream& out, COpendriveLink &link)
{
out << " Previous node: " << link.get_prev().get_index() << std::endl;
out << " Next node: " << link.get_next().get_index() << std::endl;
out << " Parent segment: " << link.get_parent_segment().get_name() << " (lane " << link.get_parent_lane().get_id() << ")" << std::endl;
out << " Road mark: ";
switch(link.get_road_mark())
{
......
......@@ -46,17 +46,21 @@ COpendriveRoad::COpendriveRoad(const COpendriveRoad& object)
new_segment_ref[object.segments[i]]=segment;
}
// update references
this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
}
COpendriveRoadSegment &COpendriveRoad::operator[](std::string &key)
COpendriveRoadSegment *COpendriveRoad::get_segment_by_id(std::string &id)
{
std::stringstream error;
for(unsigned int i=0;i<this->segments.size();i++)
{
if(this->segments[i]->get_id()==(unsigned int)std::stoi(key))
return *this->segments[i];
if(this->segments[i]->get_id()==(unsigned int)std::stoi(id))
return this->segments[i];
}
throw CException(_HERE_,"No road segment with the given ID");
error << "No road segment with the " << id << " ID";
throw CException(_HERE_,error.str());
}
void COpendriveRoad::free(void)
......@@ -85,11 +89,13 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
{
std::string predecessor_id,successor_id;
std::string predecessor_contact,successor_contact;
COpendriveRoadSegment *prev_road,*road,*next_road;
std::stringstream error;
for(OpenDRIVE::road_iterator road_it(open_drive.road().begin());road_it!=open_drive.road().end();++road_it)
{
// get current segment
COpendriveRoadSegment &road=(*this)[road_it->id().get()];
road=this->get_segment_by_id(road_it->id().get());
// get predecessor and successor
if(road_it->lane_link().present())
{
......@@ -114,14 +120,14 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
{
if(!predecessor_id.empty())
{
COpendriveRoadSegment &prev_road=(*this)[predecessor_id];
prev_road.link_segment(road);
prev_road=this->get_segment_by_id(predecessor_id);
prev_road->link_segment(road);
predecessor_id.clear();
}
if(!successor_id.empty())
{
COpendriveRoadSegment &next_road=(*this)[successor_id];
road.link_segment(next_road);
next_road=this->get_segment_by_id(successor_id);
road->link_segment(next_road);
successor_id.clear();
}
}
......@@ -136,15 +142,21 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
if(connection_it->incomingRoad().present())
incoming_road_id=connection_it->incomingRoad().get();
else
throw CException(_HERE_,"Connectivity information missing");
{
error << "Connectivity information missing for segment " << road->get_name() << ": No incomming segment";
throw CException(_HERE_,error.str());
}
if(connection_it->connectingRoad().present())
connecting_road_id=connection_it->connectingRoad().get();
else
throw CException(_HERE_,"Connectivity information missing");
{
error << "Connectivity information missing for segment " << road->get_name() << ": No connecting segment";
throw CException(_HERE_,error.str());
}
if(predecessor_id.compare(incoming_road_id)==0 && successor_id.compare(connecting_road_id)==0)// this is the connection
{
COpendriveRoadSegment &prev_road=(*this)[predecessor_id];
COpendriveRoadSegment &next_road=(*this)[successor_id];
prev_road=this->get_segment_by_id(predecessor_id);
next_road=this->get_segment_by_id(successor_id);
for(connection::laneLink_iterator lane_link_it(connection_it->laneLink().begin()); lane_link_it!=connection_it->laneLink().end();++lane_link_it)
{
int from_lane_id;
......@@ -152,27 +164,40 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
if(lane_link_it->from().present())
from_lane_id=lane_link_it->from().get();
else
throw CException(_HERE_,"Connectivity information missing");
{
error << "Connectivity information missing for segment " << road->get_name() << ": lane link missing from identifier";
throw CException(_HERE_,error.str());
}
if(lane_link_it->to().present())
to_lane_id=lane_link_it->to().get();
else
throw CException(_HERE_,"Connectivity information missing");
{
error << "Connectivity information missing for segment " << road->get_name() << ": lane link missing to identifier";
throw CException(_HERE_,error.str());
}
if(predecessor_contact=="end")
prev_road.link_segment(road,from_lane_id,false,-1,true);
prev_road->link_segment(road,from_lane_id,false,-1,true);
else
prev_road.link_segment(road,from_lane_id,true,-1,true);
TOpendriveWorldPoint end=road.get_lane(-1).get_end_point();
prev_road->link_segment(road,from_lane_id,true,-1,true);
TOpendriveWorldPoint end=road->get_lane(-1).get_end_point();
TOpendriveWorldPoint start;
if(successor_contact=="end")
{
TOpendriveWorldPoint start=next_road.get_lane(to_lane_id).get_end_point();
if(to_lane_id<0)
start=next_road->get_lane(to_lane_id).get_end_point();
else
start=next_road->get_lane(to_lane_id).get_start_point();
if(fabs(end.x-start.x)<this->resolution && fabs(end.y-start.y)<this->resolution)
road.link_segment(next_road,-1,false,to_lane_id,false);
road->link_segment(next_road,-1,false,to_lane_id,false);
}
else
{
TOpendriveWorldPoint start=next_road.get_lane(to_lane_id).get_start_point();
if(to_lane_id<0)
start=next_road->get_lane(to_lane_id).get_start_point();
else
start=next_road->get_lane(to_lane_id).get_end_point();
if(fabs(end.x-start.x)<this->resolution && fabs(end.y-start.y)<this->resolution)
road.link_segment(next_road,-1,false,to_lane_id,true);
road->link_segment(next_road,-1,false,to_lane_id,true);
}
}
}
......@@ -184,17 +209,22 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
unsigned int COpendriveRoad::add_node(COpendriveRoadNode *node)
{
std::stringstream error;
for(unsigned int i=0;i<this->nodes.size();i++)
{
if(this->nodes[i]==node)
throw CException(_HERE_,"Node already present");
{
error << "Node " << node->get_index() << " already present";
throw CException(_HERE_,error.str());
}
}
this->nodes.push_back(node);
return this->nodes.size()-1;
}
void COpendriveRoad::remove_node(COpendriveRoadNode *node)
bool COpendriveRoad::remove_node(COpendriveRoadNode *node)
{
std::vector<COpendriveRoadNode *>::iterator it;
......@@ -204,26 +234,33 @@ void COpendriveRoad::remove_node(COpendriveRoadNode *node)
{
delete *it;
it=this->nodes.erase(it);
break;
return true;
}
else
it++;
}
return false;
}
unsigned int COpendriveRoad::add_lane(COpendriveLane *lane)
{
std::stringstream error;
for(unsigned int i=0;i<this->lanes.size();i++)
{
if(this->lanes[i]==lane)
throw CException(_HERE_,"Lane already present");
{
error << "Lane " << lane->get_index() << " already present";
throw CException(_HERE_,error.str());
}
}
this->lanes.push_back(lane);
return this->lanes.size()-1;
}
void COpendriveRoad::remove_lane(COpendriveLane *lane)
bool COpendriveRoad::remove_lane(COpendriveLane *lane)
{
std::vector<COpendriveLane *>::iterator it;
......@@ -233,44 +270,37 @@ void COpendriveRoad::remove_lane(COpendriveLane *lane)
{
delete *it;
it=this->lanes.erase(it);
break;
return true;
}
else
it++;
}
return false;
}
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)
try{
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);
(*lane_it)->link_neightbour_lane((*lane_it)->left_lane);
(*lane_it)->link_neightbour_lane((*lane_it)->right_lane);
}
else