commited a lot of unstaged stuff, no idea what they do...

tue-robotics · Aug 14, 2023 · ff8f81a · ff8f81a
1 parent 495ffac
commit ff8f81a
Show file tree

Hide file tree

Showing 5 changed files with 22 additions and 1 deletion.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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)