From 6586cb76bbbd28df1179d657941a05141d29d876 Mon Sep 17 00:00:00 2001
From: Matthijs van der Burgh <MatthijsBurgh@outlook.com>
Date: Fri, 28 Apr 2023 16:15:12 +0100
Subject: [PATCH 1/3] (entity) add removePose

---
 include/ed/entity.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/include/ed/entity.h b/include/ed/entity.h
index 71b6fd4..c31ce27 100644
--- a/include/ed/entity.h
+++ b/include/ed/entity.h
@@ -110,6 +110,8 @@ class Entity
         has_pose_ = true;
     }
 
+    inline void removePose() { has_pose_ = false; }
+
     inline bool has_pose() const { return has_pose_; }
 
     inline const tue::config::DataConstPointer& data() const { return config_; }

From 40b97b06606ce3fae812e7cacf3b590425e7ccc0 Mon Sep 17 00:00:00 2001
From: Matthijs van der Burgh <MatthijsBurgh@outlook.com>
Date: Fri, 28 Apr 2023 16:15:51 +0100
Subject: [PATCH 2/3] (WM)(update_req) add removePose

---
 include/ed/update_request.h | 3 +++
 src/world_model.cpp         | 6 ++++++
 2 files changed, 9 insertions(+)

diff --git a/include/ed/update_request.h b/include/ed/update_request.h
index b987548..d23e4b9 100644
--- a/include/ed/update_request.h
+++ b/include/ed/update_request.h
@@ -131,6 +131,9 @@ class UpdateRequest
     std::map<UUID, geo::Pose3D> poses;
     void setPose(const UUID& id, const geo::Pose3D& pose) { poses[id] = pose; flagUpdated(id); }
 
+    std::vector<UUID> poses_removed;
+    void removePose(const UUID& id) { poses_removed.push_back(id); flagUpdated(id); }
+
 
     // RELATIONS
 
diff --git a/src/world_model.cpp b/src/world_model.cpp
index ed74d15..450fd94 100644
--- a/src/world_model.cpp
+++ b/src/world_model.cpp
@@ -48,6 +48,12 @@ void WorldModel::update(const UpdateRequest& req)
         e->setPose(it->second);
     }
 
+    for (const UUID& id : req.poses_removed)
+    {
+        EntityPtr e = getOrAddEntity(id, new_entities);
+        e->removePose();
+    }
+
     // Update visuals
     for(std::map<UUID, geo::ShapeConstPtr>::const_iterator it = req.visuals.begin(); it != req.visuals.end(); ++it)
     {

From 4dd527c28c4456995e03192a41d3c7e4c2e8f238 Mon Sep 17 00:00:00 2001
From: Matthijs van der Burgh <MatthijsBurgh@outlook.com>
Date: Fri, 28 Apr 2023 16:16:15 +0100
Subject: [PATCH 3/3] Add remove func to update srv

---
 src/ed.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/ed.cpp b/src/ed.cpp
index d3912fc..525892f 100644
--- a/src/ed.cpp
+++ b/src/ed.cpp
@@ -109,6 +109,7 @@ bool srvUpdate(ed_msgs::UpdateSrv::Request& req, ed_msgs::UpdateSrv::Response& r
             if (r.readGroup("pose"))
             {
                 double x, y, z;
+                std::string remove;
                 if (r.readValue("x", x) && r.readValue("y", y) && r.readValue("z", z))
                 {
                     geo::Pose3D pose = geo::Pose3D::identity();
@@ -120,6 +121,10 @@ bool srvUpdate(ed_msgs::UpdateSrv::Request& req, ed_msgs::UpdateSrv::Response& r
 
                     update_req.setPose(id, pose);
                 }
+                else if(r.readValue("remove", remove) && remove == "true")
+                {
+                    update_req.removePose(id);
+                }
                 else
                 {
                     res.response += "For entity '" + id + "': invalid pose (position).\n";