From 229556921a31d718a1809d079af28147f495a130 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 10 Nov 2023 17:57:10 +0100 Subject: [PATCH 1/2] (moveit_py) add update_frame_transforms to planning_scene_monitor (#2521) --- moveit_py/moveit/planning.pyi | 1 + .../planning_scene_monitor/planning_scene_monitor.cpp | 9 +++++++++ 2 files changed, 10 insertions(+) diff --git a/moveit_py/moveit/planning.pyi b/moveit_py/moveit/planning.pyi index 45410effd3..c2516b2a7b 100644 --- a/moveit_py/moveit/planning.pyi +++ b/moveit_py/moveit/planning.pyi @@ -57,6 +57,7 @@ class PlanningSceneMonitor: 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: ... @property def name(self) -> Any: ... diff --git a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp index f5487cc68d..9682fd6cf0 100644 --- a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp +++ b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp @@ -91,6 +91,15 @@ void initPlanningSceneMonitor(py::module& m) str: The name of this planning scene monitor. )") + .def("update_frame_transforms", &planning_scene_monitor::PlanningSceneMonitor::updateFrameTransforms, + R"( + Update the transforms for the frames that are not part of the kinematic model using tf. + + Examples of these frames are the "map" and "odom_combined" transforms. This function is automatically called + when data that uses transforms is received. + However, this function should also be called before starting a planning request, for example. + )") + .def("start_scene_monitor", &planning_scene_monitor::PlanningSceneMonitor::startSceneMonitor, R"( Starts the scene monitor. From 76a488b54df4215500b3ef9f73b0b225e7802742 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 10 Nov 2023 19:13:35 +0100 Subject: [PATCH 2/2] (moveit_py) Add Trajectory Execution Manager (#2406) * (moveit_py) add trajectory execution manager * (moveit_py) add __bool__ to ExecutionStatus * (moveit_py) Update copyright header of changed files * (moveit_py) add comment referencing issue * Rename init_trajectory_execution_manager -> initTrajectoryExecutionManager * (moveit_py) python functions snake_case * (moveit_py) fix styling --------- --- moveit_py/CMakeLists.txt | 21 +- moveit_py/moveit/core/controller_manager.pyi | 1 + moveit_py/moveit/planning.pyi | 21 ++ .../controller_manager/controller_manager.cpp | 8 +- .../moveit_ros/moveit_cpp/moveit_cpp.cpp | 8 +- .../trajectory_execution_manager.cpp | 224 ++++++++++++++++++ .../trajectory_execution_manager.h | 55 +++++ moveit_py/src/moveit/planning.cpp | 4 +- 8 files changed, 329 insertions(+), 13 deletions(-) create mode 100644 moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp create mode 100644 moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.h diff --git a/moveit_py/CMakeLists.txt b/moveit_py/CMakeLists.txt index c6a7ddb8f3..304babdf19 100644 --- a/moveit_py/CMakeLists.txt +++ b/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) diff --git a/moveit_py/moveit/core/controller_manager.pyi b/moveit_py/moveit/core/controller_manager.pyi index d3ef91f880..fc5df2530b 100644 --- a/moveit_py/moveit/core/controller_manager.pyi +++ b/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: ... diff --git a/moveit_py/moveit/planning.pyi b/moveit_py/moveit/planning.pyi index c2516b2a7b..3539ee04ac 100644 --- a/moveit_py/moveit/planning.pyi +++ b/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: @@ -61,3 +62,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: ... diff --git a/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp index 140532462e..ce9165ce0f 100644 --- a/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp +++ b/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 initExecutionStatus(py::module& m) py::class_>( 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& status) { + return static_cast(*status); + }); } } // namespace bind_controller_manager } // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp index fe7ff81098..5f4d093d32 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp +++ b/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. * @@ -166,6 +166,12 @@ void initMoveitPy(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. 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 new file mode 100644 index 0000000000..e69143a845 --- /dev/null +++ b/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 initTrajectoryExecutionManager(py::module& m) +{ + py::class_(m, "TrajectoryExecutionManager", R"( + Manages the trajectory execution. + )") + + .def("is_managing_controllers", &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("process_event", &trajectory_execution_manager::TrajectoryExecutionManager::processEvent, py::arg("event"), + R"( + Execute a named event (e.g., 'stop'). + )") + + .def("ensure_active_controllers_for_group", + &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("ensure_active_controllers_for_joints", + &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("ensure_active_controller", + &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("ensure_active_controllers", + &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("is_controller_active", &trajectory_execution_manager::TrajectoryExecutionManager::isControllerActive, + py::arg("controller"), + R"( + Check if a controller is active. + )") + + .def("are_controllers_active", &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&)) & + 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&)) & + 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 + // get_trajectories + // execute + // execute_and_wait + // wait_for_execution + // get_current_expected_trajectory_index + // get_last_execution_status + + .def("stop_execution", &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("enable_execution_duration_monitoring", + &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("set_allowed_execution_duration_scaling", + &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("set_allowed_goal_duration_margin", + &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("set_execution_velocity_scaling", + &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("set_allowed_start_tolerance", + &trajectory_execution_manager::TrajectoryExecutionManager::setAllowedStartTolerance, py::arg("tolerance"), + R"( + Set joint-value tolerance for validating trajectory's start point against current robot state. + )") + + .def("set_wait_for_trajectory_completion", + &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 + // get_controller_manager_node +} +} // namespace bind_trajectory_execution_manager +} // namespace moveit_py 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 new file mode 100644 index 0000000000..88f1d17ac5 --- /dev/null +++ b/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 +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_trajectory_execution_manager +{ + +void initTrajectoryExecutionManager(py::module& m); + +} // namespace bind_trajectory_execution_manager +} // namespace moveit_py diff --git a/moveit_py/src/moveit/planning.cpp b/moveit_py/src/moveit/planning.cpp index f89ffc50e9..5e99f70815 100644 --- a/moveit_py/src/moveit/planning.cpp +++ b/moveit_py/src/moveit/planning.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. * @@ -37,6 +37,7 @@ #include "moveit_ros/moveit_cpp/moveit_cpp.h" #include "moveit_ros/moveit_cpp/planning_component.h" #include "moveit_ros/planning_scene_monitor/planning_scene_monitor.h" +#include "moveit_ros/trajectory_execution_manager/trajectory_execution_manager.h" PYBIND11_MODULE(planning, m) { @@ -52,5 +53,6 @@ PYBIND11_MODULE(planning, m) moveit_py::bind_planning_component::initPlanningComponent(m); moveit_py::bind_planning_scene_monitor::initPlanningSceneMonitor(m); moveit_py::bind_planning_scene_monitor::initContextManagers(m); + moveit_py::bind_trajectory_execution_manager::initTrajectoryExecutionManager(m); moveit_py::bind_moveit_cpp::initMoveitPy(m); }