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_COMMON__TYPES__NAVSTATE_HPP_
#define EASYNAV_COMMON__TYPES__NAVSTATE_HPP_
 
#include <execinfo.h>
#include <cxxabi.h>
 
#include <algorithm>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <memory>
#include <mutex>
#include <optional>
#include <stdexcept>
#include <sstream>
#include <type_traits>
#include <iostream>
#include <functional>
#include <typeinfo>
#include <vector>
#include <set>
 
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(state_mutex_);
    auto it = values_.find(key);
 
    if (it == values_.end()) {
      values_[key] = std::make_shared<T>(value);
      types_[key] = typeid(T).hash_code();
      type_names_[key] = demangle(typeid(T).name());
    } else {
      if (types_[key] != typeid(T).hash_code()) {
        std::ostringstream oss;
        oss << "Type mismatch in set(\"" << key << "\")\n"
            << "  stored type  : " << type_names_[key] << "\n"
            << "  provided type: " << 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(state_mutex_);
    auto it = values_.find(key);
 
    if (it == values_.end()) {
      values_[key] = std::shared_ptr<T>(value_ptr);
      types_[key] = typeid(T).hash_code();
      type_names_[key] = demangle(typeid(T).name());
    } else {
      if (types_[key] != typeid(T).hash_code()) {
        std::ostringstream oss;
        oss << "Type mismatch in set(\"" << key << "\")\n"
            << "  stored type  : " << type_names_[key] << "\n"
            << "  provided type: " << demangle(typeid(T).name()) << "\n"
            << "Backtrace:\n" << stacktrace(1);
        throw std::runtime_error(oss.str());
      }
 
      it->second = value_ptr;
    }
  }

  void set_group(const std::string & key, const std::vector<std::string> & group_keys)
  {
    std::lock_guard<std::mutex> lock(group_mutex_);
    groups_[key] = group_keys;
 
    // Also store the group keys as a value for introspection/debugging purposes.
    set<std::vector<std::string>>(key, group_keys);
  }

  template<typename T>
  const T & get(const std::string & key) const
  {
    std::lock_guard<std::mutex> lock(state_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()) {
      std::ostringstream oss;
      oss << "Type mismatch in get(\"" << key << "\")\n"
          << "  stored type   : " << type_names_.at(key) << "\n"
          << "  requested type: " << demangle(typeid(T).name());
      throw std::runtime_error(oss.str());
    }
 
    auto ptr = std::static_pointer_cast<T>(it->second);
    return *ptr;
  }

  template<typename T>
  T get_safe(const std::string & key) const
  {
    std::lock_guard<std::mutex> lock(state_mutex_);
    auto it = values_.find(key);
 
    if (it == values_.end()) {
      throw std::runtime_error("Key not found in get_safe: " + key);
    }
 
    if (types_.at(key) != typeid(T).hash_code()) {
      std::ostringstream oss;
      oss << "Type mismatch in get_safe(\"" << key << "\")\n"
          << "  stored type   : " << type_names_.at(key) << "\n"
          << "  requested type: " << demangle(typeid(T).name());
      throw std::runtime_error(oss.str());
    }
 
    auto ptr = std::static_pointer_cast<T>(it->second);
    return *ptr;  // copy made while state_mutex_ is still held
  }

  template<typename T>
  const std::shared_ptr<T> get_ptr(const std::string & key) const
  {
    std::lock_guard<std::mutex> lock(state_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()) {
      std::ostringstream oss;
      oss << "Type mismatch in get_ptr(\"" << key << "\")\n"
          << "  stored type   : " << type_names_.at(key) << "\n"
          << "  requested type: " << demangle(typeid(T).name());
      throw std::runtime_error(oss.str());
    }
 
    return std::static_pointer_cast<T>(it->second);
  }

  template<typename T>
  const std::vector<std::shared_ptr<T>> get_group(const std::string & group_key) const
  {
    std::lock_guard<std::mutex> lock(group_mutex_);
    auto group_it = groups_.find(group_key);
 
    if (group_it == groups_.end()) {
      throw std::runtime_error("Group key not found in get_group: " + group_key);
    }
 
    const auto group_keys = group_it->second;
 
    std::vector<std::shared_ptr<T>> out;
    out.reserve(group_keys.size());
 
    for (const auto & group_key : group_keys) {
      try {
        out.push_back(get_ptr<T>(group_key));
      } catch (const std::runtime_error & e) {
        std::cerr << "Error retrieving key '" << group_key << "' from group '" << group_key <<
          "': " << e.what() << std::endl;
      }
    }
 
    return out;
  }

  template<typename T>
  std::vector<std::shared_ptr<T>> get_by_type() const
  {
    std::lock_guard<std::mutex> lock(state_mutex_);
    std::vector<std::shared_ptr<T>> out;
    const size_t target_hash = typeid(T).hash_code();
    for (const auto & kv : values_) {
      if (types_.at(kv.first) == target_hash) {
        out.push_back(std::static_pointer_cast<T>(kv.second));
      }
    }
    return out;
  }

  template<typename T>
  std::vector<std::shared_ptr<T>> get_to_vector(const std::string & key) const
  {
    std::vector<std::shared_ptr<T>> out;
    std::lock_guard<std::mutex> lock(state_mutex_);
    auto it = values_.find(key);
    if (it == values_.end()) {
      return out;
    }
    if (types_.at(key) != typeid(T).hash_code()) {
      return out;
    }
    out.push_back(std::static_pointer_cast<T>(it->second));
    return out;
  }

  template<typename T>
  std::vector<std::shared_ptr<T>> get_no_group() const
  {
    // Snapshot the set of all keys that belong to any group (under group lock).
    std::unordered_set<std::string> grouped_keys;
    {
      std::lock_guard<std::mutex> glock(group_mutex_);
      for (const auto & g : groups_) {
        for (const auto & k : g.second) {
          grouped_keys.insert(k);
        }
      }
    }
 
    // Iterate values and return those of type T whose key is not grouped.
    std::lock_guard<std::mutex> slock(state_mutex_);
    std::vector<std::shared_ptr<T>> out;
    const size_t target_hash = typeid(T).hash_code();
    for (const auto & kv : values_) {
      if (types_.at(kv.first) == target_hash &&
        grouped_keys.find(kv.first) == grouped_keys.end())
      {
        out.push_back(std::static_pointer_cast<T>(kv.second));
      }
    }
    return out;
  }

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

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

  void add_to_group(const std::string & group_key, const std::string & key)
  {
    std::lock_guard<std::mutex> lock(group_mutex_);
    auto & members = groups_[group_key];
    if (std::find(members.begin(), members.end(), key) != members.end()) {
      return;
    }
    members.push_back(key);
    set<std::vector<std::string>>(group_key, members);
  }

  std::vector<std::string> get_group_keys(const std::string & group_key) const
  {
    std::lock_guard<std::mutex> lock(group_mutex_);
    auto it = groups_.find(group_key);
    if (it == groups_.end()) {
      return {};
    }
    return it->second;
  }

  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);
      };
    auto & registry = printer_registry();
    std::lock_guard<std::mutex> lock(registry.mutex);
    registry.printers[typeid(T).hash_code()] = wrapper;
  }

  std::string debug_string() const
  {
    // Snapshot under the lock, format outside it (printers may be slow).
    struct Entry
    {
      std::string key;
      std::shared_ptr<void> ptr;
      std::optional<size_t> type;
    };
    std::vector<Entry> entries;
    {
      std::lock_guard<std::mutex> lock(state_mutex_);
      entries.reserve(values_.size());
      for (const auto & [key, ptr] : values_) {
        const auto type_it = types_.find(key);
        entries.push_back(
          {key, ptr, type_it != types_.end() ? std::optional(type_it->second) : std::nullopt});
      }
    }
    // Sorted keys: stable, readable output.
    std::sort(
      entries.begin(), entries.end(),
      [](const Entry & a, const Entry & b) {return a.key < b.key;});
 
    std::stringstream ss;
    for (const auto & [key, ptr, type] : entries) {
      ss << key << " = ";
      if (ptr) {
        if (type) {
          AnyPrinter printer;
          {
            auto & registry = printer_registry();
            std::lock_guard<std::mutex> lock(registry.mutex);
            const auto printer_it = registry.printers.find(*type);
            if (printer_it != registry.printers.end()) {
              printer = printer_it->second;
            }
          }
          if (printer) {
            ss << "[" << ptr.get() << "] : " << printer(ptr);
          } else {
            ss << "[" << ptr.get() << "] : " << *type << "]";
          }
        } 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);});
    register_printer<std::vector<std::string>>(
      [](const std::vector<std::string> & v) {
        std::ostringstream oss;
        oss << " " << v.size() << " [";
        for (std::size_t i = 0; i < v.size(); ++i) {
          if (i > 0) {oss << ", ";}
          oss << v[i];
        }
        oss << "]";
        return oss.str();
      });
  }
 
private:
  mutable std::mutex state_mutex_;  
  mutable std::mutex group_mutex_;  

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

  mutable std::unordered_map<std::string, std::vector<std::string>> groups_;

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

  mutable std::unordered_map<std::string, std::string> type_names_;

  struct PrinterRegistry
  {
    std::mutex mutex;
    std::unordered_map<size_t, AnyPrinter> printers;
  };

  static PrinterRegistry & printer_registry()
  {
    static PrinterRegistry * const registry = new PrinterRegistry();
    return *registry;
  }
};
 
}  // namespace easynav
 
#endif  // EASYNAV_COMMON__TYPES__NAVSTATE_HPP_