From 7ab329dc3e6dfd7db4d08745d6214017237cf718 Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Tue, 5 Dec 2023 01:09:55 +0100 Subject: [PATCH] (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 * Update moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp Co-authored-by: Matthijs van der Burgh * Update planning.pyi - Removed unused import * Fixes whitespace issues --------- Co-authored-by: Matthijs van der Burgh --- moveit_py/moveit/planning.pyi | 10 +++-- .../trajectory_execution_manager.cpp | 45 ++++++++++++++++--- .../trajectory_execution_manager.h | 1 + 3 files changed, 47 insertions(+), 9 deletions(-) diff --git a/moveit_py/moveit/planning.pyi b/moveit_py/moveit/planning.pyi index 0617fab139..e8adcfd8c2 100644 --- a/moveit_py/moveit/planning.pyi +++ b/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: ... diff --git a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp index 96a3f2cdb0..a995ca01ef 100644 --- a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp +++ b/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(&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(&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(), + R"( + Execute a trajectory and wait for it to finish. + )") + .def("wait_for_execution", &trajectory_execution_manager::TrajectoryExecutionManager::waitForExecution, + py::call_guard(), + 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(), R"( Stop whatever executions are active, if any. )") diff --git a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.h b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.h index 88f1d17ac5..692f6f71ac 100644 --- a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.h +++ b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.h @@ -37,6 +37,7 @@ #pragma once #include +#include #include #include #include