Easy Navigation
Loading...
Searching...
No Matches
NavState.hpp
Go to the documentation of this file.
1// Copyright 2025 Intelligent Robotics Lab
2//
3// This file is part of the project Easy Navigation (EasyNav in short)
4// licensed under the GNU General Public License v3.0.
5// See <http://www.gnu.org/licenses/> for details.
6//
7// Easy Navigation program is free software: you can redistribute it and/or modify
8// it under the terms of the GNU General Public License as published by
9// the Free Software Foundation, either version 3 of the License, or
10// (at your option) any later version.
11//
12// This program is distributed in the hope that it will be useful,
13// but WITHOUT ANY WARRANTY; without even the implied warranty of
14// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15// GNU General Public License for more details.
16//
17// You should have received a copy of the GNU General Public License
18// along with this program. If not, see <http://www.gnu.org/licenses/>.
19
26
27#ifndef EASYNAV__TYPES__NAVSTATE_HPP_
28#define EASYNAV__TYPES__NAVSTATE_HPP_
29
30#include <string>
31#include <unordered_map>
32#include <memory>
33#include <mutex>
34#include <stdexcept>
35#include <sstream>
36#include <type_traits>
37#include <iostream>
38#include <functional>
39#include <execinfo.h>
40#include <typeinfo>
41
42namespace easynav
43{
44
45
62{
63public:
69
71 virtual ~NavState() = default;
72
84 template<typename T>
85 void set(const std::string & key, const T & value)
86 {
87 std::lock_guard<std::mutex> lock(mutex_);
88 auto it = values_.find(key);
89
90 if (it == values_.end()) {
91 values_[key] = std::make_shared<T>(value);
92 types_[key] = typeid(T).hash_code();
93 } else {
94 if (types_[key] != typeid(T).hash_code()) {
95 throw std::runtime_error("Type mismatch in set for key: " + key);
96 }
97
98 auto ptr = std::static_pointer_cast<T>(it->second);
99 *ptr = value;
100 }
101 }
102
113 template<typename T>
114 const T & get(const std::string & key) const
115 {
116 std::lock_guard<std::mutex> lock(mutex_);
117 auto it = values_.find(key);
118
119 if (it == values_.end()) {
120 throw std::runtime_error("Key not found in get: " + key);
121 }
122
123 if (types_.at(key) != typeid(T).hash_code()) {
124 throw std::runtime_error("Type mismatch in get for key: " + key);
125 }
126
127 auto ptr = std::static_pointer_cast<T>(it->second);
128 return *ptr;
129 }
130
134 bool has(const std::string & key) const
135 {
136 return values_.find(key) != values_.end();
137 }
138
142 using AnyPrinter = std::function<std::string(std::shared_ptr<void>)>;
143
151 template<typename T>
152 static void register_printer(std::function<std::string(const T &)> printer)
153 {
154 auto wrapper = [printer](std::shared_ptr<void> base_ptr) -> std::string {
155 auto typed_ptr = std::static_pointer_cast<T>(base_ptr);
156 return printer(*typed_ptr);
157 };
158 type_printers_[typeid(T).hash_code()] = wrapper;
159 }
160
167 std::string debug_string() const
168 {
169 std::stringstream ss;
170 for (const auto & kv : values_) {
171 ss << kv.first << " = ";
172 auto ptr = kv.second;
173 if (ptr) {
174 auto type_it = types_.find(kv.first);
175 if (type_it != types_.end()) {
176 auto printer_it = type_printers_.find(type_it->second);
177 if (printer_it != type_printers_.end()) {
178 ss << "[" << ptr.get() << "] : " << printer_it->second(ptr);
179 } else {
180 ss << "[" << ptr.get() << "] : " << type_it->second << "]";
181 }
182 } else {
183 ss << "[" << ptr.get() << "] : unknown]";
184 }
185 } else {
186 ss << "[null]";
187 }
188 ss << std::endl;
189 }
190 return ss.str();
191 }
192
196 static void print_stacktrace()
197 {
198 void *array[50];
199 int size = backtrace(array, 50);
200 char **strings = backtrace_symbols(array, size);
201 std::cerr << "\nStack trace:\n";
202 for (int i = 0; i < size; ++i) {
203 std::cerr << strings[i] << std::endl;
204 }
205 std::cerr << std::endl;
206 free(strings);
207 }
208
212 {
213 register_printer<int>([](const int & v) {return std::to_string(v);});
214 register_printer<float>([](const float & v) {return std::to_string(v);});
215 register_printer<double>([](const double & v) {return std::to_string(v);});
216 register_printer<std::string>([](const std::string & v) {return v;});
217 register_printer<bool>([](const bool & v) {return v ? "true" : "false";});
218 register_printer<char>([](const char & v) {return std::string(1, v);});
219 }
220
221private:
222 mutable std::mutex mutex_;
223
225 mutable std::unordered_map<std::string, std::shared_ptr<void>> values_;
226
228 mutable std::unordered_map<std::string, size_t> types_;
229
231 static inline std::unordered_map<size_t, AnyPrinter> type_printers_;
232};
233
234} // namespace easynav
235
236#endif // EASYNAV__TYPES__NAVSTATE_HPP_
const T & get(const std::string &key) const
Retrieves a const reference to the value of type T associated with the given key.
Definition NavState.hpp:114
std::function< std::string(std::shared_ptr< void >)> AnyPrinter
Type alias for a generic printer function.
Definition NavState.hpp:142
static void print_stacktrace()
Prints the current C++ stack trace to standard error.
Definition NavState.hpp:196
static void register_basic_printers()
Registers default string printers for basic types: int, float, double, std::string,...
Definition NavState.hpp:211
void set(const std::string &key, const T &value)
Stores a value of type T associated with the given key.
Definition NavState.hpp:85
static void register_printer(std::function< std::string(const T &)> printer)
Registers a printer for a given type.
Definition NavState.hpp:152
NavState()
Constructs an empty NavState and registers basic type printers.
Definition NavState.hpp:65
virtual ~NavState()=default
Destructor.
bool has(const std::string &key) const
Checks whether a key exists in the NavState.
Definition NavState.hpp:134
std::string debug_string() const
Dumps all keys and their values to a formatted string.
Definition NavState.hpp:167
Definition RTTFBuffer.hpp:30