rpo2023 (#163)

tue-robotics · May 6, 2023 · 60e7b1f · 60e7b1f
2 parents c2ab283 + 4dd527c
commit 60e7b1f
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 0 deletions.
diff --git 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_; }

diff --git 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/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";

diff --git 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)
     {