From ff8f81a713e817e6c1da7b1e2862f3dd226149db Mon Sep 17 00:00:00 2001
From: PetervDooren <PetervDooren@outlook.com>
Date: Mon, 14 Aug 2023 14:30:34 +0200
Subject: [PATCH] commited a lot of unstaged stuff, no idea what they do...

---
 include/ed/entity.h                  | 11 +++++++++++
 include/ed/helpers/msg_conversions.h |  1 +
 include/ed/update_request.h          |  2 ++
 src/entity.cpp                       |  3 ++-
 src/world_model.cpp                  |  6 ++++++
 5 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/include/ed/entity.h b/include/ed/entity.h
index 2f621deb..e5ff29e9 100644
--- a/include/ed/entity.h
+++ b/include/ed/entity.h
@@ -110,6 +110,16 @@ class Entity
         has_pose_ = true;
     }
 
+    inline void setPoseFrame(const std::string& frame_name)
+    {
+        pose_frame_ = frame_name;
+    }
+
+    inline const std::string poseFrame() const
+    {
+        return pose_frame_;
+    }
+
     inline bool has_pose() const { return has_pose_; }
 
     inline const tue::config::DataConstPointer& data() const { return config_; }
@@ -256,6 +266,7 @@ class Entity
 
     bool has_pose_;
     geo::Pose3D pose_;
+    std::string pose_frame_;
 
 //    double creation_time_;
 
diff --git a/include/ed/helpers/msg_conversions.h b/include/ed/helpers/msg_conversions.h
index 54cb2b50..cb5d7d04 100644
--- a/include/ed/helpers/msg_conversions.h
+++ b/include/ed/helpers/msg_conversions.h
@@ -84,6 +84,7 @@ void convert(const ed::Entity& e, ed_msgs::EntityInfo& msg) {
     {
         geo::convert(e.pose(), msg.pose);
     }
+    msg.pose_frame = e.poseFrame();
 
     msg.last_update_time =  ros::Time(e.lastUpdateTimestamp());
 
diff --git a/include/ed/update_request.h b/include/ed/update_request.h
index 48b0d3c3..3445be9e 100644
--- a/include/ed/update_request.h
+++ b/include/ed/update_request.h
@@ -126,7 +126,9 @@ class UpdateRequest
     // POSES
 
     std::map<UUID, geo::Pose3D> poses;
+    std::map<UUID, std::string> pose_frames;
     void setPose(const UUID& id, const geo::Pose3D& pose) { poses[id] = pose; flagUpdated(id); }
+    void setPoseFrame(const UUID& id, const std::string& frame) { pose_frames[id] = frame; flagUpdated(id); }
 
 
     // RELATIONS
diff --git a/src/entity.cpp b/src/entity.cpp
index 67612ee0..8ce075d8 100644
--- a/src/entity.cpp
+++ b/src/entity.cpp
@@ -25,7 +25,8 @@ Entity::Entity(const UUID& id, const TYPE& type, const unsigned int& measurement
     shape_revision_(0),
 //    creation_time_(creation_time),
     has_pose_(false),
-    pose_(geo::Pose3D::identity())
+    pose_(geo::Pose3D::identity()),
+    pose_frame_("map")
 {
 }
 
diff --git a/src/world_model.cpp b/src/world_model.cpp
index d6d55938..fd3aa9c5 100644
--- a/src/world_model.cpp
+++ b/src/world_model.cpp
@@ -47,6 +47,12 @@ void WorldModel::update(const UpdateRequest& req)
         EntityPtr e = getOrAddEntity(it->first, new_entities);
         e->setPose(it->second);
     }
+    // Update poses
+    for(std::map<UUID, std::string>::const_iterator it = req.pose_frames.begin(); it != req.pose_frames.end(); ++it)
+    {
+        EntityPtr e = getOrAddEntity(it->first, new_entities);
+        e->setPoseFrame(it->second);
+    }
 
     // Update shapes
     for(std::map<UUID, geo::ShapeConstPtr>::const_iterator it = req.shapes.begin(); it != req.shapes.end(); ++it)