Added a function to get the previous segment.

Made some functions const.

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();
 };

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();