(moveit_py) Extend Trajectory Execution Manager

Added part of the functions from moveit#2442

moveit_py/moveit/planning.pyi

@@ -79,6 +79,10 @@ class TrajectoryExecutionManager:
     def is_managing_controllers(self, *args, **kwargs) -> Any: ...
     def process_event(self, *args, **kwargs) -> Any: ...
     def push(self, *args, **kwargs) -> Any: ...
+    def execute(self, *args, **kwargs) -> Any: ...
+    def execute_and_wait(self, *args, **kwargs) -> Any: ...
+    def wait_for_execution(self, *args, **kwargs) -> Any: ...
+    def get_last_execution_status(self, *args, **kwargs) -> Any: ...
     def set_allowed_execution_duration_scaling(self, *args, **kwargs) -> Any: ...
     def set_allowed_goal_duration_margin(self, *args, **kwargs) -> Any: ...
     def set_allowed_start_tolerance(self, *args, **kwargs) -> Any: ...

moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp

@@ -157,14 +157,32 @@ void initTrajectoryExecutionManager(py::module& m)
       // ToDo(MatthijsBurgh)
       // See https://github.com/ros-planning/moveit2/issues/2442
       // get_trajectories
-      // execute
-      // execute_and_wait
-      // wait_for_execution
       // get_current_expected_trajectory_index
-      // get_last_execution_status
-
+      .def("execute",
+           py::overload_cast<const trajectory_execution_manager::TrajectoryExecutionManager::ExecutionCompleteCallback&,
+                             bool>(&trajectory_execution_manager::TrajectoryExecutionManager::execute),
+           py::arg("callback"), py::arg("auto_clear") = true,
+           R"(
+           Execute a trajectory.
+           )")
+      .def("execute_and_wait", &trajectory_execution_manager::TrajectoryExecutionManager::executeAndWait,
+           py::call_guard<py::gil_scoped_release>(), py::arg("auto_clear") = true,
+           R"(
+           Execute a trajectory and wait for it to finish.
+           )")
+      .def("wait_for_execution", &trajectory_execution_manager::TrajectoryExecutionManager::waitForExecution,
+           py::call_guard<py::gil_scoped_release>(),
+           R"(
+           Wait for the current trajectory to finish execution.
+           )")
+      .def("get_last_execution_status",
+           &trajectory_execution_manager::TrajectoryExecutionManager::getLastExecutionStatus,
+           py::return_value_policy::move,
+           R"(
+           Get the status of the last execution.
+           )")
       .def("stop_execution", &trajectory_execution_manager::TrajectoryExecutionManager::stopExecution,
-           py::arg("auto_clear") = true,
+           py::arg("auto_clear") = true, py::call_guard<py::gil_scoped_release>(),
            R"(
            Stop whatever executions are active, if any.
            )")

moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.h

@@ -37,6 +37,7 @@
 #pragma once
 
 #include <pybind11/pybind11.h>
+#include <pybind11/functional.h>
 #include <moveit_py/moveit_py_utils/copy_ros_msg.h>
 #include <moveit_py/moveit_py_utils/ros_msg_typecasters.h>
 #include <rclcpp/rclcpp.hpp>