AWViz-ROS
Loading...
Searching...
No Matches
tf_tree.hpp
Go to the documentation of this file.
1// Copyright 2024 Kotaro Uetake.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15#ifndef AWVIZ_COMMON__TF_TREE_HPP_
16#define AWVIZ_COMMON__TF_TREE_HPP_
17
18#include <algorithm>
19#include <cstring>
20#include <memory>
21#include <optional>
22#include <string>
23#include <unordered_map>
24#include <utility>
25#include <vector>
26
27namespace awviz_common
28{
29
30constexpr const char * TF_ROOT = "map";
31
36{
37public:
44 TfFrame(const std::string & id, const std::string & parent) : id_(id), parent_(parent) {}
45
51 explicit TfFrame(const std::string & id) : id_(id), parent_("") {}
52
58 const std::string & id() const { return id_; }
59
65 const std::string & parent() const { return parent_; }
66
72 bool is_root() const { return parent_.empty(); }
73
80 bool is_static() const { return std::strcmp(parent_.c_str(), TF_ROOT) != 0 || parent_.empty(); }
81
88 bool operator==(const TfFrame & other) const
89 {
90 return id_ == other.id() && parent_ == other.parent();
91 }
92
93private:
94 std::string id_;
95 std::string parent_;
96};
97
98class TfTree
99{
100public:
104 TfTree() : frames_() { frames_.emplace(TF_ROOT, TF_ROOT); }
105
111 void emplace(const TfFrame & frame) { frames_.emplace(frame.id(), frame); }
112
119 void emplace(const std::string & id, const std::string & parent)
120 {
121 frames_.emplace(id, TfFrame(id, parent));
122 }
123
130 void emplace(std::string && id, std::string && parent)
131 {
132 auto key = id;
133 frames_.emplace(std::move(key), TfFrame(std::move(id), std::move(parent)));
134 }
135
141 const std::unordered_map<std::string, TfFrame> & get_frames() const { return frames_; }
142
149 std::optional<TfFrame> get_frame(const std::string & id) const
150 {
151 return contains(id) ? std::make_optional(frames_.at(id)) : std::nullopt;
152 }
153
160 std::optional<TfFrame> get_parent(const std::string & id) const
161 {
162 auto frame = get_frame(id);
163 return frame ? get_frame(frame->parent()) : std::nullopt;
164 }
165
172 bool contains(const std::string & id) const { return frames_.count(id) > 0; }
173
181 bool contains(const TfFrame & frame) const
182 {
183 return contains(frame.id()) && frame == frames_.at(frame.id());
184 }
185
194 std::string entity_path(const TfFrame & frame) const
195 {
196 auto current = std::make_optional<TfFrame>(frame);
197 std::string entity = "/" + current->id();
198 while (current && !current->is_root()) {
199 current = get_frame(current->parent());
200 if (current) {
201 entity = "/" + current->id() + entity;
202 } else {
203 break;
204 }
205 }
206 return entity;
207 }
208
216 bool can_link_to(const TfFrame & frame, const std::string & id) const
217 {
218 auto current = std::make_optional<TfFrame>(frame);
219 while (current && !current->is_root()) {
220 current = get_frame(current->parent());
221 }
222 return current->id() == id;
223 }
224
225private:
226 std::unordered_map<std::string, TfFrame> frames_;
227};
228} // namespace awviz_common
229
230#endif // AWVIZ_COMMON__TF_TREE_HPP_
A class to represent a TF frame information.
Definition: tf_tree.hpp:36
bool is_root() const
Indicate whether the frame is root by checking if parent_ is empty.
Definition: tf_tree.hpp:72
const std::string & parent() const
Return the parent frame ID.
Definition: tf_tree.hpp:65
bool operator==(const TfFrame &other) const
Compare with an another object.
Definition: tf_tree.hpp:88
bool is_static() const
Return whether the tf frame is static or not.
Definition: tf_tree.hpp:80
TfFrame(const std::string &id, const std::string &parent)
Construct a new object.
Definition: tf_tree.hpp:44
TfFrame(const std::string &id)
Construct a new object with empty string for parent.
Definition: tf_tree.hpp:51
const std::string & id() const
Return own frame ID.
Definition: tf_tree.hpp:58
Definition: tf_tree.hpp:99
TfTree()
Construct a object setting TF_ROOT as root.
Definition: tf_tree.hpp:104
void emplace(const TfFrame &frame)
Add a new tf frame to the tree.
Definition: tf_tree.hpp:111
void emplace(std::string &&id, std::string &&parent)
Add a new tf frame to the tree.
Definition: tf_tree.hpp:130
std::optional< TfFrame > get_parent(const std::string &id) const
Get the parent TfFrame object.
Definition: tf_tree.hpp:160
bool contains(const std::string &id) const
Whether to the specified frame ID is contained as a node of tree.
Definition: tf_tree.hpp:172
const std::unordered_map< std::string, TfFrame > & get_frames() const
Return map of all frames.
Definition: tf_tree.hpp:141
bool can_link_to(const TfFrame &frame, const std::string &id) const
Check whether the input frame is linked to the input id.
Definition: tf_tree.hpp:216
void emplace(const std::string &id, const std::string &parent)
Add a new tf frame to the tree.
Definition: tf_tree.hpp:119
std::string entity_path(const TfFrame &frame) const
Return entity path of the specified frame. The entity path will be in the format "/<Parent0>/<Parent1...
Definition: tf_tree.hpp:194
std::optional< TfFrame > get_frame(const std::string &id) const
Get the TfFrame object.
Definition: tf_tree.hpp:149
bool contains(const TfFrame &frame) const
Whether to the specified frame is contained in the tree checking both key and value.
Definition: tf_tree.hpp:181
Definition: display.hpp:31
constexpr const char * TF_ROOT
Root transformation frame.
Definition: tf_tree.hpp:30