From 2b395268a40a37050f8caa49ef3c8673bfad7181 Mon Sep 17 00:00:00 2001
From: Levi Armstrong <levi.armstrong@gmail.com>
Date: Fri, 7 Jun 2024 11:46:00 -0500
Subject: [PATCH] Add backtrace to type erasure casting to quickly identify
 where the issue location

---
 tesseract_common/CMakeLists.txt                          | 2 ++
 tesseract_common/include/tesseract_common/type_erasure.h | 9 +++++++--
 2 files changed, 9 insertions(+), 2 deletions(-)

diff --git a/tesseract_common/CMakeLists.txt b/tesseract_common/CMakeLists.txt
index 5dc4eb1dca9..3d164067635 100644
--- a/tesseract_common/CMakeLists.txt
+++ b/tesseract_common/CMakeLists.txt
@@ -67,6 +67,8 @@ target_link_libraries(
          Boost::filesystem
          Boost::serialization
          console_bridge::console_bridge
+         boost_stacktrace_backtrace
+         backtrace # important for boost::stacktrace to generate function names and line numbers
          yaml-cpp)
 target_compile_options(${PROJECT_NAME} PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
 target_compile_definitions(${PROJECT_NAME} PUBLIC ${TESSERACT_COMPILE_DEFINITIONS})
diff --git a/tesseract_common/include/tesseract_common/type_erasure.h b/tesseract_common/include/tesseract_common/type_erasure.h
index 3ff546eba26..a34e717962f 100644
--- a/tesseract_common/include/tesseract_common/type_erasure.h
+++ b/tesseract_common/include/tesseract_common/type_erasure.h
@@ -26,8 +26,11 @@
 #ifndef TESSERACT_COMMON_TYPE_ERASURE_H
 #define TESSERACT_COMMON_TYPE_ERASURE_H
 
+#define BOOST_STACKTRACE_USE_BACKTRACE
+
 #include <memory>
 #include <typeindex>
+#include <boost/stacktrace.hpp>
 #include <boost/serialization/base_object.hpp>
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/assume_abstract.hpp>
@@ -218,7 +221,8 @@ struct TypeErasureBase
   {
     if (getType() != typeid(T))
       throw std::runtime_error("TypeErasureBase, tried to cast '" + std::string(getType().name()) + "' to '" +
-                               std::string(typeid(T).name()) + "'!");
+                               std::string(typeid(T).name()) + "'\nBacktrace:\n" +
+                               boost::stacktrace::to_string(boost::stacktrace::stacktrace()) + "\n");
 
     auto* p = static_cast<uncvref_t<T>*>(value_->recover());
     return *p;
@@ -229,7 +233,8 @@ struct TypeErasureBase
   {
     if (getType() != typeid(T))
       throw std::runtime_error("TypeErasureBase, tried to cast '" + std::string(getType().name()) + "' to '" +
-                               std::string(typeid(T).name()) + "'!");
+                               std::string(typeid(T).name()) + "'\nBacktrace:\n" +
+                               boost::stacktrace::to_string(boost::stacktrace::stacktrace()) + "\n");
 
     const auto* p = static_cast<const uncvref_t<T>*>(value_->recover());
     return *p;