fix: resolve TF tree and add support of sensor_msgs::msg::CameraInfo

awviz_common/CMakeLists.txt

@@ -80,10 +80,15 @@ install(FILES
 # -------- for testing --------
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
-  set(ament_cmake_copyright_FOUND TRUE)
-  set(ament_cmake_cppcheck_FOUND TRUE)
-  set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
+
+  # -------- test transformation --------
+  ament_auto_add_gtest(test_tf_frame
+    test/transformation/test_tf_frame.cpp
+  )
+  ament_auto_add_gtest(test_tf_tree
+    test/transformation/test_tf_tree.cpp
+  )
 endif()
 
 ament_auto_package()

awviz_common/include/awviz_common/property.hpp

@@ -84,7 +84,7 @@ class RosTopicProperty
   const std::string & entity() const noexcept { return topic_; }
 
   /**
-   * @brief Return the entity path using the corresponding root and its frame ID.
+   * @brief Return the entity path using the corresponding root and its frame ID with its topic.
    *
    * @param frame_id Frame ID.
    * @return Return "/<Root>/<FrameID>/<Topic>" if the corresponding root exists, otherwise return
@@ -99,6 +99,22 @@ class RosTopicProperty
     }
   }
 
+  /**
+   * @brief Return the entity path using the corresponding root and its frame ID without its topic.
+   *
+   * @param frame_id Frame ID.
+   * @return Return "/<Root>/<FrameID>" if the corresponding root exists, otherwise return
+   * `std::nullopt`.
+   */
+  std::optional<std::string> entity_without_topic(const std::string & frame_id) const noexcept
+  {
+    if (entity_roots_ && entity_roots_->count(frame_id) > 0) {
+      return entity_roots_->at(frame_id);
+    } else {
+      return std::nullopt;
+    }
+  }
+
   bool is_initialized() const { return !type_.empty() && !topic_.empty() && entity_roots_; }
 
 private:

awviz_common/include/awviz_common/transformation/tf_tree.hpp

@@ -16,6 +16,7 @@
 #define AWVIZ_COMMON__TF_TREE_HPP_
 
 #include <algorithm>
+#include <cstring>
 #include <memory>
 #include <optional>
 #include <string>
@@ -24,6 +25,9 @@
 
 namespace awviz_common
 {
+
+constexpr const char * TF_ROOT = "map";  //!< Root transformation frame.
+
 /**
  * @brief A class to represent a TF frame information.
  */
@@ -68,11 +72,22 @@ class TfFrame
 
   /**
    * @brief Return whether the tf frame is static or not.
-   * @note Currently, this returns true if the parent id is not `"map"`.
+   * @note Currently, this returns true if the parent id is not `TF_ROOT` or is empty.
+   *
+   * @return Return true if the parent id is not `TF_ROOT` or is empty.
+   */
+  bool is_static() const { return std::strcmp(parent_.c_str(), TF_ROOT) != 0 || parent_.empty(); }
+
+  /**
+   * @brief Compare with an another object.
    *
-   * @return Return true if the parent id is not `"map"`.
+   * @param other Another object.
+   * @return Return true if both id and parent are the same.
    */
-  bool is_static() const { return parent_ != "map"; }
+  bool operator==(const TfFrame & other) const
+  {
+    return id_ == other.id() && parent_ == other.parent();
+  }
 
 private:
   std::string id_;      //!< Frame ID.
@@ -83,32 +98,23 @@ 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.
+   * @brief Construct a object setting `TF_ROOT` as root.
    */
-  void emplace(const TfFrame & frame)
-  {
-    if (!contains(frame.id())) {
-      frames_.emplace(frame.id(), frame);
-    }
-
-    if (!frame.is_root()) {
-      emplace(frame.parent());
-    }
-  }
+  TfTree() : frames_() { frames_.emplace(TF_ROOT, TF_ROOT); }
 
   /**
-   * @brief Add a new tf frame to the tree with the empty string parent.
+   * @brief Add a new tf frame to the tree.
    *
-   * @param id Frame ID. If it has been already registered, skip adding.
+   * @param frame A new tf frame. If it has been already registered, skip adding.
    */
-  void emplace(const std::string & id)
-  {
-    if (!contains(id)) {
-      frames_.emplace(id, id);
-    }
-  }
+  void emplace(const TfFrame & frame) { frames_.emplace(frame.id(), frame); }
+
+  // /**
+  //  * @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) { frames_.emplace(id, id); }
 
   /**
    * @brief Return map of all frames.
@@ -141,20 +147,63 @@ class TfTree
   }
 
   /**
-   * @brief Whether to the specified frame is contained in the tree.
+   * @brief Whether to the specified frame ID is contained as a node of 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.
+   * @brief Whether to the specified frame is contained in the tree checking both key and value.
+   *
+   * @param frame `TfFrame` object.
+   * @return true Return true if the tree contains the ID of `frame` as a key, and then if its value
+   * is equivalent to `frame`.
+   */
+  bool contains(const TfFrame & frame) const
+  {
+    return contains(frame.id()) && frame == frames_.at(frame.id());
+  }
+
+  /**
+   * @brief Return entity path of the specified frame. The entity path will be in the format
+   * `"/<Parent0>/<Parent1>/.../<FrameID>"`, where `"<Parent0>"` represents the deepest parent
+   * frame.
    *
+   * @param frame `TfFrame` Object.
+   * @return Entity path of the frame.
+   */
+  std::string entity_path(const TfFrame & frame) const
+  {
+    auto current = std::make_optional<TfFrame>(frame);
+    std::string entity = "/" + current->id();
+    while (current && !current->is_root()) {
+      current = get_frame(current->parent());
+      if (current) {
+        entity = "/" + current->id() + entity;
+      } else {
+        break;
+      }
+    }
+    return entity;
+  }
+
+  /**
+   * @brief Check whether the input frame is linked to the input `id`.
+   *
+   * @param frame `TfFrame` object.
    * @param id Frame ID.
-   * @return Returns true, if the parent frame is contained.
+   * @return Return true if the input frame can link to `id`.
    */
-  bool is_root(const std::string & id) const { return contains(id) && frames_.at(id).is_root(); }
+  bool can_link_to(const TfFrame & frame, const std::string & id) const
+  {
+    auto current = std::make_optional<TfFrame>(frame);
+    while (current && !current->is_root()) {
+      current = get_frame(current->parent());
+    }
+    return current->id() == id;
+  }
 
 private:
   std::unordered_map<std::string, TfFrame> frames_;  //!< Map to store frames.

awviz_common/include/awviz_common/transformation/transformation_manager.hpp

@@ -79,7 +79,7 @@ class TransformationManager
    *
    * @param frame `TfFrame` object.
    */
-  void log_transform(const TfFrame & frame) const;
+  void log_transform(const TfFrame & frame);
 
 private:
   rclcpp::Node::SharedPtr node_;                             //!< Node shared pointer.
@@ -90,6 +90,7 @@ class TransformationManager
   std::unique_ptr<TfTree> tf_tree_;                          //!< TfTree unique pointer.
   std::shared_ptr<std::unordered_map<std::string, std::string>>
     entities_;  //!< Map stores a entity path of a corresponding frame ID.
+  std::unordered_map<std::string, double> last_log_stamps_;
 };
 }  // namespace awviz_common
 #endif  // AWVIZ_COMMON__TRANSFORMATION_MANAGER_HPP_

awviz_common/include/awviz_common/visualization_manager.hpp

@@ -24,6 +24,7 @@
 
 #include <map>
 #include <memory>
+#include <mutex>
 #include <string>
 
 namespace awviz_common
@@ -59,6 +60,8 @@ class VisualizationManager
   rclcpp::CallbackGroup::SharedPtr parallel_callback_group_;  //!< Parallel callback group.
   rclcpp::TimerBase::SharedPtr callback_timer_;               //!< Timer callback.
 
+  std::mutex display_mutex_;
+
 private:
   /**
    * @brief Parse topics and create new ROS subscriptions every time.

awviz_common/package.xml

@@ -17,6 +17,7 @@
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

awviz_common/src/transformation/transformation_manager.cpp

@@ -14,10 +14,6 @@
 
 #include "awviz_common/transformation/transformation_manager.hpp"
 
-#include "rerun.hpp"
-
-#include <rclcpp/qos.hpp>
-
 #include <yaml-cpp/yaml.h>
 
 #include <chrono>
@@ -66,10 +62,6 @@ void TransformationManager::update_tree()
 
       const TfFrame frame(id, parent);
 
-      // If parsed frame has already been registered and static skip to update
-      if (tf_tree_->contains(id) && frame.is_static()) {
-        continue;
-      }
       tf_tree_->emplace(frame);
     }
   } catch (const YAML::Exception & ex) {
@@ -79,34 +71,31 @@ void TransformationManager::update_tree()
 
 void TransformationManager::update_entity(const TfFrame & frame)
 {
-  if (entities_->count(frame.id()) > 0) {
+  if (!tf_tree_->can_link_to(frame, TF_ROOT)) {
     return;
   }
-
-  auto current = std::make_optional<TfFrame>(frame);
-  std::string entity = "/" + current->id();
-  while (current && !current->is_root()) {
-    current = tf_tree_->get_parent(current->id());
-    if (current) {
-      entity = "/" + current->id() + entity;
-    } else {
-      break;
-    }
-  }
+  const auto entity = tf_tree_->entity_path(frame);
   entities_->emplace(frame.id(), entity);
 }
 
-void TransformationManager::log_transform(const TfFrame & frame) const
+void TransformationManager::log_transform(const TfFrame & frame)
 {
   // Root frame is skipped to log transformation
-  if (frame.is_root()) {
+  if (frame.is_root() || entities_->count(frame.id()) == 0) {
     return;
   }
 
   try {
     const auto tf_stamped =
       tf_buffer_->lookupTransform(frame.parent(), frame.id(), tf2::TimePointZero);
 
+    const auto timestamp =
+      rclcpp::Time(tf_stamped.header.stamp.sec, tf_stamped.header.stamp.nanosec).seconds();
+
+    if (last_log_stamps_.count(frame.id()) > 0 && last_log_stamps_.at(frame.id()) == timestamp) {
+      return;
+    }
+
     const auto & entity_path = entities_->at(frame.id());
 
     const rerun::Transform3D transform(
@@ -120,11 +109,10 @@ void TransformationManager::log_transform(const TfFrame & frame) const
     if (frame.is_static()) {
       stream_->log_static(entity_path, transform);
     } else {
-      stream_->set_time_seconds(
-        "timestamp",
-        rclcpp::Time(tf_stamped.header.stamp.sec, tf_stamped.header.stamp.nanosec).seconds());
+      stream_->set_time_seconds("timestamp", timestamp);
       stream_->log(entity_path, transform);
     }
+    last_log_stamps_[frame.id()] = timestamp;
   } catch (const tf2::TransformException & ex) {
     RCLCPP_ERROR_STREAM(
       node_->get_logger(),

awviz_common/src/visualization_manager.cpp

@@ -14,6 +14,7 @@
 
 #include "awviz_common/visualization_manager.hpp"
 
+#include "awviz_common/transformation/tf_tree.hpp"
 #include "awviz_common/transformation/transformation_manager.hpp"
 
 #include <chrono>
@@ -27,6 +28,8 @@ VisualizationManager::VisualizationManager(
 {
   using std::chrono_literals::operator""ms;
 
+  stream_->log_static(TF_ROOT, rerun::ViewCoordinates::RIGHT_HAND_Z_UP);
+
   display_factory_ = std::make_unique<DisplayFactory>();
   tf_manager_ = std::make_unique<TransformationManager>(node, stream);
 
@@ -51,6 +54,8 @@ void VisualizationManager::create_subscriptions()
       continue;
     }
 
+    std::lock_guard<std::mutex> lock(display_mutex_);
+
     const auto & topic_type = topic_types.front();
     const auto lookup_name = display_factory_->get_class_lookup_name(topic_type);