From 303628cc398a02656c74c54aee9b38021321a224 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 25 Mar 2023 21:40:22 +0100 Subject: [PATCH 1/9] (entity) Remove unused variables --- include/ed/entity.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/include/ed/entity.h b/include/ed/entity.h index 0ce7671..2d3f522 100644 --- a/include/ed/entity.h +++ b/include/ed/entity.h @@ -105,10 +105,6 @@ class Entity inline const tue::config::DataConstPointer& data() const { return config_; } inline void setData(const tue::config::DataConstPointer& data) { config_ = data; } - //! For debugging purposes - bool in_frustrum; - bool object_in_front; - // inline double creationTime() const { return creation_time_; } inline void setRelationTo(Idx child_idx, Idx r_idx) { relations_to_[child_idx] = r_idx; } From 7cc31923e3bf2c8a624fbb0a1c209421af23c21c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 25 Mar 2023 22:01:25 +0100 Subject: [PATCH 2/9] Remove leading slash frames --- include/ed/helpers/msg_conversions.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ed/helpers/msg_conversions.h b/include/ed/helpers/msg_conversions.h index 5544a98..d1cd3ae 100644 --- a/include/ed/helpers/msg_conversions.h +++ b/include/ed/helpers/msg_conversions.h @@ -119,7 +119,7 @@ void convert(const ed::Entity& e, ed_msgs::EntityInfo& msg) { ed_msgs::SubVolume sub_volume; convert(shape_tr, sub_volume); - sub_volume.center_point.header.frame_id = "/" + e.id().str(); + sub_volume.center_point.header.frame_id = e.id().str(); volume.subvolumes.push_back(sub_volume); } } From 114ae46b71390dd73ba4219a1c49f5d39725387b Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 25 Mar 2023 22:04:16 +0100 Subject: [PATCH 3/9] (Entity) also add collision --- include/ed/entity.h | 33 +++++++++++++++++-------- include/ed/helpers/msg_conversions.h | 2 +- include/ed/update_request.h | 15 +++++++----- include/ed/world_model.h | 15 ++++++++++-- plugins/gui_plugin.cpp | 6 ++--- plugins/robot_plugin.cpp | 2 +- src/ed.cpp | 14 +++++------ src/entity.cpp | 30 ++++++++++++++++------- src/models/model_loader.cpp | 25 ++++++++++++++----- src/rendering.cpp | 4 ++-- src/serialization/serialization.cpp | 3 ++- src/server.cpp | 2 +- src/world_model.cpp | 36 +++++++++++++++++++++------- tools/view_model.cpp | 8 +++---- 14 files changed, 134 insertions(+), 61 deletions(-) diff --git a/include/ed/entity.h b/include/ed/entity.h index 2d3f522..73c17b4 100644 --- a/include/ed/entity.h +++ b/include/ed/entity.h @@ -53,14 +53,24 @@ class Entity void addMeasurement(MeasurementConstPtr measurement); - inline geo::ShapeConstPtr shape() const { return shape_; } - void setShape(const geo::ShapeConstPtr& shape); + [[deprecated("Use visual() instead.")]] + inline geo::ShapeConstPtr shape() const { return visual(); } + inline geo::ShapeConstPtr visual() const { return visual_; } + inline geo::ShapeConstPtr collision() const { return collision_; } + [[deprecated("Use setVisual() instead.")]] + inline void setShape(const geo::ShapeConstPtr& shape) { setVisual(shape); } + void setVisual(const geo::ShapeConstPtr& visual); + void setCollision(const geo::ShapeConstPtr& collision); inline const std::map& volumes() const { return volumes_; } - inline void addVolume(const std::string& volume_name, const geo::ShapeConstPtr& volume_shape) { volumes_[volume_name] = volume_shape; ++shape_revision_; } - inline void removeVolume(const std::string& volume_name) { volumes_.erase(volume_name); ++shape_revision_; } + inline void addVolume(const std::string& volume_name, const geo::ShapeConstPtr& volume_shape) { volumes_[volume_name] = volume_shape; ++volumes_revision_; } + inline void removeVolume(const std::string& volume_name) { volumes_.erase(volume_name); ++volumes_revision_; } - inline unsigned long shapeRevision() const{ return shape_ ? shape_revision_ : 0; } + [[deprecated("Use visualRevision() instead.")]] + inline unsigned long shapeRevision() const{ return visualRevision(); } + inline unsigned long visualRevision() const{ return visual_ ? visual_revision_ : 0; } + inline unsigned long collisionRevision() const{ return collision_ ? collision_revision_ : 0; } + inline unsigned long volumesRevision() const{ return !volumes_.empty() ? volumes_revision_ : 0; } inline const ConvexHull& convexHull() const { return convex_hull_new_; } @@ -94,8 +104,8 @@ class Entity inline void setPose(const geo::Pose3D& pose) { pose_ = pose; - if (shape_) - updateConvexHullFromShape(); + if (visual_) + updateConvexHullFromVisual(); has_pose_ = true; } @@ -233,9 +243,12 @@ class Entity MeasurementConstPtr best_measurement_; unsigned int measurements_seq_; - geo::ShapeConstPtr shape_; + geo::ShapeConstPtr visual_; + geo::ShapeConstPtr collision_; std::map volumes_; - unsigned long shape_revision_; + unsigned long visual_revision_; + unsigned long collision_revision_; + unsigned long volumes_revision_; std::map convex_hull_map_; ConvexHull convex_hull_new_; @@ -255,7 +268,7 @@ class Entity void updateConvexHull(); - void updateConvexHullFromShape(); + void updateConvexHullFromVisual(); std::set flags_; diff --git a/include/ed/helpers/msg_conversions.h b/include/ed/helpers/msg_conversions.h index d1cd3ae..b2ccd5c 100644 --- a/include/ed/helpers/msg_conversions.h +++ b/include/ed/helpers/msg_conversions.h @@ -78,7 +78,7 @@ void convert(const ed::Entity& e, ed_msgs::EntityInfo& msg) { msg.z_max = 0; } - msg.has_shape = (e.shape() != nullptr); + msg.has_shape = (e.visual() != nullptr); msg.has_pose = e.has_pose(); if (e.has_pose()) { diff --git a/include/ed/update_request.h b/include/ed/update_request.h index 48b0d3c..feb6bc0 100644 --- a/include/ed/update_request.h +++ b/include/ed/update_request.h @@ -46,14 +46,17 @@ class UpdateRequest } - // SHAPES + // VISUALS + std::map visuals; + [[deprecated("Use setVisual() instead.")]] + void setShape(const UUID& id, const geo::ShapeConstPtr& shape) { setVisual(id, shape); } + void setVisual(const UUID& id, const geo::ShapeConstPtr& visual) { visuals[id] = visual; flagUpdated(id); } - std::map shapes; - void setShape(const UUID& id, const geo::ShapeConstPtr& shape) { shapes[id] = shape; flagUpdated(id); } - - - //VOLUMES + // COLLISIONS + std::map collisions; + void setCollision(const UUID& id, const geo::ShapeConstPtr& collision) { collisions[id] = collision; flagUpdated(id); } + // VOLUMES std::map > volumes_added; void addVolume(const UUID& id, const std::string Volume_name, const geo::ShapeConstPtr& Volume_shape) { std::map >::iterator it = volumes_added.find(id); diff --git a/include/ed/world_model.h b/include/ed/world_model.h index 1555113..52c7fa9 100644 --- a/include/ed/world_model.h +++ b/include/ed/world_model.h @@ -102,7 +102,14 @@ class WorldModel const std::vector& entity_revisions() const { return entity_revisions_; } - const std::vector& entity_shape_revisions() const { return entity_shape_revisions_; } + [[deprecated("Use entity_visual_revisions() instead.")]] + const std::vector& entity_shape_revisions() const { return entity_visual_revisions(); } + + const std::vector& entity_visual_revisions() const { return entity_visual_revisions_; } + + const std::vector& entity_collision_revisions() const { return entity_collision_revisions_; } + + const std::vector& entity_volumes_revisions() const { return entity_volumes_revisions_; } const PropertyKeyDBEntry* getPropertyInfo(const std::string& name) const; @@ -116,7 +123,11 @@ class WorldModel std::vector entity_revisions_; - std::vector entity_shape_revisions_; + std::vector entity_visual_revisions_; + + std::vector entity_collision_revisions_; + + std::vector entity_volumes_revisions_; std::queue entity_empty_spots_; diff --git a/plugins/gui_plugin.cpp b/plugins/gui_plugin.cpp index d5ed59d..cd6683e 100644 --- a/plugins/gui_plugin.cpp +++ b/plugins/gui_plugin.cpp @@ -273,7 +273,7 @@ ed::UUID GUIPlugin::getEntityFromClick(const cv::Point2i& p) const for(ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); ++it_entity) { const ed::EntityConstPtr& e = *it_entity; - if (!e->shape()) + if (!e->visual()) { const pcl::PointCloud& chull_points = e->convexHull().chull; @@ -307,14 +307,14 @@ void GUIPlugin::publishMapImage() { const ed::EntityConstPtr& e = *it_entity; - if (e->shape() && e->id() != "floor") + if (e->visual() && e->id() != "floor") { cv::Scalar color = idToColor(e->id()); cv::Vec3b color_vec(0.5 * color[0], 0.5 * color[1], 0.5 * color[2]); geo::Pose3D pose = projector_pose_.inverse() * e->pose(); geo::RenderOptions opt; - opt.setMesh(e->shape()->getMesh(), pose); + opt.setMesh(e->visual()->getMesh(), pose); ColorRenderResult res(map_image_, z_buffer, color_vec, projector_pose_.t.z - 3, projector_pose_.t.z + 1); diff --git a/plugins/robot_plugin.cpp b/plugins/robot_plugin.cpp index c0d6852..7e900e9 100644 --- a/plugins/robot_plugin.cpp +++ b/plugins/robot_plugin.cpp @@ -341,7 +341,7 @@ void RobotPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& req) std::string id = link->name; if (link->name.find(robot_name_) == std::string::npos) id = robot_name_ + "/" + link->name; - req.setShape(id, shape); + req.setVisual(id, shape); } } diff --git a/src/ed.cpp b/src/ed.cpp index a806f80..d3912fc 100644 --- a/src/ed.cpp +++ b/src/ed.cpp @@ -264,7 +264,7 @@ bool srvQuery(ed_msgs::Query::Request& req, ed_msgs::Query::Response& res) } // Write convex hull - if (!e->convexHull().points.empty() && wm.entity_shape_revisions()[i] > req.since_revision) + if (!e->convexHull().points.empty() && wm.entity_visual_revisions()[i] > req.since_revision) { w.writeGroup("convex_hull"); ed::serialize(e->convexHull(), w); @@ -280,10 +280,10 @@ bool srvQuery(ed_msgs::Query::Request& req, ed_msgs::Query::Response& res) } // Mesh - if (e->shape() && wm.entity_shape_revisions()[i] > req.since_revision) + if (e->visual() && wm.entity_visual_revisions()[i] > req.since_revision) { w.writeGroup("mesh"); - ed::serialize(*e->shape(), w); + ed::serialize(*e->visual(), w); w.endGroup(); } @@ -403,14 +403,14 @@ bool srvSimpleQuery(ed_msgs::SimpleQuery::Request& req, ed_msgs::SimpleQuery::Re if(req.ignore_z) center_point.z = e->pose().t.z; // Ignoring z in global frame, not in entity frame, as it can be rotated - geo::ShapeConstPtr shape = e->shape(); - if (shape) + geo::ShapeConstPtr visual = e->visual(); + if (visual) { geo::Vector3 center_point_e = e->pose().getBasis().transpose() * (center_point - e->pose().getOrigin()); if (radius > 0) - geom_ok = shape->intersect(center_point_e, radius); + geom_ok = visual->intersect(center_point_e, radius); else - geom_ok = shape->contains(center_point_e); + geom_ok = visual->contains(center_point_e); } else { diff --git a/src/entity.cpp b/src/entity.cpp index 67612ee..154142f 100644 --- a/src/entity.cpp +++ b/src/entity.cpp @@ -22,7 +22,9 @@ Entity::Entity(const UUID& id, const TYPE& type, const unsigned int& measurement last_update_timestamp_(0), measurements_(measurement_buffer_size), measurements_seq_(0), - shape_revision_(0), + visual_revision_(0), + collision_revision_(0), + volumes_revision_(0), // creation_time_(creation_time), has_pose_(false), pose_(geo::Pose3D::identity()) @@ -83,9 +85,9 @@ void Entity::updateConvexHull() // ---------------------------------------------------------------------------------------------------- -void Entity::updateConvexHullFromShape() +void Entity::updateConvexHullFromVisual() { - const std::vector& vertices = shape_->getMesh().getPoints(); + const std::vector& vertices = visual_->getMesh().getPoints(); if (vertices.empty()) return; @@ -114,17 +116,29 @@ void Entity::updateConvexHullFromShape() // ---------------------------------------------------------------------------------------------------- -void Entity::setShape(const geo::ShapeConstPtr& shape) +void Entity::setVisual(const geo::ShapeConstPtr& visual) { - if (shape_ != shape) + if (visual_ != visual) { - ++shape_revision_; - shape_ = shape; + ++visual_revision_; + visual_ = visual; - updateConvexHullFromShape(); + updateConvexHullFromVisual(); } } +// ---------------------------------------------------------------------------------------------------- + +void Entity::setCollision(const geo::ShapeConstPtr& collision) +{ + if (collision_ != collision) + { + ++collision_revision_; + collision_ = collision; + } +} + + // ---------------------------------------------------------------------------------------------------- void Entity::addMeasurement(MeasurementConstPtr measurement) diff --git a/src/models/model_loader.cpp b/src/models/model_loader.cpp index 89f5108..3f6c5e3 100644 --- a/src/models/model_loader.cpp +++ b/src/models/model_loader.cpp @@ -408,7 +408,10 @@ bool ModelLoader::create(const tue::config::DataConstPointer& data, const UUID& { geo::ShapePtr shape = loadShape(shape_model_path, r, shape_cache_, error); if (shape) - req.setShape(id, shape); + { + req.setVisual(id, shape); + req.setCollision(id, shape); + } else return false; @@ -556,8 +559,8 @@ bool ModelLoader::createSDF(const tue::config::DataConstPointer& data, const UUI } - // Shape && volumes - geo::CompositeShapePtr composite; + // visual, collision & volumes + geo::CompositeShapePtr visual_composite, collision_composite; std::map dummy_shape_cache; if (r.readArray("link")) { @@ -569,7 +572,15 @@ bool ModelLoader::createSDF(const tue::config::DataConstPointer& data, const UUI { while(r.nextArrayItem()) { - readSDFGeometry(r, composite, error, link_pose); + readSDFGeometry(r, visual_composite, error, link_pose); + } + r.endArray(); + } + if (r.readArray("collision")) + { + while(r.nextArrayItem()) + { + readSDFGeometry(r, collision_composite, error, link_pose); } r.endArray(); } @@ -591,8 +602,10 @@ bool ModelLoader::createSDF(const tue::config::DataConstPointer& data, const UUI } r.endArray(); // end array link } - if (composite) - req.setShape(id, composite); + if (visual_composite) + req.setVisual(id, visual_composite); + if (collision_composite) + req.setCollision(id, collision_composite); if(sdf_world) r.endGroup(); // end group world diff --git a/src/rendering.cpp b/src/rendering.cpp index 403362a..56f5884 100644 --- a/src/rendering.cpp +++ b/src/rendering.cpp @@ -156,7 +156,7 @@ bool renderWorldModel(const ed::WorldModel& world_model, const enum ShowVolumes const ed::EntityConstPtr& e = *it; const std::string& id = e->id().str(); - if (e->shape() && e->has_pose() && !e->hasFlag("self") && (id.size() < 5 || id.substr(id.size() - 5) != "floor")) // Filter ground plane + if (e->visual() && e->has_pose() && !e->hasFlag("self") && (id.size() < 5 || id.substr(id.size() - 5) != "floor")) // Filter ground plane { if (show_volumes == RoomVolumes && (id.size() < 4 || id.substr(0, 4) != "wall")) continue; @@ -178,7 +178,7 @@ bool renderWorldModel(const ed::WorldModel& world_model, const enum ShowVolumes } geo::Pose3D pose = cam_pose_inv * e->pose(); - renderMesh(cam, pose, e->shape()->getMesh(), color, res, flatten); + renderMesh(cam, pose, e->visual()->getMesh(), color, res, flatten); // Render volumes if (show_volumes == ModelVolumes && !e->volumes().empty()) diff --git a/src/serialization/serialization.cpp b/src/serialization/serialization.cpp index 636f8bf..bfd5c8c 100644 --- a/src/serialization/serialization.cpp +++ b/src/serialization/serialization.cpp @@ -89,7 +89,8 @@ bool deserialize(io::Reader &r, UpdateRequest& req) { geo::ShapePtr shape(new geo::Shape); ed::deserialize(r, *shape); - req.setShape(id, shape); + req.setVisual(id, shape); + req.setCollision(id, shape); r.endGroup(); } diff --git a/src/server.cpp b/src/server.cpp index 43ac1d3..e7bb556 100644 --- a/src/server.cpp +++ b/src/server.cpp @@ -173,7 +173,7 @@ void Server::reset(bool keep_all_shapes) if (e->id().str().substr(0, 6) == "sergio" || e->id().str().substr(0, 5) == "amigo" || e->id().str().substr(0, 4) == "hero") // TODO: robocup hack continue; - if (keep_all_shapes && e->shape()) + if (keep_all_shapes && e->visual()) continue; if (req_init_world->updated_entities.find(e->id()) == req_init_world->updated_entities.end()) diff --git a/src/world_model.cpp b/src/world_model.cpp index d6d5593..ed74d15 100644 --- a/src/world_model.cpp +++ b/src/world_model.cpp @@ -48,16 +48,29 @@ void WorldModel::update(const UpdateRequest& req) e->setPose(it->second); } - // Update shapes - for(std::map::const_iterator it = req.shapes.begin(); it != req.shapes.end(); ++it) + // Update visuals + for(std::map::const_iterator it = req.visuals.begin(); it != req.visuals.end(); ++it) { EntityPtr e = getOrAddEntity(it->first, new_entities); - e->setShape(it->second); + e->setVisual(it->second); Idx idx; if (findEntityIdx(e->id(), idx)) { - entity_shape_revisions_[idx] = revision_; + entity_visual_revisions_[idx] = revision_; + } + } + + // Update collisions + for(std::map::const_iterator it = req.collisions.begin(); it != req.collisions.end(); ++it) + { + EntityPtr e = getOrAddEntity(it->first, new_entities); + e->setCollision(it->second); + + Idx idx; + if (findEntityIdx(e->id(), idx)) + { + entity_collision_revisions_[idx] = revision_; } } @@ -74,7 +87,8 @@ void WorldModel::update(const UpdateRequest& req) Idx idx; if (findEntityIdx(e->id(), idx)) { - entity_shape_revisions_[idx] = revision_; + entity_visual_revisions_[idx] = revision_; + entity_collision_revisions_[idx] = revision_; } } @@ -88,7 +102,7 @@ void WorldModel::update(const UpdateRequest& req) Idx idx; if (findEntityIdx(e->id(), idx)) { - entity_shape_revisions_[idx] = revision_; + entity_volumes_revisions_[idx] = revision_; } } for (std::map >::const_iterator it = req.volumes_added.begin(); it != req.volumes_added.end(); ++it) @@ -102,7 +116,7 @@ void WorldModel::update(const UpdateRequest& req) Idx idx; if (findEntityIdx(e->id(), idx)) { - entity_shape_revisions_[idx] = revision_; + entity_volumes_revisions_[idx] = revision_; } } @@ -372,7 +386,9 @@ void WorldModel::removeEntity(const UUID& id) { entities_[it_idx->second].reset(); entity_revisions_[it_idx->second] = revision_; - entity_shape_revisions_[it_idx->second] = 0; + entity_visual_revisions_[it_idx->second] = 0; + entity_collision_revisions_[it_idx->second] = 0; + entity_volumes_revisions_[it_idx->second] = 0; entity_empty_spots_.push(it_idx->second); entity_map_.erase(it_idx); } @@ -446,7 +462,9 @@ Idx WorldModel::addNewEntity(const EntityConstPtr& e) idx = entities_.size(); entity_map_[e->id()] = idx; entities_.push_back(e); - entity_shape_revisions_.push_back(0); + entity_visual_revisions_.push_back(0); + entity_collision_revisions_.push_back(0); + entity_volumes_revisions_.push_back(0); } else { diff --git a/tools/view_model.cpp b/tools/view_model.cpp index 2bf3d58..80d05ec 100644 --- a/tools/view_model.cpp +++ b/tools/view_model.cpp @@ -153,12 +153,12 @@ int main(int argc, char **argv) { const ed::EntityConstPtr& e = *it; - if (e->shape()) + if (e->visual()) { const std::string& id = e->id().str(); if (id.size() < 5 || id.substr(id.size() - 5) != "floor") // Filter ground plane { - const std::vector& vertices = e->shape()->getMesh().getPoints(); + const std::vector& vertices = e->visual()->getMesh().getPoints(); for(unsigned int i = 0; i < vertices.size(); ++i) { const geo::Vector3& p = e->pose() * vertices[i]; @@ -172,8 +172,8 @@ int main(int argc, char **argv) } } - n_vertices += e->shape()->getMesh().getPoints().size(); - n_triangles += e->shape()->getMesh().getTriangleIs().size(); + n_vertices += e->visual()->getMesh().getPoints().size(); + n_triangles += e->visual()->getMesh().getTriangleIs().size(); } } From f9e9fc3fadd8f03d4745616248b846ff3a752e95 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 1 Apr 2023 11:08:06 +0200 Subject: [PATCH 4/9] (robot_plugin) also add collision model --- plugins/robot_plugin.cpp | 50 ++++++++++++++++++++++++++++++---------- 1 file changed, 38 insertions(+), 12 deletions(-) diff --git a/plugins/robot_plugin.cpp b/plugins/robot_plugin.cpp index 7e900e9..3630279 100644 --- a/plugins/robot_plugin.cpp +++ b/plugins/robot_plugin.cpp @@ -18,6 +18,8 @@ #include +#include + // ---------------------------------------------------------------------------------------------------- bool JointRelation::calculateTransform(const ed::Time& t, geo::Pose3D& tf) const @@ -144,12 +146,9 @@ geo::ShapePtr URDFGeometryToShape(const urdf::GeometrySharedPtr& geom) // ---------------------------------------------------------------------------------------------------- -geo::ShapePtr linkToShape(const urdf::LinkSharedPtr& link) +std::tuple LinkToShapes(const urdf::LinkSharedPtr& link) { - geo::CompositeShapePtr shape; - - if (!link->visual_array.size()) - return shape; + geo::CompositeShapePtr visual, collision; for (urdf::VisualSharedPtr& vis : link->visual_array) { @@ -169,12 +168,35 @@ geo::ShapePtr linkToShape(const urdf::LinkSharedPtr& link) if (!subshape) continue; - if (!shape) - shape.reset(new geo::CompositeShape()); - shape->addShape(*subshape, offset); + if (!visual) + visual.reset(new geo::CompositeShape()); + visual->addShape(*subshape, offset); } - return shape; + for (urdf::CollisionSharedPtr& col : link->collision_array) + { + const urdf::GeometrySharedPtr& geom = col->geometry; + if (!geom) + { + ROS_WARN_STREAM_NAMED("RobotPlugin" ,"[RobotPlugin] Robot model error: missing geometry for collision in link: '" << link->name << "'"); + continue; + } + + geo::Pose3D offset; + const urdf::Pose& o = col->origin; + offset.t = geo::Vector3(o.position.x, o.position.y, o.position.z); + offset.R.setRotation(geo::Quaternion(o.rotation.x, o.rotation.y, o.rotation.z, o.rotation.w)); + + geo::ShapePtr subshape = URDFGeometryToShape(geom); + if (!subshape) + continue; + + if (!collision) + collision.reset(new geo::CompositeShape()); + collision->addShape(*subshape, offset); + } + + return {visual, collision}; } // ---------------------------------------------------------------------------------------------------- @@ -335,13 +357,17 @@ void RobotPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& req) { const urdf::LinkSharedPtr& link = *it; - geo::ShapePtr shape = linkToShape(link); - if (shape) + geo::ShapePtr visual, collision; + std::tie(visual, collision) = LinkToShapes(link); + if (visual || collision) { std::string id = link->name; if (link->name.find(robot_name_) == std::string::npos) id = robot_name_ + "/" + link->name; - req.setVisual(id, shape); + if (visual) + req.setVisual(id, visual); + if (collision) + req.setCollision(id, collision); } } From ba512635c903d436aa7802a80ea7723485917aac Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 2 Apr 2023 21:28:35 +0200 Subject: [PATCH 5/9] Extend deprecated messages --- include/ed/entity.h | 6 +++--- include/ed/update_request.h | 2 +- include/ed/world_model.h | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/ed/entity.h b/include/ed/entity.h index 73c17b4..71b6fd4 100644 --- a/include/ed/entity.h +++ b/include/ed/entity.h @@ -53,11 +53,11 @@ class Entity void addMeasurement(MeasurementConstPtr measurement); - [[deprecated("Use visual() instead.")]] + [[deprecated("Use visual() or collision() instead.")]] inline geo::ShapeConstPtr shape() const { return visual(); } inline geo::ShapeConstPtr visual() const { return visual_; } inline geo::ShapeConstPtr collision() const { return collision_; } - [[deprecated("Use setVisual() instead.")]] + [[deprecated("Use setVisual() or setCollision() instead.")]] inline void setShape(const geo::ShapeConstPtr& shape) { setVisual(shape); } void setVisual(const geo::ShapeConstPtr& visual); void setCollision(const geo::ShapeConstPtr& collision); @@ -66,7 +66,7 @@ class Entity inline void addVolume(const std::string& volume_name, const geo::ShapeConstPtr& volume_shape) { volumes_[volume_name] = volume_shape; ++volumes_revision_; } inline void removeVolume(const std::string& volume_name) { volumes_.erase(volume_name); ++volumes_revision_; } - [[deprecated("Use visualRevision() instead.")]] + [[deprecated("Use visualRevision(), collisionRevision() or volumesRevision() instead.")]] inline unsigned long shapeRevision() const{ return visualRevision(); } inline unsigned long visualRevision() const{ return visual_ ? visual_revision_ : 0; } inline unsigned long collisionRevision() const{ return collision_ ? collision_revision_ : 0; } diff --git a/include/ed/update_request.h b/include/ed/update_request.h index feb6bc0..b987548 100644 --- a/include/ed/update_request.h +++ b/include/ed/update_request.h @@ -48,7 +48,7 @@ class UpdateRequest // VISUALS std::map visuals; - [[deprecated("Use setVisual() instead.")]] + [[deprecated("Use setVisual() or setCollision() instead.")]] void setShape(const UUID& id, const geo::ShapeConstPtr& shape) { setVisual(id, shape); } void setVisual(const UUID& id, const geo::ShapeConstPtr& visual) { visuals[id] = visual; flagUpdated(id); } diff --git a/include/ed/world_model.h b/include/ed/world_model.h index 52c7fa9..879c50e 100644 --- a/include/ed/world_model.h +++ b/include/ed/world_model.h @@ -102,7 +102,7 @@ class WorldModel const std::vector& entity_revisions() const { return entity_revisions_; } - [[deprecated("Use entity_visual_revisions() instead.")]] + [[deprecated("Use entity_visual_revisions(), entity_collision_revisions() or entity_volumes_revisions() instead.")]] const std::vector& entity_shape_revisions() const { return entity_visual_revisions(); } const std::vector& entity_visual_revisions() const { return entity_visual_revisions_; } From 4248438fc6ab76d54b12909c48f430ede4ea47f9 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 3 Apr 2023 09:05:59 +0200 Subject: [PATCH 6/9] boost::shared_ptr -> ed::shared_ptr --- include/ed/plugin_container.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ed/plugin_container.h b/include/ed/plugin_container.h index 014f090..10d7058 100644 --- a/include/ed/plugin_container.h +++ b/include/ed/plugin_container.h @@ -99,7 +99,7 @@ class PluginContainer UpdateRequestPtr update_request_; - boost::shared_ptr thread_; + ed::shared_ptr thread_; bool step_finished_; From 2c8d9a93029d966e3007b04bd320d302ed6fdc8a Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 4 Apr 2023 11:36:56 +0200 Subject: [PATCH 7/9] (ErrorContext) remove duplicate code --- src/error_context.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/error_context.cpp b/src/error_context.cpp index e6eccaa..5689f17 100644 --- a/src/error_context.cpp +++ b/src/error_context.cpp @@ -36,30 +36,30 @@ struct KeyHolder ErrorContext::ErrorContext(const char* msg, const char* value) { - ErrorContextData* data = static_cast(pthread_getspecific(key.key)); - if (!data) + ErrorContextData* _data = data(); + if (!_data) { - data = new ErrorContextData; - pthread_setspecific(key.key, data); + _data = new ErrorContextData; + pthread_setspecific(key.key, _data); } - data->stack.push_back(std::pair(msg, value)); + _data->stack.push_back(std::pair(msg, value)); } ErrorContext::~ErrorContext() { - ErrorContextData* data = static_cast(pthread_getspecific(key.key)); - if (!data) + ErrorContextData* _data = data(); + if (!_data) return; - data->stack.pop_back(); + _data->stack.pop_back(); } void ErrorContext::change(const char* msg, const char* value) { - ErrorContextData* data = static_cast(pthread_getspecific(key.key)); - data->stack.back() = std::pair(msg, value); + ErrorContextData* _data = data(); + _data->stack.back() = std::pair(msg, value); } ErrorContextData* ErrorContext::data() From a2d4d316b57b19624b82f5a4a63dc2abd6e0449e Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 4 Apr 2023 12:49:11 +0200 Subject: [PATCH 8/9] Move implementation of shape loader func to source file --- include/ed/models/shape_loader.h | 145 +---------------------------- src/models/shape_loader.cpp | 152 +++++++++++++++++++++++++++++++ 2 files changed, 157 insertions(+), 140 deletions(-) diff --git a/include/ed/models/shape_loader.h b/include/ed/models/shape_loader.h index ec03608..7fd216b 100644 --- a/include/ed/models/shape_loader.h +++ b/include/ed/models/shape_loader.h @@ -1,5 +1,5 @@ -#ifndef ED_SHAPE_LOADER_H_ -#define ED_SHAPE_LOADER_H_ +#ifndef ED_MODELS_SHAPE_LOADER_H_ +#define ED_MODELS_SHAPE_LOADER_H_ #include #include @@ -22,43 +22,8 @@ namespace models * @param height height of the cylinder * @param num_corners divided the circumference in N points and N+1 lines */ -void createCylinder(geo::Shape& shape, double radius, double height, int num_corners = 12) -{ - geo::Mesh mesh; - - // Calculate vertices - for(int i = 0; i < num_corners; ++i) - { - double a = 2 * M_PI * i / num_corners; - double x = sin(a) * radius; - double y = cos(a) * radius; - - mesh.addPoint(x, y, -height / 2); - mesh.addPoint(x, y, height / 2); - } - - // Calculate top and bottom triangles - for(int i = 1; i < num_corners - 1; ++i) - { - int i2 = 2 * i; - - // bottom - mesh.addTriangle(0, i2, i2 + 2); - - // top - mesh.addTriangle(1, i2 + 3, i2 + 1); - } +void createCylinder(geo::Shape& shape, double radius, double height, int num_corners = 12); - // Calculate side triangles - for(int i = 0; i < num_corners; ++i) - { - int j = (i + 1) % num_corners; - mesh.addTriangle(i * 2, i * 2 + 1, j * 2); - mesh.addTriangle(i * 2 + 1, j * 2 + 1, j * 2); - } - - shape.setMesh(mesh); -} /** * @brief getMiddlePoint Gets the middle point of two points in a mesh of a sphere. Uses a cache to not create double points. * The new point is placed on the radius of the sphere. @@ -69,115 +34,15 @@ void createCylinder(geo::Shape& shape, double radius, double height, int num_cor * @param radius radius of teh sphere * @return index of the inserted point */ -uint getMiddlePoint(geo::Mesh& mesh, uint i1, uint i2, std::map cache, double radius) -{ - // first check if we have it already - bool firstIsSmaller = i1 < i2; - unsigned long smallerIndex = firstIsSmaller ? i1 : i2; - unsigned long greaterIndex = firstIsSmaller ? i2 : i1; - unsigned long key = (smallerIndex << 32) + greaterIndex; - - std::map::const_iterator it = cache.find(key); - if (it != cache.end()) - return it->second; +uint getMiddlePoint(geo::Mesh& mesh, uint i1, uint i2, std::map cache, double radius); - // not in cache, calculate it - const std::vector& points = mesh.getPoints(); - geo::Vec3 p1 = points[i1]; - geo::Vec3 p2 = points[i2]; - geo::Vec3 p3((p1+p2)/2); - p3 = p3.normalized() * radius; - - // add vertex makes sure point is on unit sphere - uint i3 = mesh.addPoint(p3); - - // store it, return index - cache.insert(std::pair(key, i3)); - return i3; -} /** * @brief createSphere Create a shape of sphere * @param shape Shape object to be filled * @param radius radius of the sphere * @param recursion_level number of recursions to smooth the mesh, but rapidly increases the mesh. */ -void createSphere(geo::Shape& shape, double radius, uint recursion_level = 2) -{ - geo::Mesh mesh; - - // create 12 vertices of a icosahedron - double t = (1.0 + sqrt(5.0)) / 2.0; - - mesh.addPoint(geo::Vec3(-1, t, 0).normalized()*radius); - mesh.addPoint(geo::Vec3( 1, t, 0).normalized()*radius); - mesh.addPoint(geo::Vec3(-1, -t, 0).normalized()*radius); - mesh.addPoint(geo::Vec3( 1, -t, 0).normalized()*radius); - - mesh.addPoint(geo::Vec3( 0, -1, t).normalized()*radius); - mesh.addPoint(geo::Vec3( 0, 1, t).normalized()*radius); - mesh.addPoint(geo::Vec3( 0, -1, -t).normalized()*radius); - mesh.addPoint(geo::Vec3( 0, 1, -t).normalized()*radius); - - mesh.addPoint(geo::Vec3( t, 0, -1).normalized()*radius); - mesh.addPoint(geo::Vec3( t, 0, 1).normalized()*radius); - mesh.addPoint(geo::Vec3(-t, 0, -1).normalized()*radius); - mesh.addPoint(geo::Vec3(-t, 0, 1).normalized()*radius); - - // create 20 triangles of the icosahedron - // 5 faces around point 0 - mesh.addTriangle(0, 11, 5); - mesh.addTriangle(0, 5, 1); - mesh.addTriangle(0, 1, 7); - mesh.addTriangle(0, 7, 10); - mesh.addTriangle(0, 10, 11); - - // 5 adjacent faces - mesh.addTriangle(1, 5, 9); - mesh.addTriangle(5, 11, 4); - mesh.addTriangle(11, 10, 2); - mesh.addTriangle(10, 7, 6); - mesh.addTriangle(7, 1, 8); - - // 5 faces around point 3 - mesh.addTriangle(3, 9, 4); - mesh.addTriangle(3, 4, 2); - mesh.addTriangle(3, 2, 6); - mesh.addTriangle(3, 6, 8); - mesh.addTriangle(3, 8, 9); - - // 5 adjacent faces - mesh.addTriangle(4, 9, 5); - mesh.addTriangle(2, 4, 11); - mesh.addTriangle(6, 2, 10); - mesh.addTriangle(8, 6, 7); - mesh.addTriangle(9, 8, 1); - - for (uint i = 0; i < recursion_level; i++) - { - geo::Mesh mesh2; - std::map cache; - - const std::vector& points = mesh.getPoints(); - for (std::vector::const_iterator it = points.begin(); it != points.end(); ++it) - mesh2.addPoint(*it); - - const std::vector& triangleIs = mesh.getTriangleIs(); - for (std::vector::const_iterator it = triangleIs.begin(); it != triangleIs.end(); ++it) - { - // replace triangle by 4 triangles - uint a = getMiddlePoint(mesh2, it->i1_, it->i2_, cache, radius); - uint b = getMiddlePoint(mesh2, it->i2_, it->i3_, cache, radius); - uint c = getMiddlePoint(mesh2, it->i3_, it->i1_, cache, radius); - - mesh2.addTriangle(it->i1_, a, c); - mesh2.addTriangle(it->i2_, b, a); - mesh2.addTriangle(it->i3_, c, b); - mesh2.addTriangle(a, b, c); - } - mesh = mesh2; - } - shape.setMesh(mesh); -} +void createSphere(geo::Shape& shape, double radius, uint recursion_level = 2); } // end namespace models diff --git a/src/models/shape_loader.cpp b/src/models/shape_loader.cpp index 573ba5b..606db18 100644 --- a/src/models/shape_loader.cpp +++ b/src/models/shape_loader.cpp @@ -917,5 +917,157 @@ geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, return shape; } +// ---------------------------------------------------------------------------------------------------- + +void createCylinder(geo::Shape& shape, double radius, double height, int num_corners) +{ + geo::Mesh mesh; + + // Calculate vertices + for(int i = 0; i < num_corners; ++i) + { + double a = 2 * M_PI * i / num_corners; + double x = sin(a) * radius; + double y = cos(a) * radius; + + mesh.addPoint(x, y, -height / 2); + mesh.addPoint(x, y, height / 2); + } + + // Calculate top and bottom triangles + for(int i = 1; i < num_corners - 1; ++i) + { + int i2 = 2 * i; + + // bottom + mesh.addTriangle(0, i2, i2 + 2); + + // top + mesh.addTriangle(1, i2 + 3, i2 + 1); + } + + // Calculate side triangles + for(int i = 0; i < num_corners; ++i) + { + int j = (i + 1) % num_corners; + mesh.addTriangle(i * 2, i * 2 + 1, j * 2); + mesh.addTriangle(i * 2 + 1, j * 2 + 1, j * 2); + } + + shape.setMesh(mesh); +} + +// ---------------------------------------------------------------------------------------------------- + +uint getMiddlePoint(geo::Mesh& mesh, uint i1, uint i2, std::map cache, double radius) +{ + // first check if we have it already + bool firstIsSmaller = i1 < i2; + unsigned long smallerIndex = firstIsSmaller ? i1 : i2; + unsigned long greaterIndex = firstIsSmaller ? i2 : i1; + unsigned long key = (smallerIndex << 32) + greaterIndex; + + std::map::const_iterator it = cache.find(key); + if (it != cache.end()) + return it->second; + + // not in cache, calculate it + const std::vector& points = mesh.getPoints(); + geo::Vec3 p1 = points[i1]; + geo::Vec3 p2 = points[i2]; + geo::Vec3 p3((p1+p2)/2); + p3 = p3.normalized() * radius; + + // add vertex makes sure point is on unit sphere + uint i3 = mesh.addPoint(p3); + + // store it, return index + cache.insert(std::pair(key, i3)); + return i3; } + +// ---------------------------------------------------------------------------------------------------- + +void createSphere(geo::Shape& shape, double radius, uint recursion_level) +{ + geo::Mesh mesh; + + // create 12 vertices of a icosahedron + double t = (1.0 + sqrt(5.0)) / 2.0; + + mesh.addPoint(geo::Vec3(-1, t, 0).normalized()*radius); + mesh.addPoint(geo::Vec3( 1, t, 0).normalized()*radius); + mesh.addPoint(geo::Vec3(-1, -t, 0).normalized()*radius); + mesh.addPoint(geo::Vec3( 1, -t, 0).normalized()*radius); + + mesh.addPoint(geo::Vec3( 0, -1, t).normalized()*radius); + mesh.addPoint(geo::Vec3( 0, 1, t).normalized()*radius); + mesh.addPoint(geo::Vec3( 0, -1, -t).normalized()*radius); + mesh.addPoint(geo::Vec3( 0, 1, -t).normalized()*radius); + + mesh.addPoint(geo::Vec3( t, 0, -1).normalized()*radius); + mesh.addPoint(geo::Vec3( t, 0, 1).normalized()*radius); + mesh.addPoint(geo::Vec3(-t, 0, -1).normalized()*radius); + mesh.addPoint(geo::Vec3(-t, 0, 1).normalized()*radius); + + // create 20 triangles of the icosahedron + // 5 faces around point 0 + mesh.addTriangle(0, 11, 5); + mesh.addTriangle(0, 5, 1); + mesh.addTriangle(0, 1, 7); + mesh.addTriangle(0, 7, 10); + mesh.addTriangle(0, 10, 11); + + // 5 adjacent faces + mesh.addTriangle(1, 5, 9); + mesh.addTriangle(5, 11, 4); + mesh.addTriangle(11, 10, 2); + mesh.addTriangle(10, 7, 6); + mesh.addTriangle(7, 1, 8); + + // 5 faces around point 3 + mesh.addTriangle(3, 9, 4); + mesh.addTriangle(3, 4, 2); + mesh.addTriangle(3, 2, 6); + mesh.addTriangle(3, 6, 8); + mesh.addTriangle(3, 8, 9); + + // 5 adjacent faces + mesh.addTriangle(4, 9, 5); + mesh.addTriangle(2, 4, 11); + mesh.addTriangle(6, 2, 10); + mesh.addTriangle(8, 6, 7); + mesh.addTriangle(9, 8, 1); + + for (uint i = 0; i < recursion_level; i++) + { + geo::Mesh mesh2; + std::map cache; + + const std::vector& points = mesh.getPoints(); + for (std::vector::const_iterator it = points.begin(); it != points.end(); ++it) + mesh2.addPoint(*it); + + const std::vector& triangleIs = mesh.getTriangleIs(); + for (std::vector::const_iterator it = triangleIs.begin(); it != triangleIs.end(); ++it) + { + // replace triangle by 4 triangles + uint a = getMiddlePoint(mesh2, it->i1_, it->i2_, cache, radius); + uint b = getMiddlePoint(mesh2, it->i2_, it->i3_, cache, radius); + uint c = getMiddlePoint(mesh2, it->i3_, it->i1_, cache, radius); + + mesh2.addTriangle(it->i1_, a, c); + mesh2.addTriangle(it->i2_, b, a); + mesh2.addTriangle(it->i3_, c, b); + mesh2.addTriangle(a, b, c); + } + mesh = mesh2; + } + shape.setMesh(mesh); } + +// ---------------------------------------------------------------------------------------------------- + +} // end namespace models + +} // end namespace ed From 2d3a650ce5ff1eb0e7234b495be79dd5a4527b64 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 4 Apr 2023 12:49:30 +0200 Subject: [PATCH 9/9] Move docstrings to header --- src/models/shape_loader.cpp | 38 ---------------------------- src/models/shape_loader_private.h | 41 +++++++++++++++++++++++++++++++ 2 files changed, 41 insertions(+), 38 deletions(-) diff --git a/src/models/shape_loader.cpp b/src/models/shape_loader.cpp index 606db18..6b7196d 100644 --- a/src/models/shape_loader.cpp +++ b/src/models/shape_loader.cpp @@ -24,9 +24,6 @@ namespace ed namespace models { -/* -std::string split implementation by using delimiter as a character. Multiple delimeters are removed. -*/ /** * @brief split Implementation by using delimiter as a character. Multiple delimeters are removed. * @param strToSplit input string, which is splitted @@ -400,17 +397,6 @@ geo::ShapePtr getHeightMapShape(const std::string& image_filename, const geo::Ve // ---------------------------------------------------------------------------------------------------- -/** - * @brief getHeightMapShape convert grayscale image in a heigtmap mesh - * @param image_filename full path of grayscale image - * @param pos position of the origin of the heigtmap - * @param blockheight height of the heightmap of max grayscale value - * @param resolution_x resolution in x direction in meters - * @param resolution_y resolution in y direction in meters - * @param inverted false: CV/ROS standard (black = height); true: SDF/GAZEBO (White = height) - * @param errorerrorstream - * @return final mesh; or empty mesh in case of error - */ geo::ShapePtr getHeightMapShape(const std::string& image_filename, const geo::Vec3& pos, const double blockheight, const double resolution_x, const double resolution_y, const bool inverted, std::stringstream& error) { @@ -462,14 +448,6 @@ geo::ShapePtr getHeightMapShape(const std::string& image_filename, tue::config:: // ---------------------------------------------------------------------------------------------------- -/** - * @brief createPolygon create polygon mesh from points - * @param shape filled mesh - * @param points 2D points which define the mesh - * @param height height of the mesh - * @param error error stream - * @param create_bottom false: open bottom; true: closed bottom - */ void createPolygon(geo::Shape& shape, const std::vector& points, double height, std::stringstream& error, bool create_bottom) { TPPLPoly poly; @@ -580,14 +558,6 @@ bool readVec3Group(tue::config::Reader& cfg, geo::Vec3& v, const std::string& ve // ---------------------------------------------------------------------------------------------------- -/** - * @brief readPose read pose into Pose3D. Both ED yaml and SDF. Also reads pos(position) of SDF. - * @param cfg reader - * @param pose filled Pose3D pose - * @param pos_req position RequiredOrOptional - * @param rot_req rotation RequiredOrOptional - * @return indicates succes - */ bool readPose(tue::config::Reader& cfg, geo::Pose3D& pose, tue::config::RequiredOrOptional pos_req, tue::config::RequiredOrOptional rot_req) { double roll = 0, pitch = 0, yaw = 0; @@ -648,14 +618,6 @@ bool readPose(tue::config::Reader& cfg, geo::Pose3D& pose, tue::config::Required // ---------------------------------------------------------------------------------------------------- -/** - * @brief loadShape load the shape of a model. - * @param model_path path of the model - * @param cfg reader - * @param shape_cache cache for complex models - * @param error errorstream - * @return final mesh; or empty mesh in case of error - */ geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, std::map& shape_cache, std::stringstream& error) { diff --git a/src/models/shape_loader_private.h b/src/models/shape_loader_private.h index fcb7f5a..1a1b2c6 100644 --- a/src/models/shape_loader_private.h +++ b/src/models/shape_loader_private.h @@ -28,20 +28,61 @@ enum ModelOrFile FILE }; +/** + * @brief split Implementation by using delimiter as a character. Multiple delimeters are removed. + * @param strToSplit input string, which is splitted + * @param delimeter char on which the string is split + * @return vector of sub-strings + */ std::vector split(const std::string& strToSplit, char delimeter); std::string parseURI(const std::string& uri, ModelOrFile& uri_type); +/** + * @brief loadShape load the shape of a model. + * @param model_path path of the model + * @param cfg reader + * @param shape_cache cache for complex models + * @param error errorstream + * @return final mesh; or empty mesh in case of error + */ geo::ShapePtr loadShape(const std::string& model_path, tue::config::Reader cfg, std::map& shape_cache, std::stringstream& error); +/** + * @brief getHeightMapShape convert grayscale image in a heigtmap mesh + * @param image_filename full path of grayscale image + * @param pos position of the origin of the heigtmap + * @param blockheight height of the heightmap of max grayscale value + * @param resolution_x resolution in x direction in meters + * @param resolution_y resolution in y direction in meters + * @param inverted false: CV/ROS standard (black = height); true: SDF/GAZEBO (White = height) + * @param errorerrorstream + * @return final mesh; or empty mesh in case of error + */ geo::ShapePtr getHeightMapShape(const std::string& image_filename, const geo::Vec3& pos, const double blockheight, const double resolution_x, const double resolution_y, const bool inverted, std::stringstream& error); +/** + * @brief readPose read pose into Pose3D. Both ED yaml and SDF. Also reads pos(position) of SDF. + * @param cfg reader + * @param pose filled Pose3D pose + * @param pos_req position RequiredOrOptional + * @param rot_req rotation RequiredOrOptional + * @return indicates succes + */ bool readPose(tue::config::Reader& cfg, geo::Pose3D& pose, tue::config::RequiredOrOptional pos_req = tue::config::REQUIRED, tue::config::RequiredOrOptional rot_req = tue::config::OPTIONAL); +/** + * @brief createPolygon create polygon mesh from points + * @param shape filled mesh + * @param points 2D points which define the mesh + * @param height height of the mesh + * @param error error stream + * @param create_bottom false: open bottom; true: closed bottom + */ void createPolygon(geo::Shape& shape, const std::vector& points, double height, std::stringstream& error, bool create_bottom = true); } // end models namespace