diff --git a/include/generate_launch.h b/include/generate_launch.h index 9714ef4b33bd53d2b2ba322c84e19f64a8be146a..3dbbda01229fa82d2d504f244b7fe5fb914e972a 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 c6ce0a5d25e6f8b9707c4302346e356d582b1898..3d7eaf2ac6672e5e0295f45918ef0e89d1b7fc8a 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 a020171047bc945bc134f7f04bd37ef8a5df25d4..fee1bf23f43fdf2efef83733e3ff2d9efb1da32b 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; }