From b285915760c23bccdf7c3a38c54fbef20e7fe5f9 Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Fri, 26 Mar 2021 11:46:27 +0100
Subject: [PATCH] Add parkings yaml generation
---
include/generate_launch.h | 1 +
src/generate_launch.cpp | 47 +++++++++++++++++++++++++++++++++++++
src/opendrive_to_gazebo.cpp | 4 +++-
3 files changed, 51 insertions(+), 1 deletion(-)
diff --git a/include/generate_launch.h b/include/generate_launch.h
index 9714ef4..3dbbda0 100644
--- a/include/generate_launch.h
+++ b/include/generate_launch.h
@@ -10,6 +10,7 @@
std::string get_full_name(std::string &path,std::string &file_name,const std::string &ext);
void generate_signals_launch(std::string &path,std::string &signals_file,COpendriveRoad &road);
void generate_objects_launch(std::string &path,std::string &objects_file,COpendriveRoad &road);
+void generate_parkings_yaml(std::string &path,std::string &parkings_file,COpendriveRoad &road);
#define UNMARKEDINTERSECTION_TYPE "102"
#define UNMARKEDINTERSECTION_MARKER "alvar0"
diff --git a/src/generate_launch.cpp b/src/generate_launch.cpp
index c6ce0a5..3d7eaf2 100644
--- a/src/generate_launch.cpp
+++ b/src/generate_launch.cpp
@@ -259,3 +259,50 @@ void generate_objects_launch(std::string &path,std::string &objects_file,COpendr
throw CException(_HERE_,"Impossible to create objects launch file");
}
+void generate_parkings_yaml(std::string &path,std::string &parkings_file,COpendriveRoad &road)
+{
+ std::ofstream out_file;
+ std::string full_name,type,subtype,name,marker;
+
+
+ boost::filesystem::path dir(path);
+ if(!(boost::filesystem::exists(dir)))
+ {
+ //path does not exist
+ std::cout << "opendrive_to_gazebo: directory does not exist: " << dir << std::endl;
+ if (boost::filesystem::create_directory(dir))
+ {
+ std::cout << "opendrive_to_gazebo: created directory: " << dir << std::endl;
+ }
+ else
+ std::cout << "opendrive_to_gazebo: could not create directory: " << dir << std::endl;
+ }
+
+ full_name=get_full_name(path,parkings_file,"yaml");
+ // process the road
+ out_file.open(full_name.c_str(),std::ofstream::out);
+ if(out_file.is_open())
+ {
+ out_file << "parkings:" << std::endl;
+ for(unsigned int i=0;i<road.get_num_segments();i++)
+ {
+ const COpendriveRoadSegment &segment=road.get_segment(i);
+ for(unsigned int j=0;j<segment.get_num_objects();j++)
+ {
+ const COpendriveObject &object=segment.get_object(j);
+ if(object.is_parking_space())
+ {
+ TOpendriveWorldPose world=object.get_world_pose();
+ out_file << " - id: " << object.get_name() << "_" << object.get_id() << std::endl;
+ out_file << " x: " << world.x << std::endl;
+ out_file << " y: " << world.y << std::endl;
+ out_file << " yaw: " << world.heading << std::endl;
+ }
+ }
+ }
+ out_file.close();
+ }
+ else
+ throw CException(_HERE_,"Impossible to create parking yaml file");
+}
+
diff --git a/src/opendrive_to_gazebo.cpp b/src/opendrive_to_gazebo.cpp
index a020171..fee1bf2 100644
--- a/src/opendrive_to_gazebo.cpp
+++ b/src/opendrive_to_gazebo.cpp
@@ -4,7 +4,7 @@
int main(int argc, char *argv[])
{
- std::string road_file,path,signals_file_name,objects_file_name;
+ std::string road_file,path,signals_file_name,objects_file_name,parkings_file_name;
std::size_t slash_index, point_index;
COpendriveRoad road;
double road_scale;
@@ -21,6 +21,7 @@ int main(int argc, char *argv[])
point_index=road_file.find_last_of(".");
signals_file_name=road_file.substr(road_file.find_last_of("/")+1, point_index - slash_index -1)+"_signals";
objects_file_name=road_file.substr(road_file.find_last_of("/")+1, point_index - slash_index -1)+"_objects";
+ parkings_file_name=road_file.substr(road_file.find_last_of("/")+1, point_index - slash_index -1)+"_parkings";
if(argc==4)
{
@@ -40,6 +41,7 @@ int main(int argc, char *argv[])
generate_signals_launch(path,signals_file_name,road);
generate_objects_launch(path,objects_file_name,road);
+ generate_parkings_yaml(path,parkings_file_name,road);
}catch (CException &e){
std::cout << "[Exception caught] : " << e.what() << std::endl;
}
--
GitLab