tesseract-robotics · Levi-Armstrong · Aug 11, 2025
diff --git a/tesseract_common/CMakeLists.txt b/tesseract_common/CMakeLists.txt
@@ -17,6 +17,7 @@ if(NOT APPLE)
     Boost REQUIRED
     COMPONENTS system
                filesystem
+               thread
                serialization
                stacktrace_noop
     OPTIONAL_COMPONENTS stacktrace_backtrace stacktrace_basic)
@@ -25,6 +26,7 @@ else()
     Boost REQUIRED
     COMPONENTS system
                filesystem
+               thread
                serialization
                stacktrace_noop)
 endif()
@@ -115,6 +117,7 @@ target_link_libraries(
          ${TESSERACT_BACKTRACE_LIB}
          console_bridge::console_bridge
          yaml-cpp)
+target_link_libraries(${PROJECT_NAME} PRIVATE Boost::thread)
 target_compile_options(${PROJECT_NAME} PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
 target_compile_definitions(${PROJECT_NAME} PUBLIC ${TESSERACT_COMPILE_DEFINITIONS} ${TESSERACT_BACKTRACE_DEFINITION})
 target_clang_tidy(${PROJECT_NAME} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})

diff --git a/tesseract_common/src/allowed_collision_matrix.cpp b/tesseract_common/src/allowed_collision_matrix.cpp
@@ -25,6 +25,7 @@
 
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <boost/thread/tss.hpp>
 #include <boost/serialization/access.hpp>
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/utility.hpp>
@@ -99,7 +100,16 @@ void AllowedCollisionMatrix::removeAllowedCollision(const std::string& link_name
 
 bool AllowedCollisionMatrix::isCollisionAllowed(const std::string& link_name1, const std::string& link_name2) const
 {
+#ifdef USE_THREAD_LOCAL
   thread_local LinkNamesPair link_pair;
+#else
+  static boost::thread_specific_ptr<LinkNamesPair> link_pair_ptr;
+  if (link_pair_ptr.get() == nullptr)
+    link_pair_ptr.reset(new LinkNamesPair());
+
+  LinkNamesPair& link_pair = *link_pair_ptr;
+#endif
+
   tesseract_common::makeOrderedLinkPair(link_pair, link_name1, link_name2);
   return (lookup_table_.find(link_pair) != lookup_table_.end());
 }

diff --git a/tesseract_common/src/collision_margin_data.cpp b/tesseract_common/src/collision_margin_data.cpp
@@ -25,6 +25,7 @@
 
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <boost/thread/tss.hpp>
 #include <boost/serialization/access.hpp>
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/utility.hpp>
@@ -55,9 +56,17 @@ void CollisionMarginPairData::setCollisionMargin(const std::string& obj1, const
 std::optional<double> CollisionMarginPairData::getCollisionMargin(const std::string& obj1,
                                                                   const std::string& obj2) const
 {
-  thread_local LinkNamesPair key;
-  tesseract_common::makeOrderedLinkPair(key, obj1, obj2);
-  const auto it = lookup_table_.find(key);
+#ifdef USE_THREAD_LOCAL
+  thread_local LinkNamesPair link_pair;
+#else
+  static boost::thread_specific_ptr<LinkNamesPair> link_pair_ptr;
+  if (link_pair_ptr.get() == nullptr)
+    link_pair_ptr.reset(new LinkNamesPair());
+
+  LinkNamesPair& link_pair = *link_pair_ptr;
+#endif
+  tesseract_common::makeOrderedLinkPair(link_pair, obj1, obj2);
+  const auto it = lookup_table_.find(link_pair);
 
   if (it != lookup_table_.end())
     return it->second;

diff --git a/tesseract_kinematics/CMakeLists.txt b/tesseract_kinematics/CMakeLists.txt
@@ -12,6 +12,7 @@ endif()
 # All of these are used by all components so to avoid having them in each they are include here
 find_package(Eigen3 REQUIRED)
 find_package(console_bridge REQUIRED)
+find_package(Boost REQUIRED COMPONENTS thread)
 find_package(tesseract_scene_graph REQUIRED)
 find_package(tesseract_state_solver REQUIRED)
 find_package(tesseract_common REQUIRED)

diff --git a/tesseract_kinematics/core/CMakeLists.txt b/tesseract_kinematics/core/CMakeLists.txt
@@ -18,6 +18,7 @@ target_link_libraries(
          console_bridge::console_bridge
          boost_plugin_loader::boost_plugin_loader
          yaml-cpp)
+target_link_libraries(${PROJECT_NAME}_core PRIVATE Boost::thread)
 target_compile_options(${PROJECT_NAME}_core PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE})
 target_compile_options(${PROJECT_NAME}_core PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
 target_compile_definitions(${PROJECT_NAME}_core PUBLIC ${TESSERACT_COMPILE_DEFINITIONS})

diff --git a/tesseract_kinematics/core/src/rep_inv_kin.cpp b/tesseract_kinematics/core/src/rep_inv_kin.cpp
@@ -30,6 +30,8 @@
 #include <tesseract_scene_graph/joint.h>
 #include <tesseract_scene_graph/scene_state.h>
 
+#include <boost/thread/tss.hpp>
+
 namespace tesseract_kinematics
 {
 using Eigen::MatrixXd;
@@ -205,8 +207,26 @@ void REPInvKin::ikAt(IKSolutions& solutions,
                      Eigen::VectorXd& positioner_pose,
                      const Eigen::Ref<const Eigen::VectorXd>& seed) const
 {
+#ifdef USE_THREAD_LOCAL
   thread_local tesseract_common::TransformMap positioner_poses;
+  thread_local IKSolutions robot_solution_set;
+#else
+  static boost::thread_specific_ptr<tesseract_common::TransformMap> positioner_poses_ptr;
+  if (positioner_poses_ptr.get() == nullptr)
+    positioner_poses_ptr.reset(new tesseract_common::TransformMap());
+
+  tesseract_common::TransformMap& positioner_poses = *positioner_poses_ptr;
+
+  static boost::thread_specific_ptr<IKSolutions> robot_solution_set_ptr;
+  if (robot_solution_set_ptr.get() == nullptr)
+    robot_solution_set_ptr.reset(new IKSolutions());
+
+  IKSolutions& robot_solution_set = *robot_solution_set_ptr;
+#endif
+
   positioner_poses.clear();
+  robot_solution_set.clear();
+
   positioner_fwd_kin_->calcFwdKin(positioner_poses, positioner_pose);
   Eigen::Isometry3d positioner_tf = positioner_poses[working_frame_];
 
@@ -221,8 +241,6 @@ void REPInvKin::ikAt(IKSolutions& solutions,
   auto robot_dof = static_cast<Eigen::Index>(manip_inv_kin_->numJoints());
   auto positioner_dof = static_cast<Eigen::Index>(positioner_pose.size());
 
-  thread_local IKSolutions robot_solution_set;
-  robot_solution_set.clear();
   manip_inv_kin_->calcInvKin(robot_solution_set, robot_target_poses, seed.tail(robot_dof));
   if (robot_solution_set.empty())
     return;

diff --git a/tesseract_kinematics/core/src/rop_inv_kin.cpp b/tesseract_kinematics/core/src/rop_inv_kin.cpp
@@ -30,6 +30,8 @@
 #include <tesseract_scene_graph/joint.h>
 #include <tesseract_scene_graph/scene_state.h>
 
+#include <boost/thread/tss.hpp>
+
 namespace tesseract_kinematics
 {
 using Eigen::MatrixXd;
@@ -208,8 +210,26 @@ void ROPInvKin::ikAt(IKSolutions& solutions,
                      Eigen::VectorXd& positioner_pose,
                      const Eigen::Ref<const Eigen::VectorXd>& seed) const
 {
+#ifdef USE_THREAD_LOCAL
   thread_local tesseract_common::TransformMap positioner_poses;
+  thread_local IKSolutions robot_solution_set;
+#else
+  static boost::thread_specific_ptr<tesseract_common::TransformMap> positioner_poses_ptr;
+  if (positioner_poses_ptr.get() == nullptr)
+    positioner_poses_ptr.reset(new tesseract_common::TransformMap());
+
+  tesseract_common::TransformMap& positioner_poses = *positioner_poses_ptr;
+
+  static boost::thread_specific_ptr<IKSolutions> robot_solution_set_ptr;
+  if (robot_solution_set_ptr.get() == nullptr)
+    robot_solution_set_ptr.reset(new IKSolutions());
+
+  IKSolutions& robot_solution_set = *robot_solution_set_ptr;
+#endif
+
   positioner_poses.clear();
+  robot_solution_set.clear();
+
   positioner_fwd_kin_->calcFwdKin(positioner_poses, positioner_pose);
   Eigen::Isometry3d positioner_tf = positioner_poses[positioner_tip_link_] * positioner_to_robot_;
   Eigen::Isometry3d robot_target_pose = positioner_tf.inverse() * tip_link_poses.at(manip_tip_link_);
@@ -222,8 +242,6 @@ void ROPInvKin::ikAt(IKSolutions& solutions,
   auto robot_dof = static_cast<Eigen::Index>(manip_inv_kin_->numJoints());
   auto positioner_dof = static_cast<Eigen::Index>(positioner_pose.size());
 
-  thread_local IKSolutions robot_solution_set;
-  robot_solution_set.clear();
   manip_inv_kin_->calcInvKin(robot_solution_set, robot_target_poses, seed.tail(robot_dof));
   if (robot_solution_set.empty())
     return;

diff --git a/tesseract_kinematics/core/src/utils.cpp b/tesseract_kinematics/core/src/utils.cpp
@@ -27,6 +27,7 @@
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <Eigen/Eigenvalues>
+#include <boost/thread/tss.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_kinematics/core/utils.h>
@@ -42,10 +43,20 @@ void numericalJacobian(Eigen::Ref<Eigen::MatrixXd> jacobian,
                        const std::string& link_name,
                        const Eigen::Ref<const Eigen::Vector3d>& link_point)
 {
-  Eigen::VectorXd njvals;
-  double delta = 1e-8;
+#ifdef USE_THREAD_LOCAL
   thread_local tesseract_common::TransformMap poses;
+#else
+  static boost::thread_specific_ptr<tesseract_common::TransformMap> poses_ptr;
+  if (poses_ptr.get() == nullptr)
+    poses_ptr.reset(new tesseract_common::TransformMap());
+
+  tesseract_common::TransformMap& poses = *poses_ptr;
+#endif
+
   poses.clear();
+
+  Eigen::VectorXd njvals;
+  double delta = 1e-8;
   kin.calcFwdKin(poses, joint_values);
   Eigen::Isometry3d pose{ change_base * poses[link_name] };
 

diff --git a/tesseract_kinematics/ikfast/CMakeLists.txt b/tesseract_kinematics/ikfast/CMakeLists.txt
@@ -1,6 +1,10 @@
 add_library(${PROJECT_NAME}_ikfast INTERFACE)
-target_link_libraries(${PROJECT_NAME}_ikfast INTERFACE ${PROJECT_NAME}_core Eigen3::Eigen
-                                                       console_bridge::console_bridge)
+target_link_libraries(
+  ${PROJECT_NAME}_ikfast
+  INTERFACE ${PROJECT_NAME}_core
+            Eigen3::Eigen
+            console_bridge::console_bridge
+            Boost::thread)
 target_compile_definitions(${PROJECT_NAME}_ikfast INTERFACE -DIKFAST_NO_MAIN -DIKFAST_CLIBRARY -DIKFAST_HAS_LIBRARY)
 target_compile_options(${PROJECT_NAME}_ikfast INTERFACE ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
 target_compile_definitions(${PROJECT_NAME}_ikfast INTERFACE ${TESSERACT_COMPILE_DEFINITIONS})
@@ -21,7 +25,7 @@ configure_component(
   COMPONENT ikfast
   NAMESPACE tesseract
   TARGETS ${PROJECT_NAME}_ikfast
-  DEPENDENCIES "tesseract_kinematics COMPONENTS core")
+  DEPENDENCIES "tesseract_kinematics COMPONENTS core" "Boost COMPONENTS thread")
 
 if(TESSERACT_PACKAGE)
   cpack_component(

diff --git a/tesseract_kinematics/ikfast/include/tesseract_kinematics/ikfast/impl/ikfast_inv_kin.hpp b/tesseract_kinematics/ikfast/include/tesseract_kinematics/ikfast/impl/ikfast_inv_kin.hpp
@@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <stdexcept>
 #include <console_bridge/console.h>
 #include <tesseract_kinematics/ikfast/external/ikfast.h>
+#include <boost/thread/tss.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_kinematics/ikfast/ikfast_inv_kin.h>
@@ -98,7 +99,16 @@ inline void IKFastInvKin::calcInvKin(IKSolutions& solutions,
     // Unpack the solutions into the output vector
     const auto n_sols = ikfast_solution_set.GetNumSolutions();
 
+#ifdef USE_THREAD_LOCAL
     thread_local std::vector<IkReal> ikfast_output;
+#else
+    static boost::thread_specific_ptr<std::vector<IkReal>> ikfast_output_ptr;
+    if (ikfast_output_ptr.get() == nullptr)
+      ikfast_output_ptr.reset(new std::vector<IkReal>());
+
+    std::vector<IkReal>& ikfast_output = *ikfast_output_ptr;
+#endif
+
     ikfast_output.resize(ikfast_dof);
 
     for (std::size_t i = 0; i < n_sols; ++i)

diff --git a/tesseract_kinematics/kdl/CMakeLists.txt b/tesseract_kinematics/kdl/CMakeLists.txt
@@ -39,7 +39,8 @@ target_link_libraries(
   PUBLIC ${PROJECT_NAME}_kdl
          tesseract::tesseract_scene_graph
          tesseract::tesseract_common
-         console_bridge::console_bridge)
+         console_bridge::console_bridge
+         Boost::thread)
 target_compile_options(${PROJECT_NAME}_kdl_factories PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE})
 target_compile_options(${PROJECT_NAME}_kdl_factories PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
 target_compile_definitions(${PROJECT_NAME}_kdl_factories PUBLIC ${TESSERACT_COMPILE_DEFINITIONS})
@@ -77,7 +78,7 @@ configure_component(
   COMPONENT kdl
   NAMESPACE tesseract
   TARGETS ${PROJECT_NAME}_kdl
-  DEPENDENCIES orocos_kdl "tesseract_kinematics COMPONENTS core")
+  DEPENDENCIES orocos_kdl "tesseract_kinematics COMPONENTS core" "Boost COMPONENTS thread")
 
 if(TESSERACT_PACKAGE)
   cpack_component(

diff --git a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_fwd_kin_chain.h b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_fwd_kin_chain.h
@@ -30,6 +30,7 @@
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <kdl/chainjnttojacsolver.hpp>
+#include <boost/thread/tss.hpp>
 #include <mutex>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
@@ -110,7 +111,11 @@ class KDLFwdKinChain : public ForwardKinematics
   std::string solver_name_{ KDL_FWD_KIN_CHAIN_SOLVER_NAME };   /**< @brief Name of this solver */
   mutable std::mutex mutex_; /**< @brief KDL is not thread safe due to mutable variables in Joint Class */
 
+#ifdef USE_THREAD_LOCAL
   static thread_local KDL::JntArray kdl_joints_cache;  // NOLINT
+#else
+  static boost::thread_specific_ptr<KDL::JntArray> kdl_joints_cache_ptr;  // NOLINT
+#endif
 
   /** @brief calcFwdKin helper function */
   void calcFwdKinHelperAll(tesseract_common::TransformMap& transforms,

diff --git a/tesseract_kinematics/kdl/src/kdl_fwd_kin_chain.cpp b/tesseract_kinematics/kdl/src/kdl_fwd_kin_chain.cpp
@@ -38,7 +38,11 @@ namespace tesseract_kinematics
 using Eigen::MatrixXd;
 using Eigen::VectorXd;
 
+#ifdef USE_THREAD_LOCAL
 thread_local KDL::JntArray KDLFwdKinChain::kdl_joints_cache;  // NOLINT
+#else
+boost::thread_specific_ptr<KDL::JntArray> KDLFwdKinChain::kdl_joints_cache_ptr;  // NOLINT
+#endif
 
 KDLFwdKinChain::KDLFwdKinChain(const tesseract_scene_graph::SceneGraph& scene_graph,
                                const std::vector<std::pair<std::string, std::string>>& chains,
@@ -82,6 +86,13 @@ void KDLFwdKinChain::calcFwdKinHelperAll(tesseract_common::TransformMap& transfo
   if (joint_angles.rows() != kdl_data_.robot_chain.getNrOfJoints())
     throw std::runtime_error("kdl_joints size is not correct!");
 
+#ifndef USE_THREAD_LOCAL
+  if (kdl_joints_cache_ptr.get() == nullptr)
+    kdl_joints_cache_ptr.reset(new KDL::JntArray());
+
+  KDL::JntArray& kdl_joints_cache = *kdl_joints_cache_ptr;
+#endif
+
   if (kdl_joints_cache.rows() != joint_angles.rows())
     kdl_joints_cache.data = joint_angles;
   else
@@ -108,6 +119,13 @@ bool KDLFwdKinChain::calcJacobianHelper(KDL::Jacobian& jacobian,
                                         const Eigen::Ref<const Eigen::VectorXd>& joint_angles,
                                         int segment_num) const
 {
+#ifndef USE_THREAD_LOCAL
+  if (kdl_joints_cache_ptr.get() == nullptr)
+    kdl_joints_cache_ptr.reset(new KDL::JntArray());
+
+  KDL::JntArray& kdl_joints_cache = *kdl_joints_cache_ptr;
+#endif
+
   if (kdl_joints_cache.rows() != joint_angles.rows())
     kdl_joints_cache.data = joint_angles;
   else

diff --git a/tesseract_state_solver/CMakeLists.txt b/tesseract_state_solver/CMakeLists.txt
@@ -12,6 +12,7 @@ endif()
 # System dependencies are found with CMake's conventions
 find_package(Eigen3 REQUIRED)
 find_package(console_bridge REQUIRED)
+find_package(Boost REQUIRED COMPONENTS thread)
 find_package(orocos_kdl REQUIRED)
 find_package(tesseract_scene_graph REQUIRED)
 find_package(tesseract_common REQUIRED)
@@ -66,7 +67,8 @@ target_link_libraries(
          tesseract::tesseract_common
          tesseract::tesseract_scene_graph
          orocos-kdl
-         console_bridge::console_bridge)
+         console_bridge::console_bridge
+         Boost::thread)
 target_compile_options(${PROJECT_NAME}_kdl PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE})
 target_compile_options(${PROJECT_NAME}_kdl PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
 target_compile_definitions(${PROJECT_NAME}_kdl PUBLIC ${TESSERACT_COMPILE_DEFINITIONS})
@@ -112,6 +114,7 @@ configure_package(
     console_bridge
     tesseract_scene_graph
     tesseract_common
+    "Boost COMPONENTS thread"
   CFG_EXTRAS cmake/tesseract_state_solver-extras.cmake)
 
 # Mark cpp header files for installation

diff --git a/tesseract_state_solver/include/tesseract_state_solver/kdl/kdl_state_solver.h b/tesseract_state_solver/include/tesseract_state_solver/kdl/kdl_state_solver.h
@@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <kdl/tree.hpp>
 #include <kdl/jntarray.hpp>
 #include <kdl/treejnttojacsolver.hpp>
+#include <boost/thread/tss.hpp>
 #include <mutex>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
@@ -139,7 +140,11 @@ class KDLStateSolver : public StateSolver
   tesseract_common::KinematicLimits limits_; /**< The kinematic limits */
   mutable std::mutex mutex_; /**< @brief KDL is not thread safe due to mutable variables in Joint Class */
 
+#ifdef USE_THREAD_LOCAL
   static thread_local KDL::JntArray kdl_joints_cache;  // NOLINT
+#else
+  static boost::thread_specific_ptr<KDL::JntArray> kdl_joints_cache_ptr;  // NOLINT
+#endif
 
   void calculateTransforms(tesseract_common::TransformMap& link_transforms,
                            const KDL::JntArray& q_in,