Add parking objects as parked cars obstacles in launch file, but commented....

Add parking objects as parked cars obstacles in launch file, but commented. Create output directory if it does not exist.

include/generate_launch.h

@@ -5,6 +5,8 @@
 #include "opendrive_road_segment.h"
 #include "opendrive_signal.h"
 
+#include <boost/filesystem.hpp>
+
 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);
@@ -55,4 +57,9 @@ void generate_objects_launch(std::string &path,std::string &objects_file,COpendr
 #define SEMAPHORE_MARKER "alvar1"
 // #define SEMAPHORE_MARKER "alvar15"
 
+#define PARKED_CAR_LENGTH "0.6"
+#define PARKED_CAR_WIDTH  "0.25"
+#define PARKED_CAR_HEIGHT "0.15"
+
+
 #endif

src/CMakeLists.txt

@@ -10,11 +10,13 @@ ADD_DEFINITIONS(-D_HAVE_XSD)
 # locate the necessary dependencies
 FIND_PACKAGE(iriutils REQUIRED)
 FIND_PACKAGE(autonomous_driving_tools REQUIRED)
+FIND_PACKAGE(Boost REQUIRED COMPONENTS system filesystem)
 
 # add the necessary include directories
 INCLUDE_DIRECTORIES(../include)
 INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIRS})
 INCLUDE_DIRECTORIES(${autonomous_driving_tools_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
 
 # create the executable file
 ADD_EXECUTABLE(opendrive_to_gazebo ${sources}) 
@@ -22,4 +24,4 @@ ADD_EXECUTABLE(opendrive_to_gazebo ${sources})
 # link necessary libraries
 TARGET_LINK_LIBRARIES(opendrive_to_gazebo ${iriutils_LIBRARIES})
 TARGET_LINK_LIBRARIES(opendrive_to_gazebo ${autonomous_driving_tools_LIBRARIES})
-
+TARGET_LINK_LIBRARIES(opendrive_to_gazebo ${Boost_LIBRARIES})

src/generate_launch.cpp

@@ -117,6 +117,19 @@ void generate_signals_launch(std::string &path,std::string &signals_file,COpendr
   std::string full_name,type,subtype,name,marker;
   TOpendriveWorldPose world;
 
+  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,signals_file,"launch");
   // process the road
   out_file.open(full_name.c_str(),std::ofstream::out);
@@ -163,6 +176,19 @@ void generate_objects_launch(std::string &path,std::string &objects_file,COpendr
   std::string full_name,type,subtype,name,marker;
   TOpendriveWorldPose world;
 
+  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,objects_file,"launch");
   // process the road
   out_file.open(full_name.c_str(),std::ofstream::out);
@@ -177,13 +203,18 @@ void generate_objects_launch(std::string &path,std::string &objects_file,COpendr
       for(unsigned int j=0;j<segment.get_num_objects();j++)
       {
         const COpendriveObject &object=segment.get_object(j);
-        if(!object.is_parking_space())
+        if(object.is_parking_space())
+        {
+          out_file << "  <!--" << std::endl;
+        }
+
+        if(object.is_box() || object.is_cylinder())
         {
-          if(object.is_box() || object.is_cylinder())
+          out_file << "  <include file=\"$(find iri_object_description)/launch/spawn_object.launch\">" << std::endl;
+          out_file << "    <arg name=\"name\"  value=\"" << object.get_name() << "_" << object.get_id() << "\"/>" << std::endl;
+          if(object.is_box())
           {
-            out_file << std::endl << "  <include file=\"$(find iri_object_description)/launch/spawn_object.launch\">" << std::endl;
-            out_file << "    <arg name=\"name\"  value=\"" << object.get_name() << "_" << object.get_id() << "\"/>" << std::endl;
-            if(object.is_box())
+            if(!object.is_parking_space())
             {
               TOpendriveBox data=object.get_box_data();
               out_file << "    <arg name=\"model\"  value=\"box\"/>" << std::endl;
@@ -191,27 +222,40 @@ void generate_objects_launch(std::string &path,std::string &objects_file,COpendr
               out_file << "    <arg name=\"width\"  value=\"" << data.width << "\"/>" << std::endl;
               out_file << "    <arg name=\"height\"  value=\"" << data.height << "\"/>" << std::endl;
             }
-            else 
+            else
             {
-              TOpendriveCylinder data=object.get_cylinder_data();
-              out_file << "    <arg name=\"model\"  value=\"cyl\"/>" << std::endl;
-              out_file << "    <arg name=\"radius\"  value=\"" << data.radius << "\"/>" << std::endl;
-              out_file << "    <arg name=\"height\"  value=\"" << data.height << "\"/>" << std::endl;
+              out_file << "    <arg name=\"model\"  value=\"box\"/>" << std::endl;
+              out_file << "    <arg name=\"length\"  value=\"" << PARKED_CAR_WIDTH << "\"/>" << std::endl;
+              out_file << "    <arg name=\"width\"  value=\"" <<  PARKED_CAR_LENGTH << "\"/>" << std::endl;
+              out_file << "    <arg name=\"height\"  value=\"" << PARKED_CAR_HEIGHT << "\"/>" << std::endl;
             }
-            world=object.get_world_pose();
-            out_file << "    <arg name=\"x\"  value=\"" << world.x << "\"/>" << std::endl;
-            out_file << "    <arg name=\"y\"  value=\"" << world.y << "\"/>" << std::endl;
-            out_file << "    <arg name=\"yaw\"  value=\"" << world.heading << "\"/>" << std::endl;
-            out_file << "    <arg name=\"parent\"  value=\"$(arg parent)\"/>" << std::endl;
-            out_file << "  </include>" << std::endl;
           }
+          else
+          {
+            TOpendriveCylinder data=object.get_cylinder_data();
+            out_file << "    <arg name=\"model\"  value=\"cyl\"/>" << std::endl;
+            out_file << "    <arg name=\"radius\"  value=\"" << data.radius << "\"/>" << std::endl;
+            out_file << "    <arg name=\"height\"  value=\"" << data.height << "\"/>" << std::endl;
+          }
+          world=object.get_world_pose();
+          out_file << "    <arg name=\"x\"  value=\"" << world.x << "\"/>" << std::endl;
+          out_file << "    <arg name=\"y\"  value=\"" << world.y << "\"/>" << std::endl;
+          out_file << "    <arg name=\"yaw\"  value=\"" << world.heading << "\"/>" << std::endl;
+          out_file << "    <arg name=\"parent\"  value=\"$(arg parent)\"/>" << std::endl;
+          out_file << "  </include>" << std::endl;
         }
+
+        if(object.is_parking_space())
+        {
+          out_file << "  -->" << std::endl;
+        }
+
       }
     }
     out_file << "</launch>" << std::endl;
     out_file.close();
   }
   else
-    throw CException(_HERE_,"Impossible to create signals launch file");
+    throw CException(_HERE_,"Impossible to create objects launch file");
 }