From de21f2c3a54f1a1ae53b16816a9f7b5a4e1d2bba 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

---
 .github/workflows/windows.yml                      |  2 +-
 tesseract_common/CMakeLists.txt                    | 14 +++++++++++++-
 .../include/tesseract_common/type_erasure.h        | 13 +++++++++++--
 3 files changed, 25 insertions(+), 4 deletions(-)

diff --git a/.github/workflows/windows.yml b/.github/workflows/windows.yml
index c09f90c101b..9d18fc5db80 100644
--- a/.github/workflows/windows.yml
+++ b/.github/workflows/windows.yml
@@ -34,7 +34,7 @@ jobs:
     - name: vcpkg build
       uses: johnwason/vcpkg-action@v6
       with:
-        pkgs: fcl bullet3[multithreading,double-precision,rtti] octomap console-bridge eigen3 yaml-cpp benchmark tinyxml2 assimp orocos-kdl pcl lapack-reference boost-dll boost-filesystem boost-filesystem boost-serialization boost-program-options boost-graph urdfdom ccd[double-precision] gtest
+        pkgs: fcl bullet3[multithreading,double-precision,rtti] octomap console-bridge eigen3 yaml-cpp benchmark tinyxml2 assimp orocos-kdl pcl lapack-reference boost-dll boost-filesystem boost-filesystem boost-serialization boost-program-options boost-graph boost-stacktrace urdfdom ccd[double-precision] gtest
         triplet: x64-windows-release
         extra-args: --clean-after-build
         token: ${{ github.token }}
diff --git a/tesseract_common/CMakeLists.txt b/tesseract_common/CMakeLists.txt
index 5dc4eb1dca9..f8cab25165a 100644
--- a/tesseract_common/CMakeLists.txt
+++ b/tesseract_common/CMakeLists.txt
@@ -12,7 +12,12 @@ endif()
 include(cmake/tesseract_macros.cmake)
 list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/")
 
-find_package(Boost REQUIRED COMPONENTS system filesystem serialization)
+find_package(
+  Boost REQUIRED
+  COMPONENTS system
+             filesystem
+             serialization
+             stacktrace_backtrace)
 find_package(Eigen3 REQUIRED)
 find_package(TinyXML2 REQUIRED)
 find_package(yaml-cpp REQUIRED)
@@ -42,6 +47,12 @@ set(COVERAGE_EXCLUDE
     /*/gtest/*)
 add_code_coverage_all_targets(EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE})
 
+if(WIN32)
+  set(TESSERACT_BACKTRACE_LIB Boost::stacktrace_windbg)
+else()
+  set(TESSERACT_BACKTRACE_LIB Boost::stacktrace_backtrace)
+endif()
+
 add_library(
   ${PROJECT_NAME}
   src/allowed_collision_matrix.cpp
@@ -66,6 +77,7 @@ target_link_libraries(
          Boost::system
          Boost::filesystem
          Boost::serialization
+         ${TESSERACT_BACKTRACE_LIB}
          console_bridge::console_bridge
          yaml-cpp)
 target_compile_options(${PROJECT_NAME} PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
diff --git a/tesseract_common/include/tesseract_common/type_erasure.h b/tesseract_common/include/tesseract_common/type_erasure.h
index 3ff546eba26..3616a1f08b9 100644
--- a/tesseract_common/include/tesseract_common/type_erasure.h
+++ b/tesseract_common/include/tesseract_common/type_erasure.h
@@ -26,8 +26,15 @@
 #ifndef TESSERACT_COMMON_TYPE_ERASURE_H
 #define TESSERACT_COMMON_TYPE_ERASURE_H
 
+#ifndef _WIN32
+#define BOOST_STACKTRACE_USE_BACKTRACE
+#else
+#define BOOST_STACKTRACE_USE_WINDBG
+#endif
+
 #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 +225,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 +237,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;