Add backtrace to type erasure casting to quickly identify where the issue location

.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 }}

tesseract_common/CMakeLists.txt

@@ -12,7 +12,29 @@ endif()
 include(cmake/tesseract_macros.cmake)
 list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/")
 
-find_package(Boost REQUIRED COMPONENTS system filesystem serialization)
+if(WIN32)
+  # Need to have John Wason look into making this work on windows
+  set(TESSERACT_BACKTRACE_DEFINITION "") # BOOST_STACKTRACE_USE_WINDBG
+  set(TESSERACT_BACKTRACE_COMPONENT "") # stacktrace_windbg
+  set(TESSERACT_BACKTRACE_LIB "") # Boost::stacktrace_windbg
+else()
+  find_file(BACKTRACE_INCLUDE_FILE backtrace.h PATHS ${CMAKE_CXX_IMPLICIT_INCLUDE_DIRECTORIES})
+  if(BACKTRACE_INCLUDE_FILE)
+    set(TESSERACT_BACKTRACE_DEFINITION BOOST_STACKTRACE_USE_BACKTRACE
+                                       BOOST_STACKTRACE_BACKTRACE_INCLUDE_FILE="${BACKTRACE_INCLUDE_FILE}")
+  else()
+    set(TESSERACT_BACKTRACE_DEFINITION BOOST_STACKTRACE_USE_BACKTRACE)
+  endif()
+  set(TESSERACT_BACKTRACE_COMPONENT stacktrace_backtrace)
+  set(TESSERACT_BACKTRACE_LIB Boost::stacktrace_backtrace)
+endif()
+
+find_package(
+  Boost REQUIRED
+  COMPONENTS system
+             filesystem
+             serialization
+             ${TESSERACT_BACKTRACE_COMPONENT})
 find_package(Eigen3 REQUIRED)
 find_package(TinyXML2 REQUIRED)
 find_package(yaml-cpp REQUIRED)
@@ -66,10 +88,11 @@ 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})
-target_compile_definitions(${PROJECT_NAME} PUBLIC ${TESSERACT_COMPILE_DEFINITIONS})
+target_compile_definitions(${PROJECT_NAME} PUBLIC ${TESSERACT_COMPILE_DEFINITIONS} ${TESSERACT_BACKTRACE_DEFINITION})
 target_clang_tidy(${PROJECT_NAME} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})
 target_cxx_version(${PROJECT_NAME} PUBLIC VERSION ${TESSERACT_CXX_VERSION})
 target_code_coverage(

tesseract_common/cmake/tesseract_common-config.cmake.in

@@ -10,7 +10,7 @@ include(CMakeFindDependencyMacro)
 find_dependency(Eigen3)
 find_dependency(TinyXML2)
 find_dependency(yaml-cpp)
-find_dependency(Boost COMPONENTS system filesystem serialization)
+find_dependency(Boost COMPONENTS system filesystem serialization @TESSERACT_BACKTRACE_COMPONENT@)
 find_dependency(console_bridge)
 
 if(NOT TARGET console_bridge::console_bridge)

tesseract_common/include/tesseract_common/kinematic_limits.h

@@ -104,12 +104,12 @@ bool satisfiesLimits(const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dyna
 
   auto lower_diff_abs = (p - l0).abs();
   auto lower_diff = (lower_diff_abs <= md);
-  auto lower_relative_diff = (lower_diff_abs <= mrd * p.abs().max(l0.abs()));
+  auto lower_relative_diff = (lower_diff_abs <= mrd * (p.abs().max)(l0.abs()));
   auto lower_check = p > l0 || lower_diff || lower_relative_diff;
 
   auto upper_diff_abs = (p - l1).abs();
   auto upper_diff = (upper_diff_abs <= md);
-  auto upper_relative_diff = (upper_diff_abs <= mrd * p.abs().max(l1.abs()));
+  auto upper_relative_diff = (upper_diff_abs <= mrd * (p.abs().max)(l1.abs()));
   auto upper_check = p < l1 || upper_diff || upper_relative_diff;
 
   return (lower_check.all() && upper_check.all());
@@ -146,7 +146,7 @@ template <typename FloatType>
 void enforceLimits(Eigen::Ref<Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> values,
                    const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 2>>& limits)
 {
-  values = values.array().min(limits.col(1).array()).max(limits.col(0).array());
+  values = ((values.array().min)(limits.col(1).array()).max)(limits.col(0).array());
 }
 }  // namespace tesseract_common
 

tesseract_common/include/tesseract_common/type_erasure.h

@@ -28,6 +28,8 @@
 
 #include <memory>
 #include <typeindex>
+#include <boost/stacktrace.hpp>
+#include <boost/core/demangle.hpp>
 #include <boost/serialization/base_object.hpp>
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/assume_abstract.hpp>
@@ -217,21 +219,23 @@ struct TypeErasureBase
   T& as()
   {
     if (getType() != typeid(T))
-      throw std::runtime_error("TypeErasureBase, tried to cast '" + std::string(getType().name()) + "' to '" +
-                               std::string(typeid(T).name()) + "'!");
+      throw std::runtime_error("TypeErasureBase, tried to cast '" + boost::core::demangle(getType().name()) + "' to '" +
+                               boost::core::demangle(typeid(T).name()) + "'\nBacktrace:\n" +
+                               boost::stacktrace::to_string(boost::stacktrace::stacktrace()) + "\n");
 
-    auto* p = static_cast<uncvref_t<T>*>(value_->recover());
+    auto* p = static_cast<uncvref_t<T>*>(value_->recover());  // NOLINT
     return *p;
   }
 
   template <typename T>
   const T& as() const
   {
     if (getType() != typeid(T))
-      throw std::runtime_error("TypeErasureBase, tried to cast '" + std::string(getType().name()) + "' to '" +
-                               std::string(typeid(T).name()) + "'!");
+      throw std::runtime_error("TypeErasureBase, tried to cast '" + boost::core::demangle(getType().name()) + "' to '" +
+                               boost::core::demangle(typeid(T).name()) + "'\nBacktrace:\n" +
+                               boost::stacktrace::to_string(boost::stacktrace::stacktrace()) + "\n");
 
-    const auto* p = static_cast<const uncvref_t<T>*>(value_->recover());
+    const auto* p = static_cast<const uncvref_t<T>*>(value_->recover());  // NOLINT
     return *p;
   }
 

tesseract_kinematics/core/src/utils.cpp

@@ -77,14 +77,14 @@ void numericalJacobian(Eigen::Ref<Eigen::MatrixXd> jacobian,
                        const Eigen::Ref<const Eigen::Vector3d>& link_point)
 {
   Eigen::VectorXd njvals;
-  double delta = 1e-8;
+  constexpr double delta = 1e-8;
   tesseract_common::TransformMap poses = joint_group.calcFwdKin(joint_values);
   Eigen::Isometry3d pose = change_base * poses[link_name];
 
   for (int i = 0; i < static_cast<int>(joint_values.size()); ++i)
   {
     njvals = joint_values;
-    njvals[i] += delta;
+    njvals(i) += delta;  // NOLINT
     tesseract_common::TransformMap updated_poses = joint_group.calcFwdKin(njvals);
     Eigen::Isometry3d updated_pose = change_base * updated_poses[link_name];
 
@@ -227,6 +227,7 @@ Manipulability calcManipulability(const Eigen::Ref<const Eigen::MatrixXd>& jacob
   manip.m_linear = fn(a_linear);
   manip.m_angular = fn(a_angular);
 
+  // NOLINTNEXTLINE
   Eigen::MatrixXd a_inv = a.inverse();
   Eigen::MatrixXd a_linear_inv = a_linear.inverse();
   Eigen::MatrixXd a_angular_inv = a_angular.inverse();