Various fixes

.github/workflows/benchmarking.yml

@@ -37,15 +37,15 @@ jobs:
           vcs-file: dependencies.repos
           upstream-args: --cmake-args -DCMAKE_BUILD_TYPE=Release
           target-path: target_ws/src
-          target-args: --cmake-args -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=OFF -DTESSERACT_ENABLE_BENCHMARKING=ON -DTESSERACT_ENABLE_RUN_BENCHMARKING=OFF -DBENCHMARK_ARGS=CI_ONLY
+          target-args: --cmake-args -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=OFF -DTESSERACT_ENABLE_BENCHMARKING=ON -DTESSERACT_ENABLE_RUN_BENCHMARKING=OFF -DBENCHMARK_ARGS=CI_ONLY -DBUILD_PYTHON_INTERFACE=OFF
 
       - name: Run Benchmarks
         shell: bash
         run: |
           source $GITHUB_WORKSPACE/upstream_ws/install/setup.bash
           source $GITHUB_WORKSPACE/target_ws/install/setup.bash
           cd $GITHUB_WORKSPACE/target_ws
-          colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=OFF -DTESSERACT_ENABLE_BENCHMARKING=ON -DTESSERACT_ENABLE_RUN_BENCHMARKING=ON -DBENCHMARK_ARGS=CI_ONLY
+          colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=OFF -DTESSERACT_ENABLE_BENCHMARKING=ON -DTESSERACT_ENABLE_RUN_BENCHMARKING=ON -DBENCHMARK_ARGS=CI_ONLY -DBUILD_PYTHON_INTERFACE=OFF
           ./src/.run_combine_benchmark_results
 
       - name: Store Bullet Discrete, FCL Discrete and Environment benchmark result

.github/workflows/code_quality.yml

@@ -24,10 +24,10 @@ jobs:
         include:
           - job_type: clang-tidy
             env:
-              TARGET_CMAKE_ARGS: "-DCMAKE_CXX_COMPILER_LAUNCHER=ccache -DCMAKE_BUILD_TYPE=Debug -DTESSERACT_ENABLE_CLANG_TIDY=ON -DTESSERACT_ENABLE_TESTING=ON -DTESSERACT_ENABLE_BENCHMARKING=ON -DTESSERACT_ENABLE_RUN_BENCHMARKING=OFF -DCLANG_TIDY_NAMES=clang-tidy-17"
+              TARGET_CMAKE_ARGS: "-DCMAKE_CXX_COMPILER_LAUNCHER=ccache -DCMAKE_BUILD_TYPE=Debug -DTESSERACT_ENABLE_CLANG_TIDY=ON -DTESSERACT_ENABLE_TESTING=ON -DTESSERACT_ENABLE_BENCHMARKING=ON -DTESSERACT_ENABLE_RUN_BENCHMARKING=OFF -DCLANG_TIDY_NAMES=clang-tidy-17 -DBUILD_PYTHON_INTERFACE=OFF"
           - job_type: codecov
             env:
-              TARGET_CMAKE_ARGS: "-DCMAKE_CXX_COMPILER_LAUNCHER=ccache -DCMAKE_BUILD_TYPE=Debug -DTESSERACT_ENABLE_CODE_COVERAGE=ON -DTESSERACT_WARNINGS_AS_ERRORS=OFF"
+              TARGET_CMAKE_ARGS: "-DCMAKE_CXX_COMPILER_LAUNCHER=ccache -DCMAKE_BUILD_TYPE=Debug -DTESSERACT_ENABLE_CODE_COVERAGE=ON -DTESSERACT_WARNINGS_AS_ERRORS=OFF -DBUILD_PYTHON_INTERFACE=OFF"
     container:
       image: ubuntu:22.04
       env:

.github/workflows/mac.yml

@@ -81,6 +81,7 @@ jobs:
           --cmake-args -G "Ninja" -DVCPKG_TARGET_TRIPLET=${{ matrix.config.vcpkg_triplet }} -DCMAKE_BUILD_TYPE=Release
           -DVCPKG_APPLOCAL_DEPS=OFF -DTESSERACT_ENABLE_TESTING=ON \
           -DTESSERACT_ENABLE_BENCHMARKING=ON -DTESSERACT_ENABLE_RUN_BENCHMARKING=OFF \
+          -DBUILD_PYTHON_INTERFACE=OFF \
           -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 \
           -DOpenMP_CXX_INCLUDE_DIR=${{ matrix.config.homebrew_root }}/opt/libomp/include \
           -DOpenMP_C_INCLUDE_DIR=${{ matrix.config.homebrew_root }}/opt/libomp/include \
@@ -93,6 +94,7 @@ jobs:
           --cmake-args -G "Ninja" -DVCPKG_TARGET_TRIPLET=${{ matrix.config.vcpkg_triplet }} 
           -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=ON \
           -DTESSERACT_ENABLE_BENCHMARKING=ON -DTESSERACT_ENABLE_RUN_BENCHMARKING=OFF \
+          -DBUILD_PYTHON_INTERFACE=OFF \
           -DVCPKG_APPLOCAL_DEPS=OFF \
           -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 \
           -DOpenMP_CXX_INCLUDE_DIR=${{ matrix.config.homebrew_root }}/opt/libomp/include \

.github/workflows/nightly.yml

@@ -44,4 +44,4 @@ jobs:
           vcs-file: dependencies.repos
           upstream-args: --cmake-args -DCMAKE_BUILD_TYPE=Release
           target-path: target_ws/src
-          target-args: --cmake-args -DCMAKE_BUILD_TYPE=Debug -DTESSERACT_ENABLE_TESTING=ON -DTESSERACT_ENABLE_BENCHMARKING=ON -DTESSERACT_ENABLE_RUN_BENCHMARKING=OFF
+          target-args: --cmake-args -DCMAKE_BUILD_TYPE=Debug -DTESSERACT_ENABLE_TESTING=ON -DTESSERACT_ENABLE_BENCHMARKING=ON -DTESSERACT_ENABLE_RUN_BENCHMARKING=OFF -DBUILD_PYTHON_INTERFACE=OFF

.github/workflows/ubuntu.yml

@@ -50,4 +50,4 @@ jobs:
           vcs-file: dependencies.repos
           upstream-args: --cmake-args -DCMAKE_BUILD_TYPE=Release
           target-path: target_ws/src
-          target-args: --cmake-args -DCMAKE_BUILD_TYPE=Debug -DTESSERACT_ENABLE_TESTING=ON -DTESSERACT_ENABLE_BENCHMARKING=ON -DTESSERACT_ENABLE_RUN_BENCHMARKING=OFF -DTESSERACT_PACKAGE=ON
+          target-args: --cmake-args -DCMAKE_BUILD_TYPE=Debug -DTESSERACT_ENABLE_TESTING=ON -DTESSERACT_ENABLE_BENCHMARKING=ON -DTESSERACT_ENABLE_RUN_BENCHMARKING=OFF -DTESSERACT_PACKAGE=ON -DBUILD_PYTHON_INTERFACE=OFF -DBUILD_PYTHON_INTERFACE=OFF

.github/workflows/windows.yml

@@ -61,7 +61,7 @@ jobs:
         vcs-file: .github/workflows/windows_dependencies.repos
         upstream-args: --cmake-args -G "Ninja" -DVCPKG_TARGET_TRIPLET=${{ matrix.config.triplet }} -DCMAKE_BUILD_TYPE=Release
         target-path: target_ws/src
-        target-args: --cmake-args -G "Ninja" -DVCPKG_TARGET_TRIPLET=${{ matrix.config.triplet }} -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=ON -DTESSERACT_ENABLE_BENCHMARKING=ON -DTESSERACT_ENABLE_RUN_BENCHMARKING=OFF
+        target-args: --cmake-args -G "Ninja" -DVCPKG_TARGET_TRIPLET=${{ matrix.config.triplet }} -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=ON -DTESSERACT_ENABLE_BENCHMARKING=ON -DTESSERACT_ENABLE_RUN_BENCHMARKING=OFF -DBUILD_PYTHON_INTERFACE=OFF
 
     - name: collect logs
       if: failure()

tesseract_collision/CMakeLists.txt

@@ -61,14 +61,11 @@ if(TESSERACT_BUILD_COAL)
   add_subdirectory(coal)
 endif()
 
-# Add compiler definition to core so it can find all plugins produced
-# string(
-#   REPLACE ";"
-#           ":"
-#           CONTACT_MANAGERS_PLUGINS_STRING
-#           "${CONTACT_MANAGERS_PLUGINS}")
-# target_compile_definitions(${PROJECT_NAME}_core
-#                            PRIVATE TESSERACT_CONTACT_MANAGERS_PLUGINS="${CONTACT_MANAGERS_PLUGINS_STRING}")
+# Add compiler definition to core so it can find all plugins produced string( REPLACE ";" ":"
+# CONTACT_MANAGERS_PLUGINS_STRING
+# "${CONTACT_MANAGERS_PLUGINS}")
+# target_compile_definitions(${PROJECT_NAME}_core PRIVATE
+# TESSERACT_CONTACT_MANAGERS_PLUGINS="${CONTACT_MANAGERS_PLUGINS_STRING}")
 
 # Add compiler definition to coal so it can find all plugins produced
 string(

tesseract_collision/coal/src/coal_cast_managers.cpp

@@ -411,7 +411,7 @@ void CoalCastBVHManager::setCollisionObjectsTransform(const tesseract_common::Tr
   auto it2 = pose2.begin();
   while (it1 != pose1.end())
   {
-    assert(pose1.find(it1->first) != pose2.end());
+    assert(pose2.find(it1->first) != pose2.end());
     setCollisionObjectsTransform(it1->first, it1->second, it2->second);
     std::advance(it1, 1);
     std::advance(it2, 1);

tesseract_collision/coal/src/coal_utils.cpp

@@ -46,6 +46,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <coal/shape/convex.h>
 #include <coal/data_types.h>
 #include <coal/octree.h>
+#include <cmath>
 #include <memory>
 #include <stdexcept>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
@@ -280,7 +281,7 @@ CollisionGeometryPtr createShapePrimitive(const CollisionShapeConstPtr& geom)
 
   shape = createShapePrimitiveHelper(geom);
   CoalCollisionGeometryCache::insert(geom, shape);
-  return createShapePrimitiveHelper(geom);
+  return shape;
 }
 
 inline bool needsCollisionCheck(const CollisionObjectWrapper* cd1,
@@ -386,7 +387,7 @@ bool DistanceCallback::collide(coal::CollisionObject* o1, coal::CollisionObject*
   dist_request.gjk_initial_guess = coal::BoundingVolumeGuess;
   const double d = coal::distance(o1, o2, dist_request, dist_result);
 
-  if (d < cdata->collision_margin_data.getMaxCollisionMargin())
+  if (d <= cdata->collision_margin_data.getCollisionMargin(cd1->getName(), cd2->getName()))
   {
     const Eigen::Isometry3d& tf1 = cd1->getCollisionObjectsTransform();
     const Eigen::Isometry3d& tf2 = cd2->getCollisionObjectsTransform();
@@ -407,7 +408,9 @@ bool DistanceCallback::collide(coal::CollisionObject* o1, coal::CollisionObject*
     contact.type_id[0] = cd1->getTypeID();
     contact.type_id[1] = cd2->getTypeID();
     contact.distance = dist_result.min_distance;
-    contact.normal = (dist_result.min_distance * (contact.nearest_points[1] - contact.nearest_points[0])).normalized();
+    contact.normal =
+        (std::copysign(1.0, dist_result.min_distance) * (contact.nearest_points[1] - contact.nearest_points[0]))
+            .normalized();
 
     TESSERACT_THREAD_LOCAL tesseract_common::LinkNamesPair link_pair;
     tesseract_common::makeOrderedLinkPair(link_pair, cd1->getName(), cd2->getName());