NavState.hpp

Go to the documentation of this file.

// Copyright 2025 Intelligent Robotics Lab
//
// 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 EASYNAV__TYPES__NAVSTATE_HPP_
#define EASYNAV__TYPES__NAVSTATE_HPP_
 
#include <string>
#include <unordered_map>
#include <memory>
#include <mutex>
#include <stdexcept>
#include <sstream>
#include <type_traits>
#include <iostream>
#include <functional>
#include <execinfo.h>
#include <typeinfo>
#include <cxxabi.h>
#include <execinfo.h>
 
namespace easynav
{

inline std::string demangle(const char * name)
{
  int status = 0;
  char * p = abi::__cxa_demangle(name, nullptr, nullptr, &status);
  std::string out = (status == 0 && p) ? p : name;
  std::free(p);
  return out;
}

inline std::string stacktrace(std::size_t skip = 1, std::size_t max_frames = 64)
{
  void * buf[128];
  const int n = backtrace(buf, static_cast<int>(std::min<std::size_t>(max_frames, 128)));
  char ** syms = backtrace_symbols(buf, n);
  std::ostringstream oss;
  for (int i = static_cast<int>(skip); i < n; ++i) {
    oss << "#" << (i - static_cast<int>(skip)) << " " << syms[i] << "\n";
  }
  std::free(syms);
  return oss.str();
}

class NavState
{
public:
  NavState()
  {
    register_basic_printers();
  }

  virtual ~NavState() = default;

  template<typename T>
  void set(const std::string & key, const T & value)
  {
    std::lock_guard<std::mutex> lock(mutex_);
    auto it = values_.find(key);
 
    if (it == values_.end()) {
      values_[key] = std::make_shared<T>(value);
      types_[key] = typeid(T).hash_code();
    } else {
      if (types_[key] != typeid(T).hash_code()) {
        std::ostringstream oss;
        oss << "Type mismatch in set(\"" << key << "\")\n"
            << "  expected(hash): " << types_[key] << "\n"
            << "  provided     : " << demangle(typeid(T).name()) << "\n"
            << "Backtrace:\n" << stacktrace(1);
        throw std::runtime_error(oss.str());
      }
 
      auto ptr = std::static_pointer_cast<T>(it->second);
      *ptr = value;
    }
  }

  template<typename T>
  void set(const std::string & key, const std::shared_ptr<T> value_ptr)
  {
    std::lock_guard<std::mutex> lock(mutex_);
    auto it = values_.find(key);
 
    if (it == values_.end()) {
      values_[key] = std::shared_ptr<T>(value_ptr);
      types_[key] = typeid(T).hash_code();
    } else {
      if (types_[key] != typeid(T).hash_code()) {
        std::ostringstream oss;
        oss << "Type mismatch in set(\"" << key << "\")\n"
            << "  expected(hash): " << types_[key] << "\n"
            << "  provided     : " << demangle(typeid(T).name()) << "\n"
            << "Backtrace:\n" << stacktrace(1);
        throw std::runtime_error(oss.str());
      }
 
      auto ptr = std::static_pointer_cast<T>(it->second);
      ptr = std::shared_ptr<T>(value_ptr);
    }
  }

  template<typename T>
  const T & get(const std::string & key) const
  {
    std::lock_guard<std::mutex> lock(mutex_);
    auto it = values_.find(key);
 
    if (it == values_.end()) {
      throw std::runtime_error("Key not found in get: " + key);
    }
 
    if (types_.at(key) != typeid(T).hash_code()) {
      throw std::runtime_error("Type mismatch in get for key: " + key);
    }
 
    auto ptr = std::static_pointer_cast<T>(it->second);
    return *ptr;
  }

  template<typename T>
  const std::shared_ptr<T> get_ptr(const std::string & key) const
  {
    std::lock_guard<std::mutex> lock(mutex_);
    auto it = values_.find(key);
 
    if (it == values_.end()) {
      throw std::runtime_error("Key not found in get: " + key);
    }
 
    if (types_.at(key) != typeid(T).hash_code()) {
      throw std::runtime_error("Type mismatch in get for key: " + key);
    }
 
    return std::static_pointer_cast<T>(it->second);
  }

  bool has(const std::string & key) const
  {
    return values_.find(key) != values_.end();
  }

  using AnyPrinter = std::function<std::string(std::shared_ptr<void>)>;

  template<typename T>
  static void register_printer(std::function<std::string(const T &)> printer)
  {
    auto wrapper = [printer](std::shared_ptr<void> base_ptr) -> std::string {
        auto typed_ptr = std::static_pointer_cast<T>(base_ptr);
        return printer(*typed_ptr);
      };
    type_printers_[typeid(T).hash_code()] = wrapper;
  }

  std::string debug_string() const
  {
    std::stringstream ss;
    for (const auto & kv : values_) {
      ss << kv.first << " = ";
      auto ptr = kv.second;
      if (ptr) {
        auto type_it = types_.find(kv.first);
        if (type_it != types_.end()) {
          auto printer_it = type_printers_.find(type_it->second);
          if (printer_it != type_printers_.end()) {
            ss << "[" << ptr.get() << "] : " << printer_it->second(ptr);
          } else {
            ss << "[" << ptr.get() << "] : " << type_it->second << "]";
          }
        } else {
          ss << "[" << ptr.get() << "] : unknown]";
        }
      } else {
        ss << "[null]";
      }
      ss << std::endl;
    }
    return ss.str();
  }

  static void print_stacktrace()
  {
    void *array[50];
    int size = backtrace(array, 50);
    char **strings = backtrace_symbols(array, size);
    std::cerr << "\nStack trace:\n";
    for (int i = 0; i < size; ++i) {
      std::cerr << strings[i] << std::endl;
    }
    std::cerr << std::endl;
    free(strings);
  }

  static void register_basic_printers()
  {
    register_printer<int>([](const int & v) {return std::to_string(v);});
    register_printer<float>([](const float & v) {return std::to_string(v);});
    register_printer<double>([](const double & v) {return std::to_string(v);});
    register_printer<std::string>([](const std::string & v) {return v;});
    register_printer<bool>([](const bool & v) {return v ? "true" : "false";});
    register_printer<char>([](const char & v) {return std::string(1, v);});
  }
 
private:
  mutable std::mutex mutex_;  

  mutable std::unordered_map<std::string, std::shared_ptr<void>> values_;

  mutable std::unordered_map<std::string, size_t> types_;

  static inline std::unordered_map<size_t, AnyPrinter> type_printers_;
};
 
}  // namespace easynav
 
#endif  // EASYNAV__TYPES__NAVSTATE_HPP_