diff --git a/include/adc_circuit.h b/include/adc_circuit.h index 05dfe7164d552b39ce7039dbacfa2ef22fee4c55..1dd45972a7d1eac536d8d67b13a3954163a8133f 100644 --- a/include/adc_circuit.h +++ b/include/adc_circuit.h @@ -13,7 +13,6 @@ #define SIGNS_LAUNCH_FILE "spawn_signs.launch" #define OBJECTS_LAUNCH_FILE "spawn_objects.launch" -#define ROAD_SCALE 10 /** * \class CAdcCircuit @@ -27,6 +26,7 @@ class CAdcCircuit { private: std::vector<CAdcRoad*> roads;///< Variable to store an access each road data. + double road_scale;///< The scale of the road to divide by the objects and signals poses. /** * \brief Function to add a road. @@ -108,6 +108,14 @@ class CAdcCircuit * */ void load(std::unique_ptr<OpenDRIVE> &open_drive); + + /** + * \brief Function to set road_Scale. + * + * \param road_scale The scale of the road to divide by the objects and signals poses. + * + */ + void set_road_scale(double road_scale=1.0); }; #endif diff --git a/include/opendrive_to_gazebo.h b/include/opendrive_to_gazebo.h index 93f3239e241f9d1abb4163753cf518cfe4911024..dd9b24f356a3303426186bace3235ba9db152a95 100644 --- a/include/opendrive_to_gazebo.h +++ b/include/opendrive_to_gazebo.h @@ -18,9 +18,11 @@ class COpenDriveFormat public: /** * \brief Default constructor. + * + * \param road_scale The scale of the road to divide by the objects and signals poses. * */ - COpenDriveFormat(); + COpenDriveFormat(double road_scale); /** * \brief Function to store on adc_circuit the useful information of a .xodr file. diff --git a/src/adc_circuit.cpp b/src/adc_circuit.cpp index 037482bc154cba24e6fef2bb31111a06d0deccfc..fbfa272c7b6026582282ad52a90132ce54f125e8 100644 --- a/src/adc_circuit.cpp +++ b/src/adc_circuit.cpp @@ -6,11 +6,12 @@ CAdcCircuit::CAdcCircuit() { - + this->road_scale = 1.0; } CAdcCircuit::~CAdcCircuit() { + this->road_scale = 1.0; clear_roads(); } @@ -77,11 +78,11 @@ void CAdcCircuit::generate_spawn_launch_files(std::string &dir) //sign info for (unsigned int i = 0; i < this->roads.size(); i++) - this->roads[i]->append_signs_spawn(signs_filename, ROAD_SCALE); + this->roads[i]->append_signs_spawn(signs_filename, this->road_scale); //object info for (unsigned int i = 0; i < this->roads.size(); i++) - this->roads[i]->append_objects_spawn(objects_filename, ROAD_SCALE); + this->roads[i]->append_objects_spawn(objects_filename, this->road_scale); //End oss.str(""); @@ -112,3 +113,8 @@ void CAdcCircuit::load(std::unique_ptr<OpenDRIVE> &open_drive) add_road(road); } } + +void CAdcCircuit::set_road_scale(double road_scale) +{ + this->road_scale = road_scale; +} diff --git a/src/examples/opendrive_to_gazebo_test.cpp b/src/examples/opendrive_to_gazebo_test.cpp index ae20ae6358e2c513b7927152fd7eabcfb301b0e3..38152678048b40bdb8974eeabd48cbe264f1f605 100644 --- a/src/examples/opendrive_to_gazebo_test.cpp +++ b/src/examples/opendrive_to_gazebo_test.cpp @@ -3,25 +3,43 @@ #include <iostream> #include <vector> #include "adc_signals.h" +#include <stdexcept> int main(int argc, char *argv[]) { - if (argc != 3) + if (argc != 3 && argc != 4) { std::cerr << "ERROR: Missing input arguments.The usage of this program is:" << std::endl; - std::cerr << "opendrive_to_gazebo_test <xodr input file path> <output launch file direcctory>" << std::endl; + std::cerr << "opendrive_to_gazebo_test <xodr input file path> <output launch file direcctory> [optional]<road_scale>" << std::endl; return -1; } - COpenDriveFormat open_drive_format; std::string xml_file = argv[1]; std::string launch_dir = argv[2]; + double road_scale; int slash_index, point_index; slash_index = xml_file.find_last_of("/"); point_index = xml_file.find_last_of("."); std::string file_name = xml_file.substr(xml_file.find_last_of("/")+1, point_index - slash_index -1); - std::cout << "file name " << file_name << std::endl; std::string launch_base_name = launch_dir + file_name; + if (argc == 4) + { + try + { + road_scale = std::stod(argv[3]); + } + catch (const std::invalid_argument& e) + { + std::cerr << "ERROR: Invalid road_scale, it must be a double: " << e.what() << std::endl; + return -1; + } + } + else + { + road_scale = 1.0; + } + COpenDriveFormat open_drive_format(road_scale); + try { open_drive_format.load(xml_file, false); diff --git a/src/opendrive_to_gazebo.cpp b/src/opendrive_to_gazebo.cpp index 5bc308b8f9fca8dd0ca99bdcdd897a70ddd94ba6..5ee7c8a4a8a62dd17faa2091016c99ae4609d05b 100644 --- a/src/opendrive_to_gazebo.cpp +++ b/src/opendrive_to_gazebo.cpp @@ -16,8 +16,9 @@ #include "xml/OpenDRIVE_1.4H.hxx" #endif -COpenDriveFormat::COpenDriveFormat() +COpenDriveFormat::COpenDriveFormat(double road_scale) { + adc_circuit.set_road_scale(road_scale); } void COpenDriveFormat::load(std::string &filename, bool debug)