(moveit_py) Extend Trajectory Execution Manager (#2569)

* (moveit_py) Extend Trajectory Execution Manager
Added part of the functions from #2442

* PR-remarks

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

Co-authored-by: Matthijs van der Burgh <[email protected]>

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

Co-authored-by: Matthijs van der Burgh <[email protected]>

* Update planning.pyi - Removed unused import

* Fixes whitespace issues

---------

Co-authored-by: Matthijs van der Burgh <[email protected]>

moveit_py/moveit/planning.pyi

@@ -52,17 +52,17 @@ class PlanningComponent:
 class PlanningSceneMonitor:
     def __init__(self, *args, **kwargs) -> None: ...
     def clear_octomap(self, *args, **kwargs) -> Any: ...
+    def process_attached_collision_object(self, *args, **kwargs) -> Any: ...
+    def process_collision_object(self, *args, **kwargs) -> Any: ...
     def read_only(self, *args, **kwargs) -> Any: ...
     def read_write(self, *args, **kwargs) -> Any: ...
+    def request_planning_scene_state(self, *args, **kwargs) -> Any: ...
     def start_scene_monitor(self, *args, **kwargs) -> Any: ...
     def start_state_monitor(self, *args, **kwargs) -> Any: ...
     def stop_scene_monitor(self, *args, **kwargs) -> Any: ...
     def stop_state_monitor(self, *args, **kwargs) -> Any: ...
     def update_frame_transforms(self, *args, **kwargs) -> Any: ...
     def wait_for_current_robot_state(self, *args, **kwargs) -> Any: ...
-    def request_planning_scene_state(self, *args, **kwargs) -> Any: ...
-    def process_collision_object(self, *args, **kwargs) -> Any: ...
-    def process_attached_collision_object(self, *args, **kwargs) -> Any: ...
     @property
     def name(self) -> Any: ...
 
@@ -75,6 +75,9 @@ class TrajectoryExecutionManager:
     def ensure_active_controllers(self, *args, **kwargs) -> Any: ...
     def ensure_active_controllers_for_group(self, *args, **kwargs) -> Any: ...
     def ensure_active_controllers_for_joints(self, *args, **kwargs) -> Any: ...
+    def execute(self, *args, **kwargs) -> Any: ...
+    def execute_and_wait(self, *args, **kwargs) -> Any: ...
+    def get_last_execution_status(self, *args, **kwargs) -> Any: ...
     def is_controller_active(self, *args, **kwargs) -> Any: ...
     def is_managing_controllers(self, *args, **kwargs) -> Any: ...
     def process_event(self, *args, **kwargs) -> Any: ...
@@ -85,3 +88,4 @@ class TrajectoryExecutionManager:
     def set_execution_velocity_scaling(self, *args, **kwargs) -> Any: ...
     def set_wait_for_trajectory_completion(self, *args, **kwargs) -> Any: ...
     def stop_execution(self, *args, **kwargs) -> Any: ...
+    def wait_for_execution(self, *args, **kwargs) -> Any: ...

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

@@ -157,14 +157,47 @@ 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"(
+           Start the execution of pushed trajectories.
 
+           This does not wait for completion, but calls a callback when done.
+           )")
+      .def(
+          "execute",
+          py::overload_cast<const trajectory_execution_manager::TrajectoryExecutionManager::ExecutionCompleteCallback&,
+                            const trajectory_execution_manager::TrajectoryExecutionManager::PathSegmentCompleteCallback&,
+                            bool>(&trajectory_execution_manager::TrajectoryExecutionManager::execute),
+          py::arg("callback"), py::arg("part_callback"), py::arg("auto_clear") = true,
+          R"(
+          Start the execution of pushed trajectories.
+
+          This does not wait for completion, but calls a callback when done. A callback is also called for every
+          trajectory part that completes successfully.
+           )")
+      .def("execute_and_wait", &trajectory_execution_manager::TrajectoryExecutionManager::executeAndWait,
+           py::arg("auto_clear") = true, py::call_guard<py::gil_scoped_release>(),
+           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.
+           )")
+      // ToDo(MatthijsBurgh)
+      // See https://github.com/ros-planning/moveit2/issues/2442
+      // get_current_expected_trajectory_index
+      .def("get_last_execution_status",
+           &trajectory_execution_manager::TrajectoryExecutionManager::getLastExecutionStatus,
+           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>