From 0952fb6d6807cc2651c8a09fe29261aa1cbc106d Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Mon, 27 Apr 2020 19:03:30 +0200
Subject: [PATCH] Added calculate_signals_pose and get_signals
---
include/adc_circuit.h | 3 +++
include/adc_road.h | 2 ++
include/adc_signals.h | 6 ++++-
include/open_drive_format.h | 3 +++
src/adc_circuit.cpp | 13 +++++++++++
src/adc_road.cpp | 29 +++++++++++++++++++++++++
src/adc_signals.cpp | 13 +++++++++--
src/examples/open_drive_format_test.cpp | 10 ++++++++-
src/open_drive_format.cpp | 12 +++++++++-
9 files changed, 86 insertions(+), 5 deletions(-)
diff --git a/include/adc_circuit.h b/include/adc_circuit.h
index 3466d4f..c8a60f9 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 006728d..9965ccf 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 a5739ae..9fe046d 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 063d326..5825a5d 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 1de10c8..d84c1c5 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 76041e7..a71b0eb 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 1de4de8..4c28755 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 96542a3..f3ee739 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 5a3b049..b70d22f 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;
--
GitLab