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

Moved the opendrive geometry from the lane to the segment. It describes the center lane.

The links keep the splines to generate the trajectory.
Solved a bug with the computation of the curvature for lanes other than the central one.
parent f8b52ca5
......@@ -6,12 +6,13 @@
class COpendriveArc : public COpendriveGeometry
{
friend class COpendriveRoadNode;
friend class COpendriveRoadSegment;
private:
double curvature;
protected:
COpendriveArc();
COpendriveArc(const COpendriveArc &object);
virtual bool transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
virtual bool transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
virtual void print(std::ostream &out);
virtual void load_params(const planView::geometry_type &geometry_info);
virtual std::string get_name(void);
......
......@@ -11,6 +11,7 @@
class COpendriveGeometry
{
friend class COpendriveRoadNode;
friend class COpendriveRoadSegment;
private:
protected:
COpendriveGeometry();
......@@ -20,7 +21,7 @@ class COpendriveGeometry
double min_s; ///< Starting track coordenate "s" for the geometry.
double max_s; ///< Ending track coordenate "s" for the geometry.
TOpendriveWorldPose pose;
virtual bool transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const = 0;
virtual bool transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const = 0;
virtual void print(std::ostream &out);
virtual void load_params(const planView::geometry_type &geometry_info) = 0;
virtual std::string get_name(void)=0;
......@@ -30,14 +31,15 @@ class COpendriveGeometry
void set_min_s(double s);
public:
virtual COpendriveGeometry *clone(void) = 0;
bool get_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
bool get_world_pose(TOpendriveTrackPose &track,TOpendriveWorldPose &world) const;
bool get_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
bool get_world_pose(const TOpendriveTrackPose &track,TOpendriveWorldPose &world) const;
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;
TOpendriveWorldPose get_start_pose(void) const;
TOpendriveWorldPose get_end_pose(void) const;
void operator=(const COpendriveGeometry &object);
friend std::ostream& operator<<(std::ostream& out, COpendriveGeometry &geom);
virtual ~COpendriveGeometry();
......
......@@ -71,13 +71,14 @@ class COpendriveLane
double get_width(void) const;
double get_speed(void) const;
double get_offset(void) const;
double get_center_offset(void) const;
unsigned int get_index(void) const;
int get_id(void) const;
TOpendriveWorldPose get_start_pose(void) const;
TOpendriveWorldPose get_end_pose(void) const;
double get_length(void) const;
TOpendriveLocalPose transform_to_local_pose(TOpendriveTrackPose &track);
TOpendriveWorldPose transform_to_world_pose(TOpendriveTrackPose &track);
TOpendriveLocalPose transform_to_local_pose(TOpendriveTrackPose &track) const;
TOpendriveWorldPose transform_to_world_pose(TOpendriveTrackPose &track) const;
unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double angle_tol=0.1);
friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
~COpendriveLane();
......
......@@ -6,11 +6,12 @@
class COpendriveLine : public COpendriveGeometry
{
friend class COpendriveRoadNode;
friend class COpendriveRoadSegment;
private:
protected:
COpendriveLine();
COpendriveLine(const COpendriveGeometry &object);
virtual bool transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
virtual bool transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
virtual void print(std::ostream &out);
virtual void load_params(const planView::geometry_type &geometry_info);
virtual std::string get_name(void);
......
......@@ -36,8 +36,8 @@ class COpendriveLink
void generate(double start_curvature,double end_curvature,double length,bool lane);
void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lanei_refs,node_up_ref_t &node_refs);
bool clean_references(node_up_ref_t &refs);
void update_start_pose(TOpendriveWorldPose *start=NULL);
void update_end_pose(TOpendriveWorldPose *end=NULL);
TPoint update_start_pose(TOpendriveWorldPose *start=NULL);
TPoint update_end_pose(TOpendriveWorldPose *end=NULL);
public:
const COpendriveRoadNode &get_prev(void) const;
const COpendriveRoadNode &get_next(void) const;
......
......@@ -14,6 +14,7 @@ typedef struct
class COpendriveParamPoly3 : public COpendriveGeometry
{
friend class COpendriveRoadNode;
friend class COpendriveRoadSegment;
private:
TOpendrivePoly3Params u;
TOpendrivePoly3Params v;
......@@ -21,7 +22,7 @@ class COpendriveParamPoly3 : public COpendriveGeometry
protected:
COpendriveParamPoly3();
COpendriveParamPoly3(const COpendriveParamPoly3 &object);
virtual bool transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
virtual bool transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
virtual void print(std::ostream &out);
virtual void load_params(const planView::geometry_type &geometry_info);
virtual std::string get_name(void);
......
......@@ -10,8 +10,10 @@
typedef struct
{
COpendriveLane * lane;
COpendriveGeometry *geometry;
COpendriveRoadSegment *segment;
double start_curvature;
double end_curvature;
double length;
}TOpendriveRoadNodeParent;
class COpendriveRoadNode
......@@ -31,7 +33,6 @@ class COpendriveRoadNode
COpendriveRoadNode();
COpendriveRoadNode(const COpendriveRoadNode &object);
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);
......@@ -42,8 +43,7 @@ class COpendriveRoadNode
void set_scale_factor(double scale);
void set_index(unsigned int index);
void set_pose(TOpendriveWorldPose &start);
void add_parent(const planView::geometry_type &geom_info,COpendriveLane *lane,COpendriveRoadSegment *segment);
void add_parent(COpendriveGeometry *geometry,COpendriveLane *lane,COpendriveRoadSegment *segment);
void add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double length,double start_curvature,double end_curvature);
TOpendriveRoadNodeParent *get_parent_with_lane(const COpendriveLane *lane);
TOpendriveRoadNodeParent *get_parent_with_segment(const COpendriveRoadSegment *segment);
bool updated(node_up_ref_t &refs);
......@@ -59,7 +59,6 @@ class COpendriveRoadNode
unsigned int get_num_parents(void) const;
const COpendriveRoadSegment& get_parent_segment(unsigned int index) const;
const COpendriveLane &get_parent_lane(unsigned int index) const;
const COpendriveGeometry &get_geometry(unsigned int index) const;
TOpendriveWorldPose get_pose(void) const;
unsigned int get_num_links(void) const;
const COpendriveLink &get_link(unsigned int index) const;
......
......@@ -11,6 +11,12 @@
#include "opendrive_object.h"
#include "opendrive_road.h"
#include "opendrive_road_node.h"
#include "opendrive_geometry.h"
typedef struct{
COpendriveGeometry *opendrive;
CG2Spline *spline;
}TOpendriveRoadSegmentGeometry;
class COpendriveRoadSegment
{
......@@ -20,6 +26,7 @@ class COpendriveRoadSegment
private:
std::map<int,COpendriveLane *> lanes;
std::vector<COpendriveRoadNode *> nodes;
std::vector<TOpendriveRoadSegmentGeometry> geometries;
COpendriveRoad *parent_road;
double resolution;
double scale_factor;
......@@ -51,8 +58,9 @@ class COpendriveRoadSegment
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(const planView::geometry_type &geom_info,COpendriveLane *lane);
void add_geometries(OpenDRIVE::road_type &road_info);
void add_nodes(void);
void add_node(TOpendriveRoadSegmentGeometry &geometry,COpendriveLane *lane);
void add_empty_node(TOpendriveWorldPose &pose,COpendriveLane *lane);
void remove_node(COpendriveRoadNode *node);
bool has_node(COpendriveRoadNode *node);
......@@ -80,6 +88,7 @@ class COpendriveRoadSegment
const COpendriveObject &get_object(unsigned int index) const;
unsigned int get_num_connecting(void) const;
const COpendriveRoadSegment &get_connecting(unsigned int index) const;
double get_length(void);
TOpendriveLocalPose transform_to_local_pose(const TOpendriveTrackPose &track);
TOpendriveWorldPose transform_to_world_pose(const TOpendriveTrackPose &track);
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment);
......
......@@ -6,13 +6,14 @@
class COpendriveSpiral : public COpendriveGeometry
{
friend class COpendriveRoadNode;
friend class COpendriveRoadSegment;
private:
double start_curvature;
double end_curvature;
protected:
COpendriveSpiral();
COpendriveSpiral(const COpendriveSpiral &object);
virtual bool transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
virtual bool transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
virtual void print(std::ostream &out);
virtual void load_params(const planView::geometry_type &geometry_info);
virtual std::string get_name(void);
......
......@@ -11,7 +11,7 @@ COpendriveArc::COpendriveArc(const COpendriveArc &object) : COpendriveGeometry(o
this->curvature=object.curvature;
}
bool COpendriveArc::transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
bool COpendriveArc::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
{
double alpha;
......
......@@ -62,12 +62,12 @@ void COpendriveGeometry::set_min_s(double s)
this->min_s=s*this->scale_factor;
}
bool COpendriveGeometry::get_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
bool COpendriveGeometry::get_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
{
return this->transform_local_pose(track,local);
}
bool COpendriveGeometry::get_world_pose(TOpendriveTrackPose &track,TOpendriveWorldPose &world) const
bool COpendriveGeometry::get_world_pose(const TOpendriveTrackPose &track,TOpendriveWorldPose &world) const
{
TOpendriveLocalPose local;
......@@ -116,6 +116,19 @@ TOpendriveWorldPose COpendriveGeometry::get_start_pose(void) const
return tmp_pose;
}
TOpendriveWorldPose COpendriveGeometry::get_end_pose(void) const
{
TOpendriveTrackPose track_pose;
TOpendriveWorldPose world_pose;
track_pose.s=this->get_length();
track_pose.t=0.0;
track_pose.heading=0.0;
this->get_world_pose(track_pose,world_pose);
return world_pose;
}
void COpendriveGeometry::operator=(const COpendriveGeometry &object)
{
this->min_s = object.min_s;
......
......@@ -628,6 +628,14 @@ double COpendriveLane::get_offset(void) const
return this->offset/this->scale_factor;
}
double COpendriveLane::get_center_offset(void) const
{
if(this->id<0)
return (this->offset-this->width/2.0)/this->scale_factor;
else
return (this->offset+this->width/2.0)/this->scale_factor;
}
unsigned int COpendriveLane::get_index(void) const
{
return this->index;
......@@ -642,40 +650,20 @@ TOpendriveWorldPose COpendriveLane::get_start_pose(void) const
{
TOpendriveTrackPose track_pose;
TOpendriveWorldPose world_pose;
TOpendriveRoadNodeParent *parent;
std::stringstream error;
track_pose.heading=0.0;
track_pose.t=0.0;
if(this->id<0)// right lane
{
track_pose.t=this->get_offset()-this->get_width()/2.0;
track_pose.s=0.0;
parent=this->nodes[0]->get_parent_with_lane(this);
if(parent!=NULL)
parent->geometry->get_world_pose(track_pose,world_pose);
else
{
error << "Node " << this->nodes[0]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
throw CException(_HERE_,error.str());
}
track_pose.heading=0.0;
}
else
{
track_pose.t=this->get_offset()+this->get_width()/2.0;
parent=this->nodes[0]->get_parent_with_lane(this);
if(parent!=NULL)
{
track_pose.s=parent->geometry->get_length();
parent->geometry->get_world_pose(track_pose,world_pose);
}
else
{
error << "Node " << this->nodes[0]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
throw CException(_HERE_,error.str());
}
track_pose.s=this->segment->get_length();
track_pose.heading=3.14159;
}
if(this->id>0)
world_pose.heading=normalize_angle(world_pose.heading+3.14159);
world_pose=this->transform_to_world_pose(track_pose);
return world_pose;
}
......@@ -684,141 +672,57 @@ TOpendriveWorldPose COpendriveLane::get_end_pose(void) const
{
TOpendriveTrackPose track_pose;
TOpendriveWorldPose world_pose;
TOpendriveRoadNodeParent *parent;
std::stringstream error;
track_pose.heading=0.0;
if(this->id>0)// left lane
{
track_pose.t=this->get_offset()+this->get_width()/2.0;
track_pose.s=0.0;
parent=this->nodes[this->nodes.size()-1]->get_parent_with_lane(this);
if(parent!=NULL)
parent->geometry->get_world_pose(track_pose,world_pose);
else
{
error << "Node " << this->nodes[this->nodes.size()-1]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
throw CException(_HERE_,error.str());
}
track_pose.t=0.0;
if(this->id<0)// right_lane
{
track_pose.s=this->segment->get_length();
track_pose.heading=0.0;
}
else
{
track_pose.t=this->get_offset()-this->get_width()/2.0;
parent=this->nodes[this->nodes.size()-1]->get_parent_with_lane(this);
if(parent!=NULL)
{
track_pose.s=parent->geometry->get_length();
parent->geometry->get_world_pose(track_pose,world_pose);
}
else
{
error << "Node " << this->nodes[this->nodes.size()-1]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
throw CException(_HERE_,error.str());
}
{
track_pose.s=0.0;
track_pose.heading=3.14159;
}
if(this->id>0)
world_pose.heading=normalize_angle(world_pose.heading+3.14159);
world_pose=this->transform_to_world_pose(track_pose);
return world_pose;
}
double COpendriveLane::get_length(void) const
{
TOpendriveRoadNodeParent *parent;
std::stringstream error;
double length=0.0;
for(unsigned int i=0;i<this->nodes.size();i++)
{
parent=this->nodes[i]->get_parent_with_lane(this);
if(parent!=NULL)
length+=parent->geometry->get_length();
else
for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
{
error << "Node " << this->nodes[i]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
throw CException(_HERE_,error.str());
if(&this->nodes[i]->get_link(j).get_parent_lane()==this)
length+=this->nodes[i]->links[j]->get_length();
}
}
return length;
}
TOpendriveLocalPose COpendriveLane::transform_to_local_pose(TOpendriveTrackPose &track)
TOpendriveLocalPose COpendriveLane::transform_to_local_pose(TOpendriveTrackPose &track) const
{
TOpendriveLocalPose local;
TOpendriveRoadNodeParent *parent;
std::stringstream error;
TOpendriveTrackPose pose;
if(track.s<0.0)
{
error << "Invalid track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ")";
throw CException(_HERE_,error.str());
}
for(unsigned int i=0;i<this->nodes.size();i++)
{
parent=this->nodes[i]->get_parent_with_lane(this);
if(parent!=NULL)
{
if(track.s<=parent->geometry->get_length())
{
if(!parent->geometry->get_local_pose(track,local))
{
error << "Impossible to transform to local coordinates pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") in lane " << parent->lane->get_id() << " of segment " << parent->segment->get_name();
throw CException(_HERE_,error.str());
}
return local;
}
else
track.s-=parent->geometry->get_length();
}
else
{
error << "Node " << this->nodes[i]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
throw CException(_HERE_,error.str());
}
}
error << "Track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") does not belong to lane " << this->id << " of segment " << this->segment->get_name();
throw CException(_HERE_,error.str());
pose=track;
pose.t+=this->get_center_offset();
return this->segment->transform_to_local_pose(pose);
}
TOpendriveWorldPose COpendriveLane::transform_to_world_pose(TOpendriveTrackPose &track)
TOpendriveWorldPose COpendriveLane::transform_to_world_pose(TOpendriveTrackPose &track) const
{
TOpendriveWorldPose world;
TOpendriveRoadNodeParent *parent;
std::stringstream error;
if(track.s<0.0)
{
error << "Invalid track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ")";
throw CException(_HERE_,error.str());
}
for(unsigned int i=0;i<this->nodes.size();i++)
{
parent=this->nodes[i]->get_parent_with_lane(this);
if(parent!=NULL)
{
if(track.s<=parent->geometry->get_length())
{
if(!parent->geometry->get_world_pose(track,world))
{
error << "Impossible to transform to local coordinates pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") in lane " << parent->lane->get_id() << " of segment " << parent->segment->get_name();
throw CException(_HERE_,error.str());
}
return world;
}
else
track.s-=parent->geometry->get_length();
}
else
{
error << "Node " << this->nodes[i]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
throw CException(_HERE_,error.str());
}
}
TOpendriveTrackPose pose;
error << "Track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") does not belong to lane " << this->id << " of segment " << this->segment->get_name();
throw CException(_HERE_,error.str());
pose=track;
pose.t+=this->get_center_offset();
return this->segment->transform_to_world_pose(pose);
}
unsigned int COpendriveLane::get_closest_node_index(TOpendriveWorldPose &pose,double angle_tol)
......
......@@ -10,7 +10,7 @@ COpendriveLine::COpendriveLine(const COpendriveGeometry &object) : COpendriveGeo
}
bool COpendriveLine::transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
bool COpendriveLine::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
{
local.u=track.s;
local.v=track.t;
......
......@@ -53,11 +53,34 @@ void COpendriveLink::set_parent_lane(COpendriveLane *lane)
void COpendriveLink::set_resolution(double res)
{
this->resolution=res;
if(this->spline!=NULL)
this->spline->generate(res);
}
void COpendriveLink::set_scale_factor(double scale)
{
TPoint spline_start,spline_end;
if(this->spline!=NULL)
{
this->spline->get_start_point(spline_start);
spline_start.x*=this->scale_factor/scale;
spline_start.y*=this->scale_factor/scale;
spline_start.curvature*=scale/this->scale_factor;
this->spline->set_start_point(spline_start);
this->spline->get_end_point(spline_end);
spline_end.x*=this->scale_factor/scale;
spline_end.y*=this->scale_factor/scale;
spline_end.curvature*=scale/this->scale_factor;
this->spline->set_end_point(spline_end);
this->spline->generate(this->resolution);
}
this->scale_factor=scale;
}
void COpendriveLink::generate(double start_curvature,double end_curvature,double length,bool lane)
......@@ -105,13 +128,13 @@ bool COpendriveLink::clean_references(node_up_ref_t &refs)
return false;
}
void COpendriveLink::update_start_pose(TOpendriveWorldPose *start)
TPoint COpendriveLink::update_start_pose(TOpendriveWorldPose *start)
{
TPoint start_pose;
TPoint start_pose={0};
double length;
if(start==NULL)
return;
return start_pose;
if(this->spline!=NULL)
{
length=this->spline->find_closest_point(start->x,start->y,start_pose);
......@@ -119,21 +142,25 @@ void COpendriveLink::update_start_pose(TOpendriveWorldPose *start)
this->spline->set_start_point(start_pose);
this->spline->generate(this->resolution,length);
}
return start_pose;
}
void COpendriveLink::update_end_pose(TOpendriveWorldPose *end)
TPoint COpendriveLink::update_end_pose(TOpendriveWorldPose *end)
{
TPoint end_pose;
TPoint end_pose={0};
double length;
if(end==NULL)
return;
return end_pose;
if(this->spline!=NULL)
{
length=this->spline->find_closest_point(end->x,end->y,end_pose);
this->spline->set_end_point(end_pose);
this->spline->generate(this->resolution,length);
}
return end_pose;
}
const COpendriveRoadNode &COpendriveLink::get_prev(void) const
......
......@@ -27,7 +27,7 @@ COpendriveParamPoly3::COpendriveParamPoly3(const COpendriveParamPoly3 &object) :
this->normalized=object.normalized;
}
bool COpendriveParamPoly3::transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
bool COpendriveParamPoly3::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
{
double p = (this->normalized ? track.s/((this->max_s - this->min_s)/this->scale_factor):track.s);
double p2 = p*p;
......
......@@ -741,9 +741,9 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
// add the last segment
node=this->nodes[path_nodes[path_nodes.size()-1]];
link_index=node->get_closest_link(end_pose,3.14159);
link=node->links[link_index];
if(link==NULL)
if(link_index==(unsigned int)-1)
throw CException(_HERE_,"The provided path has unconnected nodes");
link=node->links[link_index];
segment=link->segment;
if(new_segment_ref.find(segment)==new_segment_ref.end())
{
......@@ -786,7 +786,26 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>