Added functions to get a part of the road.

Added functions to update all the references when a new road is created.

include/opendrive_road.h

@@ -5,28 +5,37 @@
 #include "xml/OpenDRIVE_1.4H.hxx"
 #endif
 
+#include "opendrive_common.h"
 #include "opendrive_road_segment.h"
 #include "opendrive_road_node.h"
-
-class COpendriveRoadSegment;
-class COpendriveRoadNode;
+#include "opendrive_lane.h"
 
 class COpendriveRoad
 {
   friend class COpendriveRoadSegment;
   friend class COpendriveRoadNode;
+  friend class COpendriveLane;
   private:
     std::vector<COpendriveRoadSegment *> segments;
     std::vector<COpendriveRoadNode *> nodes;
+    std::vector<COpendriveLane *> lanes;
     double resolution;
     double scale_factor;
     double min_road_length;
   protected:
     COpendriveRoadSegment &operator[](std::string &key);
+    void free(void);
     void link_segments(OpenDRIVE &open_drive);
     unsigned int add_node(COpendriveRoadNode *node);
-    bool node_exists_at(const TOpendriveWorldPoint &pose);
-    COpendriveRoadNode* get_node_at(const TOpendriveWorldPoint &pose) const;
+    void remove_node(COpendriveRoadNode *node);
+    unsigned int add_lane(COpendriveLane *lane);
+    void remove_lane(COpendriveLane *lane);
+    void add_segment(COpendriveRoadSegment *segment,std::vector<unsigned int> &old_path,std::vector<unsigned int> &new_path);
+    void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
+    void clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
+    void prune(std::vector<unsigned int> &path_nodes);
+    bool node_exists_at(TOpendriveWorldPoint &pose);
+    COpendriveRoadNode* get_node_at(TOpendriveWorldPoint &pose);
   public:
     COpendriveRoad();
     COpendriveRoad(const COpendriveRoad& object);
@@ -42,9 +51,11 @@ class COpendriveRoad
     unsigned int get_num_nodes(void) const;
     const COpendriveRoadNode& get_node(unsigned int index) const;
     const COpendriveRoadSegment& operator[](std::size_t index);
-    unsigned int get_closest_node(double x, double y,double heading,double angle_tol=0.1);
-    double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1);
-    void get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1);
+    unsigned int get_closest_node(TOpendriveWorldPoint &point,double angle_tol=0.1);
+    double get_closest_point(TOpendriveWorldPoint &point,TOpendriveWorldPoint &closest_point,double angle_tol=0.1);
+    void get_closest_points(TOpendriveWorldPoint &point,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1);
+    void get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPoint &start_point,TOpendriveWorldPoint &end_point,COpendriveRoad &new_road);
+    void get_sub_road(TOpendriveWorldPoint &start_point,double length,COpendriveRoad &new_road);
     void operator=(const COpendriveRoad& object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
     ~COpendriveRoad();

src/opendrive_road.cpp

@@ -11,41 +11,41 @@ COpendriveRoad::COpendriveRoad()
   this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
   this->segments.clear();
   this->nodes.clear();
+  this->lanes.clear();
 }
 
 COpendriveRoad::COpendriveRoad(const COpendriveRoad& object)
 {
   COpendriveRoadSegment *segment;
   COpendriveRoadNode *node;
-  std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> new_segment_ref;
-  std::map<COpendriveRoadNode *,COpendriveRoadNode *> new_node_ref;
+  COpendriveLane *lane;
+  segment_up_ref_t new_segment_ref;
+  node_up_ref_t new_node_ref;
+  lane_up_ref_t new_lane_ref;
 
+  this->free();
   this->resolution=object.resolution;
   this->scale_factor=object.scale_factor;
   this->min_road_length=object.min_road_length;
-  this->segments.clear();
+  for(unsigned int i=0;i<object.lanes.size();i++)
+  {
+    lane=new COpendriveLane(*object.lanes[i]);
+    this->lanes.push_back(lane);
+    new_lane_ref[object.lanes[i]]=lane;
+  }
   for(unsigned int i=0;i<object.nodes.size();i++)
   {
     node=new COpendriveRoadNode(*object.nodes[i]);
     this->nodes.push_back(node);
     new_node_ref[object.nodes[i]]=node;
   }
-  // update references
-  for(unsigned int i=0;i<this->nodes.size();i++)
-    this->nodes[i]->update_references(new_node_ref);
-
   for(unsigned int i=0;i<object.segments.size();i++)
   {
-    segment=new COpendriveRoadSegment(*object.segments[i],new_node_ref,this);
+    segment=new COpendriveRoadSegment(*object.segments[i]);
     this->segments.push_back(segment);
     new_segment_ref[object.segments[i]]=segment;
   }
-  this->nodes.clear();
   // update references
-  for(unsigned int i=0;i<this->segments.size();i++)
-    this->segments[i]->update_references(new_segment_ref);
-  for(unsigned int i=0;i<this->nodes.size();i++)
-    this->nodes[i]->update_references(new_segment_ref);
 }
 
 COpendriveRoadSegment &COpendriveRoad::operator[](std::string &key)
@@ -59,6 +59,28 @@ COpendriveRoadSegment &COpendriveRoad::operator[](std::string &key)
   throw CException(_HERE_,"No road segment with the given ID");
 }
 
+void COpendriveRoad::free(void)
+{
+  for(unsigned int i=0;i<this->segments.size();i++)
+  {
+    delete this->segments[i];
+    this->segments[i]=NULL;
+  }
+  this->segments.clear();
+  for(unsigned int i=0;i<this->nodes.size();i++)
+  {
+    delete this->nodes[i];
+    this->nodes[i]=NULL;
+  }
+  this->nodes.clear();
+  for(unsigned int i=0;i<this->lanes.size();i++)
+  {
+    delete this->lanes[i];
+    this->lanes[i]=NULL;
+  }
+  this->lanes.clear();
+}
+
 void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
 {
   std::string predecessor_id,successor_id;
@@ -172,7 +194,237 @@ unsigned int COpendriveRoad::add_node(COpendriveRoadNode *node)
   return this->nodes.size()-1;
 }
 
-bool COpendriveRoad::node_exists_at(const TOpendriveWorldPoint &pose)
+void COpendriveRoad::remove_node(COpendriveRoadNode *node)
+{
+  std::vector<COpendriveRoadNode *>::iterator it;
+
+  for(it=this->nodes.begin();it!=this->nodes.end();)
+  {
+    if((*it)->get_index()==node->get_index())
+    {
+      delete *it;
+      it=this->nodes.erase(it);
+      break;
+    }
+    else
+      it++;
+  }
+}
+
+unsigned int COpendriveRoad::add_lane(COpendriveLane *lane)
+{
+  for(unsigned int i=0;i<this->lanes.size();i++)
+  {
+    if(this->lanes[i]==lane)
+      throw CException(_HERE_,"Lane already present");
+  }
+  this->lanes.push_back(lane);
+
+  return this->lanes.size()-1;
+}
+
+void COpendriveRoad::remove_lane(COpendriveLane *lane)
+{
+  std::vector<COpendriveLane *>::iterator it;
+
+  for(it=this->lanes.begin();it!=this->lanes.end();)
+  {
+    if((*it)->get_index()==lane->get_index())
+    {
+      delete *it;
+      it=this->lanes.erase(it);
+      break;
+    }
+    else
+      it++;
+  }
+}
+
+void COpendriveRoad::add_segment(COpendriveRoadSegment *segment,std::vector<unsigned int> &old_path,std::vector<unsigned int> &new_path)
+{
+  for(unsigned int i=0;i<this->segments.size();i++)
+    if(this->segments[i]->get_id()==segment->get_id())// segment is already present
+      return;
+  // add the new segment
+  this->segments.push_back(segment);
+  // add the lanes
+  for(unsigned int i=1;i<=segment->get_num_right_lanes();i++)
+  {
+    segment->lanes[-i]->set_index(this->lanes.size());
+    this->lanes.push_back(segment->lanes[-i]);
+  }
+  for(unsigned int i=1;i<=segment->get_num_left_lanes();i++)
+  {
+    segment->lanes[i]->set_index(this->lanes.size());
+    this->lanes.push_back(segment->lanes[i]);
+  }
+  // add the nodes
+  for(unsigned int i=0;i<segment->get_num_nodes();i++)
+  {
+    for(unsigned int j=0;j<old_path.size();j++)
+      if(old_path[j]==segment->nodes[i]->index)
+        new_path.push_back(this->nodes.size());
+    segment->nodes[i]->set_index(this->nodes.size());
+    this->nodes.push_back(segment->nodes[i]);
+  }
+}
+
+void COpendriveRoad::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
+{
+  std::vector<COpendriveRoadSegment *>::iterator seg_it;
+  std::vector<COpendriveLane *>::iterator lane_it;
+  std::vector<COpendriveRoadNode *>::iterator node_it;
+
+  for(seg_it=this->segments.begin();seg_it!=this->segments.end();seg_it++)
+  {
+    if(segment_refs.find(*seg_it)!=segment_refs.end())
+    {
+      (*seg_it)=segment_refs[*seg_it];
+      (*seg_it)->update_references(segment_refs,lane_refs,node_refs);
+    }
+    else if((*seg_it)->updated(segment_refs))
+      (*seg_it)->update_references(segment_refs,lane_refs,node_refs);
+  }
+  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
+    if(lane_refs.find(*lane_it)!=lane_refs.end())
+      (*lane_it)=lane_refs[*lane_it];
+  for(node_it=this->nodes.begin();node_it!=this->nodes.end();node_it++)
+    if(node_refs.find(*node_it)!=node_refs.end())
+      (*node_it)=node_refs[*node_it];
+}
+
+void COpendriveRoad::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
+{
+  std::vector<COpendriveRoadSegment *>::iterator seg_it;
+  std::vector<COpendriveLane *>::iterator lane_it;
+  std::vector<COpendriveRoadNode *>::iterator node_it;
+
+  for(seg_it=this->segments.begin();seg_it!=this->segments.end();)
+  {
+    if(segment_refs.find(*seg_it)!=segment_refs.end())
+    {
+      (*seg_it)=segment_refs[*seg_it];
+      (*seg_it)->clean_references(segment_refs,lane_refs,node_refs);
+      seg_it++;
+    }
+    else if((*seg_it)->updated(segment_refs))
+    {
+      (*seg_it)->clean_references(segment_refs,lane_refs,node_refs);
+      seg_it++;
+    }
+    else
+      seg_it=this->segments.erase(seg_it);
+  }
+  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
+  {
+    if(lane_refs.find(*lane_it)!=lane_refs.end())
+    {
+      (*lane_it)=lane_refs[*lane_it];
+      lane_it++;
+    }
+    else if(!(*lane_it)->updated(lane_refs))
+      lane_it=this->lanes.erase(lane_it);
+    else
+      lane_it++;
+  }
+  for(node_it=this->nodes.begin();node_it!=this->nodes.end();)
+  {
+    if(node_refs.find(*node_it)!=node_refs.end())
+    {
+      (*node_it)=node_refs[*node_it];
+      node_it++;
+    }
+    else if(!(*node_it)->updated(node_refs))
+      node_it=this->nodes.erase(node_it);
+    else
+      node_it++;
+  }
+}
+
+void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
+{
+  COpendriveLane *neightbour_lane;
+  std::vector<COpendriveLane *>::iterator lane_it;
+  bool present;
+
+  // remove all nodes and lanes not present in the path
+  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
+  {
+    present=false;
+    for(unsigned int i=0;i<path_nodes.size();i++)
+    {
+      if((*lane_it)->has_node(path_nodes[i]))
+      {
+        present=true;
+        break;
+      }
+    } 
+    if(!present)
+    {
+      neightbour_lane=(*lane_it)->left_lane;
+      while(neightbour_lane!=NULL)
+      {
+        for(unsigned int i=0;i<path_nodes.size();i++)
+        {
+          if(neightbour_lane->has_node(path_nodes[i]))
+          {
+            present=true;
+            break;
+          }
+        }
+        if(present)
+          break;
+        neightbour_lane=neightbour_lane->left_lane;
+      }
+      neightbour_lane=(*lane_it)->right_lane;
+      while(neightbour_lane!=NULL)
+      {
+        for(unsigned int i=0;i<path_nodes.size();i++)
+        {
+          if(neightbour_lane->has_node(path_nodes[i]))
+          {
+            present=true;
+            break;
+          }
+        }
+        if(present)
+          break;
+        neightbour_lane=neightbour_lane->right_lane;
+      }
+      if(!present)
+      {
+        (*lane_it)->segment->remove_lane(*lane_it);
+        for(unsigned int i=0;i<(*lane_it)->nodes.size();i++)
+          this->remove_node((*lane_it)->nodes[i]);
+        delete *lane_it;
+        lane_it=this->lanes.erase(lane_it);
+      }
+      else
+        lane_it++;
+    }
+    else
+      lane_it++;
+  }
+
+  // remove links to non-consecutive nodes
+  for(unsigned int i=0;i<this->nodes.size();i++)
+  {
+    for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
+    {
+      for(unsigned int k=0;k<path_nodes.size()-1;k++)
+      {
+        if(path_nodes[k]==this->nodes[i]->links[j]->prev->index)
+          if(path_nodes[k+1]!=this->nodes[i]->links[j]->next->index)
+          {
+            this->nodes[i]->remove_link(this->nodes[i]->links[j]);
+            break;
+          }
+      }
+    }
+  } 
+}
+
+bool COpendriveRoad::node_exists_at(TOpendriveWorldPoint &pose)
 {
   TOpendriveWorldPoint node_pose;
 
@@ -186,7 +438,7 @@ bool COpendriveRoad::node_exists_at(const TOpendriveWorldPoint &pose)
   return false;
 }
 
-COpendriveRoadNode* COpendriveRoad::get_node_at(const TOpendriveWorldPoint &pose) const
+COpendriveRoadNode* COpendriveRoad::get_node_at(TOpendriveWorldPoint &pose)
 {
   TOpendriveWorldPoint node_pose;
 
@@ -200,6 +452,7 @@ COpendriveRoadNode* COpendriveRoad::get_node_at(const TOpendriveWorldPoint &pose
   return NULL;
 }
 
+
 void COpendriveRoad::load(const std::string &filename)
 {
   struct stat buffer;
@@ -207,9 +460,7 @@ void COpendriveRoad::load(const std::string &filename)
   if(stat(filename.c_str(),&buffer)==0)
   {
     // delete any previous data
-    for(unsigned int i=0;i<this->segments.size();i++)
-      delete this->segments[i];
-    this->segments.clear();
+    this->free();
     // try to open the specified file
     try{
       std::unique_ptr<OpenDRIVE> open_drive(OpenDRIVE_(filename.c_str(), xml_schema::flags::dont_validate));
@@ -317,16 +568,16 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
 }
 
 
-unsigned int COpendriveRoad::get_closest_node(double x, double y,double heading,double angle_tol)
+unsigned int COpendriveRoad::get_closest_node(TOpendriveWorldPoint &point,double angle_tol)
 {
   double dist,min_dist=std::numeric_limits<double>::max();
-  TOpendriveWorldPoint point;
+  TOpendriveWorldPoint closest_point;
   unsigned int closest_index;
 
   for(unsigned int i=0;i<this->nodes.size();i++)
   { 
-    this->nodes[i]->get_closest_point(x,y,heading,point,angle_tol);
-    dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
+    this->nodes[i]->get_closest_point(point,closest_point,angle_tol);
+    dist=sqrt(pow(closest_point.x-point.x,2.0)+pow(closest_point.y-point.y,2.0));
     if(dist<min_dist)
     { 
       min_dist=dist;
@@ -337,20 +588,20 @@ unsigned int COpendriveRoad::get_closest_node(double x, double y,double heading,
   return closest_index;
 }
 
-double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol)
+double COpendriveRoad::get_closest_point(TOpendriveWorldPoint &point,TOpendriveWorldPoint &closest_point,double angle_tol)
 {
   double dist,min_dist=std::numeric_limits<double>::max();
-  TOpendriveWorldPoint point;
+  TOpendriveWorldPoint point_found;
   double length,closest_length;
 
   for(unsigned int i=0;i<this->nodes.size();i++)
   {
-    length=this->nodes[i]->get_closest_point(x,y,heading,point,angle_tol);
-    dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
+    length=this->nodes[i]->get_closest_point(point,point_found,angle_tol);
+    dist=sqrt(pow(point_found.x-point.x,2.0)+pow(point_found.y-point.y,2.0));
     if(dist<min_dist)
     {
       min_dist=dist;
-      closest_point=point;
+      closest_point=point_found;
       closest_length=length;
     }
   }
@@ -358,7 +609,7 @@ double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpen
   return closest_length;
 }
 
-void COpendriveRoad::get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol)
+void COpendriveRoad::get_closest_points(TOpendriveWorldPoint &point,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol)
 {
   std::vector<TOpendriveWorldPoint> points;
   std::vector<double> lengths;
@@ -367,7 +618,7 @@ void COpendriveRoad::get_closest_points(double x, double y,double heading,std::v
   closest_lengths.clear();
   for(unsigned int i=0;i<this->nodes.size();i++)
   {
-    this->nodes[i]->get_closest_points(x,y,heading,points,lengths,dist_tol,angle_tol);
+    this->nodes[i]->get_closest_points(point,points,lengths,dist_tol,angle_tol);
     for(unsigned int j=0;j<points.size();i++)
     {
       closest_points.push_back(points[i]);
@@ -376,40 +627,132 @@ void COpendriveRoad::get_closest_points(double x, double y,double heading,std::v
   }
 }
 
+void COpendriveRoad::get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPoint &start_point,TOpendriveWorldPoint &end_point,COpendriveRoad &new_road)
+{
+  segment_up_ref_t new_segment_ref;
+  lane_up_ref_t new_lane_ref;
+  node_up_ref_t new_node_ref;
+  COpendriveRoadNode *node,*next_node;
+  COpendriveRoadSegment *segment,*new_segment;
+  std::vector<unsigned int> new_path_nodes;
+  unsigned int link_index;
+  int node_side;
+  bool added;
+
+  new_road.free();
+  new_road.set_resolution(this->resolution);
+  new_road.set_scale_factor(this->scale_factor);
+  new_road.set_min_road_length(this->min_road_length);
+
+  if(path_nodes.size()==1)
+  {
+    node=this->nodes[path_nodes[path_nodes.size()-1]];
+    link_index=node->get_closest_link(end_point);
+    segment=node->links[link_index]->segment;
+    node_side=segment->get_node_side(node);
+    new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,node_side,&start_point,&end_point);
+    new_road.add_segment(new_segment,path_nodes,new_path_nodes);
+    new_segment_ref[segment]=new_segment;
+  }
+  else
+  {
+    for(unsigned int i=0;i<path_nodes.size()-1;i++)
+    {
+      node=this->nodes[path_nodes[i]];
+      next_node=this->nodes[path_nodes[i+1]];
+      segment=node->get_link_with(next_node)->segment;
+      if(segment->has_node(next_node))
+        continue;
+      else
+      {
+        if(i==0)
+        {
+          node_side=segment->get_node_side(node);
+          new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,node_side,&start_point,NULL);
+        }
+        else
+          new_segment=segment->clone(new_node_ref,new_lane_ref);
+        new_road.add_segment(new_segment,path_nodes,new_path_nodes);
+        new_segment_ref[segment]=new_segment;
+      }
+    }
+    // add the last segment
+    node=this->nodes[path_nodes[path_nodes.size()-1]];
+    link_index=node->get_closest_link(end_point,3.14159);
+    segment=node->links[link_index]->segment;
+    node_side=segment->get_node_side(node);
+    new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,node_side,NULL,&end_point);
+    new_road.add_segment(new_segment,path_nodes,new_path_nodes);
+    new_segment_ref[segment]=new_segment;
+  }
+  new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);
+  // add additional nodes not explicitly in the path
+/*
+  for(unsigned int i=0;i<path_nodes.size();i++)
+  {
+    added=false;
+    node=this->nodes[path_nodes[i]];
+    for(unsigned int j=i+1;j<path_nodes.size();j++)
+    {
+      next_node=this->nodes[path_nodes[j]];
+      for(unsigned int k=0;k<this->segments.size();k++)
+        if(this->segments[k]->connects_nodes(node,next_node))
+        {
+          new_segment=this->segments[k]->clone(new_node_ref,new_lane_ref);
+          new_road.add_segment(new_segment,path_nodes,new_path_nodes);
+          new_segment_ref[segment]=new_segment;
+          added=true;
+          break;
+        }
+      if(added)
+        break;
+    }
+  }
+*/
+  // remove unconnected elements
+  new_road.prune(new_path_nodes);
+  new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref);
+}
+
+void COpendriveRoad::get_sub_road(TOpendriveWorldPoint &start_point,double length,COpendriveRoad &road)
+{
+
+}
+
 void COpendriveRoad::operator=(const COpendriveRoad& object)
 {
   COpendriveRoadSegment *segment;
   COpendriveRoadNode *node;
-  std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> new_references;
+  COpendriveLane *lane;
+  segment_up_ref_t new_segment_ref;
+  node_up_ref_t new_node_ref;
+  lane_up_ref_t new_lane_ref;
 
+  this->free();
   this->resolution=object.resolution;
   this->scale_factor=object.scale_factor;
   this->min_road_length=object.min_road_length;
-  for(unsigned int i=0;i<this->segments.size();i++)
+  for(unsigned int i=0;i<object.lanes.size();i++)
   {
-    delete this->segments[i];
-    this->segments[i]=NULL;
+    lane=new COpendriveLane(*object.lanes[i]);
+    this->lanes.push_back(lane);
+    new_lane_ref[object.lanes[i]]=lane;
   }
-  this->segments.clear();
-  for(unsigned int i=0;i<object.segments.size();i++)
-  {
-    segment=new COpendriveRoadSegment(*object.segments[i]);
-    this->segments.push_back(segment);
-    new_references[object.segments[i]]=segment;
-  }