diff --git a/include/adc_circuit.h b/include/adc_circuit.h
index 3466d4fc581c537ccd323595f63b48aea0050297..c8a60f97981f19c1c7f015a2cfa08c3fc6b51546 100644
--- a/include/adc_circuit.h
+++ b/include/adc_circuit.h
@@ -1,6 +1,7 @@
#ifndef _ADC_CIRCUIT_H
#define _ADC_CIRCUIT_H
+#include <vector>
#include "adc_road.h"
#include <vector>
@@ -15,6 +16,8 @@ class CAdcCircuit
void clear_roads(void);
void add_road(CAdcRoad &road);
void debug(void);
+ void calculate_signals_pose(bool debug = false);
+ void get_signals(std::vector<CAdcSignals> &signals);
};
#endif
\ No newline at end of file
diff --git a/include/adc_road.h b/include/adc_road.h
index 006728da4d359087c57299fb9859ef0de7db04b8..9965ccfa02e4ff01ae27e83f57e25547d39ae8da 100644
--- a/include/adc_road.h
+++ b/include/adc_road.h
@@ -30,6 +30,8 @@ class CAdcRoad
void set_length(double length);
double get_length(void);
void debug(void);
+ void calculate_signals_pose(bool debug = false);
+ void get_signals(std::vector<CAdcSignals> &signals);
};
#endif
diff --git a/include/adc_signals.h b/include/adc_signals.h
index a5739aed70532a29c3d9af7c8e9f82780c6cd3e4..9fe046d7d1f54b0d88d06cb84bd6b1979e10ff6e 100644
--- a/include/adc_signals.h
+++ b/include/adc_signals.h
@@ -26,6 +26,9 @@ class CAdcSignals
double s;
double t;
double heading;
+ double world_x;
+ double world_y;
+ double world_yaw;
std::string type;
std::string sub_type;
int value;
@@ -38,7 +41,8 @@ class CAdcSignals
int get_id(void);
void get_pose(double &s, double &t, double &heading);
void get_sign_info(std::string &type, std::string &sub_type, int &value, std::string &text);
- void debug(void);
+ void set_world_pose(double x, double y, double yaw);
+ void debug(bool world_pose = false);
};
#endif
diff --git a/include/open_drive_format.h b/include/open_drive_format.h
index 063d3267f1add86132dfbe12f7d3fb56bbbfa2da..5825a5d7e3d68ab3ee4cb4722d583b2229c2f722 100644
--- a/include/open_drive_format.h
+++ b/include/open_drive_format.h
@@ -4,12 +4,15 @@
#include <string>
#include <vector>
#include "adc_circuit.h"
+#include "adc_signals.h"
class COpenDriveFormat
{
public:
COpenDriveFormat();
void load(std::string &filename, bool debug = false);
+ void calculate_signals_pose(bool debug = false);
+ void get_signals(std::vector<CAdcSignals> &signals);
~COpenDriveFormat();
private:
CAdcCircuit adc_circuit;
diff --git a/src/adc_circuit.cpp b/src/adc_circuit.cpp
index 1de10c830cd229861d04388c5c8f5cf0261b5e88..d84c1c58154f042a9f931c4a698d68ccf8075df8 100644
--- a/src/adc_circuit.cpp
+++ b/src/adc_circuit.cpp
@@ -27,4 +27,17 @@ void CAdcCircuit::debug(void)
{
for (unsigned int i = 0; i < this->roads.size(); i++)
this->roads[i].debug();
+}
+
+void CAdcCircuit::calculate_signals_pose(bool debug)
+{
+ for (unsigned int i = 0; i < this->roads.size(); i++)
+ this->roads[i].calculate_signals_pose(debug);
+}
+
+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);
}
\ No newline at end of file
diff --git a/src/adc_road.cpp b/src/adc_road.cpp
index 76041e7bc4b8250016e9d6e05a907863667e43f2..a71b0ebe5e7f62dd23f3c42e7a6511af7dab81cc 100644
--- a/src/adc_road.cpp
+++ b/src/adc_road.cpp
@@ -82,4 +82,33 @@ void CAdcRoad::debug(void)
this->geometries[i]->debug();
for (unsigned int i = 0; i < this->signals.size(); i++)
this->signals[i].debug();
+}
+
+void CAdcRoad::calculate_signals_pose(bool debug)
+{
+ double s, t, heading, x, y, yaw;
+ for (unsigned int i = 0; i < this->signals.size(); i++)
+ {
+ this->signals[i].get_pose(s, t, heading);
+ for (unsigned int j = 0; j < this->geometries.size(); j++)
+ {
+ if (this->geometries[j]->on_range(s))
+ {
+ this->geometries[j]->get_world_pose(s, t, heading, x, y, yaw);
+ this->signals[i].set_world_pose(x, y, yaw);
+ break;
+ }
+ }
+ if (debug)
+ {
+ std::cout << "Road " << this->name << " id = " << this->id << "; length = " << this->length << std::endl;
+ this->signals[i].debug(debug);
+ }
+ }
+}
+
+void CAdcRoad::get_signals(std::vector<CAdcSignals> &signals)
+{
+ for (unsigned int i = 0; i < this->signals.size(); i++)
+ signals.push_back(this->signals[i]);
}
\ No newline at end of file
diff --git a/src/adc_signals.cpp b/src/adc_signals.cpp
index 1de4de82b567dfcc04a9845d5128a63eacd74e10..4c2875515dfc4f041a1fb4c7b77405bfaa2ead49 100644
--- a/src/adc_signals.cpp
+++ b/src/adc_signals.cpp
@@ -31,7 +31,7 @@ CAdcSignals::CAdcSignals(int id, double s, double t, double heading, std::string
this->id = id;
this->s = s;
this->t = t;
- this->heading = heading + (reverse ? 0.0 : M_PI);
+ this->heading = heading + (reverse ? M_PI : 0.0);
this->type = type;
this->sub_type = sub_type;
this->value = value;
@@ -54,9 +54,18 @@ void CAdcSignals::get_sign_info(std::string &type, std::string &sub_type, int &v
text = this->text;
}
-void CAdcSignals::debug(void)
+void CAdcSignals::set_world_pose(double x, double y, double yaw)
+{
+ this->world_x = x;
+ this->world_y = y;
+ this->world_yaw = yaw;
+}
+
+void CAdcSignals::debug(bool world_pose)
{
std::cout << " Signal id = " << this->id << "; type = " << this->type << ", subtype = " << this->sub_type << std::endl;
std::cout << " value = " << this->value << "; text = " << this->text << std::endl;
std::cout << " pose: s = " << this->s << ", t = " << this->t << ", heading = " << this->heading << std::endl;
+ if (world_pose)
+ std::cout << " world pose: x = " << this->world_x << ", y = " << this->world_y << ", yaw = " << this->world_yaw << std::endl;
}
diff --git a/src/examples/open_drive_format_test.cpp b/src/examples/open_drive_format_test.cpp
index 96542a3fc4bd96ac6c222663e9e72a180f301099..f3ee7397509ed679983aa5e7ac6ec0d18aa3533e 100644
--- a/src/examples/open_drive_format_test.cpp
+++ b/src/examples/open_drive_format_test.cpp
@@ -1,6 +1,8 @@
#include "open_drive_format.h"
#include "exceptions.h"
#include <iostream>
+#include <vector>
+#include "adc_signals.h"
int main(int argc, char *argv[])
{
@@ -9,7 +11,13 @@ int main(int argc, char *argv[])
// std::string xml_file = "../src/xml/atlatec_vires.xodr";
try
{
- open_drive_format.load(xml_file, true);
+ open_drive_format.load(xml_file, false);
+ 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);
}
catch (CException &e)
{
diff --git a/src/open_drive_format.cpp b/src/open_drive_format.cpp
index 5a3b049fd2bfe18a2aab2ba42fb16781f19af284..b70d22f08f89d5e3fc1454cc5689d014387fe0fe 100644
--- a/src/open_drive_format.cpp
+++ b/src/open_drive_format.cpp
@@ -124,7 +124,7 @@ void COpenDriveFormat::load(std::string &filename, bool debug)
}
if (debug)
- adc_circuit.debug();
+ this->adc_circuit.debug();
std::cout << "Done." << std::endl;
}catch (const xml_schema::exception& e){
std::ostringstream os;
@@ -137,6 +137,16 @@ void COpenDriveFormat::load(std::string &filename, bool debug)
throw CException(_HERE_,"The configuration file does not exist");
}
+void COpenDriveFormat::calculate_signals_pose(bool debug)
+{
+ this->adc_circuit.calculate_signals_pose(debug);
+}
+
+void COpenDriveFormat::get_signals(std::vector<CAdcSignals> &signals)
+{
+ this->adc_circuit.get_signals(signals);
+}
+
// void COpenDriveFormat::load(std::string &filename)
// {
// struct stat buffer;