diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index e4546e94..f988941a 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -33,6 +33,15 @@ else () set(CMAKE_CXX_STANDARD 14) endif () +# check for MOVEIT_MASTER +include(CheckIncludeFileCXX) +set(CMAKE_REQUIRED_INCLUDES ${moveit_core_INCLUDE_DIRS}) +CHECK_INCLUDE_FILE_CXX(moveit/collision_detection/collision_env.h MOVEIT_MASTER) +if(NOT MOVEIT_MASTER) + set(MOVEIT_MASTER 0) +endif() +add_definitions(-DMOVEIT_MASTER=${MOVEIT_MASTER}) + set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor) add_subdirectory(src) diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index b0ae608f..56d87cb6 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -110,8 +110,13 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene& bool failure = false; while (!failure) { res.clear(); +#if MOVEIT_MASTER + scene.getCollisionEnv()->checkRobotCollision(req, res, scene.getCurrentState(), + scene.getAllowedCollisionMatrix()); +#else scene.getCollisionWorld()->checkRobotCollision(req, res, *scene.getCollisionRobotUnpadded(), scene.getCurrentState(), scene.getAllowedCollisionMatrix()); +#endif if (!res.collision) return result;