diff --git a/include/opendrive_road_segment.h b/include/opendrive_road_segment.h
index fb263febcec659ee1675d451d9b074e7f6ff9a09..7a635d70b2e91bd11c9b80809a0c30df1ee18efb 100644
--- a/include/opendrive_road_segment.h
+++ b/include/opendrive_road_segment.h
@@ -63,17 +63,15 @@ class COpendriveRoadSegment
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);
- int get_node_side(COpendriveRoadNode *node);
+ bool has_node(COpendriveRoadNode *node) const;
void link_neightbour_lanes(lanes::laneSection_type &lane_section);
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,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL);
- COpendriveRoadSegment *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref);
- COpendriveRoadSegment *get_next_segment(int &side);
+ COpendriveRoadSegment *get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL) const;
+ COpendriveRoadSegment *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref) const;
public:
std::string get_name(void) const;
unsigned int get_id(void) const;
@@ -89,16 +87,19 @@ 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);
- TOpendriveWorldPose get_pose_at(double length);
- double get_curvature_at(double length);
+ double get_length(void) const;
+ TOpendriveWorldPose get_pose_at(double length) const;
+ double get_curvature_at(double length) const;
TOpendriveLocalPose transform_to_local_pose(const TOpendriveTrackPose &track);
TOpendriveWorldPose transform_to_world_pose(const TOpendriveTrackPose &track);
const COpendriveRoadNode &get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
- const COpendriveLane &get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1);
+ const COpendriveLane &get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
int get_closest_lane_id(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
+ const COpendriveRoadSegment &get_next_segment(int &side,bool &valid) const;
+ const COpendriveRoadSegment &get_prev_segment(int &side,bool &valid) const;
+ int get_pose_side(TOpendriveWorldPose &pose) const;
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment);
~COpendriveRoadSegment();
};
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index c48d4765bccadfad4a915da4f8b0d61d767a144b..41e6f6b312e67834399f50da5efe05827ea754f4 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -521,7 +521,7 @@ void COpendriveRoadSegment::remove_node(COpendriveRoadNode *node)
}
}
-bool COpendriveRoadSegment::has_node(COpendriveRoadNode *node)
+bool COpendriveRoadSegment::has_node(COpendriveRoadNode *node) const
{
for(unsigned int i=0;i<this->nodes.size();i++)
if(this->nodes[i]==node)
@@ -530,25 +530,20 @@ bool COpendriveRoadSegment::has_node(COpendriveRoadNode *node)
return false;
}
-int COpendriveRoadSegment::get_node_side(COpendriveRoadNode *node)
+int COpendriveRoadSegment::get_pose_side(TOpendriveWorldPose &pose) const
{
- std::map<int,COpendriveLane *>::iterator it;
+ const COpendriveRoadNode *node;
+ std::map<int,COpendriveLane *>::const_iterator it;
std::stringstream error;
+ double distance;
- if(this->has_node(node))
- {
- for(it=this->lanes.begin();it!=this->lanes.end();it++)
- if(it->second->has_node(node))
- return it->first;
+ node=&this->get_closest_node(pose,distance,3.14159);
+ for(it=this->lanes.begin();it!=this->lanes.end();it++)
+ if(it->second->has_node((COpendriveRoadNode *)node))
+ return it->first;
- error << "Segment " << this->name << " does not include node " << node->get_index();
- throw CException(_HERE_,error.str());
- }
- else
- {
- error << "Segment " << this->name << " does not include node " << node->get_index();
- throw CException(_HERE_,error.str());
- }
+ error << "Segment " << this->name << " does not include node " << node->get_index();
+ throw CException(_HERE_,error.str());
}
void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_section)
@@ -690,7 +685,7 @@ bool COpendriveRoadSegment::connects_segments(COpendriveRoadSegment *segment1,CO
return false;
}
-COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start,TOpendriveWorldPose *end)
+COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start,TOpendriveWorldPose *end) const
{
TOpendriveWorldPose *start_pose,*end_pose;
std::map<int,COpendriveLane *>::iterator lane_it;
@@ -708,7 +703,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
if(start==NULL && end==NULL)
return this->clone(new_node_ref,new_lane_ref,new_link_ref);
new_segment=new COpendriveRoadSegment(*this);
- new_segment_ref[this]=new_segment;
+ new_segment_ref[(COpendriveRoadSegment *)this]=new_segment;
if(node_side<0)
{
start_pose=start;
@@ -770,7 +765,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
return new_segment;
}
-COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref)
+COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref) const
{
std::map<int,COpendriveLane *>::iterator lane_it;
lane_up_ref_t::iterator lane_ref_it;
@@ -781,7 +776,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,
node_up_ref_t::iterator node_ref_it;
new_segment=new COpendriveRoadSegment(*this);
- new_segment_ref[this]=new_segment;
+ new_segment_ref[(COpendriveRoadSegment *)this]=new_segment;
/* get the sublanes */
for(lane_it=new_segment->lanes.begin();lane_it!=new_segment->lanes.end();lane_it++)
{
@@ -793,7 +788,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,
return new_segment;
}
-COpendriveRoadSegment *COpendriveRoadSegment::get_next_segment(int &side)
+const COpendriveRoadSegment &COpendriveRoadSegment::get_next_segment(int &side,bool &valid) const
{
std::vector<COpendriveRoadSegment *> segment_candidates;
std::vector<int> side_candidates;
@@ -848,16 +843,90 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_next_segment(int &side)
}
}
if(segment_candidates.size()==0)
- return NULL;
+ {
+ valid=false;
+ return *this;
+ }
+ else if(segment_candidates.size()>1)
+ throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
+ else
+ {
+ valid=true;
+ side=side_candidates[0];
+ return *segment_candidates[0];
+ }
+}
+
+const COpendriveRoadSegment &COpendriveRoadSegment::get_prev_segment(int &side,bool &valid) const
+{
+ std::vector<COpendriveRoadSegment *> segment_candidates;
+ std::vector<int> side_candidates;
+ bool already_present;
+
+ if(side<0)
+ {
+ for(unsigned int i=1;i<=this->get_num_right_lanes();i++)
+ {
+ const COpendriveLane &lane=this->get_lane(-i);
+ if(lane.get_num_prev_lanes()>1)
+ throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
+ if(lane.get_num_prev_lanes()==1)
+ {
+ already_present=false;
+ for(unsigned int k=0;k<segment_candidates.size();k++)
+ if(segment_candidates[k]==lane.get_prev_lane(0).segment)
+ {
+ already_present=true;
+ break;
+ }
+ if(!already_present)
+ {
+ segment_candidates.push_back(lane.get_prev_lane(0).segment);
+ side_candidates.push_back(lane.get_prev_lane(0).get_id());
+ }
+ }
+ }
+ }
+ else
+ {
+ for(unsigned int i=1;i<=this->get_num_left_lanes();i++)
+ {
+ const COpendriveLane &lane=this->get_lane(i);
+ if(lane.get_num_prev_lanes()>1)
+ throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
+ if(lane.get_num_prev_lanes()==1)
+ {
+ already_present=false;
+ for(unsigned int k=0;k<segment_candidates.size();k++)
+ if(segment_candidates[k]==lane.get_prev_lane(0).segment)
+ {
+ already_present=true;
+ break;
+ }
+ if(!already_present)
+ {
+ segment_candidates.push_back(lane.get_prev_lane(0).segment);
+ side_candidates.push_back(lane.get_prev_lane(0).get_id());
+ }
+ }
+ }
+ }
+ if(segment_candidates.size()==0)
+ {
+ valid=false;
+ return *this;
+ }
else if(segment_candidates.size()>1)
throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
else
{
+ valid=true;
side=side_candidates[0];
- return segment_candidates[0];
+ return *segment_candidates[0];
}
}
+
void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
{
std::map<int,COpendriveLane *>::const_iterator lane_it;
@@ -1034,17 +1103,17 @@ const COpendriveRoadSegment &COpendriveRoadSegment::get_connecting(unsigned int
}
}
-double COpendriveRoadSegment::get_length(void)
+double COpendriveRoadSegment::get_length(void) const
{
double length=0.0;
for(unsigned int i=0;i<this->geometries.size();i++)
- length+=this->geometries[i].opendrive->get_length();
+ length+=this->geometries[i].spline->get_length();
return length;
}
-TOpendriveWorldPose COpendriveRoadSegment::get_pose_at(double length)
+TOpendriveWorldPose COpendriveRoadSegment::get_pose_at(double length) const
{
TOpendriveWorldPose world_pose{0};
TPoint spline_pose;
@@ -1066,7 +1135,7 @@ TOpendriveWorldPose COpendriveRoadSegment::get_pose_at(double length)
return world_pose;
}
-double COpendriveRoadSegment::get_curvature_at(double length)
+double COpendriveRoadSegment::get_curvature_at(double length) const
{
double curvature;
TPoint spline_pose;
@@ -1207,12 +1276,18 @@ int COpendriveRoadSegment::get_closest_lane_id(TOpendriveWorldPose &pose,double
return closest_id;
}
-const COpendriveLane &COpendriveRoadSegment::get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol)
+const COpendriveLane &COpendriveRoadSegment::get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
+ std::map<int,COpendriveLane *>::const_iterator lane_it;
+
int closest_id=this->get_closest_lane_id(pose,distance,angle_tol);
if(closest_id==0)
throw CException(_HERE_,"Impossible to find the closest lane");
- return *this->lanes[closest_id];
+ for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
+ if(lane_it->first==closest_id)
+ return *lane_it->second;
+
+ throw CException(_HERE_,"Impossible to find the closest lane");
}
double COpendriveRoadSegment::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
@@ -1246,7 +1321,6 @@ double COpendriveRoadSegment::get_closest_pose(TOpendriveWorldPose &pose,TOpendr
{
for(unsigned int i=0;i<closest_index;i++)
distance+=this->geometries[i].spline->get_length();
- distance+=min_dist;
}
else
distance=std::numeric_limits<double>::max();