diff --git a/include/adc_circuit.h b/include/adc_circuit.h index c8a60f97981f19c1c7f015a2cfa08c3fc6b51546..9189e8d09e79cae43e0a7a8f4e6cd010df3723a2 100644 --- a/include/adc_circuit.h +++ b/include/adc_circuit.h @@ -1,6 +1,7 @@ #ifndef _ADC_CIRCUIT_H #define _ADC_CIRCUIT_H +#include <string> #include <vector> #include "adc_road.h" #include <vector> @@ -18,6 +19,7 @@ class CAdcCircuit void debug(void); void calculate_signals_pose(bool debug = false); void get_signals(std::vector<CAdcSignals> &signals); + void generate_spwan_launch_file(std::string &filename); }; #endif \ No newline at end of file diff --git a/include/adc_road.h b/include/adc_road.h index 9965ccfa02e4ff01ae27e83f57e25547d39ae8da..c3ca57130140cfb308907f5c4d019169c2490ab9 100644 --- a/include/adc_road.h +++ b/include/adc_road.h @@ -32,6 +32,7 @@ class CAdcRoad void debug(void); void calculate_signals_pose(bool debug = false); void get_signals(std::vector<CAdcSignals> &signals); + void append_signs_spawn(std::string &filename); }; #endif diff --git a/include/adc_signals.h b/include/adc_signals.h index 9fe046d7d1f54b0d88d06cb84bd6b1979e10ff6e..288553e9370d1a0059e4e9d02f2d96980969a44f 100644 --- a/include/adc_signals.h +++ b/include/adc_signals.h @@ -1,23 +1,46 @@ #ifndef _ADC_SIGNALS_H #define _ADC_SIGNALS_H #include <string> +#include <map> -// UNMARKEDINTERSECTION 102 -// STOPANDGIVEWAY 206 -// PARKINGAREA ??? -// HAVEWAY ??? -// AHEADONLY 209 -// AHEADONLYSUB 30 -// GIVEWAY 205 -// PEDESTRIANCROSSING ??? -// ROUNDABOUT ??? -// NOOVERTAKING 276 -// NOENTRYVEHICULARTRAFFIC 267 -// TESTCOURSEA9 ??? -// ONEWAYSTREET 220 -// ROADWORKS 123 -// KMH50 274 -// KMH100 274 +#define UNMARKEDINTERSECTION_TYPE "102" +#define UNMARKEDINTERSECTION_MARKER "alvar0" +#define STOPANDGIVEWAY_TYPE "206" +#define STOPANDGIVEWAY_MARKER "alvar1" +#define PARKINGAREA_TYPE "???" +#define PARKINGAREA_MARKER "alvar2" +#define HAVEWAY_TYPE "???" +#define HAVEWAY_MARKER "alvar3" +#define AHEADONLY_TYPE "209" +#define AHEADONLY_SUB_TYPE "30" +#define AHEADONLY_MARKER "alvar4" +#define GIVEWAY_TYPE "205" +#define GIVEWAY_MARKER "alvar5" +#define PEDESTRIANCROSSING_TYPE "???" +#define PEDESTRIANCROSSING_MARKER "alvar6" +#define ROUNDABOUT_TYPE "???" +#define ROUNDABOUT_MARKER "alvar7" +#define NOOVERTAKING_TYPE "276" +#define NOOVERTAKING_MARKER "alvar8" +#define NOENTRYVEHICULARTRAFFIC_TYPE "267" +#define NOENTRYVEHICULARTRAFFIC_MARKER "alvar9" +#define TESTCOURSEA9_TYPE "???" +#define TESTCOURSEA9_MARKER "alvar10" +#define ONEWAYSTREET_TYPE "220" +#define ONEWAYSTREET_MARKER "alvar11" +#define ROADWORKS_TYPE "123" +#define ROADWORKS_MARKER "alvar12" +#define MAX_VEL_TYPE "274" +#define MAX_VEL_50_MARKER "alvar13" +#define MAX_VEL_100_MARKER "alvar14" +#define SEMAPHORE_TYPE "1000001" +#define SEMAPHORE_MARKER "alvar15" + +typedef struct +{ + std::string type; + std::string marker; +}Sign_urdf_info; class CAdcSignals { @@ -33,6 +56,8 @@ class CAdcSignals std::string sub_type; int value; std::string text; + static std::map<std::string, Sign_urdf_info> urdf_map; + static void set_urdf_map(void); public: CAdcSignals(); CAdcSignals(int id, double s, double t, double heading, std::string type, std::string sub_type, int value, std::string text, bool reverse = false); @@ -43,6 +68,7 @@ class CAdcSignals void get_sign_info(std::string &type, std::string &sub_type, int &value, std::string &text); void set_world_pose(double x, double y, double yaw); void debug(bool world_pose = false); + void append_sign_spawn(std::string &filename); }; #endif diff --git a/include/open_drive_format.h b/include/open_drive_format.h index 5825a5d7e3d68ab3ee4cb4722d583b2229c2f722..0fa1aa0721eae2d76bdb61192b2d64860d55cc30 100644 --- a/include/open_drive_format.h +++ b/include/open_drive_format.h @@ -13,6 +13,7 @@ class COpenDriveFormat void load(std::string &filename, bool debug = false); void calculate_signals_pose(bool debug = false); void get_signals(std::vector<CAdcSignals> &signals); + void generate_spwan_launch_file(std::string &filename); ~COpenDriveFormat(); private: CAdcCircuit adc_circuit; diff --git a/src/adc_circuit.cpp b/src/adc_circuit.cpp index d84c1c58154f042a9f931c4a698d68ccf8075df8..d483a8801bbcb64fd95637a908c4743c7783fb1f 100644 --- a/src/adc_circuit.cpp +++ b/src/adc_circuit.cpp @@ -1,5 +1,8 @@ #include "adc_circuit.h" - +#include <sstream> +#include <iostream> +#include <fstream> +#include "exceptions.h" CAdcCircuit::CAdcCircuit() { @@ -40,4 +43,37 @@ void CAdcCircuit::get_signals(std::vector<CAdcSignals> &signals) std::vector<CAdcSignals>().swap(signals); for (unsigned int i = 0; i < this->roads.size(); i++) this->roads[i].get_signals(signals); +} + +void CAdcCircuit::generate_spwan_launch_file(std::string &filename) +{ + std::remove(filename.c_str()); + //First the header + std::ostringstream oss; + oss << "<?xml version=\"1.0\"?>" << std::endl << std::endl; + oss << "<launch>" << std::endl; + oss << " <arg name=\"parent\" default=\"map\"/>" << std::endl; + oss << " <arg name=\"rviz\" default=\"false\"/>" << std::endl; + + std::ofstream out_file; + out_file.open(filename.c_str(), std::ios_base::app); + if (!out_file.is_open()) + throw CException(_HERE_,"CAdcCircuit::generate_spwan_launch_file -> The output file can't be open"); + out_file << oss.str() << std::endl; + out_file.close(); + + //sign info + for (unsigned int i = 0; i < this->roads.size(); i++) + this->roads[i].append_signs_spawn(filename); + + //End + oss.str(""); + oss.clear(); + oss << std::endl << "</launch>" << std::endl; + out_file.open(filename.c_str(), std::ios_base::app); + if (!out_file.is_open()) + throw CException(_HERE_,"CAdcCircuit::generate_spwan_launch_file -> The output file can't be open"); + out_file << oss.str() << std::endl; + out_file.close(); + } \ No newline at end of file diff --git a/src/adc_road.cpp b/src/adc_road.cpp index a71b0ebe5e7f62dd23f3c42e7a6511af7dab81cc..cf5d4d10ebcf0fd431121daa143fc8762ff42708 100644 --- a/src/adc_road.cpp +++ b/src/adc_road.cpp @@ -111,4 +111,10 @@ void CAdcRoad::get_signals(std::vector<CAdcSignals> &signals) { for (unsigned int i = 0; i < this->signals.size(); i++) signals.push_back(this->signals[i]); +} + +void CAdcRoad::append_signs_spawn(std::string &filename) +{ + for (unsigned int i = 0; i < this->signals.size(); i++) + this->signals[i].append_sign_spawn(filename); } \ No newline at end of file diff --git a/src/adc_signals.cpp b/src/adc_signals.cpp index 4c2875515dfc4f041a1fb4c7b77405bfaa2ead49..c61f8e9a639087dac325513a93f0d39440a5f543 100644 --- a/src/adc_signals.cpp +++ b/src/adc_signals.cpp @@ -1,6 +1,11 @@ #include "adc_signals.h" #include <cmath> #include <iostream> +#include <sstream> +#include <fstream> +#include "exceptions.h" + +std::map<std::string, Sign_urdf_info> CAdcSignals::urdf_map; CAdcSignals::CAdcSignals() { @@ -8,10 +13,14 @@ CAdcSignals::CAdcSignals() this->s = 0; this->t = 0; this->heading = 0; + this->world_x = 0; + this->world_y = 0; + this->world_yaw = 0; this->type = ""; this->sub_type = ""; this->value = 0; this->text = ""; + set_urdf_map(); } CAdcSignals::~CAdcSignals() @@ -20,6 +29,9 @@ CAdcSignals::~CAdcSignals() this->s = 0; this->t = 0; this->heading = 0; + this->world_x = 0; + this->world_y = 0; + this->world_yaw = 0; this->type = ""; this->sub_type = ""; this->value = 0; @@ -36,6 +48,73 @@ CAdcSignals::CAdcSignals(int id, double s, double t, double heading, std::string this->sub_type = sub_type; this->value = value; this->text = text; + this->world_x = 0; + this->world_y = 0; + this->world_yaw = 0; + set_urdf_map(); +} +void CAdcSignals::set_urdf_map(void) +{ + if (urdf_map.size() == 0) + { + Sign_urdf_info urdf_info; + urdf_info.type = "unmarked_intersection"; + urdf_info.marker = UNMARKEDINTERSECTION_MARKER; + urdf_map[UNMARKEDINTERSECTION_TYPE] = urdf_info; + urdf_info.type = "stop"; + urdf_info.marker = STOPANDGIVEWAY_MARKER; + urdf_map[STOPANDGIVEWAY_TYPE] = urdf_info; + urdf_info.type = "parking"; + urdf_info.marker = PARKINGAREA_MARKER; + urdf_map[PARKINGAREA_TYPE] = urdf_info; + urdf_info.type = "haveway"; + urdf_info.marker = HAVEWAY_MARKER; + urdf_map[HAVEWAY_TYPE] = urdf_info; + urdf_info.type = "ahead_only"; + urdf_info.marker = AHEADONLY_MARKER; + std::stringstream ss; + ss << AHEADONLY_TYPE << "." << AHEADONLY_SUB_TYPE; + urdf_map[ss.str()] = urdf_info; + urdf_info.type = "giveway"; + urdf_info.marker = GIVEWAY_MARKER; + urdf_map[GIVEWAY_TYPE] = urdf_info; + urdf_info.type = "pedestrian_crossing"; + urdf_info.marker = PEDESTRIANCROSSING_MARKER; + urdf_map[PEDESTRIANCROSSING_TYPE] = urdf_info; + urdf_info.type = "roundabout"; + urdf_info.marker = ROUNDABOUT_MARKER; + urdf_map[ROUNDABOUT_TYPE] = urdf_info; + urdf_info.type = "no_overtaking"; + urdf_info.marker = NOOVERTAKING_MARKER; + urdf_map[NOOVERTAKING_TYPE] = urdf_info; + urdf_info.type = "no_entry"; + urdf_info.marker = NOENTRYVEHICULARTRAFFIC_MARKER; + urdf_map[NOENTRYVEHICULARTRAFFIC_TYPE] = urdf_info; + urdf_info.type = "test_course"; + urdf_info.marker = TESTCOURSEA9_MARKER; + urdf_map[TESTCOURSEA9_TYPE] = urdf_info; + urdf_info.type = "one_way"; + urdf_info.marker = ONEWAYSTREET_MARKER; + urdf_map[ONEWAYSTREET_TYPE] = urdf_info; + urdf_info.type = "roadworks"; + urdf_info.marker = ROADWORKS_MARKER; + urdf_map[ROADWORKS_TYPE] = urdf_info; + urdf_info.type = "max_50"; + urdf_info.marker = MAX_VEL_50_MARKER; + ss.str(""); + ss.clear(); + ss << MAX_VEL_TYPE << ".50"; + urdf_map[ss.str()] = urdf_info; + urdf_info.type = "max_100"; + urdf_info.marker = MAX_VEL_100_MARKER; + ss.str(""); + ss.clear(); + ss << MAX_VEL_TYPE << ".100"; + urdf_map[ss.str()] = urdf_info; + urdf_info.type = "semaphore"; + urdf_info.marker = SEMAPHORE_MARKER; + urdf_map[SEMAPHORE_TYPE] = urdf_info; + } } void CAdcSignals::get_pose(double &s, double &t, double &heading) @@ -69,3 +148,43 @@ void CAdcSignals::debug(bool world_pose) if (world_pose) std::cout << " world pose: x = " << this->world_x << ", y = " << this->world_y << ", yaw = " << this->world_yaw << std::endl; } + +void CAdcSignals::append_sign_spawn(std::string &filename) +{ + std::map<std::string, Sign_urdf_info>::iterator it = urdf_map.end(); + + std::stringstream ss; + if (this->type == AHEADONLY_TYPE) + ss << AHEADONLY_TYPE << "." << AHEADONLY_SUB_TYPE; + else if (this->type == MAX_VEL_TYPE) + ss << MAX_VEL_TYPE << "." << this->value; + else + ss << this->type; + it = urdf_map.find(ss.str()); + if (it == urdf_map.end()) + { + std::cout << "WARNING: Unknown signal type " << this->type << std::endl; + return; + } + + + std::ostringstream oss; + oss << std::endl << " <include file=\"$(find sign_description)/launch/spwan.launch\">" << std::endl; + oss << " <arg name=\"name\" value=\"" << it->second.type << this->id << "\"/>" << std::endl; + oss << " <arg name=\"model\" value=\"" << (this->type == SEMAPHORE_TYPE ? "semaphore": "sign") << "\"/>" << std::endl; + oss << " <arg name=\"tag\" value=\"" << it->second.marker << "\"/>" << std::endl; + oss << " <arg name=\"type\" value=\"" << it->second.type << "\"/>" << std::endl; + oss << " <arg name=\"x\" value=\"" << this->world_x << "\"/>" << std::endl; + oss << " <arg name=\"y\" value=\"" << this->world_y << "\"/>" << std::endl; + oss << " <arg name=\"yaw\" value=\"" << this->world_yaw << "\"/>" << std::endl; + oss << " <arg name=\"parent\" value=\"$(arg parent)\"/>" << std::endl; + oss << " <arg name=\"rviz\" value=\"$(arg rviz)\"/>" << std::endl; + oss << " </include>" << std::endl; + std::ofstream out_file; + out_file.open(filename.c_str(), std::ios_base::app); + if (!out_file.is_open()) + throw CException(_HERE_,"CAdcCircuit::generate_spwan_launch_file -> The output file can't be open"); + out_file << oss.str() << std::endl; + out_file.close(); + +} \ No newline at end of file diff --git a/src/examples/open_drive_format_test.cpp b/src/examples/open_drive_format_test.cpp index f3ee7397509ed679983aa5e7ac6ec0d18aa3533e..704c1058db4d7a2c7e5853f836359823c3e93eba 100644 --- a/src/examples/open_drive_format_test.cpp +++ b/src/examples/open_drive_format_test.cpp @@ -8,6 +8,7 @@ int main(int argc, char *argv[]) { COpenDriveFormat open_drive_format; std::string xml_file = "../src/xml/atlatec_generic.xodr"; + std::string launch_file = "../spawn_signs.launch"; // std::string xml_file = "../src/xml/atlatec_vires.xodr"; try { @@ -15,9 +16,10 @@ int main(int argc, char *argv[]) open_drive_format.calculate_signals_pose(false); std::vector<CAdcSignals> signals; - open_drive_format.get_signals(signals); - for (unsigned int i = 0; i < signals.size(); i++) - signals[i].debug(true); + // open_drive_format.get_signals(signals); + // for (unsigned int i = 0; i < signals.size(); i++) + // signals[i].debug(true); + open_drive_format.generate_spwan_launch_file(launch_file); } catch (CException &e) { diff --git a/src/open_drive_format.cpp b/src/open_drive_format.cpp index b70d22f08f89d5e3fc1454cc5689d014387fe0fe..a17ba5b24c56e1ade4dcc24b68f3d7259391e2fd 100644 --- a/src/open_drive_format.cpp +++ b/src/open_drive_format.cpp @@ -22,6 +22,7 @@ COpenDriveFormat::COpenDriveFormat() void COpenDriveFormat::load(std::string &filename, bool debug) { + std::cout << "Loading .xodr info..." << std::endl; struct stat buffer; if(stat(filename.c_str(),&buffer)==0) @@ -123,9 +124,9 @@ void COpenDriveFormat::load(std::string &filename, bool debug) this->adc_circuit.add_road(road); } + std::cout << "Loading .xodr info done." << std::endl; if (debug) this->adc_circuit.debug(); - std::cout << "Done." << std::endl; }catch (const xml_schema::exception& e){ std::ostringstream os; os << e; @@ -139,7 +140,9 @@ void COpenDriveFormat::load(std::string &filename, bool debug) void COpenDriveFormat::calculate_signals_pose(bool debug) { + std::cout << "Calculating world signs poses..." << std::endl; this->adc_circuit.calculate_signals_pose(debug); + std::cout << "Calculating world signs poses done" << std::endl; } void COpenDriveFormat::get_signals(std::vector<CAdcSignals> &signals) @@ -147,6 +150,12 @@ void COpenDriveFormat::get_signals(std::vector<CAdcSignals> &signals) this->adc_circuit.get_signals(signals); } +void COpenDriveFormat::generate_spwan_launch_file(std::string &filename) +{ + std::cout << "Generating " << filename << " ..." << std::endl; + this->adc_circuit.generate_spwan_launch_file(filename); + std::cout << "Generating " << filename << " done" << std::endl; +} // void COpenDriveFormat::load(std::string &filename) // { // struct stat buffer;