Add parkings yaml generation

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"

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");
+}
+

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