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_; } 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 poses; void setPose(const UUID& id, const geo::Pose3D& pose) { poses[id] = pose; flagUpdated(id); } + std::vector 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 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"; 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::const_iterator it = req.visuals.begin(); it != req.visuals.end(); ++it) {