Python can interupt task planning

raiserobotics · Jun 17, 2024 · 21499d6 · 21499d6
1 parent 8fa66a6
commit 21499d6
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 2 deletions.
diff --git a/core/python/pybind11 b/core/python/pybind11
diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt
@@ -37,7 +37,8 @@ add_library(${PROJECT_NAME}
 	solvers/pipeline_planner.cpp
 	solvers/multi_planner.cpp
 )
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} fmt::fmt)
+find_package(pybind11 REQUIRED)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} fmt::fmt pybind11::module)
 target_include_directories(${PROJECT_NAME}
 	PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
 	PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>

diff --git a/core/src/task.cpp b/core/src/task.cpp
@@ -48,6 +48,8 @@
 
 #include <functional>
 
+#include <pybind11/pybind11.h>
+
 namespace {
 std::string rosNormalizeName(const std::string& name) {
 	std::string n;
@@ -236,6 +238,8 @@ moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
 	auto impl = pimpl();
 	init();
 
+	pybind11::gil_scoped_release release;
+
 	// Print state and return success if there are solutions otherwise the input error_code
 	const auto success_or = [this](const int32_t error_code) -> int32_t {
 		if (numSolutions() > 0)
@@ -258,6 +262,9 @@ moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
 		if (impl->introspection_)
 			impl->introspection_->publishTaskState();
 	};
+
+	pybind11::gil_scoped_acquire acquire;
+
 	return success_or(moveit::core::MoveItErrorCode::PLANNING_FAILED);
 }