From b1fc7a4b1e4de56eb8b080475eaa5f54c6ddada5 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 13 Jan 2021 17:09:20 +0100
Subject: [PATCH] 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.
---
include/opendrive_arc.h | 3 +-
include/opendrive_geometry.h | 8 +-
include/opendrive_lane.h | 5 +-
include/opendrive_line.h | 3 +-
include/opendrive_link.h | 4 +-
include/opendrive_param_poly3.h | 3 +-
include/opendrive_road_node.h | 9 +-
include/opendrive_road_segment.h | 13 +-
include/opendrive_spiral.h | 3 +-
src/opendrive_arc.cpp | 2 +-
src/opendrive_geometry.cpp | 17 +-
src/opendrive_lane.cpp | 166 ++++---------------
src/opendrive_line.cpp | 2 +-
src/opendrive_link.cpp | 39 ++++-
src/opendrive_param_poly3.cpp | 2 +-
src/opendrive_road.cpp | 23 ++-
src/opendrive_road_node.cpp | 216 ++++++------------------
src/opendrive_road_segment.cpp | 275 ++++++++++++++++++++++++-------
src/opendrive_spiral.cpp | 2 +-
19 files changed, 410 insertions(+), 385 deletions(-)
diff --git a/include/opendrive_arc.h b/include/opendrive_arc.h
index e5bc79f..683b321 100644
--- a/include/opendrive_arc.h
+++ b/include/opendrive_arc.h
@@ -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);
diff --git a/include/opendrive_geometry.h b/include/opendrive_geometry.h
index d54840d..f9c7285 100644
--- a/include/opendrive_geometry.h
+++ b/include/opendrive_geometry.h
@@ -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();
diff --git a/include/opendrive_lane.h b/include/opendrive_lane.h
index 6b91343..4208f4c 100644
--- a/include/opendrive_lane.h
+++ b/include/opendrive_lane.h
@@ -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();
diff --git a/include/opendrive_line.h b/include/opendrive_line.h
index 3116aa9..b544bce 100644
--- a/include/opendrive_line.h
+++ b/include/opendrive_line.h
@@ -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);
diff --git a/include/opendrive_link.h b/include/opendrive_link.h
index ae6701b..96a2789 100644
--- a/include/opendrive_link.h
+++ b/include/opendrive_link.h
@@ -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;
diff --git a/include/opendrive_param_poly3.h b/include/opendrive_param_poly3.h
index 2b03eb1..c40fdae 100644
--- a/include/opendrive_param_poly3.h
+++ b/include/opendrive_param_poly3.h
@@ -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);
diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index 9951642..49fa7a6 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -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;
diff --git a/include/opendrive_road_segment.h b/include/opendrive_road_segment.h
index d00bb7f..37dd02c 100644
--- a/include/opendrive_road_segment.h
+++ b/include/opendrive_road_segment.h
@@ -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);
diff --git a/include/opendrive_spiral.h b/include/opendrive_spiral.h
index 12b0a36..106597e 100644
--- a/include/opendrive_spiral.h
+++ b/include/opendrive_spiral.h
@@ -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);
diff --git a/src/opendrive_arc.cpp b/src/opendrive_arc.cpp
index b8567a0..4d18947 100644
--- a/src/opendrive_arc.cpp
+++ b/src/opendrive_arc.cpp
@@ -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;
diff --git a/src/opendrive_geometry.cpp b/src/opendrive_geometry.cpp
index 82d9cd2..dc3ef7b 100644
--- a/src/opendrive_geometry.cpp
+++ b/src/opendrive_geometry.cpp
@@ -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;
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index 53dbe87..57f9d9e 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -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)
diff --git a/src/opendrive_line.cpp b/src/opendrive_line.cpp
index 3f82f4d..9b0fa5e 100644
--- a/src/opendrive_line.cpp
+++ b/src/opendrive_line.cpp
@@ -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;
diff --git a/src/opendrive_link.cpp b/src/opendrive_link.cpp
index 5aff42f..ddffa45 100644
--- a/src/opendrive_link.cpp
+++ b/src/opendrive_link.cpp
@@ -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
diff --git a/src/opendrive_param_poly3.cpp b/src/opendrive_param_poly3.cpp
index 6405540..e57b486 100644
--- a/src/opendrive_param_poly3.cpp
+++ b/src/opendrive_param_poly3.cpp
@@ -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;
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index 6ca93c1..9b300bd 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -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>
void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double length,COpendriveRoad &road)
{
+ segment_up_ref_t new_segment_ref;
+ lane_up_ref_t new_lane_ref;
+ node_up_ref_t new_node_ref;
+ COpendriveRoadSegment *segment,*new_segment;
+ COpendriveRoadNode *node;
+ unsigned int node_index;
+ int node_size;
+ // get the first segment
+/*
+ node_index=this->get_closest_nodes(start_pose,3.14159);
+ if(node_index==(unsigned int)-1)
+ throw CException(_HERE_,"Start position does not beloang to the road");
+ node=this->nodes[node_index];
+ if(node->get_num_parents()>1)
+ throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
+ segment=node->parents[0].segment;
+ node_side=segment->get_node_side(node);
+ new_segment=gegment->get_sub_sugment(new_node_ref,new_lane_ref,node_side,&start_pose,NULL);
+*/
}
void COpendriveRoad::operator=(const COpendriveRoad& object)
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index a3cfb58..f9a990f 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -22,13 +22,7 @@ COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
this->resolution=object.resolution;
this->scale_factor=object.scale_factor;
this->pose=object.pose;
- this->parents.resize(object.parents.size());
- for(unsigned int i=0;i<object.parents.size();i++)
- {
- this->parents[i].geometry=object.parents[i].geometry->clone();
- this->parents[i].lane=object.parents[i].lane;
- this->parents[i].segment=object.parents[i].segment;
- }
+ this->parents=object.parents;
this->links.clear();
for(unsigned int i=0;i<object.links.size();i++)
{
@@ -40,7 +34,7 @@ COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
bool COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane)
{
- TOpendriveWorldPose lane_end,node_pose;
+ TOpendriveWorldPose node_pose;
double start_curvature,end_curvature,length;
COpendriveLink *new_link;
bool lane_found=false;
@@ -59,22 +53,13 @@ bool COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
node_pose=node->get_pose();
for(unsigned int i=0;i<this->parents.size();i++)
{
- try{
- lane_end=this->parents[i].lane->get_end_pose();
- if(fabs(lane_end.x-node_pose.x)<this->resolution && fabs(lane_end.y-node_pose.y)<this->resolution)
- {
- this->parents[i].geometry->get_curvature(start_curvature,end_curvature);
- length=this->parents[i].geometry->get_length();
- if(this->parents[i].lane->get_id()>0)
- {
- start_curvature*=-1.0;
- end_curvature*=-1.0;
- }
- lane_found=true;
- break;
- }
- }
- catch(CException &e){
+ if(this->parents[i].lane==lane && this->parents[i].segment==segment)
+ {
+ start_curvature=this->parents[i].start_curvature;
+ end_curvature=this->parents[i].end_curvature;
+ length=this->parents[i].length;
+ lane_found=true;
+ break;
}
}
if(!lane_found)
@@ -156,6 +141,13 @@ void COpendriveRoadNode::set_scale_factor(double scale)
for(unsigned int i=0;i<this->links.size();i++)
this->links[i]->set_scale_factor(scale);
+
+ for(unsigned int i=0;i<this->parents.size();i++)
+ {
+ this->parents[i].start_curvature*=scale/this->scale_factor;
+ this->parents[i].end_curvature*=scale/this->scale_factor;
+ this->parents[i].length*=this->scale_factor/scale;
+ }
}
void COpendriveRoadNode::set_index(unsigned int index)
@@ -168,41 +160,18 @@ void COpendriveRoadNode::set_pose(TOpendriveWorldPose &start)
this->pose=start;
}
-void COpendriveRoadNode::add_parent(const planView::geometry_type &geom_info,COpendriveLane *lane,COpendriveRoadSegment *segment)
-{
- COpendriveGeometry *geometry;
- std::stringstream text;
-
- // import geometry
- if(geom_info.line().present())
- geometry=new COpendriveLine();
- else if(geom_info.spiral().present())
- geometry=new COpendriveSpiral();
- else if(geom_info.arc().present())
- geometry=new COpendriveArc();
- else if(geom_info.paramPoly3().present())
- geometry=new COpendriveParamPoly3();
- else
- {
- text << "Unsupported or missing geometry in road " << segment->get_name() << " (lane " << lane->get_id() << ")";
- throw CException(_HERE_,text.str());
- }
- geometry->set_scale_factor(this->scale_factor);
- geometry->load(geom_info);
- this->add_parent(geometry,lane,segment);
-}
-
-void COpendriveRoadNode::add_parent(COpendriveGeometry *geometry,COpendriveLane *lane,COpendriveRoadSegment *segment)
+void COpendriveRoadNode::add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double length,double start_curvature,double end_curvature)
{
TOpendriveRoadNodeParent new_parent;
for(unsigned int i=0;i<this->parents.size();i++)
- if(parents[i].geometry==geometry &&this->parents[i].lane==lane && this->parents[i].segment==segment)
+ if(this->parents[i].lane==lane && this->parents[i].segment==segment)
return;
-
new_parent.lane=lane;
new_parent.segment=segment;
- new_parent.geometry=geometry;
+ new_parent.start_curvature=start_curvature;
+ new_parent.end_curvature=end_curvature;
+ new_parent.length=length;
this->parents.push_back(new_parent);
}
@@ -227,11 +196,6 @@ TOpendriveRoadNodeParent *COpendriveRoadNode::get_parent_with_segment(const COpe
void COpendriveRoadNode::free(void)
{
- for(unsigned int i=0;i<this->parents.size();i++)
- {
- delete this->parents[i].geometry;
- this->parents[i].geometry=NULL;
- }
this->parents.clear();
for(unsigned int i=0;i<this->links.size();i++)
{
@@ -244,55 +208,6 @@ void COpendriveRoadNode::free(void)
this->links.clear();
}
-void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriveLane *lane,COpendriveRoadSegment *segment)
-{
- TOpendriveTrackPose track_pose;
- COpendriveGeometry *geometry;
- std::stringstream text;
-
- this->free();
- // import geometry
- if(geom_info.line().present())
- geometry=new COpendriveLine();
- else if(geom_info.spiral().present())
- geometry=new COpendriveSpiral();
- else if(geom_info.arc().present())
- geometry=new COpendriveArc();
- else if(geom_info.paramPoly3().present())
- geometry=new COpendriveParamPoly3();
- else
- {
- text << "Unsupported or missing geometry in road " << segment->get_name() << " (lane " << lane->get_id() << ")";
- throw CException(_HERE_,text.str());
- }
- geometry->set_scale_factor(this->scale_factor);
- geometry->load(geom_info);
- // import start position
- if(lane->get_id()<0)
- {
- track_pose.t=lane->get_offset()-lane->get_width()/2.0;
- track_pose.s=0.0;
- }
- else
- {
- track_pose.t=lane->get_offset()+lane->get_width()/2.0;
- track_pose.s=geometry->get_length();
- }
- track_pose.heading=0.0;
- if(!geometry->get_world_pose(track_pose,this->pose))
- {
- delete geometry;
- text << "Impossible to get world coordinates " << segment->get_name() << " (lane " << lane->get_id() << ")";
- throw CException(_HERE_,text.str());
- }
- else
- {
- if(lane->get_id()>0)
- this->pose.heading=normalize_angle(this->pose.heading+3.14159);
- }
- this->add_parent(geometry,lane,segment);
-}
-
bool COpendriveRoadNode::updated(node_up_ref_t &refs)
{
node_up_ref_t::iterator updated_it;
@@ -350,10 +265,7 @@ void COpendriveRoadNode::clean_references(segment_up_ref_t &segment_refs,lane_up
for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
{
if(!parent_it->segment->updated(segment_refs) || !parent_it->lane->updated(lane_refs))
- {
- delete parent_it->geometry;
parent_it=this->parents.erase(parent_it);
- }
else
parent_it++;
}
@@ -364,29 +276,11 @@ void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldP
{
std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
std::vector<COpendriveLink *>::iterator link_it;
- double length;
+ double new_length;
+ TPoint new_pose;
if(start==NULL)
return;
- // remove the references to all lanes and segments except for lane
- for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
- {
- if(parent_it->lane!=lane)
- {
- delete parent_it->geometry;
- parent_it=this->parents.erase(parent_it);
- }
- else
- parent_it++;
- }
- length=this->get_closest_pose(*start,this->pose,3.14159);
- // update geometry
- for(unsigned int i=0;i<this->parents.size();i++)
- {
- if(lane->get_id()<0)
- this->parents[i].geometry->set_start_pose(pose);
- this->parents[i].geometry->set_max_s(this->parents[i].geometry->get_max_s()-length);
- }
// update the links
for(link_it=this->links.begin();link_it!=this->links.end();)
{
@@ -397,36 +291,35 @@ void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldP
}
else
{
- (*link_it)->update_start_pose(start);
+ new_pose=(*link_it)->update_start_pose(start);
+ new_length=(*link_it)->get_length();
link_it++;
}
}
+ // remove the references to all lanes and segments except for lane
+ for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
+ {
+ if(parent_it->lane!=lane)
+ parent_it=this->parents.erase(parent_it);
+ else
+ {
+ parent_it++;
+ parent_it->length=new_length;
+ parent_it->start_curvature=new_pose.curvature;
+ }
+ }
+
}
void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPose *end)
{
std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
std::vector<COpendriveLink *>::iterator link_it;
- TOpendriveWorldPose end_pose;
- double length;
+ double new_length;
+ TPoint new_pose;
if(end==NULL)
return;
- // remove the references to all lanes and segments except for lane
- for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
- {
- if(parent_it->lane!=lane)
- {
- delete parent_it->geometry;
- parent_it=this->parents.erase(parent_it);
- }
- else
- parent_it++;
- }
- length=this->get_closest_pose(*end,end_pose,3.14159);
- // update geometry
- for(unsigned int i=0;i<this->parents.size();i++)
- this->parents[i].geometry->set_max_s(length);;
// update the links
for(link_it=this->links.begin();link_it!=this->links.end();)
{
@@ -437,10 +330,23 @@ void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPos
}
else
{
- (*link_it)->update_end_pose(end);
+ new_pose=(*link_it)->update_end_pose(end);
+ new_length=(*link_it)->get_length();
link_it++;
}
}
+ // remove the references to all lanes and segments except for lane
+ for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
+ {
+ if(parent_it->lane!=lane)
+ parent_it=this->parents.erase(parent_it);
+ else
+ {
+ parent_it++;
+ parent_it->length=new_length;
+ parent_it->end_curvature=new_pose.curvature;
+ }
+ }
}
double COpendriveRoadNode::get_resolution(void) const
@@ -489,19 +395,6 @@ const COpendriveLane &COpendriveRoadNode::get_parent_lane(unsigned int index) co
}
}
-const COpendriveGeometry &COpendriveRoadNode::get_geometry(unsigned int index) const
-{
- std::stringstream text;
-
- if(index>=0 && index<this->parents.size())
- return *this->parents[index].geometry;
- else
- {
- text << "Invalid parent index " << index << " for node " << this->index << " (has " << this->parents.size() << " parents)";
- throw CException(_HERE_,text.str());
- }
-}
-
TOpendriveWorldPose COpendriveRoadNode::get_pose(void) const
{
return this->pose;
@@ -660,7 +553,6 @@ std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node)
{
out << " Parent " << i << ":" << std::endl;
out << " segment " << node.get_parent_segment(i).get_name() << " (lane " << node.get_parent_lane(i).get_id() << ")" << std::endl;
- out << *node.parents[i].geometry;
}
out << " Number of links: " << node.links.size() << std::endl;
for(unsigned int i=0;i<node.links.size();i++)
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index 80dc2bb..5b40b47 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -17,13 +17,11 @@ COpendriveRoadSegment::COpendriveRoadSegment()
this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
this->resolution=DEFAULT_RESOLUTION;
this->scale_factor=DEFAULT_SCALE_FACTOR;
+ this->geometries.clear();
}
COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object)
{
- COpendriveSignal *new_signal;
- COpendriveObject *new_object;
-
this->name=object.name;
this->id=object.id;
this->lanes=object.lanes;
@@ -32,22 +30,23 @@ COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object
this->nodes=object.nodes;
this->parent_road=object.parent_road;
this->signals.clear();
+ this->signals.resize(object.signals.size());
for(unsigned int i=0;i<object.signals.size();i++)
- {
- new_signal=new COpendriveSignal(*object.signals[i]);
- this->signals.push_back(new_signal);
- }
- this->objects.clear();
- for(unsigned int i=0;i<object.objects.size();i++)
- {
- new_object=new COpendriveObject(*object.objects[i]);
- this->objects.push_back(new_object);
- }
+ this->signals[i]=new COpendriveSignal(*object.signals[i]);
+ this->objects.resize(object.objects.size());
+ for(unsigned int i=0;i<object.signals.size();i++)
+ this->objects[i]=new COpendriveObject(*object.objects[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;
+ this->geometries.resize(object.geometries.size());
+ for(unsigned int i=0;i<object.geometries.size();i++)
+ {
+ this->geometries[i].opendrive=object.geometries[i].opendrive->clone();
+ this->geometries[i].spline=new CG2Spline(*object.geometries[i].spline);
+ }
}
void COpendriveRoadSegment::free(void)
@@ -69,6 +68,14 @@ void COpendriveRoadSegment::free(void)
}
this->objects.clear();
this->connecting.clear();
+ for(unsigned int i=0;i<this->geometries.size();i++)
+ {
+ delete this->geometries[i].opendrive;
+ this->geometries[i].opendrive=NULL;
+ delete this->geometries[i].spline;
+ this->geometries[i].spline=NULL;
+ }
+ this->geometries.clear();
}
void COpendriveRoadSegment::set_resolution(double res)
@@ -79,6 +86,9 @@ void COpendriveRoadSegment::set_resolution(double res)
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
lane_it->second->set_resolution(res);
+
+ for(unsigned int i=0;i<this->geometries.size();i++)
+ this->geometries[i].spline->generate(res);
}
void COpendriveRoadSegment::set_scale_factor(double scale)
@@ -93,6 +103,9 @@ void COpendriveRoadSegment::set_scale_factor(double scale)
this->signals[i]->set_scale_factor(scale);
for(unsigned int i=0;i<this->objects.size();i++)
this->objects[i]->set_scale_factor(scale);
+
+ for(unsigned int i=0;i<this->geometries.size();i++)
+ this->geometries[i].opendrive->set_scale_factor(scale);
}
void COpendriveRoadSegment::set_min_road_length(double length)
@@ -120,6 +133,62 @@ void COpendriveRoadSegment::set_center_mark(opendrive_mark_t mark)
this->center_mark=mark;
}
+void COpendriveRoadSegment::add_geometries(OpenDRIVE::road_type &road_info)
+{
+ planView::geometry_iterator geom_info;
+ TOpendriveRoadSegmentGeometry geometry;
+ TOpendriveWorldPose start_pose,end_pose;
+ TPoint spline_start,spline_end;
+ double start_curvature,end_curvature;
+ std::stringstream error;
+
+ for(unsigned int i=0;i<this->geometries.size();i++)
+ {
+ delete this->geometries[i].opendrive;
+ delete this->geometries[i].spline;
+ }
+ this->geometries.clear();
+ for(geom_info=road_info.planView().geometry().begin(); geom_info!=road_info.planView().geometry().end(); ++geom_info)
+ {
+ // import geometry
+ if(geom_info->line().present())
+ geometry.opendrive=new COpendriveLine();
+ else if(geom_info->spiral().present())
+ geometry.opendrive=new COpendriveSpiral();
+ else if(geom_info->arc().present())
+ geometry.opendrive=new COpendriveArc();
+ else if(geom_info->paramPoly3().present())
+ geometry.opendrive=new COpendriveParamPoly3();
+ else
+ {
+ error << "Unsupported or missing geometry in road " << this->name;
+ throw CException(_HERE_,error.str());
+ }
+ geometry.opendrive->set_scale_factor(this->scale_factor);
+ geometry.opendrive->load(*geom_info);
+ if(geometry.opendrive->get_length()>this->min_road_length)
+ {
+ // create spline
+ geometry.spline=new CG2Spline();
+ start_pose=geometry.opendrive->get_start_pose();
+ end_pose=geometry.opendrive->get_end_pose();
+ geometry.opendrive->get_curvature(start_curvature,end_curvature);
+ spline_start.x=start_pose.x;
+ spline_start.y=start_pose.y;
+ spline_start.heading=start_pose.heading;
+ spline_start.curvature=start_curvature;
+ geometry.spline->set_start_point(spline_start);
+ spline_end.x=end_pose.x;
+ spline_end.y=end_pose.y;
+ spline_end.heading=end_pose.heading;
+ spline_end.curvature=end_curvature;
+ geometry.spline->set_end_point(spline_end);
+ geometry.spline->generate(this->resolution,geometry.opendrive->get_length());
+ this->geometries.push_back(geometry);
+ }
+ }
+}
+
bool COpendriveRoadSegment::updated(segment_up_ref_t &refs)
{
segment_up_ref_t::iterator updated_it;
@@ -298,36 +367,36 @@ void COpendriveRoadSegment::remove_lane(COpendriveLane *lane)
}
}
-void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
+void COpendriveRoadSegment::add_nodes(void)
{
- planView::geometry_iterator geom_it;
double lane_offset;
- for(geom_it=road_info.planView().geometry().begin(); geom_it!=road_info.planView().geometry().end(); ++geom_it)
+ for(unsigned int j=0;j<this->geometries.size();j++)
{
lane_offset=0.0;
for(int i=-1;i>=-this->num_right_lanes;i--)
{
this->lanes[i]->set_offset(lane_offset);
- this->add_node(*geom_it,this->lanes[i]);
+ this->add_node(this->geometries[j],this->lanes[i]);
lane_offset-=this->lanes[i]->width;
}
}
- for(geom_it=road_info.planView().geometry().end(); geom_it!=road_info.planView().geometry().begin();)
+ for(unsigned int j=0;j<this->geometries.size();j++)
{
- geom_it--;
lane_offset=0.0;
for(int i=1;i<=this->num_left_lanes;i++)
{
this->lanes[i]->set_offset(lane_offset);
- this->add_node(*geom_it,this->lanes[i]);
+ this->add_node(this->geometries[this->geometries.size()-1-j],this->lanes[i]);
lane_offset+=this->lanes[i]->width;
}
}
}
-void COpendriveRoadSegment::add_node(const planView::geometry_type &geom_info,COpendriveLane *lane)
+void COpendriveRoadSegment::add_node(TOpendriveRoadSegmentGeometry &geometry,COpendriveLane *lane)
{
+ TOpendriveTrackPose track_pose;
+ double start_curvature,end_curvature,length;
COpendriveRoadNode *new_node;
TOpendriveWorldPose node_pose;
unsigned int node_index;
@@ -337,31 +406,54 @@ void COpendriveRoadSegment::add_node(const planView::geometry_type &geom_info,CO
new_node=new COpendriveRoadNode();
new_node->set_resolution(this->resolution);
new_node->set_scale_factor(this->scale_factor);
- new_node->load(geom_info,lane,this);
- if(new_node->get_geometry(0).get_length()>this->min_road_length)
+ // import start position
+ track_pose.t=lane->get_center_offset();
+ track_pose.heading=0.0;
+ if(lane->get_id()<0)
{
- node_pose=new_node->get_pose();
- if(this->parent_road->node_exists_at(node_pose))
- {
- delete new_node;
- new_node=this->parent_road->get_node_at(node_pose);
- new_node->add_parent(geom_info,lane,this);
- }
- else
+ track_pose.s=0.0;
+ geometry.opendrive->get_world_pose(track_pose,node_pose);
+ }
+ else
+ {
+ track_pose.s=geometry.opendrive->get_length();
+ geometry.opendrive->get_world_pose(track_pose,node_pose);
+ node_pose.heading=normalize_angle(node_pose.heading+3.14159);
+ }
+ new_node->set_pose(node_pose);
+ if(this->parent_road->node_exists_at(node_pose))
+ {
+ delete new_node;
+ new_node=this->parent_road->get_node_at(node_pose);
+ }
+ else
+ {
+ node_index=this->parent_road->add_node(new_node);
+ new_node->set_index(node_index);
+ }
+ geometry.opendrive->get_curvature(start_curvature,end_curvature);
+ if(start_curvature>0)
+ start_curvature=1.0/((1.0/start_curvature)-lane->get_center_offset());
+ else
+ start_curvature=1.0/((1.0/start_curvature)+lane->get_center_offset());
+ if(end_curvature>0)
+ end_curvature=1.0/((1.0/end_curvature)-lane->get_center_offset());
+ else
+ end_curvature=1.0/((1.0/end_curvature)+lane->get_center_offset());
+ length=geometry.opendrive->get_length();
+ if(lane->get_id()<0)
+ new_node->add_parent(lane,this,length,start_curvature,end_curvature);
+ else
+ new_node->add_parent(lane,this,length,-end_curvature,-start_curvature);
+ for(unsigned int i=0;i<this->nodes.size();i++)
+ if(this->nodes[i]==new_node)
{
- node_index=this->parent_road->add_node(new_node);
- new_node->set_index(node_index);
+ exist=true;
+ break;
}
- for(unsigned int i=0;i<this->nodes.size();i++)
- if(this->nodes[i]==new_node)
- {
- exist=true;
- break;
- }
- if(!exist)
- this->nodes.push_back(new_node);
- lane->add_node(new_node);
- }
+ if(!exist)
+ this->nodes.push_back(new_node);
+ lane->add_node(new_node);
}catch(CException &e){
this->free();
if(!this->parent_road->remove_node(new_node))
@@ -595,6 +687,9 @@ 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;
+ std::vector<TOpendriveRoadSegmentGeometry>::iterator geom_it;
+ TPoint new_point;
+ double length;
if(start==NULL && end==NULL)
return this->clone(new_node_ref,new_lane_ref);
@@ -610,7 +705,41 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
new_lane_ref[lane_it->second]=new_lane;
}
new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);
- // update signals and objects
+ // update geometry
+ for(geom_it=this->geometries.begin();geom_it!=this->geometries.end();)
+ {
+ if(start!=NULL)
+ {
+ length=geom_it->spline->find_closest_point(start->x,start->y,new_point);
+ if(length>geom_it->opendrive->get_length())
+ geom_it=this->geometries.erase(geom_it);
+ else
+ {
+ geom_it->spline->set_start_point(new_point);
+ geom_it->spline->generate(this->resolution);
+ geom_it->opendrive->set_start_pose(*start);
+ geom_it->opendrive->set_max_s(geom_it->opendrive->get_max_s()-length);
+ break;
+ }
+ }
+ }
+ for(geom_it=this->geometries.begin();geom_it!=this->geometries.end();)
+ {
+ if(end!=NULL)
+ {
+ length=geom_it->spline->find_closest_point(end->x,end->y,new_point);
+ if(length<geom_it->opendrive->get_length())
+ {
+ geom_it->spline->set_end_point(new_point);
+ geom_it->spline->generate(this->resolution);
+ geom_it->opendrive->set_max_s(length);;
+ geom_it=this->geometries.erase(geom_it+1,this->geometries.end());
+ break;
+ }
+ else
+ geom_it++;
+ }
+ }
return new_segment;
}
@@ -665,11 +794,13 @@ void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
else
lane_section=road_info.lanes().laneSection().begin();
- // add lanes
try{
+ // add lanes
this->add_lanes(*lane_section);
+ // add geometries
+ this->add_geometries(road_info);
// add road nodes
- this->add_nodes(road_info);
+ this->add_nodes();
// link lanes
this->link_neightbour_lanes(*lane_section);
// add road signals
@@ -812,6 +943,16 @@ const COpendriveRoadSegment &COpendriveRoadSegment::get_connecting(unsigned int
}
}
+double COpendriveRoadSegment::get_length(void)
+{
+ double length=0.0;
+
+ for(unsigned int i=0;i<this->geometries.size();i++)
+ length+=this->geometries[i].opendrive->get_length();
+
+ return length;
+}
+
TOpendriveLocalPose COpendriveRoadSegment::transform_to_local_pose(const TOpendriveTrackPose &track)
{
TOpendriveTrackPose pose;
@@ -824,16 +965,23 @@ TOpendriveLocalPose COpendriveRoadSegment::transform_to_local_pose(const TOpendr
throw CException(_HERE_,error.str());
}
pose=track;
- if(this->num_right_lanes>0)
- local=this->lanes[-1]->transform_to_local_pose(pose);
- else
+ for(unsigned int i=0;i<this->geometries.size();i++)
{
- pose.s=this->lanes[1]->get_length()-track.s;
- pose.heading=normalize_angle(track.heading+3.14159);
- local=this->lanes[1]->transform_to_local_pose(pose);
+ if((pose.s-this->geometries[i].opendrive->get_length())<=this->resolution)
+ {
+ if(!this->geometries[i].opendrive->get_local_pose(pose,local))
+ {
+ error << "Impossible to transform to local coordinates pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") in segment " << this->name;
+ throw CException(_HERE_,error.str());
+ }
+ return local;
+ }
+ else
+ pose.s-=this->geometries[i].opendrive->get_length();
}
- return local;
+ error << "Track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") does not belong to segment " << this->name;
+ throw CException(_HERE_,error.str());
}
TOpendriveWorldPose COpendriveRoadSegment::transform_to_world_pose(const TOpendriveTrackPose &track)
@@ -848,16 +996,23 @@ TOpendriveWorldPose COpendriveRoadSegment::transform_to_world_pose(const TOpendr
throw CException(_HERE_,error.str());
}
pose=track;
- if(this->num_right_lanes>0)
- world=this->lanes[-1]->transform_to_world_pose(pose);
- else
+ for(unsigned int i=0;i<this->geometries.size();i++)
{
- pose.s=this->lanes[1]->get_length()-track.s;
- pose.heading=normalize_angle(track.heading+3.14159);
- world=this->lanes[1]->transform_to_world_pose(pose);
+ if((pose.s-this->geometries[i].opendrive->get_length())<=this->resolution)
+ {
+ if(!this->geometries[i].opendrive->get_world_pose(pose,world))
+ {
+ error << "Impossible to transform to world coordinates pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") in segment " << this->name;
+ throw CException(_HERE_,error.str());
+ }
+ return world;
+ }
+ else
+ pose.s-=this->geometries[i].opendrive->get_length();
}
- return world;
+ error << "Track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") does not belong to segment " << this->name;
+ throw CException(_HERE_,error.str());
}
std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment)
diff --git a/src/opendrive_spiral.cpp b/src/opendrive_spiral.cpp
index 1e21a50..089ad95 100644
--- a/src/opendrive_spiral.cpp
+++ b/src/opendrive_spiral.cpp
@@ -12,7 +12,7 @@ COpendriveSpiral::COpendriveSpiral(const COpendriveSpiral &object) : COpendriveG
this->end_curvature=object.end_curvature;
}
-bool COpendriveSpiral::transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
+bool COpendriveSpiral::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
{
return true;
}
--
GitLab