From 64206b45ed21258c821f6916d6f94fa5c3980d68 Mon Sep 17 00:00:00 2001 From: Levi Armstrong 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 | 27 +++++++++++++++++-- .../include/tesseract_common/type_erasure.h | 16 ++++++----- tesseract_kinematics/core/src/utils.cpp | 5 ++-- 4 files changed, 39 insertions(+), 11 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..8a4f2973e3c 100644 --- a/tesseract_common/CMakeLists.txt +++ b/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( diff --git a/tesseract_common/include/tesseract_common/type_erasure.h b/tesseract_common/include/tesseract_common/type_erasure.h index 3ff546eba26..cbdc1ffd6c4 100644 --- a/tesseract_common/include/tesseract_common/type_erasure.h +++ b/tesseract_common/include/tesseract_common/type_erasure.h @@ -28,6 +28,8 @@ #include #include +#include +#include #include #include #include @@ -217,10 +219,11 @@ 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*>(value_->recover()); + auto* p = static_cast*>(value_->recover()); // NOLINT return *p; } @@ -228,10 +231,11 @@ struct TypeErasureBase 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*>(value_->recover()); + const auto* p = static_cast*>(value_->recover()); // NOLINT return *p; } diff --git a/tesseract_kinematics/core/src/utils.cpp b/tesseract_kinematics/core/src/utils.cpp index d70a5c5b8d7..7c423f9c6b4 100644 --- a/tesseract_kinematics/core/src/utils.cpp +++ b/tesseract_kinematics/core/src/utils.cpp @@ -77,14 +77,14 @@ void numericalJacobian(Eigen::Ref jacobian, const Eigen::Ref& 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(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& 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();