diff --git a/include/generate_launch.h b/include/generate_launch.h
index ca4da82840dd105f3927af3770459286f9b5c0af..3dbbda01229fa82d2d504f244b7fe5fb914e972a 100644
--- a/include/generate_launch.h
+++ b/include/generate_launch.h
@@ -5,9 +5,12 @@
#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);
+void generate_parkings_yaml(std::string &path,std::string &parkings_file,COpendriveRoad &road);
#define UNMARKEDINTERSECTION_TYPE "102"
#define UNMARKEDINTERSECTION_MARKER "alvar0"
@@ -55,4 +58,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..3d7eaf2ac6672e5e0295f45918ef0e89d1b7fc8a 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,87 @@ 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");
+}
+
+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;
}