Added generate_spwan_launch_file

include/adc_circuit.h

 #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

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

include/adc_signals.h

 #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

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;

src/adc_circuit.cpp

 #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

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

src/adc_signals.cpp

 #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

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)
   {

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;