feat(awviz_common): add a class to mange transform frames and entity

Doxyfile

@@ -1,4 +1,4 @@
-PROJECT_NAME = AWViz ROS C++ API
+PROJECT_NAME = AWViz-ROS C++ API Reference
 
 DOXYFILE_ENCODING = UTF-8
 

README.md

@@ -35,3 +35,11 @@ awviz
 
 Some ROS messages are already covered by built-in plugins defined in `awviz_plugin`.
 For the detail of supported ROS messages and plugin customization, please refer to [docs/PLUGIN.md](./docs/PLUGIN.md).
+
+## Contribution
+
+Please refer to [docs/CONTRIBUTING.md](./docs/CONTRIBUTING.md).
+
+## API Reference
+
+Please refer to [AWViz-ROS C++ API Reference](https://ktro2828.github.io/awviz-ros/).

awviz_common/CMakeLists.txt

@@ -38,9 +38,9 @@ target_include_directories(${PROJECT_NAME} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>
 )
-target_link_libraries(${PROJECT_NAME} rerun_sdk)
+target_link_libraries(${PROJECT_NAME} rerun_sdk yaml-cpp)
 # NOTE: ament_target_dependencies ensures that all dependency include directories are ordered correctly when using an overlay workspace
-ament_target_dependencies(${PROJECT_NAME} rclcpp pluginlib)
+ament_target_dependencies(${PROJECT_NAME} rclcpp pluginlib tf2 tf2_ros)
 
 # -------- install targets --------
 install(

awviz_common/awviz_commonConfig.cmake.in

@@ -4,5 +4,8 @@ include(CMakeFindDependencyMacro)
 
 find_dependency(rclcpp)
 find_dependency(pluginlib)
+find_dependency(tf2)
+find_dependency(tf2_ros)
+find_dependency(yaml-cpp)
 
 include("${CMAKE_CURRENT_LIST_DIR}/@[email protected]")

awviz_common/include/awviz_common/display.hpp

@@ -24,6 +24,7 @@
 
 #include <memory>
 #include <string>
+#include <unordered_map>
 
 namespace awviz_common
 {
@@ -50,7 +51,9 @@ class Display
    * @param topic Name of topic.
    * @param entity Entity path of the record.
    */
-  virtual void setStatus(const std::string & topic, const std::string & entity) = 0;
+  virtual void setStatus(
+    const std::string & topic,
+    const std::shared_ptr<std::unordered_map<std::string, std::string>> entity_roots) = 0;
 
   /**
    * @brief Start to display.
@@ -66,12 +69,12 @@ class Display
    * @brief Return true if the initialization is completed.
    * @return bool Return the value of the private member named `is_initialized_`.
    */
-  bool isInitialized() const { return is_initialized_; }
+  virtual bool isInitialized() const { return is_initialized_; }
 
 protected:
-  rclcpp::Node::SharedPtr node_;
-  std::shared_ptr<rerun::RecordingStream> stream_;
-  bool is_initialized_;
+  rclcpp::Node::SharedPtr node_;                    //!< Node shared pointer.
+  std::shared_ptr<rerun::RecordingStream> stream_;  //!< RecordingStream shared pointer.
+  bool is_initialized_;                             //!< Whether the object has been initialized.
 };
 
 /**
@@ -113,10 +116,12 @@ class RosTopicDisplay : public Display
    * @param topic Name of topic.
    * @param entity Entity path of the record.
    */
-  void setStatus(const std::string & topic, const std::string & entity) override
+  void setStatus(
+    const std::string & topic,
+    const std::shared_ptr<std::unordered_map<std::string, std::string>> entity_roots) override
   {
     property_.setTopic(topic);
-    property_.setEntity(entity);
+    property_.setEntityRoots(entity_roots);
   }
 
   /**
@@ -129,11 +134,14 @@ class RosTopicDisplay : public Display
    */
   void end() override { unsubscribe(); }
 
+  bool isInitialized() const override { return is_initialized_ && property_.isInitialized(); }
+
 protected:
   static constexpr const char * TIMELINE_NAME = "timestamp";  //!< Entity name of timeline record.
 
   /**
    * @brief Start to subscribing the specified topic.
+   * @todo Currently, `rclcpp::SensorDataQoS` is used for QoS profile setting.
    */
   virtual void subscribe()
   {
@@ -154,7 +162,8 @@ class RosTopicDisplay : public Display
 
   /**
    * @brief Log subscribed ROS message to recording stream.
-   * @param msg ROS message.
+   * @param msg Constant shared pointer of ROS message.
+   * @note Currently, if the corresponding entity path doesn't exist this just logs warning as text.
    */
   virtual void logToStream(typename MsgType::ConstSharedPtr msg) = 0;
 

awviz_common/include/awviz_common/property.hpp

@@ -15,7 +15,10 @@
 #ifndef AWVIZ_COMMON__PROPERTY_HPP_
 #define AWVIZ_COMMON__PROPERTY_HPP_
 
+#include <memory>
+#include <optional>
 #include <string>
+#include <unordered_map>
 
 namespace awviz_common
 {
@@ -31,10 +34,12 @@ class RosTopicProperty
    * @brief Construct instance.
    * @param type ROS msg type.
    * @param topic Name of topic.
-   * @param entity Entity path to log.
+   * @param entity_roots Root entity paths to recording.
    */
-  RosTopicProperty(const std::string & type, const std::string & topic, const std::string & entity)
-  : type_(type), topic_(topic), entity_(entity)
+  RosTopicProperty(
+    const std::string & type, const std::string & topic,
+    const std::shared_ptr<std::unordered_map<std::string, std::string>> entity_roots)
+  : type_(type), topic_(topic), entity_roots_(entity_roots)
   {
   }
 
@@ -54,7 +59,11 @@ class RosTopicProperty
    * @brief Set entity path of record.
    * @param entity Entity path of record.
    */
-  void setEntity(const std::string & entity) { entity_ = entity; }
+  void setEntityRoots(
+    const std::shared_ptr<std::unordered_map<std::string, std::string>> entity_roots)
+  {
+    entity_roots_ = entity_roots;
+  }
 
   /**
    * @brief Get ROS message type.
@@ -69,15 +78,34 @@ class RosTopicProperty
   const std::string & topic() const noexcept { return topic_; }
 
   /**
-   * @brief Get entity path of record.
-   * @return Entity path of record.
+   * @brief Return the entity path using topic name.
+   * @return Topic name used as entity path.
+   */
+  const std::string & entity() const noexcept { return topic_; }
+
+  /**
+   * @brief Return the entity path using the corresponding root and its frame ID.
+   *
+   * @param frame_id Frame ID.
+   * @return Return "/<Root>/<FrameID>/<Topic>" if the corresponding root exists, otherwise return
+   * `std::nullopt`.
    */
-  const std::string & entity() const noexcept { return entity_; }
+  std::optional<std::string> entity(const std::string & frame_id) const noexcept
+  {
+    if (entity_roots_ && entity_roots_->count(frame_id) > 0) {
+      return entity_roots_->at(frame_id) + topic_;
+    } else {
+      return std::nullopt;
+    }
+  }
+
+  bool isInitialized() const { return !type_.empty() && !topic_.empty() && entity_roots_; }
 
 private:
-  std::string type_;    //!< Type of ROS message.
-  std::string topic_;   //!< Name of topic.
-  std::string entity_;  //!< Entity path of recording.
+  std::string type_;   //!< Type of ROS message.
+  std::string topic_;  //!< Name of topic.
+  std::shared_ptr<std::unordered_map<std::string, std::string>>
+    entity_roots_;  //!< Entity root paths of recording.
 };
 }  // namespace awviz_common
 

awviz_common/include/awviz_common/transformation/tf_tree.hpp

@@ -0,0 +1,164 @@
+// Copyright 2024 Kotaro Uetake.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AWVIZ_COMMON__TF_TREE_HPP_
+#define AWVIZ_COMMON__TF_TREE_HPP_
+
+#include <algorithm>
+#include <memory>
+#include <optional>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace awviz_common
+{
+/**
+ * @brief A class to represent a TF frame information.
+ */
+class TfFrame
+{
+public:
+  /**
+   * @brief Construct a new object.
+   *
+   * @param id Frame ID.
+   * @param parent Parent frame ID.
+   */
+  TfFrame(const std::string & id, const std::string & parent) : id_(id), parent_(parent) {}
+
+  /**
+   * @brief Construct a new object with empty string for parent.
+   *
+   * @param id Frame ID.
+   */
+  explicit TfFrame(const std::string & id) : id_(id), parent_("") {}
+
+  /**
+   * @brief Return own frame ID.
+   *
+   * @return Own frame ID.
+   */
+  const std::string & id() const { return id_; }
+
+  /**
+   * @brief Return the parent frame ID.
+   *
+   * @return Parent frame ID.
+   */
+  const std::string & parent() const { return parent_; }
+
+  /**
+   * @brief Indicate whether the frame is root by checking if `parent_` is empty.
+   *
+   * @return Return true, if the `parent_` is empty.
+   */
+  bool is_root() const { return parent_.empty(); }
+
+  /**
+   * @brief Return whether the tf frame is static or not.
+   * @note Currently, this returns true if the parent id is not `"map"`.
+   *
+   * @return Return true if the parent id is not `"map"`.
+   */
+  bool is_static() const { return parent_ != "map"; }
+
+private:
+  std::string id_;      //!< Frame ID.
+  std::string parent_;  //!< Parent frame ID.
+};
+
+class TfTree
+{
+public:
+  /**
+   * @brief Add a new tf frame to the tree.
+   *
+   * @param frame A new tf frame. If it has been already registered, skip adding.
+   */
+  void emplace(const TfFrame & frame)
+  {
+    if (!contains(frame.id())) {
+      frames_.emplace(frame.id(), frame);
+    }
+
+    if (!frame.is_root()) {
+      emplace(frame.parent());
+    }
+  }
+
+  /**
+   * @brief Add a new tf frame to the tree with the empty string parent.
+   *
+   * @param id Frame ID. If it has been already registered, skip adding.
+   */
+  void emplace(const std::string & id)
+  {
+    if (!contains(id)) {
+      frames_.emplace(id, id);
+    }
+  }
+
+  /**
+   * @brief Return map of all frames.
+   *
+   * @return Shared pointer of all frames.
+   */
+  const std::unordered_map<std::string, TfFrame> & get_frames() const { return frames_; }
+
+  /**
+   * @brief Get the `TfFrame` object.
+   *
+   * @param id Frame ID.
+   * @return Corresponding `TfFrame` object. If there is no target `TfFrame`, returns `nullptr`.
+   */
+  std::optional<TfFrame> get_frame(const std::string & id) const
+  {
+    return contains(id) ? std::make_optional(frames_.at(id)) : std::nullopt;
+  }
+
+  /**
+   * @brief Get the parent `TfFrame` object.
+   *
+   * @param id Frame ID.
+   * @return Parent `TfFrame` object. If there is no parent, returns `nullptr`.
+   */
+  std::optional<TfFrame> get_parent(const std::string & id) const
+  {
+    auto frame = get_frame(id);
+    return frame ? get_frame(frame->parent()) : std::nullopt;
+  }
+
+  /**
+   * @brief Whether to the specified frame is contained in the tree.
+   *
+   * @param id Frame ID.
+   * @return Returns true, if the frame is contained.
+   */
+  bool contains(const std::string & id) const { return frames_.count(id) > 0; }
+
+  /**
+   * @brief Whether to the parent of specified frame is contained in the tree.
+   *
+   * @param id Frame ID.
+   * @return Returns true, if the parent frame is contained.
+   */
+  bool is_root(const std::string & id) const { return contains(id) && frames_.at(id).is_root(); }
+
+private:
+  std::unordered_map<std::string, TfFrame> frames_;  //!< Map to store frames.
+};
+}  // namespace awviz_common
+
+#endif  // AWVIZ_COMMON__TF_TREE_HPP_