add pose to the volumes list

tue-robotics · Apr 12, 2023 · 495ffac · 495ffac
1 parent 6ac5521
commit 495ffac
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 10 deletions.
diff --git a/include/ed/entity.h b/include/ed/entity.h
@@ -22,9 +22,14 @@
 #include <set>
 #include <vector>
 
+
 namespace ed
 {
 
+struct semanticGeometry{
+    geo::Pose3D pose;
+    geo::ShapeConstPtr shape; 
+};
 // ----------------------------------------------------------------------------------------------------
 
 class Entity
@@ -56,8 +61,13 @@ class Entity
     inline geo::ShapeConstPtr shape() const { return shape_; }
     void setShape(const geo::ShapeConstPtr& shape);
 
-    inline const std::map<std::string, geo::ShapeConstPtr>& 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 const std::map<std::string, semanticGeometry>& volumes() const { return volumes_; }
+    inline void addVolume(const std::string& volume_name, const geo::ShapeConstPtr& volume_shape) {
+        volumes_[volume_name] = semanticGeometry();
+        volumes_[volume_name].pose = geo::Pose3D::identity();
+        volumes_[volume_name].shape = volume_shape;
+        ++shape_revision_;
+        }
     inline void removeVolume(const std::string& volume_name) { volumes_.erase(volume_name); ++shape_revision_; }
 
     inline unsigned long shapeRevision() const{ return shape_ ? shape_revision_ : 0; }
@@ -238,7 +248,7 @@ class Entity
     unsigned int measurements_seq_;
 
     geo::ShapeConstPtr shape_;
-    std::map<std::string, geo::ShapeConstPtr> volumes_;
+    std::map<std::string, semanticGeometry> volumes_;
     unsigned long shape_revision_;
 
     std::map<std::string, MeasurementConvexHull> convex_hull_map_;

diff --git a/include/ed/helpers/msg_conversions.h b/include/ed/helpers/msg_conversions.h
@@ -102,12 +102,12 @@ void convert(const ed::Entity& e, ed_msgs::EntityInfo& msg) {
 
     if (!e.volumes().empty())
     {
-        for (std::map<std::string, geo::ShapeConstPtr>::const_iterator it = e.volumes().begin(); it != e.volumes().end(); ++it)
+        for (std::map<std::string, ed::semanticGeometry>::const_iterator it = e.volumes().begin(); it != e.volumes().end(); ++it)
         {
             ed_msgs::Volume volume;
             volume.name = it->first;
 
-            geo::CompositeShapeConstPtr composite = std::dynamic_pointer_cast<const geo::CompositeShape>(it->second);
+            geo::CompositeShapeConstPtr composite = std::dynamic_pointer_cast<const geo::CompositeShape>(it->second.shape);
             if (composite)
             {
                 const std::vector<std::pair<geo::ShapePtr, geo::Transform> >&  shapes = composite->getShapes();
@@ -126,7 +126,7 @@ void convert(const ed::Entity& e, ed_msgs::EntityInfo& msg) {
             else
             {
                 ed_msgs::SubVolume sub_volume;
-                convert(it->second, sub_volume);
+                convert(it->second.shape, sub_volume);
                 volume.subvolumes.push_back(sub_volume);
             }
             msg.volumes.push_back(volume);

diff --git a/src/rendering.cpp b/src/rendering.cpp
@@ -183,18 +183,18 @@ bool renderWorldModel(const ed::WorldModel& world_model, const enum ShowVolumes
             // Render volumes
             if (show_volumes == ModelVolumes && !e->volumes().empty())
             {
-                for (std::map<std::string, geo::ShapeConstPtr>::const_iterator it = e->volumes().begin(); it != e->volumes().end(); ++it)
+                for (std::map<std::string, ed::semanticGeometry>::const_iterator it = e->volumes().begin(); it != e->volumes().end(); ++it)
                 {
-                    renderMesh(cam, pose, it->second->getMesh(), cv::Vec3b(0, 0, 255), res, flatten); // Red
+                    renderMesh(cam, pose, it->second.shape->getMesh(), cv::Vec3b(0, 0, 255), res, flatten); // Red
                 }
             }
         }
         else if (show_volumes == RoomVolumes && e->types().find("room") != e->types().end())
         {
             geo::Pose3D pose = cam_pose_inv * e->pose();
-            for (std::map<std::string, geo::ShapeConstPtr>::const_iterator it = e->volumes().begin(); it != e->volumes().end(); ++it)
+            for (std::map<std::string, ed::semanticGeometry>::const_iterator it = e->volumes().begin(); it != e->volumes().end(); ++it)
             {
-                renderMesh(cam, pose, it->second->getMesh(), cv::Vec3b(0, 0, 255), res, flatten); // Red
+                renderMesh(cam, pose, it->second.shape->getMesh(), cv::Vec3b(0, 0, 255), res, flatten); // Red
             }
         }