diff --git a/include/adc_circuit.h b/include/adc_circuit.h
index 20f9ff560b83f9bf712319d812032b05cf54b0d1..a17de92b432f4bb1ec79507cbfd105471d9e90e3 100644
--- a/include/adc_circuit.h
+++ b/include/adc_circuit.h
@@ -62,14 +62,14 @@ class CAdcCircuit
void debug(void);
/**
- * \brief Function to calculate the world pose of each sign.
+ * \brief Function to calculate the world pose of each sign and object.
*
- * It calls CAdcRoad::calculate_signals_pose function of each road.
+ * It calls CAdcRoad::calculate_signals_pose and CAdcRoad::calculate_objects_pose function of each road.
*
- * \param debug Boolean to print all the information stored on each signal at
+ * \param debug Boolean to print all the information stored on each signal nad object at
* the end of the function.
*/
- void calculate_signals_pose(bool debug = false);
+ void calculate_world_poses(bool debug = false);
/**
* \brief Function to get all the signs presents on circuit.
diff --git a/include/adc_object.h b/include/adc_object.h
new file mode 100644
index 0000000000000000000000000000000000000000..d7d87b3589489c5e1daaa7511176fda569951c14
--- /dev/null
+++ b/include/adc_object.h
@@ -0,0 +1,231 @@
+#ifndef _ADC_OBJECT_H
+#define _ADC_OBJECT_H
+#include <string>
+#include <map>
+
+#ifdef _HAVE_XSD
+#include "../src/xml/OpenDRIVE_1.4H.hxx"
+#endif
+
+
+/**
+ * \struct Object_corner.
+ *
+ * \brief Corner "s" and "t" coordenates with height.
+ */
+typedef struct
+{
+ double s;
+ double t;
+ double world_x;
+ double world_y;
+ double height;
+}Object_corner;
+
+/**
+ * \class CAdcObject
+ *
+ * \brief Class to store the data of a Object.
+ *
+ */
+
+class CAdcObject
+{
+ private:
+ int id; ///< Object's id.
+ double s; ///< Object's track coordenate "s".
+ double t; ///< Object's track coordenate "t".
+ double heading; ///< Object's track heading.
+ double world_x; ///< Object's world coordenate "x".
+ double world_y; ///< Object's world coordenate "y".
+ double world_yaw; ///< Object's world yaw.
+ double valid_length; ///< Object's valid length.
+ double length; ///< Object's length.
+ double width; ///< Object's width.
+ double radius; ///< Object's radius.
+ double height; ///< Object's height.
+ std::string type; ///< Object's OpenDrive type.
+ std::string name; ///< Object's name.
+ bool outline; ///< Boolean to know if a object is an outline.
+ std::vector<Object_corner> corners; ///< Outline object corners.
+
+ public:
+ /**
+ * \brief Default constructor.
+ *
+ *
+ */
+ CAdcObject();
+
+ /**
+ * \brief Default constructor.
+ *
+ *
+ */
+ CAdcObject(const CAdcObject &Object);
+
+ /**
+ * \brief Class constructor for a "box" object.
+ *
+ *
+ * \param id Object's id.
+ *
+ * \param s Object's track coordenate "s".
+ *
+ * \param t Object's track coordenate "t".
+ *
+ * \param heading Object's track heading.
+ *
+ * \param type Object's OpenDrive type.
+ *
+ * \param name Object's OpenDrive name.
+ *
+ * \param width Object's width.
+ *
+ * \param length Object's length.
+ *
+ * \param height Object's height.
+ *
+ * \param valid_length Object's valid_length.
+ *
+ */
+ CAdcObject(int id, double s, double t, double heading, std::string type, std::string name, double width, double length, double height, double valid_length);
+
+ /**
+ * \brief Class constructor for a "Cylinder" object.
+ *
+ *
+ * \param id Object's id.
+ *
+ * \param s Object's track coordenate "s".
+ *
+ * \param t Object's track coordenate "t".
+ *
+ * \param heading Object's track heading.
+ *
+ * \param type Object's OpenDrive type.
+ *
+ * \param name Object's OpenDrive name.
+ *
+ * \param radius Object's radius.
+ *
+ * \param height Object's height.
+ *
+ * \param valid_length Object's valid_length.
+ *
+ */
+ CAdcObject(int id, double s, double t, double heading, std::string type, std::string name, double radius, double height, double valid_length);
+
+ /**
+ * \brief Class constructor for a "Cylinder" object.
+ *
+ *
+ * \param id Object's id.
+ *
+ * \param s Object's track coordenate "s".
+ *
+ * \param t Object's track coordenate "t".
+ *
+ * \param heading Object's track heading.
+ *
+ * \param type Object's OpenDrive type.
+ *
+ * \param name Object's OpenDrive name.
+ *
+ * \param valid_length Object's valid_length.
+ *
+ * \param corners Object's corners.
+ *
+ */
+ CAdcObject(int id, double s, double t, double heading, std::string type, std::string name, double valid_length, std::vector<Object_corner> &corners);
+
+ /**
+ * \brief Default destructor.
+ *
+ */
+ ~CAdcObject();
+
+ /**
+ * \brief assign operator = overload
+ *
+ */
+ void operator = (const CAdcObject &Object);
+
+ /**
+ * \brief Function to get Object's track pose.
+ *
+ * \param s Output Object's track coordenate "s".
+ *
+ * \param t Output Object's track coordenate "t".
+ *
+ * \param heading Output Object's track heading.
+ *
+ */
+ void get_pose(double &s, double &t, double &heading);
+
+ /**
+ * \brief Function to set Object's world pose.
+ *
+ * \param x Object's world coordenate "x".
+ *
+ * \param y Object's world coordenate "y".
+ *
+ * \param yaw Object's world yaw.
+ *
+ */
+ void set_world_pose(double x, double y, double yaw);
+
+ /**
+ * \brief Function to add Object's corner world pose.
+ *
+ * \param index Corner index
+ *
+ * \param x Corner's world coordenate "x".
+ *
+ * \param y Corner's world coordenate "y".
+ *
+ */
+ void add_corner_world_pose(unsigned int index, double x, double y);
+
+ /**
+ * \brief Function to get Object's corners.
+ *
+ * \param corners Corners vector.
+ *
+ */
+ void get_corners(std::vector<Object_corner> &corners);
+
+ /**
+ * \brief Function to know if an objects is a outline.
+ *
+ * \return Outline value.
+ *
+ */
+ bool is_outline(void);
+
+ /**
+ * \brief Function to print its info.
+ *
+ * \param world_pose To print world pose or not.
+ *
+ */
+ void debug(bool world_pose = false);
+
+ /**
+ * \brief Function to append each object spwan lines on a .launch file.
+ *
+ * \param filename The route to the output .launch file.
+ *
+ */
+ void append_object_spawn(std::string &filename);
+
+ /**
+ * \brief Function to load the .xodr info.
+ *
+ * \param object_info The object generated with XSD.
+ *
+ */
+ void load(std::auto_ptr<objects::object_type> &object_info);
+};
+
+#endif
diff --git a/include/adc_road.h b/include/adc_road.h
index a69d0aa0c28e278f5c6732e606502bc1ae441e2c..a5b691b01c855293d994227dc0431b60eaaff3ac 100644
--- a/include/adc_road.h
+++ b/include/adc_road.h
@@ -5,6 +5,7 @@
#include <string>
#include "adc_geometries.h"
#include "adc_signals.h"
+#include "adc_object.h"
#ifdef _HAVE_XSD
#include "../src/xml/OpenDRIVE_1.4H.hxx"
@@ -25,6 +26,7 @@ class CAdcRoad
double length;///< Road's length.
std::vector<CAdcGeometry*> geometries;///< Road's geometries.
std::vector<CAdcSignals*> signals; ///< Road's signals.
+ std::vector<CAdcObject*> objects; ///< Road's objects.
/**
* \brief Function to clear geometries.
@@ -54,6 +56,20 @@ class CAdcRoad
*/
inline void add_signal(CAdcSignals* sign);
+ /**
+ * \brief Function to clear signals.
+ *
+ */
+ inline void clear_objects(void);
+
+ /**
+ * \brief Function to add a sign.
+ *
+ * \param sign The road's sign to add.
+ *
+ */
+ inline void add_object(CAdcObject* object);
+
public:
/**
* \brief Default constructor.
@@ -136,13 +152,26 @@ class CAdcRoad
*
* For each sign, it gets its track pose and check on wich
* geometry it belongs. After that it calculates its local pose and
- * finally, the worl pose.
+ * finally, the world pose.
*
* \param debug Boolean to print all the information stored on each signal at
* the end of the function.
*/
void calculate_signals_pose(bool debug = false);
+ /**
+ * \brief Function to calculate the world pose of each object.
+ *
+ * For each object, it gets its track pose and check on wich
+ * geometry it belongs. After that it calculates its local pose and
+ * finally, the world pose. In case of an outline object it also calculates
+ * the world pose of each corner.
+ *
+ * \param debug Boolean to print all the information stored on each signal at
+ * the end of the function.
+ */
+ void calculate_objects_pose(bool debug = false);
+
/**
* \brief Function to get all the signs presents on circuit.
*
diff --git a/include/opendrive_to_gazebo.h b/include/opendrive_to_gazebo.h
index 9ebf5adb3e28fb58161ce18c62525bb3aaaf2e21..68f3727116e97dce184bc78db12c7cb1bfabd0a4 100644
--- a/include/opendrive_to_gazebo.h
+++ b/include/opendrive_to_gazebo.h
@@ -35,15 +35,15 @@ class COpenDriveFormat
void load(std::string &filename, bool debug = false);
/**
- * \brief Function to calculate the world pose of each sign.
+ * \brief Function to calculate the world pose of each sign and object.
*
- * It calls CAdcCircuit::calculate_signals_pose function.
+ * It calls CAdcCircuit::calculate_world_poses function.
*
* \param debug Boolean to print all the information stored on each signal at
* the end of the function.
*
*/
- void calculate_signals_pose(bool debug = false);
+ void calculate_world_poses(bool debug = false);
/**
* \brief Function to get all the signs presents on circuit.
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 9631942b5e57a82b5fce9aaca7c094fc1f534d30..513a54beb7aea38ca81df4022bd3c75ee975dce0 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,7 +1,7 @@
# driver source files
-SET(sources opendrive_to_gazebo.cpp adc_circuit.cpp adc_road.cpp adc_geometries.cpp adc_signals.cpp)
+SET(sources opendrive_to_gazebo.cpp adc_circuit.cpp adc_road.cpp adc_geometries.cpp adc_signals.cpp adc_object.cpp)
# application header files
-SET(headers ../include/opendrive_to_gazebo.h ../include/adc_circuit.h ../include/adc_road.h ../include/adc_geometries.h ../include/adc_signals.h)
+SET(headers ../include/opendrive_to_gazebo.h ../include/adc_circuit.h ../include/adc_road.h ../include/adc_geometries.h ../include/adc_signals.h ../include/adc_object.h)
# locate the necessary dependencies
FIND_PACKAGE(iriutils REQUIRED)
diff --git a/src/adc_circuit.cpp b/src/adc_circuit.cpp
index e3c5366f8ddfb0065dc9050bcda6ec5c63c70678..6df6a81d9f9af60c42a51ebb3950608806688956 100644
--- a/src/adc_circuit.cpp
+++ b/src/adc_circuit.cpp
@@ -32,10 +32,13 @@ void CAdcCircuit::debug(void)
this->roads[i]->debug();
}
-void CAdcCircuit::calculate_signals_pose(bool debug)
+void CAdcCircuit::calculate_world_poses(bool debug)
{
for (unsigned int i = 0; i < this->roads.size(); i++)
+ {
this->roads[i]->calculate_signals_pose(debug);
+ this->roads[i]->calculate_objects_pose(debug);
+ }
}
void CAdcCircuit::get_signals(std::vector<CAdcSignals*> &signals)
diff --git a/src/adc_object.cpp b/src/adc_object.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ec715dd11ee60cf4c6641c6483d4d051f9448df4
--- /dev/null
+++ b/src/adc_object.cpp
@@ -0,0 +1,252 @@
+#include "adc_object.h"
+#include <cmath>
+#include <iostream>
+#include <sstream>
+#include <fstream>
+#include "exceptions.h"
+
+CAdcObject::CAdcObject()
+{
+ this->id = 0;
+ this->s = 0;
+ this->t = 0;
+ this->heading = 0;
+ this->world_x = 0;
+ this->world_y = 0;
+ this->world_yaw = 0;
+ this->valid_length = 0;
+ this->length = 0;
+ this->width = 0;
+ this->radius = 0;
+ this->height = 0;
+ this->type = "";
+ this->name = "";
+ this->outline = false;
+ std::vector<Object_corner>().swap(this->corners);
+}
+
+CAdcObject::~CAdcObject()
+{
+ this->id = 0;
+ this->s = 0;
+ this->t = 0;
+ this->heading = 0;
+ this->world_x = 0;
+ this->world_y = 0;
+ this->world_yaw = 0;
+ this->valid_length = 0;
+ this->length = 0;
+ this->width = 0;
+ this->radius = 0;
+ this->height = 0;
+ this->type = "";
+ this->name = "";
+ this->outline = false;
+ std::vector<Object_corner>().swap(this->corners);
+}
+
+CAdcObject::CAdcObject(const CAdcObject & object)
+{
+ this->id = object.id;
+ this->s = object.s;
+ this->t = object.t;
+ this->heading = object.heading;
+ this->world_x = object.world_x;
+ this->world_y = object.world_y;
+ this->world_yaw = object.world_yaw;
+ this->valid_length = object.valid_length;
+ this->length = object.length;
+ this->width = object.width;
+ this->radius = object.radius;
+ this->height = object.height;
+ this->type = object.type;
+ this->name = object.name;
+ this->outline = object.outline;
+ std::vector<Object_corner>().swap(this->corners);
+ for (int i = 0; i < object.corners.size(); i++)
+ this->corners.push_back(object.corners[i]);
+}
+
+CAdcObject::CAdcObject(int id, double s, double t, double heading, std::string type, std::string name, double width, double length, double height, double valid_length)
+{
+ this->id = id;
+ this->s = s;
+ this->t = t;
+ this->heading = heading;
+ this->world_x = 0;
+ this->world_y = 0;
+ this->world_yaw = 0;
+ this->valid_length = valid_length;
+ this->length = length;
+ this->width = width;
+ this->radius = 0.0;
+ this->height = height;
+ this->type = type;
+ this->name = name;
+ this->outline = false;
+ std::vector<Object_corner>().swap(this->corners);
+}
+
+CAdcObject::CAdcObject(int id, double s, double t, double heading, std::string type, std::string name, double radius, double height, double valid_length)
+{
+ this->id = id;
+ this->s = s;
+ this->t = t;
+ this->heading = heading;
+ this->world_x = 0;
+ this->world_y = 0;
+ this->world_yaw = 0;
+ this->valid_length = valid_length;
+ this->length = 0.0;
+ this->width = 0.0;
+ this->radius = radius;
+ this->height = height;
+ this->type = type;
+ this->name = name;
+ this->outline = false;
+ std::vector<Object_corner>().swap(this->corners);
+}
+
+CAdcObject::CAdcObject(int id, double s, double t, double heading, std::string type, std::string name, double valid_length, std::vector<Object_corner> &corners)
+{
+ this->id = id;
+ this->s = s;
+ this->t = t;
+ this->heading = heading;
+ this->world_x = 0;
+ this->world_y = 0;
+ this->world_yaw = 0;
+ this->valid_length = valid_length;
+ this->length = 0.0;
+ this->width = 0.0;
+ this->radius = 0.0;
+ this->height = 0.0;
+ this->type = type;
+ this->name = name;
+ this->outline = false;
+ std::vector<Object_corner>().swap(this->corners);
+ for (int i = 0; i < corners.size(); i++)
+ this->corners.push_back(corners[i]);
+}
+
+void CAdcObject::operator = (const CAdcObject &object)
+{
+ this->id = object.id;
+ this->s = object.s;
+ this->t = object.t;
+ this->heading = object.heading;
+ this->world_x = object.world_x;
+ this->world_y = object.world_y;
+ this->world_yaw = object.world_yaw;
+ this->valid_length = object.valid_length;
+ this->length = object.length;
+ this->width = object.width;
+ this->radius = object.radius;
+ this->height = object.height;
+ this->type = object.type;
+ this->name = object.name;
+ this->outline = object.outline;
+ std::vector<Object_corner>().swap(this->corners);
+ for (int i = 0; i < object.corners.size(); i++)
+ this->corners.push_back(object.corners[i]);
+}
+
+void CAdcObject::get_pose(double &s, double &t, double &heading)
+{
+ s = this->s;
+ t = this->t;
+ heading = this->heading;
+}
+
+void CAdcObject::set_world_pose(double x, double y, double yaw)
+{
+ this->world_x = x;
+ this->world_y = y;
+ this->world_yaw = yaw;
+}
+
+void CAdcObject::add_corner_world_pose(unsigned int index, double x, double y)
+{
+ this->corners[index].world_x = x;
+ this->corners[index].world_y = y;
+}
+
+void CAdcObject::get_corners(std::vector<Object_corner> &corners)
+{
+ std::vector<Object_corner>().swap(corners);
+ //resize better than push_back
+ for (int i = 0; i < this->corners.size(); i++)
+ corners.push_back(this->corners[i]);
+}
+
+bool CAdcObject::is_outline(void)
+{
+ return this->outline;
+}
+
+void CAdcObject::debug(bool world_pose)
+{
+ std::cout << " Object id = " << this->id << "; type = " << this->type << ", name = " << this->name << std::endl;
+ std::cout << " length = " << this->length << "; width = " << this->width << "; radius = " << this->radius << "; height = " << height << std::endl;
+ std::cout << " pose: s = " << this->s << ", t = " << this->t << ", heading = " << this->heading << "; valid_length = " << valid_length << std::endl;
+ if (world_pose)
+ std::cout << " world pose: x = " << this->world_x << ", y = " << this->world_y << ", yaw = " << this->world_yaw << std::endl;
+ if (outline)
+ {
+ for (int i = 0; i < this->corners.size(); i++)
+ {
+ std::cout << " corner_" << i << ": s = " << this->corners[i].s << ", t = " << this->corners[i].t << ", height = " << this->corners[i].height << std::endl;
+ if (world_pose)
+ std::cout << " world_corner_" << i << ": x = " << this->corners[i].world_x << ", y = " << this->corners[i].world_y << std::endl;
+ }
+ }
+}
+
+void CAdcObject::append_object_spawn(std::string &filename)
+{
+
+}
+
+void CAdcObject::load(std::auto_ptr<objects::object_type> &object_info)
+{
+ std::stringstream ss(object_info->id().get());
+ ss >> this->id;
+ this->s = (object_info->s().present() ? object_info->s().get() : 0.0);
+ this->t = (object_info->t().present() ? object_info->t().get() : 0.0);
+ this->heading = (object_info->hdg().present() ? object_info->hdg().get() : 0.0);
+ this->valid_length = (object_info->validLength().present() ? object_info->validLength().get() : 0.0);
+ this->length = (object_info->length().present() ? object_info->length().get() : 0.0);
+ this->width = (object_info->width().present() ? object_info->width().get() : 0.0);
+ this->radius = (object_info->radius().present() ? object_info->radius().get() : 0.0);
+ this->height = (object_info->height().present() ? object_info->height().get() : 0.0);
+ this->type = (object_info->type().present() ? object_info->type().get() : "");
+ this->name = (object_info->name().present() ? object_info->name().get() : "");
+ //radius and length and width warning
+ if (object_info->outline().present())
+ {
+ this->outline = true;
+ std::vector<Object_corner>().swap(this->corners);
+
+ for (outline::cornerRoad_iterator cornerRoad_it(object_info->outline().get().cornerRoad().begin()); cornerRoad_it != object_info->outline().get().cornerRoad().end(); ++cornerRoad_it)
+ {
+ Object_corner corner;
+ corner.s = (cornerRoad_it->s().present() ? cornerRoad_it->s().get() : 0.0);
+ corner.t = (cornerRoad_it->t().present() ? cornerRoad_it->t().get() : 0.0);
+ corner.height = (cornerRoad_it->height().present() ? cornerRoad_it->height().get() : 0.0);
+ this->corners.push_back(corner);
+ }
+
+ for (outline::cornerLocal_iterator cornerLocal_it(object_info->outline().get().cornerLocal().begin()); cornerLocal_it != object_info->outline().get().cornerLocal().end(); ++cornerLocal_it)
+ {
+ Object_corner corner;
+ double u = (cornerLocal_it->u().present() ? cornerLocal_it->u().get() : 0.0);
+ double v = (cornerLocal_it->v().present() ? cornerLocal_it->v().get() : 0.0);
+
+
+ corner.s = this->s + u*cos(this->heading) - v*sin(this->heading);
+ corner.t = this->t + u*sin(this->heading) + v*cos(this->heading);
+ corner.height = (cornerLocal_it->height().present() ? cornerLocal_it->height().get() : 0.0);
+ this->corners.push_back(corner);
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/adc_road.cpp b/src/adc_road.cpp
index ab008abc39a9cc00e659439059d03377716aa13e..3980358932450c6b0f116b5fc92e7b1438381e3d 100644
--- a/src/adc_road.cpp
+++ b/src/adc_road.cpp
@@ -75,6 +75,18 @@ void CAdcRoad::add_signal(CAdcSignals* sign)
this->signals.push_back(sign);
}
+void CAdcRoad::clear_objects(void)
+{
+ for (unsigned int i = 0; i < this->objects.size(); i++)
+ delete this->objects[i];
+ std::vector<CAdcObject*>().swap(this->objects);
+}
+
+void CAdcRoad::add_object(CAdcObject* object)
+{
+ this->objects.push_back(object);
+}
+
void CAdcRoad::set_id(int id)
{
this->id = id;
@@ -102,6 +114,8 @@ void CAdcRoad::debug(void)
this->geometries[i]->debug();
for (unsigned int i = 0; i < this->signals.size(); i++)
this->signals[i]->debug();
+ for (unsigned int i = 0; i < this->objects.size(); i++)
+ this->objects[i]->debug();
}
void CAdcRoad::calculate_signals_pose(bool debug)
@@ -127,6 +141,46 @@ void CAdcRoad::calculate_signals_pose(bool debug)
}
}
+void CAdcRoad::calculate_objects_pose(bool debug)
+{
+ double s, t, heading, x, y, yaw;
+ for (unsigned int i = 0; i < this->objects.size(); i++)
+ {
+ if (this->objects[i]->is_outline())
+ {
+ std::vector<Object_corner> corners;
+ this->objects[i]->get_corners(corners);
+ for (unsigned int j = 0; j < corners.size(); j++)
+ {
+ if (this->geometries[j]->on_range(corners[j].s))
+ {
+ this->geometries[j]->get_world_pose(corners[j].s, corners[j].t, 0.0, x, y, yaw);
+ this->objects[i]->add_corner_world_pose(j, x, y);
+ break;
+ }
+ }
+ }
+ else
+ {
+ this->objects[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->objects[i]->set_world_pose(x, y, yaw);
+ break;
+ }
+ }
+ }
+ if (debug)
+ {
+ std::cout << "Road " << this->name << " id = " << this->id << "; length = " << this->length << std::endl;
+ this->objects[i]->debug(debug);
+ }
+ }
+}
+
void CAdcRoad::get_signals(std::vector<CAdcSignals*> &signals)
{
for (unsigned int i = 0; i < this->signals.size(); i++)
@@ -197,4 +251,16 @@ void CAdcRoad::load(std::auto_ptr<OpenDRIVE::road_type> &road_info)
add_signal(sign);
}
}
+
+ ////////// Objects atributes
+ if (road_info->objects().present())
+ {
+ for (objects::object_iterator object_it(road_info->objects().get().object().begin()); object_it != road_info->objects().get().object().end(); ++object_it)
+ {
+ CAdcObject *obj = new CAdcObject();
+ std::auto_ptr<objects::object_type> object_info(new objects::object_type(*object_it));
+ obj->load(object_info);
+ add_object(obj);
+ }
+ }
}
\ No newline at end of file
diff --git a/src/adc_signals.cpp b/src/adc_signals.cpp
index 17d795d63b7cfe584d6de959883018ffdf68d381..99a649ded5822467571ace8564bd2385ca67c4b3 100644
--- a/src/adc_signals.cpp
+++ b/src/adc_signals.cpp
@@ -150,7 +150,6 @@ void CAdcSignals::set_urdf_map(void)
void CAdcSignals::get_pose(double &s, double &t, double &heading)
{
- id = this->id;
s = this->s;
t = this->t;
heading = this->heading;
diff --git a/src/examples/opendrive_to_gazebo_test.cpp b/src/examples/opendrive_to_gazebo_test.cpp
index c90354ed8b4215eae2a424a05e575ce43523c0f1..beca6fef3702bf84dbad5d1d19f18480d37e882c 100644
--- a/src/examples/opendrive_to_gazebo_test.cpp
+++ b/src/examples/opendrive_to_gazebo_test.cpp
@@ -13,7 +13,7 @@ int main(int argc, char *argv[])
try
{
open_drive_format.load(xml_file, false);
- open_drive_format.calculate_signals_pose(false);
+ open_drive_format.calculate_world_poses(true);
std::vector<CAdcSignals*> signals;
// open_drive_format.get_signals(signals);
diff --git a/src/opendrive_to_gazebo.cpp b/src/opendrive_to_gazebo.cpp
index 1bb2c498f62b50986460d95a5de9bb2c08bdc52d..3ca162b7e61d6b77e250d09a877707ba0449f80b 100644
--- a/src/opendrive_to_gazebo.cpp
+++ b/src/opendrive_to_gazebo.cpp
@@ -48,11 +48,11 @@ void COpenDriveFormat::load(std::string &filename, bool debug)
throw CException(_HERE_,"The .xodr file does not exist");
}
-void COpenDriveFormat::calculate_signals_pose(bool debug)
+void COpenDriveFormat::calculate_world_poses(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;
+ std::cout << "Calculating world signs and objects poses..." << std::endl;
+ this->adc_circuit.calculate_world_poses(debug);
+ std::cout << "Calculating world signs and objects poses done" << std::endl;
}
void COpenDriveFormat::get_signals(std::vector<CAdcSignals*> &signals)