Added functions to get the start and end position of a lane.

Added a function to return the length of a lane.
Solved a bug with the names of the road marks.
Solved a bug when kinking two nodes in the same lane.

include/opendrive_lane.h

@@ -45,6 +45,9 @@ class COpendriveLane
     double get_speed(void) const;
     double get_offset(void) const;
     int get_id(void) const;
+    TOpendriveWorldPoint get_start_point(void);
+    TOpendriveWorldPoint get_end_point(void);
+    double get_length(void);
     void operator=(const COpendriveLane &object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
     ~COpendriveLane();

src/opendrive_lane.cpp

@@ -94,15 +94,10 @@ void COpendriveLane::add_node(COpendriveRoadNode *node)
     if(this->nodes[i]==node)
       return;
   // add the new node
-  for(unsigned int i=0;i<node->get_num_lanes();i++)
-  {
-    if(&node->get_lane(i)==this)
-      node->set_index(i,this->nodes.size());
-  }
   this->nodes.push_back(node);
   // link the new node with the previous one in the current lane, if any
   if(this->nodes.size()>1)
-    this->nodes[this->nodes.size()-1]->add_link(node,OD_MARK_NONE);
+    this->nodes[this->nodes.size()-2]->add_link(node,OD_MARK_NONE);
 }
 
 void COpendriveLane::set_resolution(double res)
@@ -179,13 +174,13 @@ void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegmen
         mark=OD_MARK_SOLID;
       else if(lane_info.roadMark().begin()->type1().get()=="broken")
         mark=OD_MARK_BROKEN;
-      else if(lane_info.roadMark().begin()->type1().get()=="solid_solid")
+      else if(lane_info.roadMark().begin()->type1().get()=="solid solid")
         mark=OD_MARK_SOLID_SOLID;
-      else if(lane_info.roadMark().begin()->type1().get()=="solid_broken")
+      else if(lane_info.roadMark().begin()->type1().get()=="solid broken")
         mark=OD_MARK_SOLID_BROKEN;
-      else if(lane_info.roadMark().begin()->type1().get()=="broken_solid")
+      else if(lane_info.roadMark().begin()->type1().get()=="broken solid")
         mark=OD_MARK_BROKEN_SOLID;
-      else if(lane_info.roadMark().begin()->type1().get()=="broken_broken")
+      else if(lane_info.roadMark().begin()->type1().get()=="broken broken")
         mark=OD_MARK_BROKEN_BROKEN;
       else
         mark=OD_MARK_NONE;
@@ -239,6 +234,82 @@ int COpendriveLane::get_id(void) const
   return this->id;
 }
 
+TOpendriveWorldPoint COpendriveLane::get_start_point(void)
+{
+  TOpendriveTrackPoint track_point;
+  TOpendriveWorldPoint world_point;
+
+  track_point.heading=0.0;
+  if(this->id<0)// right lane
+  {
+    track_point.t=this->get_offset()-this->get_width()/2.0;
+    track_point.s=0.0;
+    for(unsigned int i=0;i<this->nodes[0]->get_num_lanes();i++)
+      if(this->nodes[0]->get_lane(i).get_id()==this->id)
+        this->nodes[0]->get_geometry(i).get_world_pose(track_point,world_point);
+  }
+  else
+  {
+    track_point.t=this->get_offset()+this->get_width()/2.0;
+    for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
+      if(this->nodes[this->nodes.size()-1]->get_lane(i).get_id()==this->id)
+      {
+        track_point.s=this->nodes[this->nodes.size()-1]->get_geometry(i).get_length();
+        this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);
+      }
+  }
+  if(this->get_id()>0)
+    world_point.heading=normalize_angle(world_point.heading+3.14159);
+
+  return world_point;
+}
+
+TOpendriveWorldPoint COpendriveLane::get_end_point(void)
+{
+  TOpendriveTrackPoint track_point;
+  TOpendriveWorldPoint world_point;
+  
+  track_point.heading=0.0;
+  if(this->id>0)// left lane
+  { 
+    track_point.t=this->get_offset()-this->get_width()/2.0;
+    track_point.s=0.0;
+    for(unsigned int i=0;i<this->nodes[0]->get_num_lanes();i++)
+      if(this->nodes[0]->get_lane(i).get_id()==this->id)
+        this->nodes[0]->get_geometry(i).get_world_pose(track_point,world_point);
+  }
+  else
+  { 
+    track_point.t=this->get_offset()+this->get_width()/2.0;
+    for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
+      if(this->nodes[this->nodes.size()-1]->get_lane(i).get_id()==this->id)
+      {
+        track_point.s=this->nodes[this->nodes.size()-1]->get_geometry(i).get_length();
+        this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);
+      }
+  }
+  if(this->get_id()>0)
+    world_point.heading=normalize_angle(world_point.heading+3.14159);
+
+  return world_point;
+}
+
+double COpendriveLane::get_length(void)
+{
+  double length=0.0;
+
+  for(unsigned int i=0;i<this->nodes.size();i++)
+  {
+    for(unsigned int j=0;j<this->nodes[i]->get_num_lanes();j++)
+    {
+      if(this->nodes[i]->get_lane(j).get_id()==this->id)
+        length+=this->nodes[i]->get_geometry(j).get_length();
+    }
+  }
+
+  return length;
+}
+
 void COpendriveLane::operator=(const COpendriveLane &object)
 {
   COpendriveRoadNode *node;
@@ -325,10 +396,7 @@ std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
   }
   out << "    Number of nodes: " << lane.nodes.size() << std::endl;
   for(unsigned int i=0;i<lane.nodes.size();i++)
-  {
-    out << "    Node " << i << ":" << std::endl;
     out << *lane.nodes[i];
-  }
 
   return out;
 }