opendrive_road.cpp

#include "opendrive_road.h"
#include "exceptions.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <math.h>

COpendriveRoad::COpendriveRoad()
{
  this->resolution=DEFAULT_RESOLUTION;
  this->scale_factor=DEFAULT_SCALE_FACTOR;
  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;
  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<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;
  }
  for(unsigned int i=0;i<object.segments.size();i++)
  {
    segment=new COpendriveRoadSegment(*object.segments[i]);
    this->segments.push_back(segment);
    new_segment_ref[object.segments[i]]=segment;
  }
  // update references
  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
}

COpendriveRoadSegment *COpendriveRoad::get_segment_by_id(std::string &id)
{
  std::stringstream error;

  for(unsigned int i=0;i<this->segments.size();i++)
  {
    if(this->segments[i]->get_id()==(unsigned int)std::stoi(id))
      return this->segments[i];
  }

  error << "No road segment with the " << id << " ID";
  throw CException(_HERE_,error.str());
}

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;
  std::string predecessor_contact,successor_contact;
  COpendriveRoadSegment *prev_road,*road,*next_road;
  std::stringstream error;

  for(OpenDRIVE::road_iterator road_it(open_drive.road().begin());road_it!=open_drive.road().end();++road_it)
  {
    // get current segment
    road=this->get_segment_by_id(road_it->id().get());
    // get predecessor and successor
    if(road_it->lane_link().present())
    {
      if(road_it->lane_link().get().predecessor().present())// predecessor present
      {
        if(road_it->lane_link().get().predecessor().get().elementType().get()=="road")// previous segment is a road
        {
          predecessor_id=road_it->lane_link().get().predecessor().get().elementId().get();
          predecessor_contact=road_it->lane_link().get().predecessor().get().contactPoint().get();
        }
      }
      if(road_it->lane_link().get().successor().present())// successor present
      {
        if(road_it->lane_link().get().successor().get().elementType().get()=="road")
        {
          successor_id=road_it->lane_link().get().successor().get().elementId().get();
          successor_contact=road_it->lane_link().get().successor().get().contactPoint().get();
        }
      }
    }
    if(std::stoi(road_it->junction().get())==-1)// non junction road segments
    {
      if(!predecessor_id.empty())
      {
        prev_road=this->get_segment_by_id(predecessor_id);
        prev_road->link_segment(road);
        predecessor_id.clear();
      }
      if(!successor_id.empty())
      {
        next_road=this->get_segment_by_id(successor_id);
        road->link_segment(next_road);
        successor_id.clear();
      }
    }
    else// junction segment
    {
      for(OpenDRIVE::junction_iterator junction_it(open_drive.junction().begin());junction_it!=open_drive.junction().end();++junction_it)
      {
        for(junction::connection_iterator connection_it(junction_it->connection().begin()); connection_it!=junction_it->connection().end();++connection_it)
        {
          std::string incoming_road_id;
          std::string connecting_road_id;
          if(connection_it->incomingRoad().present())
            incoming_road_id=connection_it->incomingRoad().get();
          else
          {
            error << "Connectivity information missing for segment " << road->get_name() << ": No incomming segment";
            throw CException(_HERE_,error.str());
          }
          if(connection_it->connectingRoad().present())
            connecting_road_id=connection_it->connectingRoad().get();
          else
          {
            error << "Connectivity information missing for segment " << road->get_name() << ": No connecting segment";
            throw CException(_HERE_,error.str());
          }
          if(predecessor_id.compare(incoming_road_id)==0 && successor_id.compare(connecting_road_id)==0)// this is the connection
          {
            prev_road=this->get_segment_by_id(predecessor_id);
            next_road=this->get_segment_by_id(successor_id);
            for(connection::laneLink_iterator lane_link_it(connection_it->laneLink().begin()); lane_link_it!=connection_it->laneLink().end();++lane_link_it)
            {
              int from_lane_id;
              int to_lane_id;
              if(lane_link_it->from().present())
                from_lane_id=lane_link_it->from().get();
              else
              {
                error << "Connectivity information missing for segment " << road->get_name() << ": lane link missing from identifier";
                throw CException(_HERE_,error.str());
              }
              if(lane_link_it->to().present())
                to_lane_id=lane_link_it->to().get();
              else
              {
                error << "Connectivity information missing for segment " << road->get_name() << ": lane link missing to identifier";
                throw CException(_HERE_,error.str());
              }
              if(predecessor_contact=="end")
                prev_road->link_segment(road,from_lane_id,false,-1,true);
              else 
                prev_road->link_segment(road,from_lane_id,true,-1,true);
              TOpendriveWorldPose end=road->get_lane(-1).get_end_pose();
              TOpendriveWorldPose start;
              if(successor_contact=="end")
              {
                if(to_lane_id<0)
                  start=next_road->get_lane(to_lane_id).get_end_pose();
                else
                  start=next_road->get_lane(to_lane_id).get_start_pose();
                if(fabs(end.x-start.x)<this->resolution && fabs(end.y-start.y)<this->resolution)
                  road->link_segment(next_road,-1,false,to_lane_id,false);
              }
              else
              {
                if(to_lane_id<0)
                  start=next_road->get_lane(to_lane_id).get_start_pose();
                else
                  start=next_road->get_lane(to_lane_id).get_end_pose();
                if(fabs(end.x-start.x)<this->resolution && fabs(end.y-start.y)<this->resolution)
                  road->link_segment(next_road,-1,false,to_lane_id,true);
              }
            }
          }
        }
      }
    }
  }
}

unsigned int COpendriveRoad::add_node(COpendriveRoadNode *node)
{
  std::stringstream error;

  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    if(this->nodes[i]==node)
    {
      error << "Node " << node->get_index() << " already present";
      throw CException(_HERE_,error.str());
    }
  }
  this->nodes.push_back(node);

  return this->nodes.size()-1;
}

bool 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);
      return true;
    }
    else
      it++;
  }

  return false;
}

unsigned int COpendriveRoad::add_lane(COpendriveLane *lane)
{
  std::stringstream error;

  for(unsigned int i=0;i<this->lanes.size();i++)
  {
    if(this->lanes[i]==lane)
    {
      error << "Lane " << lane->get_index() << " already present";
      throw CException(_HERE_,error.str());
    }
  }
  this->lanes.push_back(lane);

  return this->lanes.size()-1;
}

bool 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);
      return true;
    }
    else
      it++;
  }

  return false;
}

void COpendriveRoad::complete_open_lanes(void)
{
  std::vector<COpendriveLane *>::iterator lane_it;
  TOpendriveWorldPose end_pose;

  // remove all nodes and lanes not present in the path
  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
  {
    if((*lane_it)->next.empty())// lane is not connected
    {
      try{
        end_pose=(*lane_it)->get_end_pose();
        if(!this->node_exists_at(end_pose))// there is no node at the end pose
        {
          (*lane_it)->segment->add_empty_node(end_pose,(*lane_it));
          (*lane_it)->segment->link_neightbour_lanes();
        }
        else
          lane_it++;
      }catch(CException &e){
        lane_it++;
      }
    }
    else
      lane_it++;
  }   
}

bool COpendriveRoad::has_segment(COpendriveRoadSegment *segment)
{
  for(unsigned int i=0;i<this->segments.size();i++)
    if(this->segments[i]==segment)
      return true;
  
  return false;
}

void COpendriveRoad::add_segment(COpendriveRoadSegment *segment)
{
  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++)
    this->lanes.push_back(segment->lanes[-i]);
  for(unsigned int i=1;i<=segment->get_num_left_lanes();i++)
    this->lanes.push_back(segment->lanes[i]);
  // add the nodes
  for(unsigned int i=0;i<segment->get_num_nodes();i++)
    this->nodes.push_back(segment->nodes[i]);
  // update the road reference
  segment->set_parent_road(this);
}

std::vector<unsigned int> COpendriveRoad::update_path(node_up_ref_t &node_refs,std::vector<unsigned int> &path)
{
  std::vector<unsigned int> new_path;
  node_up_ref_t::iterator node_it;

  for(unsigned int i=0;i<path.size();i++)
  {
    for(node_it=node_refs.begin();node_it!=node_refs.end();node_it++)
    {
      if(node_it->first->get_index()==path[i])
      {
        new_path.push_back(node_it->second->get_index());
        break;
      }
    }
  }

  return new_path;
}

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((*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_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_it)->updated(node_refs))
      node_it=this->nodes.erase(node_it);
    else
      node_it++;
  }
}

void COpendriveRoad::reindex(void)
{
  // reindex lanes
  for(unsigned int i=0;i<this->get_num_lanes();i++)
    this->lanes[i]->set_index(i);
  // reindex nodes
  for(unsigned int i=0;i<this->get_num_nodes();i++)
    this->nodes[i]->set_index(i);
}

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_with_index(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_with_index(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_with_index(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]);
        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++)
    {
      if(!this->has_segment(this->nodes[i]->links[j]->segment))
        this->nodes[i]->remove_link(this->nodes[i]->links[j]);
    }
  } 
}

bool COpendriveRoad::node_exists_at(TOpendriveWorldPose &pose)
{
  TOpendriveWorldPose node_pose;

  for(unsigned int i=0;i<nodes.size();i++)
  {
    node_pose=this->nodes[i]->get_pose();
    if(std::abs(pose.x-node_pose.x)<0.01 && std::abs(pose.y-node_pose.y)<0.01)
      return true;
  }

  return false;
}

COpendriveRoadNode* COpendriveRoad::get_node_at(TOpendriveWorldPose &pose)
{
  TOpendriveWorldPose node_pose;

  for(unsigned int i=0;i<nodes.size();i++)
  {
    node_pose=this->nodes[i]->get_pose();
    if(std::abs(pose.x-node_pose.x)<0.01 && std::abs(pose.y-node_pose.y)<0.01)
      return this->nodes[i];
  }

  return NULL;
}

void COpendriveRoad::load(const std::string &filename)
{
  struct stat buffer;

  if(stat(filename.c_str(),&buffer)==0)
  {
    // delete any previous data
    this->free();
    // try to open the specified file
    try{
      std::unique_ptr<OpenDRIVE> open_drive(OpenDRIVE_(filename.c_str(), xml_schema::flags::dont_validate));
      for (OpenDRIVE::road_iterator road_it(open_drive->road().begin()); road_it != open_drive->road().end(); ++road_it)
      {
        try{
          COpendriveRoadSegment *segment=new COpendriveRoadSegment();
          segment->set_resolution(this->resolution);
          segment->set_scale_factor(this->scale_factor);
          segment->set_min_road_length(this->min_road_length);
          segment->set_parent_road(this);
          segment->load(*road_it);
          this->segments.push_back(segment);
        }catch(CException &e){
          std::cout << e.what() << std::endl;
        }
      }
      // link segments
      this->link_segments(*open_drive);
    }catch (const xml_schema::exception& e){
      std::ostringstream os;
      os << e;
      /* handle exceptions */
      throw CException(_HERE_,os.str());
    }
  }
  else
    throw CException(_HERE_,"The .xodr file does not exist");
}

void COpendriveRoad::set_resolution(double res)
{
  this->resolution=res;

  // change the resolution of all current nodes
  for(unsigned int i=0;i<this->segments.size();i++)
    this->segments[i]->set_resolution(res);
}

double COpendriveRoad::get_resolution(void) const
{
  return this->resolution;
}

void COpendriveRoad::set_scale_factor(double scale)
{
  this->scale_factor=scale;

  // change the resolution of all current nodes
  for(unsigned int i=0;i<this->segments.size();i++)
    this->segments[i]->set_scale_factor(scale);
}

double COpendriveRoad::get_scale_factor(void) const
{
  return this->scale_factor;
}

void COpendriveRoad::set_min_road_length(double length)
{
  this->min_road_length=length;

  for(unsigned int i=0;i<this->segments.size();i++)
    this->segments[i]->set_min_road_length(length);
}

double COpendriveRoad::get_min_road_length(void) const
{
  return this->min_road_length;
}

unsigned int COpendriveRoad::get_num_segments(void) const
{
  return this->segments.size();
}

const COpendriveRoadSegment& COpendriveRoad::get_segment(unsigned int index) const
{
  std::stringstream error;

  if(index>=0 && index<this->segments.size())
    return *this->segments[index];
  else
  {
    error << "Invalid segment index " << index;
    throw CException(_HERE_,error.str());
  }
}

unsigned int COpendriveRoad::get_num_lanes(void) const
{
  return this->lanes.size();
}

const COpendriveLane &COpendriveRoad::get_lane(unsigned int index) const
{
  std::stringstream error;

  if(index>=0 && index<this->lanes.size())
    return *this->lanes[index];
  else
  {
    error << "Invalid lane index " << index;
    throw CException(_HERE_,error.str());
  }
}

unsigned int COpendriveRoad::get_num_nodes(void) const
{
  return this->nodes.size();
}

const COpendriveRoadNode& COpendriveRoad::get_node(unsigned int index) const
{
  std::stringstream error;

  if(index>=0 && index<this->nodes.size())
    return *this->nodes[index];
  else
  {
    error << "Invalid node index " << index;
    throw CException(_HERE_,error.str());
  }
}

const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
{
  std::stringstream error;

  if(index>=0 && index<this->segments.size())
    return *this->segments[index];
  else
  {
    error << "Invalid node index " << index;
    throw CException(_HERE_,error.str());
  }
}

unsigned int COpendriveRoad::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
  double dist,min_dist=std::numeric_limits<double>::max(),length;
  TOpendriveWorldPose closest_pose;
  unsigned int closest_index=-1;

  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    length=this->nodes[i]->get_closest_pose(pose,closest_pose,false,angle_tol);
    if(length<std::numeric_limits<double>::max())
    {
      dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
      if(dist<min_dist)
      {
        min_dist=dist;
        closest_index=i;
        distance=length;
      }
    }
  }

  return closest_index;
}

const COpendriveRoadNode &COpendriveRoad::get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
  unsigned int closest_index;

  closest_index=this->get_closest_node_index(pose,distance,angle_tol);
  if(closest_index==(unsigned int)-1)
    throw CException(_HERE_,"Impossible to find the closest node");
  return *this->nodes[closest_index];
}

unsigned int COpendriveRoad::get_closest_lane_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
  double dist,min_dist=std::numeric_limits<double>::max(),length;
  TOpendriveWorldPose closest_pose;
  unsigned int closest_index=-1;

  for(unsigned int i=0;i<this->lanes.size();i++)
  { 
    length=this->lanes[i]->get_closest_pose(pose,closest_pose,angle_tol);
    if(length<std::numeric_limits<double>::max())
    {
      dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
      if(dist<min_dist)
      { 
        min_dist=dist;
        closest_index=i;
        distance=length;
      }
    }
  }

  return closest_index;
}

const COpendriveLane &COpendriveRoad::get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
  unsigned int closest_index;

  closest_index=this->get_closest_lane_index(pose,distance,angle_tol);
  if(closest_index==(unsigned int)-1)
    throw CException(_HERE_,"Impossible to find the closest lane");
  return *this->lanes[closest_index];
}

unsigned int COpendriveRoad::get_closest_segment_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
  double dist,min_dist=std::numeric_limits<double>::max(),length;
  TOpendriveWorldPose closest_pose;
  unsigned int closest_index=-1;

  for(unsigned int i=0;i<this->segments.size();i++)
  { 
    length=this->segments[i]->get_closest_pose(pose,closest_pose,angle_tol);
    if(length<std::numeric_limits<double>::max())
    {
      dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
      if(dist<min_dist)
      { 
        min_dist=dist;
        closest_index=i;
        distance=length;
      }
    }
  }

  return closest_index;
}

const COpendriveRoadSegment &COpendriveRoad::get_closest_segment(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
  unsigned int closest_index;

  closest_index=this->get_closest_segment_index(pose,distance,angle_tol);
  if(closest_index==(unsigned int)-1)
    throw CException(_HERE_,"Impossible to find the closest segment");
  return *this->segments[closest_index];
}

double COpendriveRoad::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
{
  double dist,min_dist=std::numeric_limits<double>::max();
  TOpendriveWorldPose pose_found;
  double length,closest_length;

  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    length=this->nodes[i]->get_closest_pose(pose,pose_found,false,angle_tol);
    if(length<std::numeric_limits<double>::max())
    {
      dist=sqrt(pow(pose_found.x-pose.x,2.0)+pow(pose_found.y-pose.y,2.0));
      if(dist<min_dist)
      {
        min_dist=dist;
        closest_pose=pose_found;
        closest_length=length;
      }
    }
  }

  return closest_length;
}

void COpendriveRoad::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol) const
{
  std::vector<TOpendriveWorldPose> poses;
  std::vector<double> lengths;
  bool already_added;

  closest_poses.clear();
  closest_lengths.clear();
  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    this->nodes[i]->get_closest_poses(pose,poses,lengths,dist_tol,false,angle_tol);
    for(unsigned int j=0;j<poses.size();j++)
    {
      already_added=false;
      for(unsigned int k=0;k<closest_poses.size();k++)
        if(fabs(closest_poses[k].x-poses[j].x)<this->resolution &&
           fabs(closest_poses[k].y-poses[j].y)<this->resolution)
        {
          already_added=true;
          break;
        }
      if(!already_added)
      {
        closest_poses.push_back(poses[j]);
        closest_lengths.push_back(lengths[j]);
      }
    }
  }
}

std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPose &start_pose,TOpendriveWorldPose &end_pose,COpendriveRoad &new_road)
{
  segment_up_ref_t new_segment_ref;
  lane_up_ref_t new_lane_ref;
  node_up_ref_t new_node_ref;
  link_up_ref_t new_link_ref;
  COpendriveRoadNode *node,*next_node;
  COpendriveRoadSegment *segment,*new_segment;
//  CopendriveRoadSegment *original_seg1,*original_seg2;
  COpendriveLink *link;
  std::vector<unsigned int> new_path_nodes;
  unsigned int link_index;

  new_path_nodes.resize(path_nodes.size());

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

  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]];
    link=node->get_link_with(next_node);
    if(link==NULL)
      throw CException(_HERE_,"The provided path has unconnected nodes");
    segment=link->segment;
    if(new_segment_ref.find(segment)==new_segment_ref.end())
    {
      new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref);
      new_road.add_segment(new_segment);
      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_pose);
  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())
  {
    new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref);
    new_road.add_segment(new_segment);
    new_segment_ref[segment]=new_segment;
  }

  // add additional nodes not explicitly in the path
/*
  for(unsigned int i=0;i<this->segments.size();i++)
  {
    for(unsigned int j=0;j<new_road.segments.size();j++)
    {
      original_seg1=new_road.segments[j]->get_original_segment(new_segment_ref);
      for(unsigned int k=j+1;k<new_road.segments.size();k++)
      {
        original_seg2=new_road.segments[k]->get_original_segment(new_segment_ref);
        if(this->segments[i]->connects_segments(original_seg1,original_seg2)
        {
          if(!new_road.has_segment(new_segment_ref[this->segments[i]]))
          {
            new_segment=this->segment[k]->clone(new_node_ref,new_lane_ref,new_link_ref);
            new_road.add_segment(new_segment);
            new_segment_ref[segment]=new_segment;
          }
        }
      }
    }
  }
*/
  new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);
  new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref);
  // remove unconnected elements
  new_road.prune(path_nodes);
  new_road.reindex();
  new_road.complete_open_lanes();

  return new_road.update_path(new_node_ref,path_nodes);
}

void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double &length,COpendriveRoad &new_road)
{
  segment_up_ref_t new_segment_ref;
  lane_up_ref_t new_lane_ref;
  node_up_ref_t new_node_ref;
  link_up_ref_t new_link_ref;
  unsigned int segment_index,node_index;
  TOpendriveWorldPose end_pose;
  COpendriveRoadSegment *segment,*new_segment,*next_segment;
  COpendriveRoadNode *node;
  double rem_length=length,distance,start_length;
  int node_side;

  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);
  node_index=this->get_closest_node_index(start_pose,distance);
  if(node_index==(unsigned int)-1)
    throw CException(_HERE_,"Start position does not belang to the road");
  node=this->nodes[node_index];
  segment_index=this->get_closest_segment_index(start_pose,distance);
  if(segment_index==(unsigned int)-1)
    throw CException(_HERE_,"Start position does not belang to the road");
  segment=this->segments[segment_index];
  node_side=segment->get_node_side(node);
  new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,&start_pose,NULL);
  start_length=new_segment->get_length()-distance;
  if(rem_length<start_length)
  {
    if(node_side<0)
      end_pose=new_segment->get_pose_at(rem_length);
    else
      end_pose=new_segment->get_pose_at(new_segment->get_length()-rem_length);
    segment=new_segment;
    new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,NULL,&end_pose);
    delete segment;
  }
  rem_length-=new_segment->get_length();
  new_road.add_segment(new_segment);
  new_segment_ref[segment]=new_segment;
  while(rem_length>0)
  {
    next_segment=segment->get_next_segment(node_side);
    if(next_segment==NULL)
      break;
    if((rem_length-next_segment->get_length())<0.0)
    {
      if(node_side<0)
        end_pose=next_segment->get_pose_at(rem_length);
      else
        end_pose=next_segment->get_pose_at(next_segment->get_length()-rem_length);
      new_segment=next_segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,NULL,&end_pose);
    }
    else
      new_segment=next_segment->clone(new_node_ref,new_lane_ref,new_link_ref);
    rem_length-=new_segment->get_length();
    new_road.add_segment(new_segment);
    new_segment_ref[next_segment]=new_segment;
    segment=next_segment;
  }
  length-=rem_length;
  new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);
  new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref);
  new_road.reindex();
  new_road.complete_open_lanes();
}

void COpendriveRoad::operator=(const COpendriveRoad& object)
{
  COpendriveRoadSegment *segment;
  COpendriveRoadNode *node;
  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<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]);