From da458e359bac97d015f0e98e9a4a62169bf183b8 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 25 Aug 2023 13:12:00 +0200
Subject: [PATCH] Added support to save road data in opendrive format.
---
include/common.h | 4 +-
include/opendrive/opendrive_junction.h | 10 +-
include/opendrive/opendrive_road_segment.h | 8 +-
include/road_map.h | 3 +
src/opendrive/opendrive_junction.cpp | 137 ++++++++++++++-
src/opendrive/opendrive_road_segment.cpp | 191 ++++++++++++++++++++-
src/road_map.cpp | 128 ++++++++++++++
7 files changed, 473 insertions(+), 8 deletions(-)
diff --git a/include/common.h b/include/common.h
index 8cac3c9..1112440 100644
--- a/include/common.h
+++ b/include/common.h
@@ -8,9 +8,11 @@ class COpendriveRoadSegment;
class CRoad;
typedef std::pair<COpendriveRoadSegment *,std::vector<CRoad *> > opendrive_map_pair_t;
-
typedef std::vector<opendrive_map_pair_t> opendrive_road_map_t;
+typedef std::pair<std::vector<CRoad *>,std::vector<COpendriveRoadSegment *> > opendrive_inverse_map_pair_t;
+typedef std::vector<opendrive_inverse_map_pair_t> opendrive_road_inverse_map_t;
+
double normalize_angle(double angle);
double diff_angle(double angle1,double angle2);
diff --git a/include/opendrive/opendrive_junction.h b/include/opendrive/opendrive_junction.h
index 6613ecc..4a0a03d 100644
--- a/include/opendrive/opendrive_junction.h
+++ b/include/opendrive/opendrive_junction.h
@@ -1,7 +1,7 @@
#ifndef _OPENDRIVE_JUNCTION_H
#define _OPENDRIVE_JUNCTION_H
-#include "road_map.h"
+#include "opendrive/opendrive_road_segment.h"
typedef struct{
int from_lane_id;
@@ -19,16 +19,22 @@ typedef struct{
class COpendriveJunction
{
friend class CRoadMap;
+ friend class CJunction;
+ friend class COpendriveRoadSegment;
+ friend class COpendriveLane;
private:
std::vector<TJunctionConnection> connections;
std::string name;
unsigned int id;
protected:
- void set_name(std::string &name);
+ void set_name(const std::string &name);
void set_id(unsigned int id);
void load(OpenDRIVE::junction_type &junction_info);
void save(OpenDRIVE::junction_type &junction_info);
void convert(CJunction **junction,opendrive_road_map_t &road_map);
+ static bool get_road_data(unsigned int id,opendrive_road_inverse_map_t &road_map,COpendriveRoadSegment **road,bool incomming);
+ static void generate_road_segment(CRoadSegment *segment,unsigned int ¤t_id,unsigned int road_in,int lane_in,unsigned int road_out,int lane_out,COpendriveRoadSegment *new_segment);
+ static void convert(CJunction *junction_in,unsigned int ¤t_id,COpendriveJunction *junction_out,std::vector<COpendriveRoadSegment *> &segments,opendrive_road_inverse_map_t &road_map);
public:
COpendriveJunction();
std::string get_name(void) const;
diff --git a/include/opendrive/opendrive_road_segment.h b/include/opendrive/opendrive_road_segment.h
index 2d9f68f..f6127f1 100644
--- a/include/opendrive/opendrive_road_segment.h
+++ b/include/opendrive/opendrive_road_segment.h
@@ -27,6 +27,7 @@ class COpendriveRoadSegment
friend class CRoadMap;
friend class CRoad;
friend class COpendriveLane;
+ friend class COpendriveJunction;
private:
std::vector<COpendriveGeometry *> geometries;
std::vector<COpendriveLane *> right_lanes;
@@ -45,14 +46,19 @@ class COpendriveRoadSegment
void free(void);
void load(OpenDRIVE::road_type &road_info);
void convert(CRoad **left_road,CRoad **right_road,double resolution);
+ static void convert(CRoad *left_road,CRoad *right_road,unsigned int ¤t_id,std::vector<COpendriveRoadSegment *> &segments);
void save(OpenDRIVE::road_type **road_info);
- void set_name(std::string &name);
+ void set_name(const std::string &name);
void set_id(unsigned int id);
void set_center_mark(opendrive_mark_t mark);
void add_lanes(lanes::laneSection_type &lane_section);
void add_right_lane(const ::lane &lane_info);
+ void add_right_lane(COpendriveLane *lane);
void add_left_lane(const ::lane &lane_info);
+ void add_left_lane(COpendriveLane *lane);
bool add_geometries(OpenDRIVE::road_type &road_info);
+ void add_geometry(COpendriveGeometry *geometry,double resolution);
+ void set_junction_id(unsigned int id);
public:
COpendriveRoadSegment();
/**
diff --git a/include/road_map.h b/include/road_map.h
index b19f9bd..428f25d 100644
--- a/include/road_map.h
+++ b/include/road_map.h
@@ -45,11 +45,13 @@ class CRoadMap
CJunction *get_junction_by_index(unsigned int index);
void get_segment_by_id(unsigned int id,CRoadSegment **segment);
void remove_road_by_index(unsigned int index);
+ void set_opposite_direction_roads_by_index(unsigned int right_road,unsigned int left_road);
void remove_junction_by_index(unsigned int index);
void merge_roads(opendrive_road_map_t &road_map);
public:
CRoadMap();
void load_opendrive(const std::string &filename);
+ void save_opendrive(const std::string &filename);
void set_resolution(double resolution);
double get_resolution(void);
std::vector<unsigned int> get_path_sub_roadmap(std::vector<unsigned int> &segment_ids,CRoadMap &new_road_map);
@@ -64,6 +66,7 @@ class CRoadMap
void add_road(CRoad *road);
bool has_road(unsigned int id);
void remove_road_by_id(unsigned int id);
+ void set_opposite_direction_roads_by_id(unsigned int right_road,unsigned int left_road);
void add_junction(CJunction *junction);
bool has_junction(unsigned int id);
void remove_junction_by_id(unsigned int id);
diff --git a/src/opendrive/opendrive_junction.cpp b/src/opendrive/opendrive_junction.cpp
index 297f31a..a5bf706 100644
--- a/src/opendrive/opendrive_junction.cpp
+++ b/src/opendrive/opendrive_junction.cpp
@@ -1,4 +1,5 @@
#include "opendrive/opendrive_junction.h"
+#include "opendrive/opendrive_param_poly3.h"
#include "exceptions.h"
@@ -11,7 +12,7 @@ COpendriveJunction::COpendriveJunction()
this->connections.clear();
}
-void COpendriveJunction::set_name(std::string &name)
+void COpendriveJunction::set_name(const std::string &name)
{
this->name=name;
}
@@ -180,6 +181,140 @@ void COpendriveJunction::convert(CJunction **junction,opendrive_road_map_t &road
}
}
+bool COpendriveJunction::get_road_data(unsigned int id,opendrive_road_inverse_map_t &road_map,COpendriveRoadSegment **road,bool incomming)
+{
+ for(unsigned int i=0;i<road_map.size();i++)
+ {
+ if(road_map[i].first.size()>0)
+ {
+ if(road_map[i].first[0]->get_id()==id)
+ {
+ if(incomming)
+ (*road)=road_map[i].second[road_map[i].second.size()-1];
+ else
+ (*road)=road_map[i].second[0];
+ return true;
+ }
+ }
+ if(road_map[i].first.size()>1)
+ {
+ if(road_map[i].first[1]->get_id()==id)
+ {
+ if(incomming)
+ (*road)=road_map[i].second[0];
+ else
+ (*road)=road_map[i].second[road_map[i].second.size()-1];
+ return false;
+ }
+ }
+ }
+ throw CException(_HERE_,"No road data for the given ID");
+}
+
+void COpendriveJunction::generate_road_segment(CRoadSegment *segment,unsigned int ¤t_id,unsigned int road_in,int lane_in,unsigned int road_out,int lane_out,COpendriveRoadSegment *new_segment)
+{
+ std::stringstream txt;
+ double lateral_offset_in,lateral_offset_out;
+ TPoint start_point,end_point;
+ TOpendriveWorldPose start_pose,end_pose;
+ COpendriveParamPoly3 *new_geometry;
+ COpendriveLane *new_lane;
+
+ txt << "junction_" << road_in << "_" << lane_in << "_" << road_out << "_" << lane_out;
+ new_segment->set_name(txt.str());
+ new_segment->set_id(current_id);
+ current_id++;
+ new_segment->set_center_mark(OD_MARK_NONE);
+ new_segment->set_junction_id(segment->get_parent_junction().get_id());
+ // add geometry
+ lateral_offset_in=-(abs(lane_in)-1)*segment->get_lane_width();
+ segment->get_point_at(0.0,lateral_offset_in,start_point);
+ start_pose.x=start_point.x;
+ start_pose.y=start_point.y;
+ start_pose.heading=start_point.heading;
+ lateral_offset_out=-(abs(lane_out)-1)*segment->get_lane_width();
+ segment->get_point_at(segment->get_length(),lateral_offset_out,end_point);
+ end_pose.x=end_point.x;
+ end_pose.y=end_point.y;
+ end_pose.heading=end_point.heading;
+ new_geometry=new COpendriveParamPoly3(start_pose,end_pose);
+ new_segment->add_geometry(new_geometry,segment->get_resolution());
+ // add a single right lane
+ new_lane=new COpendriveLane();
+ new_lane->set_speed(segment->get_lane_speed());
+ new_lane->set_width(segment->get_lane_width());
+ new_lane->set_id(-1);
+ new_lane->set_right_mark(OD_MARK_NONE);
+ new_segment->add_right_lane(new_lane);
+}
+
+void COpendriveJunction::convert(CJunction *junction_in,unsigned int ¤t_id,COpendriveJunction *junction_out,std::vector<COpendriveRoadSegment *> &segments,opendrive_road_inverse_map_t &road_map)
+{
+ std::stringstream txt;
+ CRoadSegment *segment;
+ COpendriveRoadSegment *incomming_segment,*outgoing_segment,*new_segment;
+ TJunctionConnection new_connection;
+ TJunctionLink new_link;
+ bool incomming_right,outgoing_right;
+ int incomming_lane,outgoing_lane;
+ unsigned int ids=0,incomming_id,outgoing_id;
+
+ if(junction_in==NULL)
+ throw CException(_HERE_,"Invalid junction reference");
+ segments.clear();
+ txt << "junction" << junction_in->get_id();
+ junction_out->set_name(txt.str());
+ junction_out->set_id(junction_in->get_id());
+ for(unsigned int i=0;i<junction_in->get_num_incomming_roads();i++)
+ {
+ // get incomming opendrive road info
+ incomming_id=junction_in->get_incomming_road_by_index(i)->get_id();
+ incomming_right=COpendriveJunction::get_road_data(incomming_id,road_map,&incomming_segment,true);
+ new_connection.incomming_road_id=incomming_segment->get_id();
+ new_connection.end_contact_point=false;
+ for(unsigned int j=0;j<junction_in->get_num_outgoing_roads();j++)
+ {
+ outgoing_id=junction_in->get_outgoing_road_by_index(j)->get_id();
+ outgoing_right=COpendriveJunction::get_road_data(outgoing_id,road_map,&outgoing_segment,false);
+ new_connection.connecting_road_id=outgoing_segment->get_id();
+ new_connection.links.clear();
+ if(junction_in->are_roads_linked_by_id(incomming_id,outgoing_id))
+ {
+ new_connection.id=ids;
+ ids++;
+ segment=&junction_in->get_segment_between_by_id(incomming_id,outgoing_id);
+ // create road from segment
+ for(unsigned int k=0;k<segment->get_num_lanes_in();k++)
+ {
+ if(incomming_right)
+ incomming_lane=-(k+1);
+ else
+ incomming_lane=k+1;
+ for(unsigned int l=0;l<segment->get_num_lanes_out();l++)
+ {
+ if(outgoing_right)
+ outgoing_lane=-(k+1);
+ else
+ outgoing_lane=k+1;
+ if(segment->are_lanes_linked(k,l))
+ {
+ new_link.from_lane_id=incomming_lane;
+ new_link.to_lane_id=outgoing_lane;
+ new_connection.links.push_back(new_link);
+ // create links
+ new_segment=new COpendriveRoadSegment();
+ COpendriveJunction::generate_road_segment(segment,current_id,incomming_segment->get_id(),incomming_lane,outgoing_segment->get_id(),outgoing_lane,new_segment);
+ segments.push_back(new_segment);
+ }
+ }
+ }
+ if(new_connection.links.size()>0)
+ junction_out->connections.push_back(new_connection);
+ }
+ }
+ }
+}
+
std::string COpendriveJunction::get_name(void) const
{
return this->name;
diff --git a/src/opendrive/opendrive_road_segment.cpp b/src/opendrive/opendrive_road_segment.cpp
index a8fddb3..61e28b2 100644
--- a/src/opendrive/opendrive_road_segment.cpp
+++ b/src/opendrive/opendrive_road_segment.cpp
@@ -56,7 +56,7 @@ void COpendriveRoadSegment::free(void)
}
}
-void COpendriveRoadSegment::set_name(std::string &name)
+void COpendriveRoadSegment::set_name(const std::string &name)
{
this->name=name;
}
@@ -101,6 +101,30 @@ bool COpendriveRoadSegment::add_geometries(OpenDRIVE::road_type &road_info)
return true;
}
+void COpendriveRoadSegment::add_geometry(COpendriveGeometry *geometry,double resolution)
+{
+ TOpendriveWorldPose start_pose,end_pose;
+
+ if(geometry==NULL)
+ throw CException(_HERE_,"Invalid geometry reference");
+ if(this->geometries.size()==0)
+ this->geometries.push_back(geometry);
+ else
+ {
+ start_pose=geometry->get_start_pose();
+ end_pose=this->geometries[this->geometries.size()-1]->get_end_pose();
+ if(sqrt(pow(start_pose.x-end_pose.x,2.0)+pow(start_pose.y-end_pose.y,2.0))<=resolution)
+ this->geometries.push_back(geometry);
+ else
+ throw CException(_HERE_,"geometries do not coincide");
+ }
+}
+
+void COpendriveRoadSegment::set_junction_id(unsigned int id)
+{
+ this->junction_id=id;
+}
+
void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
{
right::lane_iterator r_lane_it;
@@ -137,6 +161,16 @@ void COpendriveRoadSegment::add_right_lane(const ::lane &lane_info)
}
}
+void COpendriveRoadSegment::add_right_lane(COpendriveLane *lane)
+{
+ if(lane==NULL)
+ throw CException(_HERE_,"Invalid right lane reference");
+ for(unsigned int i=0;i<this->right_lanes.size();i++)
+ if(this->right_lanes[i]->get_id()==lane->get_id())
+ throw CException(_HERE_,"A lane with the same ID has already been added");
+ this->right_lanes.push_back(lane);
+}
+
void COpendriveRoadSegment::add_left_lane(const ::lane &lane_info)
{
COpendriveLane *new_lane;
@@ -154,6 +188,16 @@ void COpendriveRoadSegment::add_left_lane(const ::lane &lane_info)
}
}
+void COpendriveRoadSegment::add_left_lane(COpendriveLane *lane)
+{
+ if(lane==NULL)
+ throw CException(_HERE_,"Invalid left lane reference");
+ for(unsigned int i=0;i<this->left_lanes.size();i++)
+ if(this->left_lanes[i]->get_id()==lane->get_id())
+ throw CException(_HERE_,"A lane with the same ID has already been added");
+ this->left_lanes.push_back(lane);
+}
+
bool COpendriveRoadSegment::is_junction(void) const
{
if(this->junction_id==(unsigned int)-1)
@@ -466,6 +510,127 @@ void COpendriveRoadSegment::convert(CRoad **left_road,CRoad **right_road,double
}
}
+void COpendriveRoadSegment::convert(CRoad *left_road,CRoad *right_road,unsigned int ¤t_id,std::vector<COpendriveRoadSegment *> &segments)
+{
+ COpendriveLane *new_lane;
+ COpendriveRoadSegment *new_segment;
+ CRoadSegment *right_segment,*left_segment;
+ std::stringstream txt;
+ TPoint start_point,end_point;
+ TOpendriveWorldPose start_pose,end_pose;
+ COpendriveParamPoly3 *new_geometry;
+
+ if(right_road==NULL)
+ throw CException(_HERE_,"Invalid right road reference");
+ segments.clear();
+ for(unsigned int i=0;i<right_road->get_num_segments();i++)
+ {
+ right_segment=right_road->get_segment_by_index(i);
+ new_segment=new COpendriveRoadSegment();
+ txt.str("");
+ txt << "road" << current_id;
+ new_segment->set_name(txt.str());
+ new_segment->set_id(current_id);
+ new_segment->set_center_mark(OD_MARK_SOLID);
+ if(right_segment->has_parent_junction())
+ new_segment->set_junction_id(right_segment->get_parent_junction().get_id());
+ else
+ new_segment->set_junction_id(-1);
+ // add geometry
+ right_segment->get_start_point(start_point);
+ start_pose.x=start_point.x;
+ start_pose.y=start_point.y;
+ start_pose.heading=start_point.heading;
+ right_segment->get_end_point(end_point);
+ end_pose.x=end_point.x;
+ end_pose.y=end_point.y;
+ end_pose.heading=end_point.heading;
+ new_geometry=new COpendriveParamPoly3(start_pose,end_pose);
+ new_segment->add_geometry(new_geometry,right_road->get_resolution());
+ // add right lanes
+ for(unsigned int j=0;j<right_road->get_num_lanes();j++)
+ {
+ new_lane=new COpendriveLane();
+ new_lane->set_speed(right_road->get_lane_speed());
+ new_lane->set_width(right_road->get_lane_width());
+ new_lane->set_id(-(j+1));
+ if(j==(right_road->get_num_lanes()-1))
+ new_lane->set_right_mark(OD_MARK_SOLID);
+ else
+ {
+ if(right_segment->are_lanes_linked(j,j+1))
+ new_lane->set_right_mark(OD_MARK_BROKEN);
+ else
+ new_lane->set_right_mark(OD_MARK_SOLID);
+ }
+ new_segment->add_right_lane(new_lane);
+ }
+ // add left lanes
+ if(left_road!=NULL)
+ {
+ left_segment=left_road->get_segment_by_index(left_road->get_num_segments()-1-i);
+ for(unsigned int j=0;j<left_road->get_num_lanes();j++)
+ {
+ new_lane=new COpendriveLane();
+ new_lane->set_speed(left_road->get_lane_speed());
+ new_lane->set_width(left_road->get_lane_width());
+ new_lane->set_id(j+1);
+ if(j==(left_road->get_num_lanes()-1))
+ new_lane->set_right_mark(OD_MARK_SOLID);
+ else
+ {
+ if(left_segment->are_lanes_linked(j,j+1))
+ new_lane->set_right_mark(OD_MARK_BROKEN);
+ else
+ new_lane->set_right_mark(OD_MARK_SOLID);
+ }
+ new_segment->add_left_lane(new_lane);
+ }
+ }
+ // add connectivity
+ if(right_segment==right_road->get_first_segment())
+ {
+ if(right_road->exist_prev_junction())
+ {
+ new_segment->predecessor_exists=true;
+ new_segment->predecessor.road=false;
+ new_segment->predecessor.id=right_road->get_prev_junction().get_id();
+ new_segment->predecessor.end_contact=false;
+ }
+ else
+ new_segment->predecessor_exists=false;
+ }
+ else
+ {
+ new_segment->predecessor_exists=true;
+ new_segment->predecessor.road=true;
+ new_segment->predecessor.id=current_id-1;
+ new_segment->predecessor.end_contact=false;
+ }
+ if(right_segment==right_road->get_last_segment())
+ {
+ if(right_road->exist_next_junction())
+ {
+ new_segment->successor_exists=true;
+ new_segment->successor.road=false;
+ new_segment->successor.id=right_road->get_next_junction().get_id();
+ new_segment->successor.end_contact=true;
+ }
+ else
+ new_segment->successor_exists=false;
+ }
+ else
+ {
+ new_segment->successor_exists=true;
+ new_segment->successor.road=true;
+ new_segment->successor.id=current_id+1;
+ new_segment->successor.end_contact=true;
+ }
+ segments.push_back(new_segment);
+ current_id++;
+ }
+}
+
void COpendriveRoadSegment::save(OpenDRIVE::road_type **road_info)
{
::lanes lanes;
@@ -522,7 +687,7 @@ void COpendriveRoadSegment::save(OpenDRIVE::road_type **road_info)
section=new laneSection(center);
// lane section
section->s(0.0);
- for(unsigned int i=1;i<=this->left_lanes.size();i++)
+ for(unsigned int i=0;i<this->left_lanes.size();i++)
{
::lane new_lane;
this->left_lanes[i]->save(new_lane);
@@ -554,8 +719,28 @@ void COpendriveRoadSegment::save(OpenDRIVE::road_type **road_info)
road_type.type("town");
(*road_info)->type().push_back(road_type);
// link information
- (*road_info)->junction(std::to_string(this->junction_id));
+ (*road_info)->junction(std::to_string((int)this->junction_id));
/* get connectivity info from junction object */
+ if(this->predecessor.road)
+ predecessor.elementType("road");
+ else
+ predecessor.elementType("junction");
+ predecessor.elementId(std::to_string(this->predecessor.id));
+ if(this->predecessor.end_contact)
+ predecessor.contactPoint("end");
+ else
+ predecessor.contactPoint("start");
+ link.predecessor(predecessor);
+ if(this->successor.road)
+ successor.elementType("road");
+ else
+ successor.elementType("junction");
+ successor.elementId(std::to_string(this->successor.id));
+ if(this->successor.end_contact)
+ successor.contactPoint("end");
+ else
+ successor.contactPoint("start");
+ link.successor(successor);
(*road_info)->lane_link(link);
}
diff --git a/src/road_map.cpp b/src/road_map.cpp
index 0f7b6e7..e412b36 100644
--- a/src/road_map.cpp
+++ b/src/road_map.cpp
@@ -6,6 +6,8 @@
#include <sys/types.h>
#include <sys/stat.h>
+#include <chrono>
+#include <fstream>
CRoadMap::CRoadMap()
{
@@ -108,6 +110,35 @@ void CRoadMap::remove_road_by_index(unsigned int index)
}
}
+void CRoadMap::set_opposite_direction_roads_by_index(unsigned int right_road,unsigned int left_road)
+{
+ CRoad *left,*right;
+ CRoadSegment *left_segment,*right_segment;
+ TPoint left_point,right_point;
+
+ if(right_road<0 || right_road >= this->roads.size())
+ throw CException(_HERE_,"Invalid right road index");
+ if(left_road<0 || left_road >= this->roads.size())
+ throw CException(_HERE_,"Invalid left road index");
+
+ left=this->get_road_by_index(left_road);
+ right=this->get_road_by_index(right_road);
+ // check geometry
+ if(left->get_num_segments()!=right->get_num_segments())
+ throw CException(_HERE_,"The number of segments in both roads do not match");
+ for(unsigned int i=0;i<right->get_num_segments();i++)
+ {
+ left_segment=left->get_segment_by_index(left->get_num_segments()-1-i);
+ left_segment->get_start_point(left_point);
+ right_segment=right->get_segment_by_index(i);
+ right_segment->get_end_point(right_point);
+ if(sqrt(pow(right_point.x-left_point.x,2.0)+pow(right_point.y-left_point.y,2.0))>this->resolution)
+ throw CException(_HERE_,"Intermediate geometry points do not match");
+ }
+ left->set_opposite_direction_road(right);
+ right->set_opposite_direction_road(left);
+}
+
void CRoadMap::remove_junction_by_index(unsigned int index)
{
std::vector<CJunction *>::iterator it;
@@ -237,10 +268,14 @@ void CRoadMap::load_opendrive(const std::string &filename)
junction->convert(&new_junction,road_map);
if(new_junction!=NULL)
this->add_junction(new_junction);
+ delete junction;
}catch(CException &e){
std::cout << e.what() << std::endl;
}
}
+ /* free all opendrive segments */
+ for(unsigned int i=0;i<road_map.size();i++)
+ delete road_map[i].first;
}catch (const xml_schema::exception& e){
this->free();
std::ostringstream os;
@@ -253,6 +288,90 @@ void CRoadMap::load_opendrive(const std::string &filename)
throw CException(_HERE_,"The .xodr file does not exist");
}
+void CRoadMap::save_opendrive(const std::string &filename)
+{
+ std::vector<CRoad *> processed_roads;
+ opendrive_inverse_map_pair_t map_pair;
+ opendrive_road_inverse_map_t road_map;
+ CRoad *opposite_road;
+ std::vector<COpendriveRoadSegment *> junction_segments,segments;
+ COpendriveJunction *junction;
+ xml_schema::namespace_infomap map;
+ ::header opendrive_header;
+ ::OpenDRIVE *opendrive_data;
+ OpenDRIVE::road_type *new_road;
+ unsigned int road_id=0;
+
+ try{
+ std::ofstream output_file(filename.c_str(),std::ios_base::out);
+ opendrive_header.revMajor(1.0);
+ opendrive_header.revMinor(1.0);
+ opendrive_header.name("iri");
+ opendrive_header.version(1.0);
+ opendrive_header.north(0.0);
+ opendrive_header.south(0.0);
+ opendrive_header.east(0.0);
+ opendrive_header.west(0.0);
+ std::time_t current_time = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
+ opendrive_header.date(std::ctime(¤t_time));
+ opendrive_data=new ::OpenDRIVE(opendrive_header);
+ for(unsigned int i=0;i<this->roads.size();i++)
+ {
+ if(std::find(processed_roads.begin(),processed_roads.end(),this->roads[i])==processed_roads.end())
+ {
+ map_pair.first.clear();
+ map_pair.second.clear();
+ processed_roads.push_back(this->roads[i]);
+ map_pair.first.push_back(this->roads[i]);
+ if(this->roads[i]->has_opposite_direction_road())
+ {
+ opposite_road=&this->roads[i]->get_opposite_direction_road();
+ processed_roads.push_back(opposite_road);
+ map_pair.first.push_back(opposite_road);
+ }
+ else
+ opposite_road=NULL;
+ COpendriveRoadSegment::convert(opposite_road,this->roads[i],road_id,segments);
+ for(unsigned j=0;j<segments.size();j++)
+ {
+ segments[j]->save(&new_road);
+ opendrive_data->road().push_back(*new_road);
+ delete new_road;
+ map_pair.second.push_back(segments[j]);
+ }
+ road_map.push_back(map_pair);
+ }
+ }
+ for(unsigned int i=0;i<this->junctions.size();i++)
+ {
+ junction=new COpendriveJunction();
+ junction->convert(this->junctions[i],road_id,junction,junction_segments,road_map);
+ OpenDRIVE::junction_type new_junction;
+ junction->save(new_junction);
+ opendrive_data->junction().push_back(new_junction);
+ for(unsigned int j=0;j<junction_segments.size();j++)
+ {
+ junction_segments[j]->save(&new_road);
+ opendrive_data->road().push_back(*new_road);
+ delete new_road;
+ delete junction_segments[j];
+ }
+ delete junction;
+ }
+ for(unsigned int i=0;i<road_map.size();i++)
+ for(unsigned int j=0;j<road_map[i].second.size();j++)
+ delete road_map[i].second[j];
+ map[""].name="";
+ map[""].schema="OpenDRIVE_1.4H.xsd";
+ OpenDRIVE_(output_file,*opendrive_data,map);
+ }catch (const xml_schema::exception& e){
+ std::ostringstream os;
+ os << e;
+ /* handle exceptions */
+ throw CException(_HERE_,os.str());
+ }
+}
+
void CRoadMap::set_resolution(double resolution)
{
this->resolution=resolution;
@@ -502,6 +621,15 @@ void CRoadMap::remove_road_by_id(unsigned int id)
this->remove_road_by_index(index);
}
+void CRoadMap::set_opposite_direction_roads_by_id(unsigned int right_road,unsigned int left_road)
+{
+ unsigned int left_index,right_index;
+
+ right_index=CRoad::get_index_by_id(this->roads,right_road);
+ left_index=CRoad::get_index_by_id(this->roads,left_road);
+ this->set_opposite_direction_roads_by_index(right_index,left_index);
+}
+
void CRoadMap::add_junction(CJunction *junction)
{
if(junction==NULL)
--
GitLab