(moveit_py) Add Trajectory Execution Manager

moveit_py/CMakeLists.txt

@@ -61,16 +61,19 @@ target_link_libraries(core PRIVATE moveit_ros_planning::moveit_cpp
 configure_build_install_location(core)
 
 pybind11_add_module(planning
-	src/moveit/planning.cpp
-        src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp
-	src/moveit/moveit_ros/moveit_cpp/planning_component.cpp
-	src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp
-	)
+    src/moveit/planning.cpp
+    src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp
+    src/moveit/moveit_ros/moveit_cpp/planning_component.cpp
+    src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp
+    src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp
+)
 target_link_libraries(planning PRIVATE moveit_ros_planning::moveit_cpp
-				       moveit_ros_planning::moveit_planning_scene_monitor
-				       moveit_core::moveit_utils
-				       rclcpp::rclcpp
-				       moveit_py_utils)
+    moveit_ros_planning::moveit_planning_scene_monitor
+    moveit_ros_planning::moveit_trajectory_execution_manager
+    moveit_core::moveit_utils
+    rclcpp::rclcpp
+    moveit_py_utils
+)
 configure_build_install_location(planning)
 
 

moveit_py/moveit/core/controller_manager.pyi

@@ -4,3 +4,4 @@ class ExecutionStatus:
     def __init__(self, *args, **kwargs) -> None: ...
     @property
     def status(self) -> Any: ...
+    def __bool__(self) -> Any: ...

moveit_py/moveit/planning.pyi

@@ -16,6 +16,7 @@ class MoveItPy:
     def get_planning_component(self, *args, **kwargs) -> Any: ...
     def get_planning_scene_monitor(self, *args, **kwargs) -> Any: ...
     def get_robot_model(self, *args, **kwargs) -> Any: ...
+    def get_trajactory_execution_manager(self, *args, **kwargs) -> Any: ...
     def shutdown(self, *args, **kwargs) -> Any: ...
 
 class MultiPipelinePlanRequestParameters:
@@ -60,3 +61,23 @@ class PlanningSceneMonitor:
     def wait_for_current_robot_state(self, *args, **kwargs) -> Any: ...
     @property
     def name(self) -> Any: ...
+
+class TrajectoryExecutionManager:
+    def __init__(self, *args, **kwargs) -> None: ...
+    def areControllersActive(self, *args, **kwargs) -> Any: ...
+    def clear(self, *args, **kwargs) -> Any: ...
+    def enableExecutionDurationMonitoring(self, *args, **kwargs) -> Any: ...
+    def ensureActiveController(self, *args, **kwargs) -> Any: ...
+    def ensureActiveControllers(self, *args, **kwargs) -> Any: ...
+    def ensureActiveControllersForGroup(self, *args, **kwargs) -> Any: ...
+    def ensureActiveControllersForJoints(self, *args, **kwargs) -> Any: ...
+    def isControllerActive(self, *args, **kwargs) -> Any: ...
+    def isManagingControllers(self, *args, **kwargs) -> Any: ...
+    def processEvent(self, *args, **kwargs) -> Any: ...
+    def push(self, *args, **kwargs) -> Any: ...
+    def setAllowedExecutionDurationScaling(self, *args, **kwargs) -> Any: ...
+    def setAllowedGoalDurationMargin(self, *args, **kwargs) -> Any: ...
+    def setAllowedStartTolerance(self, *args, **kwargs) -> Any: ...
+    def setExecutionVelocityScaling(self, *args, **kwargs) -> Any: ...
+    def setWaitForTrajectoryCompletion(self, *args, **kwargs) -> Any: ...
+    def stopExecution(self, *args, **kwargs) -> Any: ...

moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp

@@ -14,7 +14,7 @@
  *     copyright notice, this list of conditions and the following
  *     disclaimer in the documentation and/or other materials provided
  *     with the distribution.
- *   * Neither the name of PickNik Inc. nor the names of its
+ *   * Neither the name of the copyright holder nor the names of its
  *     contributors may be used to endorse or promote products derived
  *     from this software without specific prior written permission.
  *
@@ -46,7 +46,11 @@ void init_execution_status(py::module& m)
 
   py::class_<moveit_controller_manager::ExecutionStatus, std::shared_ptr<moveit_controller_manager::ExecutionStatus>>(
       controller_manager, "ExecutionStatus", R"( Execution status of planned robot trajectory. )")
-      .def_property_readonly("status", &moveit_controller_manager::ExecutionStatus::asString);
+      .def_property_readonly("status", &moveit_controller_manager::ExecutionStatus::asString)
+
+      .def("__bool__", [](std::shared_ptr<moveit_controller_manager::ExecutionStatus>& status) {
+        return static_cast<bool>(*status);
+      });
 }
 }  // namespace bind_controller_manager
 }  // namespace moveit_py

moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp

@@ -14,7 +14,7 @@
  *     copyright notice, this list of conditions and the following
  *     disclaimer in the documentation and/or other materials provided
  *     with the distribution.
- *   * Neither the name of PickNik Inc. nor the names of its
+ *   * Neither the name of the copyright holder nor the names of its
  *     contributors may be used to endorse or promote products derived
  *     from this software without specific prior written permission.
  *
@@ -160,6 +160,12 @@ void init_moveit_py(py::module& m)
            Returns the planning scene monitor.
            )")
 
+      .def("get_trajactory_execution_manager", &moveit_cpp::MoveItCpp::getTrajectoryExecutionManagerNonConst,
+           py::return_value_policy::reference,
+           R"(
+           Returns the trajectory execution manager.
+           )")
+
       .def("get_robot_model", &moveit_cpp::MoveItCpp::getRobotModel, py::return_value_policy::reference,
            R"(
            Returns robot model.

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

@@ -0,0 +1,224 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2023, Matthijs van der Burgh
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Matthijs van der Burgh */
+
+#include "trajectory_execution_manager.h"
+
+namespace moveit_py
+{
+namespace bind_trajectory_execution_manager
+{
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_py.bind_trajectory_execution_manager");
+
+void init_trajectory_execution_manager(py::module& m)
+{
+  py::class_<trajectory_execution_manager::TrajectoryExecutionManager,
+             trajectory_execution_manager::TrajectoryExecutionManagerPtr>(m, "TrajectoryExecutionManager", R"(
+      Manages the trajectory execution.
+      )")
+
+      .def("isManagingControllers", &trajectory_execution_manager::TrajectoryExecutionManager::isManagingControllers,
+           R"(
+           If this function returns true, then this instance of the manager is allowed to load/unload/switch controllers.
+           )")
+
+      .def("processEvent", &trajectory_execution_manager::TrajectoryExecutionManager::processEvent, py::arg("event"),
+           R"(
+           Execute a named event (e.g., 'stop').
+           )")
+
+      .def("ensureActiveControllersForGroup",
+           &trajectory_execution_manager::TrajectoryExecutionManager::ensureActiveControllersForGroup, py::arg("group"),
+           R"(
+           Make sure the active controllers are such that trajectories that actuate joints in the specified group can be executed.
+
+           If manage_controllers_ is false and the controllers that happen to be active do not cover the joints in the group to be actuated, this function fails.
+           )")
+
+      .def("ensureActiveControllersForJoints",
+           &trajectory_execution_manager::TrajectoryExecutionManager::ensureActiveControllersForJoints,
+           py::arg("joints"),
+           R"(
+           Make sure the active controllers are such that trajectories that actuate joints in the specified set can be executed.
+
+           If manage_controllers_ is false and the controllers that happen to be active do not cover the joints to be actuated, this function fails.
+           )")
+
+      .def("ensureActiveController", &trajectory_execution_manager::TrajectoryExecutionManager::ensureActiveController,
+           py::arg("controller"),
+           R"(
+           Make sure a particular controller is active.
+
+           If manage_controllers_ is false and the controllers that happen to be active to not include the one specified as argument, this function fails.
+           )")
+
+      .def("ensureActiveControllers",
+           &trajectory_execution_manager::TrajectoryExecutionManager::ensureActiveControllers, py::arg("controllers"),
+           R"(
+           Make sure a particular set of controllers are active.
+
+           If manage_controllers_ is false and the controllers that happen to be active to not include the ones specified as argument, this function fails.
+          )")
+
+      .def("isControllerActive", &trajectory_execution_manager::TrajectoryExecutionManager::isControllerActive,
+           py::arg("controller"),
+           R"(
+           Check if a controller is active.
+           )")
+
+      .def("areControllersActive", &trajectory_execution_manager::TrajectoryExecutionManager::areControllersActive,
+           py::arg("controllers"),
+           R"(
+           Check if a set of controllers are active
+           )")
+
+      .def("push",
+           (bool(trajectory_execution_manager::TrajectoryExecutionManager::*)(const moveit_msgs::msg::RobotTrajectory&,
+                                                                              const std::string&)) &
+               trajectory_execution_manager::TrajectoryExecutionManager::push,
+           py::arg("trajectory"), py::arg("controller") = "",
+           R"(
+           Add a trajectory for future execution. Optionally specify a controller to use for the trajectory.
+
+           If no controller is specified, a default is used.
+           )")
+
+      .def("push",
+           (bool(trajectory_execution_manager::TrajectoryExecutionManager::*)(
+               const trajectory_msgs::msg::JointTrajectory&, const std::string&)) &
+               trajectory_execution_manager::TrajectoryExecutionManager::push,
+           py::arg("trajectory"), py::arg("controller") = "",
+           R"(
+           Add a trajectory for future execution. Optionally specify a controller to use for the trajectory.
+
+           If no controller is specified, a default is used.
+           )")
+
+      .def("push",
+           (bool(trajectory_execution_manager::TrajectoryExecutionManager::*)(const moveit_msgs::msg::RobotTrajectory&,
+                                                                              const std::vector<std::string>&)) &
+               trajectory_execution_manager::TrajectoryExecutionManager::push,
+           py::arg("trajectory"), py::arg("controllers"),
+           R"(
+           Add a trajectory for future execution.
+
+           Optionally specify a set of controllers to consider using for the trajectory.
+           Multiple controllers can be used simultaneously to execute the different parts of the trajectory.
+           If multiple controllers can be used, preference is given to the already loaded ones.
+           If no controller is specified, a default is used.
+           )")
+
+      .def("push",
+           (bool(trajectory_execution_manager::TrajectoryExecutionManager::*)(
+               const trajectory_msgs::msg::JointTrajectory&, const std::vector<std::string>&)) &
+               trajectory_execution_manager::TrajectoryExecutionManager::push,
+           py::arg("trajectory"), py::arg("controllers"),
+           R"(
+           Add a trajectory for future execution.
+
+           Optionally specify a set of controllers to consider using for the trajectory.
+           Multiple controllers can be used simultaneously to execute the different parts of the trajectory.
+           If multiple controllers can be used, preference is given to the already loaded ones.
+           If no controller is specified, a default is used.
+           )")
+
+      // ToDo(MatthijsBurgh)
+      // See https://github.com/ros-planning/moveit2/issues/2442
+      // getTrajectories
+      // execute
+      // executeAndWait
+      // waitForExecution
+      // getCurrentExpectedTrajectoryIndex
+      // getLastExecutionStatus
+
+      .def("stopExecution", &trajectory_execution_manager::TrajectoryExecutionManager::stopExecution,
+           py::arg("auto_clear") = true,
+           R"(
+           Stop whatever executions are active, if any.
+           )")
+
+      .def("clear", &trajectory_execution_manager::TrajectoryExecutionManager::clear,
+           R"(
+           Clear the trajectories to execute.
+           )")
+
+      .def("enableExecutionDurationMonitoring",
+           &trajectory_execution_manager::TrajectoryExecutionManager::enableExecutionDurationMonitoring,
+           py::arg("flag"),
+           R"(
+           Enable or disable the monitoring of trajectory execution duration.
+
+           If a controller takes longer than expected, the trajectory is canceled.
+           )")
+
+      .def("setAllowedExecutionDurationScaling",
+           &trajectory_execution_manager::TrajectoryExecutionManager::setAllowedExecutionDurationScaling,
+           py::arg("scaling"),
+           R"(
+           When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution.
+           )")
+
+      .def("setAllowedGoalDurationMargin",
+           &trajectory_execution_manager::TrajectoryExecutionManager::setAllowedGoalDurationMargin, py::arg("margin"),
+           R"(
+           When determining the expected duration of a trajectory, this additional margin s applied after scalign to allow more than the expected execution time before triggering trajectory cancel.
+           )")
+
+      .def("setExecutionVelocityScaling",
+           &trajectory_execution_manager::TrajectoryExecutionManager::setExecutionVelocityScaling, py::arg("scaling"),
+           R"(
+           Before sending a trajectory to a controller, scale the velocities by the factor specified.
+
+           By default, this is 1.0
+           )")
+
+      .def("setAllowedStartTolerance",
+           &trajectory_execution_manager::TrajectoryExecutionManager::setAllowedStartTolerance, py::arg("tolerance"),
+           R"(
+           Set joint-value tolerance for validating trajectory's start point against current robot state.
+           )")
+
+      .def("setWaitForTrajectoryCompletion",
+           &trajectory_execution_manager::TrajectoryExecutionManager::setWaitForTrajectoryCompletion, py::arg("flag"),
+           R"(
+           Enable or disable waiting for trajectory completion.
+           )");
+
+  // ToDo(MatthijsBurgh)
+  // https://github.com/ros-planning/moveit2/issues/2442
+  // getControllerManagerNode
+}
+}  // namespace bind_trajectory_execution_manager
+}  // namespace moveit_py

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

@@ -0,0 +1,55 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2023, Matthijs van der Burgh
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Matthijs van der Burgh */
+
+#pragma once
+
+#include <pybind11/pybind11.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>
+#include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
+
+namespace py = pybind11;
+
+namespace moveit_py
+{
+namespace bind_trajectory_execution_manager
+{
+
+void init_trajectory_execution_manager(py::module& m);
+
+}  // namespace bind_trajectory_execution_manager
+}  // namespace moveit_py