tf_tree.hpp

Go to the documentation of this file.

// 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 <cstring>
#include <memory>
#include <optional>
#include <string>
#include <unordered_map>
#include <vector>
 
namespace awviz_common
{
 
constexpr const char * TF_ROOT = "map";  
 
class TfFrame
{
public:
  TfFrame(const std::string & id, const std::string & parent) : id_(id), parent_(parent) {}
 
  explicit TfFrame(const std::string & id) : id_(id), parent_("") {}
 
  const std::string & id() const { return id_; }
 
  const std::string & parent() const { return parent_; }
 
  bool is_root() const { return parent_.empty(); }
 
  bool is_static() const { return std::strcmp(parent_.c_str(), TF_ROOT) != 0 || parent_.empty(); }
 
  bool operator==(const TfFrame & other) const
  {
    return id_ == other.id() && parent_ == other.parent();
  }
 
private:
  std::string id_;      
  std::string parent_;  
};
 
class TfTree
{
public:
  TfTree() : frames_() { frames_.emplace(TF_ROOT, TF_ROOT); }
 
  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); }
 
  const std::unordered_map<std::string, TfFrame> & get_frames() const { return frames_; }
 
  std::optional<TfFrame> get_frame(const std::string & id) const
  {
    return contains(id) ? std::make_optional(frames_.at(id)) : std::nullopt;
  }
 
  std::optional<TfFrame> get_parent(const std::string & id) const
  {
    auto frame = get_frame(id);
    return frame ? get_frame(frame->parent()) : std::nullopt;
  }
 
  bool contains(const std::string & id) const { return frames_.count(id) > 0; }
 
  bool contains(const TfFrame & frame) const
  {
    return contains(frame.id()) && frame == frames_.at(frame.id());
  }
 
  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;
  }
 
  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_;  
};
}  // namespace awviz_common
 
#endif  // AWVIZ_COMMON__TF_TREE_HPP_