diff --git a/include/generate_launch.h b/include/generate_launch.h
index ca4da82840dd105f3927af3770459286f9b5c0af..9714ef4b33bd53d2b2ba322c84e19f64a8be146a 100644
--- a/include/generate_launch.h
+++ b/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
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index a2a7f7c0851616671a85345810b6ba4c274a69e3..c1d983598c1dc378534c2178ecac7447b662793b 100644
--- a/src/CMakeLists.txt
+++ b/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})
diff --git a/src/generate_launch.cpp b/src/generate_launch.cpp
index b1c4170a0bb840580326d712cf23128a8cd240d5..c6ce0a5d25e6f8b9707c4302346e356d582b1898 100644
--- a/src/generate_launch.cpp
+++ b/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");
}