diff --git a/include/adc_object.h b/include/adc_object.h index d7d87b3589489c5e1daaa7511176fda569951c14..aa4e9bab5a13f66985e63f74be388c0776875da2 100644 --- a/include/adc_object.h +++ b/include/adc_object.h @@ -11,7 +11,7 @@ /** * \struct Object_corner. * - * \brief Corner "s" and "t" coordenates with height. + * \brief Corner "s" and "t" coordenates with height and world "x" e "y" coordenates. */ typedef struct { @@ -22,6 +22,23 @@ typedef struct double height; }Object_corner; +/** + * \struct Object_repeat. + * + * \brief Object repeat information. + */ +typedef struct +{ + double s; + double t; + double heading; + double distance; + int n; + std::vector<double> world_x; + std::vector<double> world_y; + std::vector<double> world_yaw; +}Object_repeat; + /** * \class CAdcObject * @@ -47,7 +64,9 @@ class CAdcObject 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. + std::vector<std::vector<Object_corner> > corners; ///< Outline object corners. + bool repeat; ///< Boolean to know if a object is repeated over distance. + std::vector<Object_repeat> repeat_info; ///< Repeat object info. public: /** @@ -137,7 +156,7 @@ class CAdcObject * \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); + CAdcObject(int id, double s, double t, double heading, std::string type, std::string name, double valid_length, std::vector<std::vector<Object_corner> > &corners); /** * \brief Default destructor. @@ -178,14 +197,30 @@ class CAdcObject /** * \brief Function to add Object's corner world pose. * - * \param index Corner index + * \param i Object index + * + * \param j 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); + void add_corner_world_pose(unsigned int i, unsigned int j, double x, double y); + + /** + * \brief Function to add Object's repeat_info world pose. + * + * \param i Repeat info index + * + * \param x Objects's world coordenate "x". + * + * \param y Objects's world coordenate "y". + * + * \param yaw Object's world yaw. + * + */ + void add_repeat_info_world_pose(unsigned int i, double x, double y, double yaw); /** * \brief Function to get Object's corners. @@ -193,7 +228,15 @@ class CAdcObject * \param corners Corners vector. * */ - void get_corners(std::vector<Object_corner> &corners); + void get_corners(std::vector<std::vector<Object_corner> > &corners); + + /** + * \brief Function to get Object's repeat info. + * + * \param corners Repeat info vector. + * + */ + void get_repeat_info(std::vector<Object_repeat> &repeat_info); /** * \brief Function to know if an objects is a outline. @@ -203,6 +246,14 @@ class CAdcObject */ bool is_outline(void); + /** + * \brief Function to know if an objects is has repeat info. + * + * \return Repeat value. + * + */ + bool is_repeat(void); + /** * \brief Function to print its info. * @@ -222,6 +273,8 @@ class CAdcObject /** * \brief Function to load the .xodr info. * + * If an object is a continuous object is loaded as an outline object. + * * \param object_info The object generated with XSD. * */ diff --git a/src/adc_geometries.cpp b/src/adc_geometries.cpp index a0bfa9c90c880d3a7ace1c0e96abc5bf6097a981..7875ae76430737e365744bbd2e3f2ba13421d5fa 100644 --- a/src/adc_geometries.cpp +++ b/src/adc_geometries.cpp @@ -63,7 +63,7 @@ bool CAdcGeometry::get_world_pose(double s, double t, double heading, double &x, bool CAdcGeometry::on_range(double s) { - if ((s < this->max_s) && (s > this->min_s)) + if ((s < this->max_s) && (s >= this->min_s)) return true; return false; } diff --git a/src/adc_object.cpp b/src/adc_object.cpp index ec715dd11ee60cf4c6641c6483d4d051f9448df4..41601e44891f4f38997b11ca055c8b028717aea8 100644 --- a/src/adc_object.cpp +++ b/src/adc_object.cpp @@ -22,7 +22,9 @@ CAdcObject::CAdcObject() this->type = ""; this->name = ""; this->outline = false; - std::vector<Object_corner>().swap(this->corners); + this->repeat = false; + std::vector<std::vector<Object_corner> >().swap(this->corners); + std::vector<Object_repeat>().swap(this->repeat_info); } CAdcObject::~CAdcObject() @@ -42,7 +44,9 @@ CAdcObject::~CAdcObject() this->type = ""; this->name = ""; this->outline = false; - std::vector<Object_corner>().swap(this->corners); + this->repeat = false; + std::vector<std::vector<Object_corner> >().swap(this->corners); + std::vector<Object_repeat>().swap(this->repeat_info); } CAdcObject::CAdcObject(const CAdcObject & object) @@ -62,9 +66,20 @@ CAdcObject::CAdcObject(const CAdcObject & object) this->type = object.type; this->name = object.name; this->outline = object.outline; - std::vector<Object_corner>().swap(this->corners); + this->repeat = object.repeat; + + std::vector<std::vector<Object_corner> >().swap(this->corners); for (int i = 0; i < object.corners.size(); i++) - this->corners.push_back(object.corners[i]); + { + std::vector<Object_corner> corner_aux; + for (int j = 0; j < object.corners[i].size(); j++) + corner_aux.push_back(object.corners[i][j]); + this->corners.push_back(corner_aux); + } + + std::vector<Object_repeat>().swap(this->repeat_info); + for (int i = 0; i < object.repeat_info.size(); i++) + this->repeat_info.push_back(object.repeat_info[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) @@ -84,7 +99,9 @@ CAdcObject::CAdcObject(int id, double s, double t, double heading, std::string t this->type = type; this->name = name; this->outline = false; - std::vector<Object_corner>().swap(this->corners); + this->repeat = false; + std::vector<std::vector<Object_corner> >().swap(this->corners); + std::vector<Object_repeat>().swap(this->repeat_info); } CAdcObject::CAdcObject(int id, double s, double t, double heading, std::string type, std::string name, double radius, double height, double valid_length) @@ -104,10 +121,12 @@ CAdcObject::CAdcObject(int id, double s, double t, double heading, std::string t this->type = type; this->name = name; this->outline = false; - std::vector<Object_corner>().swap(this->corners); + this->repeat = false; + std::vector<std::vector<Object_corner> >().swap(this->corners); + std::vector<Object_repeat>().swap(this->repeat_info); } -CAdcObject::CAdcObject(int id, double s, double t, double heading, std::string type, std::string name, double valid_length, std::vector<Object_corner> &corners) +CAdcObject::CAdcObject(int id, double s, double t, double heading, std::string type, std::string name, double valid_length, std::vector<std::vector<Object_corner> > &corners) { this->id = id; this->s = s; @@ -123,10 +142,17 @@ CAdcObject::CAdcObject(int id, double s, double t, double heading, std::string t this->height = 0.0; this->type = type; this->name = name; - this->outline = false; - std::vector<Object_corner>().swap(this->corners); + this->outline = true; + this->repeat = false; + std::vector<std::vector<Object_corner> >().swap(this->corners); for (int i = 0; i < corners.size(); i++) - this->corners.push_back(corners[i]); + { + std::vector<Object_corner> corner_aux; + for (int j = 0; j < corners[i].size(); j++) + corner_aux.push_back(corners[i][j]); + this->corners.push_back(corner_aux); + } + std::vector<Object_repeat>().swap(this->repeat_info); } void CAdcObject::operator = (const CAdcObject &object) @@ -146,9 +172,18 @@ void CAdcObject::operator = (const CAdcObject &object) this->type = object.type; this->name = object.name; this->outline = object.outline; - std::vector<Object_corner>().swap(this->corners); + this->repeat = object.repeat; + std::vector<std::vector<Object_corner> >().swap(this->corners); for (int i = 0; i < object.corners.size(); i++) - this->corners.push_back(object.corners[i]); + { + std::vector<Object_corner> corner_aux; + for (int j = 0; j < object.corners[i].size(); j++) + corner_aux.push_back(object.corners[i][j]); + this->corners.push_back(corner_aux); + } + std::vector<Object_repeat>().swap(this->repeat_info); + for (int i = 0; i < object.repeat_info.size(); i++) + this->repeat_info.push_back(object.repeat_info[i]); } void CAdcObject::get_pose(double &s, double &t, double &heading) @@ -165,18 +200,23 @@ void CAdcObject::set_world_pose(double x, double y, double yaw) this->world_yaw = yaw; } -void CAdcObject::add_corner_world_pose(unsigned int index, double x, double y) +void CAdcObject::add_corner_world_pose(unsigned int i, unsigned int j, double x, double y) { - this->corners[index].world_x = x; - this->corners[index].world_y = y; + this->corners[i][j].world_x = x; + this->corners[i][j].world_y = y; } -void CAdcObject::get_corners(std::vector<Object_corner> &corners) +void CAdcObject::get_corners(std::vector<std::vector<Object_corner> > &corners) { - std::vector<Object_corner>().swap(corners); + std::vector<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]); + { + std::vector<Object_corner> corner_aux; + for (int j = 0; j < this->corners[i].size(); j++) + corner_aux.push_back(this->corners[i][j]); + corners.push_back(corner_aux); + } } bool CAdcObject::is_outline(void) @@ -184,6 +224,25 @@ bool CAdcObject::is_outline(void) return this->outline; } +bool CAdcObject::is_repeat(void) +{ + return this->repeat; +} + +void CAdcObject::get_repeat_info(std::vector<Object_repeat> &repeat_info) +{ + std::vector<Object_repeat>().swap(this->repeat_info); + for (int i = 0; i < this->repeat_info.size(); i++) + repeat_info.push_back(this->repeat_info[i]); +} + +void CAdcObject::add_repeat_info_world_pose(unsigned int i, double x, double y, double yaw) +{ + this->repeat_info[i].world_x.push_back(x); + this->repeat_info[i].world_y.push_back(y); + this->repeat_info[i].world_yaw.push_back(yaw); +} + void CAdcObject::debug(bool world_pose) { std::cout << " Object id = " << this->id << "; type = " << this->type << ", name = " << this->name << std::endl; @@ -191,13 +250,32 @@ void CAdcObject::debug(bool world_pose) 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) + if (this->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; + std::cout << " Subobject_" << i << ":" << std::endl; + for (int j = 0; j < this->corners[i].size(); j++) + { + std::cout << " corner_" << j << ": s = " << this->corners[i][j].s << ", t = " << this->corners[i][j].t << ", height = " << this->corners[i][j].height << std::endl; + if (world_pose) + std::cout << " world_corner_" << j << ": x = " << this->corners[i][j].world_x << ", y = " << this->corners[i][j].world_y << std::endl; + } + } + } + if (this->repeat) + { + int count = 0; + for (int i = 0; i < this->repeat_info.size(); i++) + { + for (int j = 0; j < this->repeat_info[i].n; j++) + { + std::cout << " Subobject_" << count << ":" << std::endl; + std::cout << " pose: s = " << this->repeat_info[i].s + j*this->repeat_info[i].distance << ", t = " << this->t << ", heading = " << this->heading << std::endl; + if (world_pose) + std::cout << " world pose: x = " << this->repeat_info[i].world_x[j] << ", y = " << this->repeat_info[i].world_y[j] << ", yaw = " << this->repeat_info[i].world_yaw[j] << std::endl; + count++; + } } } } @@ -222,18 +300,21 @@ void CAdcObject::load(std::auto_ptr<objects::object_type> &object_info) 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 + std::vector<std::vector<Object_corner> >().swap(this->corners); + std::vector<Object_repeat>().swap(this->repeat_info); + if (object_info->outline().present()) { this->outline = true; - std::vector<Object_corner>().swap(this->corners); + std::vector<Object_corner> corner_aux; 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); + corner_aux.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) @@ -246,7 +327,48 @@ void CAdcObject::load(std::auto_ptr<objects::object_type> &object_info) 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); + corner_aux.push_back(corner); + } + this->corners.push_back(corner_aux); + } + for (object::repeat_iterator repeat_it(object_info->repeat().begin()); repeat_it != object_info->repeat().end(); ++repeat_it) + { + if (repeat_it->distance().present() && repeat_it->distance().get() == 0.0) + { + this->outline = true; + std::vector<Object_corner> corner_aux; + Object_corner corner; + corner.s = (repeat_it->s().present() ? repeat_it->s().get() : this->s); + corner.t = (repeat_it->tStart().present() ? repeat_it->tStart().get() : this->t); + corner.height = (repeat_it->heightStart().present() ? repeat_it->heightStart().get() : this->height); + corner_aux.push_back(corner); + + corner.t += (repeat_it->widthStart().present() ? repeat_it->widthStart().get() : this->width); + corner_aux.push_back(corner); + + corner.s += (repeat_it->length().present() ? repeat_it->length().get() : this->valid_length); + double t_end_corner = (repeat_it->tEnd().present() ? repeat_it->tEnd().get() : this->t); + corner.t = t_end_corner + (repeat_it->widthEnd().present() ? repeat_it->widthEnd().get() : this->width); + corner.height = (repeat_it->heightEnd().present() ? repeat_it->heightEnd().get() : this->height); + corner_aux.push_back(corner); + + corner.t = t_end_corner; + corner_aux.push_back(corner); + + this->corners.push_back(corner_aux); + } + else if (repeat_it->distance().present()) + { + this->repeat = true; + Object_repeat repeat_aux; + repeat_aux.distance = repeat_it->distance().get(); + double length = (repeat_it->length().present() ? repeat_it->length().get() : this->valid_length); + repeat_aux.n = (repeat_aux.distance == 0.0 ? 1 : (int) std::floor(length/repeat_aux.distance)); + repeat_aux.s = (repeat_it->s().present() ? repeat_it->s().get() : this->s); + repeat_aux.t = this->t; + repeat_aux.heading = this->heading; + + this->repeat_info.push_back(repeat_aux); } } } \ No newline at end of file diff --git a/src/adc_road.cpp b/src/adc_road.cpp index 3980358932450c6b0f116b5fc92e7b1438381e3d..4e1130106b5cf94deaeaf46b20e42142012d8292 100644 --- a/src/adc_road.cpp +++ b/src/adc_road.cpp @@ -143,25 +143,53 @@ void CAdcRoad::calculate_signals_pose(bool debug) void CAdcRoad::calculate_objects_pose(bool debug) { - double s, t, heading, x, y, yaw; + double x, y, yaw; for (unsigned int i = 0; i < this->objects.size(); i++) { if (this->objects[i]->is_outline()) { - std::vector<Object_corner> corners; + std::vector<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)) + for (unsigned int l = 0; l < corners[j].size(); l++) { - 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; + for (unsigned int k = 0; k < this->geometries.size(); k++) + { + if (this->geometries[k]->on_range(corners[j][l].s)) + { + this->geometries[k]->get_world_pose(corners[j][l].s, corners[j][l].t, 0.0, x, y, yaw); + this->objects[i]->add_corner_world_pose(j, l, x, y); + break; + } + } + } + } + } + else if (this->objects[i]->is_repeat()) + { + std::vector<Object_repeat> repeat_info; + this->objects[i]->get_repeat_info(repeat_info); + for (unsigned int j = 0; j < repeat_info.size(); j++) + { + for (unsigned int l = 0; l < repeat_info[j].n; l++) + { + double s = repeat_info[j].s + repeat_info[j].distance*l; + for (unsigned int k = 0; k < this->geometries.size(); k++) + { + if (this->geometries[k]->on_range(s)) + { + this->geometries[k]->get_world_pose(s, repeat_info[j].t, repeat_info[j].heading, x, y, yaw); + this->objects[i]->add_repeat_info_world_pose(j, x, y, yaw); + break; + } + } } } } else { + double s, t, heading; this->objects[i]->get_pose(s, t, heading); for (unsigned int j = 0; j < this->geometries.size(); j++) {