From 602e85909061c2ad42e81cf33ec0a63848492a71 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 30 Oct 2023 18:46:41 +0100 Subject: [PATCH 01/11] Refactor planning pipeline chain Move plan_request_adapters into planning interface package Add callRequestAdapterChain function Differentiate between request and response adapters Restore planning_pipeline.cpp Add response_adapters parameter and cleanup adapter chain calls Add response adapter loading chain Address compiler errors Fix parameter type Fix compilation errors Rename planning_request_adapters to *_adapter Cleanup + fix errors Fix plugins and print description Remove unused adapters Planners plan, Adapters adapt ... Format + fix compilation error Simplify by removing callRequest/ResponseAdapterChain Remove unneeded PlannerFn Make clang tidy happy Add loadPluginVector template and make callAdapterChain private Add documentation Remove TODOs Cleanups and move templates into annonymous namespace Add more debug information and fix bugs in adapters Fix bugs Remove commented out CMake lines Update parameter description Apply suggestions from code review Co-authored-by: Abishalini Sivaraman Address review Remove instrospection dir Make parameters read only Make sure that pipeline does not abort if no request adapter is configured Remove anonymous namespace in header Move adapter params into separate namespace Delete outdated unittest Apply suggestions from code review Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> ruckig -> Ruckig! Address comments part 2 Remove wrong request_adapters initilization from PILZ config Update migration guide Update logging Remove response argument from PlanningRequestAdapter::adapt Make adapter naming consistent Add pipeline state publisher Pull stacked constraints check out of planning pipeline Update planning request adapters Format! Remove unused params and fix clang tidy Fix error Move validity check and path publisher in separate adapters Fix DISPLAY_PATH_TOPIC Update config yamls Add MoveItStatus and return with request adapter Revert outdated change Request adapters adapt returns void now Planner solve function returns void now Update license in header Apply suggestions from code review Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Update migration guide Address review Don't make initialize purely virtual & address review --- MIGRATION.md | 32 +- .../default_configs/chomp_planning.yaml | 20 +- .../default_configs/ompl_planning.yaml | 19 +- ...lz_industrial_motion_planner_planning.yaml | 2 +- .../default_configs/stomp_planning.yaml | 17 +- moveit_core/CMakeLists.txt | 2 - moveit_core/planning_interface/CMakeLists.txt | 1 + .../planning_interface/planning_interface.h | 10 +- .../planning_interface/planning_request.h | 2 +- .../planning_request_adapter.h | 75 ++++ .../planning_interface/planning_response.h | 5 - .../planning_response_adapter.h | 74 ++++ .../planning_request_adapter/CMakeLists.txt | 28 -- .../planning_request_adapter.h | 127 ------ .../src/planning_request_adapter.cpp | 159 -------- .../test_planning_request_adapter_chain.cpp | 204 ---------- .../include/moveit/utils/moveit_status.h | 45 +-- .../chomp_interface/chomp_planning_context.h | 4 +- .../src/chomp_planning_context.cpp | 15 +- .../chomp_motion_planner/chomp_planner.h | 2 +- .../src/chomp_planner.cpp | 26 +- .../chomp_optimizer_adapter/CHANGELOG.rst | 164 -------- .../chomp_optimizer_adapter/CMakeLists.txt | 53 --- ...p_optimizer_adapter_plugin_description.xml | 9 - .../chomp/chomp_optimizer_adapter/package.xml | 28 -- .../src/chomp_optimizer_adapter.cpp | 216 ---------- .../model_based_planning_context.h | 4 +- .../src/model_based_planning_context.cpp | 119 +++--- .../test/test_planning_context_manager.cpp | 9 +- .../planning_context_base.h | 41 +- .../trajectory_generator.h | 4 +- .../src/trajectory_generator.cpp | 11 +- .../test/test_utils.cpp | 6 +- .../src/unittest_planning_context.cpp | 15 +- ...t_trajectory_blender_transition_window.cpp | 6 +- .../unittest_trajectory_generator_circ.cpp | 52 +-- .../unittest_trajectory_generator_common.cpp | 46 +-- .../src/unittest_trajectory_generator_lin.cpp | 24 +- .../src/unittest_trajectory_generator_ptp.cpp | 19 +- moveit_planners/stomp/CMakeLists.txt | 1 - .../stomp_moveit_planning_context.hpp | 4 +- .../src/stomp_moveit_planning_context.cpp | 10 +- .../src/stomp_moveit_smoothing_adapter.cpp | 128 ------ .../stomp/stomp_moveit_plugin_description.xml | 6 - .../prbt_moveit_config/launch/demo.launch.py | 4 +- .../test/launch/hybrid_planning_common.py | 3 +- .../cartesian_path_service_capability.cpp | 8 +- .../move_action_capability.cpp | 12 +- .../plan_service_capability.cpp | 8 +- moveit_ros/planning/CMakeLists.txt | 9 +- ...lt_request_adapters_plugin_description.xml | 33 ++ ...t_response_adapters_plugin_description.xml | 27 ++ .../planning/introspection/CMakeLists.txt | 10 - .../src/list_planning_adapter_plugins.cpp | 86 ---- .../planning_pipeline/planning_pipeline.h | 189 ++++++--- .../res/planning_pipeline_parameters.yaml | 17 +- .../src/planning_pipeline.cpp | 370 +++++++----------- .../CMakeLists.txt | 16 +- .../default_plan_request_adapter_params.yaml | 45 --- .../res/default_request_adapter_params.yaml | 12 + .../src/check_for_stacked_constraints.cpp | 98 +++++ .../src/check_start_state_bounds.cpp | 202 ++++++++++ .../src/check_start_state_collision.cpp | 101 +++++ .../src/fix_start_state_bounds.cpp | 221 ----------- .../src/fix_start_state_collision.cpp | 193 --------- .../src/fix_start_state_path_constraints.cpp | 160 -------- .../src/resolve_constraint_frames.cpp | 43 +- ...unds.cpp => validate_workspace_bounds.cpp} | 57 ++- ...ng_request_adapters_plugin_description.xml | 46 --- .../CMakeLists.txt | 18 + .../res/default_response_adapter_params.yaml | 28 ++ .../src/add_ruckig_traj_smoothing.cpp | 52 ++- .../src/add_time_optimal_parameterization.cpp | 57 +-- .../src/display_motion_path.cpp | 103 +++++ .../src/validate_path.cpp | 154 ++++++++ .../ompl-chomp_planning_pipeline.launch.xml | 9 +- 76 files changed, 1630 insertions(+), 2605 deletions(-) create mode 100644 moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h create mode 100644 moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h delete mode 100644 moveit_core/planning_request_adapter/CMakeLists.txt delete mode 100644 moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h delete mode 100644 moveit_core/planning_request_adapter/src/planning_request_adapter.cpp delete mode 100644 moveit_core/planning_request_adapter/test/test_planning_request_adapter_chain.cpp rename moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp => moveit_core/utils/include/moveit/utils/moveit_status.h (60%) delete mode 100644 moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst delete mode 100644 moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt delete mode 100644 moveit_planners/chomp/chomp_optimizer_adapter/chomp_optimizer_adapter_plugin_description.xml delete mode 100644 moveit_planners/chomp/chomp_optimizer_adapter/package.xml delete mode 100644 moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp delete mode 100644 moveit_planners/stomp/src/stomp_moveit_smoothing_adapter.cpp create mode 100644 moveit_ros/planning/default_request_adapters_plugin_description.xml create mode 100644 moveit_ros/planning/default_response_adapters_plugin_description.xml delete mode 100644 moveit_ros/planning/introspection/CMakeLists.txt delete mode 100644 moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp delete mode 100644 moveit_ros/planning/planning_request_adapter_plugins/res/default_plan_request_adapter_params.yaml create mode 100644 moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml create mode 100644 moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp create mode 100644 moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp create mode 100644 moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp delete mode 100644 moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp delete mode 100644 moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp delete mode 100644 moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp rename moveit_ros/planning/planning_request_adapter_plugins/src/{fix_workspace_bounds.cpp => validate_workspace_bounds.cpp} (54%) delete mode 100644 moveit_ros/planning/planning_request_adapters_plugin_description.xml create mode 100644 moveit_ros/planning/planning_response_adapter_plugins/CMakeLists.txt create mode 100644 moveit_ros/planning/planning_response_adapter_plugins/res/default_response_adapter_params.yaml rename moveit_ros/planning/{planning_request_adapter_plugins => planning_response_adapter_plugins}/src/add_ruckig_traj_smoothing.cpp (59%) rename moveit_ros/planning/{planning_request_adapter_plugins => planning_response_adapter_plugins}/src/add_time_optimal_parameterization.cpp (55%) create mode 100644 moveit_ros/planning/planning_response_adapter_plugins/src/display_motion_path.cpp create mode 100644 moveit_ros/planning/planning_response_adapter_plugins/src/validate_path.cpp diff --git a/MIGRATION.md b/MIGRATION.md index 26b22a65fd..04baab23c5 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -3,19 +3,39 @@ API changes in MoveIt releases ## ROS Rolling -- `lockSceneRead()` and `lockSceneWrite()` are now protected member functions, for internal use only. To lock the planning scene, use LockedPlanningSceneRO or LockedPlanningSceneRW: +- [10/2023] Planning request adapters are now separated into PlanRequest (preprocessing) and PlanResponse (postprocessing) adapters. The adapters are configured with ROS parameter vectors (vector order corresponds to execution order). Please update your pipeline configurations for example like this: +```diff +- request_adapters: >- +- default_planner_request_adapters/AddTimeOptimalParameterization +- default_planner_request_adapters/ResolveConstraintFrames +- default_planner_request_adapters/FixWorkspaceBounds +- default_planner_request_adapters/FixStartStateBounds +- default_planner_request_adapters/FixStartStateCollision +- default_planner_request_adapters/FixStartStatePathConstraints ++ # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. ++ request_adapters: ++ - default_planning_request_adapters/ResolveConstraintFrames ++ - default_planning_request_adapters/ValidateWorkspaceBounds ++ - default_planning_request_adapters/CheckStartStateBounds ++ - default_planning_request_adapters/CheckStartStateCollision ++ response_adapters: ++ - default_planning_response_adapters/AddTimeOptimalParameterization ++ - default_planning_response_adapters/ValidateSolution ++ - default_planning_response_adapters/DisplayMotionPath +``` +- [2021] `lockSceneRead()` and `lockSceneWrite()` are now protected member functions, for internal use only. To lock the planning scene, use ``LockedPlanningSceneRO`` or ``LockedPlanningSceneRW``: ``` planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor); moveit::core::RobotModelConstPtr model = ls->getRobotModel(); ``` -- ServoServer was renamed to ServoNode -- `CollisionObject` messages are now defined with a `Pose`, and shapes and subframes are defined relative to the object's pose. This makes it easier to place objects with subframes and multiple shapes in the scene. This causes several changes: +- [2021] ServoServer was renamed to ServoNode +- [2021] `CollisionObject` messages are now defined with a `Pose`, and shapes and subframes are defined relative to the object's pose. This makes it easier to place objects with subframes and multiple shapes in the scene. This causes several changes: - `getFrameTransform()` now returns this pose instead of the first shape's pose. - The Rviz plugin's manipulation tab now uses the object's pose instead of the shape pose to evaluate if object's are in the region of interest. - Planning scene geometry text files (`.scene`) have changed format. Loading old files is still supported. You can add a line `0 0 0 0 0 0 1` under each line with an asterisk to upgrade old files. -- add API for passing RNG to setToRandomPositionsNearBy -- Static member variable interface of the CollisionDetectorAllocatorTemplate for the string NAME was replaced with a virtual method `getName`. -- Enhance `RDFLoader` to load from string parameter OR string topic (and add the ability to publish a string topic). +- [2021] Add API for passing RNG to setToRandomPositionsNearBy +- [2021] Static member variable interface of the CollisionDetectorAllocatorTemplate for the string NAME was replaced with a virtual method `getName`. +- [2021] Enhance `RDFLoader` to load from string parameter OR string topic (and add the ability to publish a string topic). ## ROS Noetic - RobotModel no longer overrides empty URDF collision geometry by matching the visual geometry of the link. diff --git a/moveit_configs_utils/default_configs/chomp_planning.yaml b/moveit_configs_utils/default_configs/chomp_planning.yaml index fa0d77c5ae..b632d65c05 100644 --- a/moveit_configs_utils/default_configs/chomp_planning.yaml +++ b/moveit_configs_utils/default_configs/chomp_planning.yaml @@ -1,12 +1,14 @@ planning_plugin: chomp_interface/CHOMPPlanner enable_failure_recovery: true -jiggle_fraction: 0.05 -request_adapters: >- - default_planner_request_adapters/AddTimeOptimalParameterization - default_planner_request_adapters/ResolveConstraintFrames - default_planner_request_adapters/FixWorkspaceBounds - default_planner_request_adapters/FixStartStateBounds - default_planner_request_adapters/FixStartStateCollision - default_planner_request_adapters/FixStartStatePathConstraints +# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath + ridge_factor: 0.01 -start_state_max_bounds_error: 0.1 diff --git a/moveit_configs_utils/default_configs/ompl_planning.yaml b/moveit_configs_utils/default_configs/ompl_planning.yaml index 36c2c762c8..9c02c44bdd 100644 --- a/moveit_configs_utils/default_configs/ompl_planning.yaml +++ b/moveit_configs_utils/default_configs/ompl_planning.yaml @@ -1,10 +1,11 @@ planning_plugin: ompl_interface/OMPLPlanner -start_state_max_bounds_error: 0.1 -jiggle_fraction: 0.05 -request_adapters: >- - default_planner_request_adapters/AddTimeOptimalParameterization - default_planner_request_adapters/ResolveConstraintFrames - default_planner_request_adapters/FixWorkspaceBounds - default_planner_request_adapters/FixStartStateBounds - default_planner_request_adapters/FixStartStateCollision - default_planner_request_adapters/FixStartStatePathConstraints +# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath diff --git a/moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml b/moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml index 92fd5907be..9925a81a8b 100644 --- a/moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml +++ b/moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml @@ -1,5 +1,5 @@ planning_plugin: pilz_industrial_motion_planner/CommandPlanner -request_adapters: "" +# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. default_planner_config: PTP capabilities: >- pilz_industrial_motion_planner/MoveGroupSequenceAction diff --git a/moveit_configs_utils/default_configs/stomp_planning.yaml b/moveit_configs_utils/default_configs/stomp_planning.yaml index 47d311501b..38120524cb 100644 --- a/moveit_configs_utils/default_configs/stomp_planning.yaml +++ b/moveit_configs_utils/default_configs/stomp_planning.yaml @@ -1,11 +1,14 @@ planning_plugin: stomp_moveit/StompPlanner -request_adapters: >- - default_planner_request_adapters/AddTimeOptimalParameterization - default_planner_request_adapters/ResolveConstraintFrames - default_planner_request_adapters/FixWorkspaceBounds - default_planner_request_adapters/FixStartStateBounds - default_planner_request_adapters/FixStartStateCollision - default_planner_request_adapters/FixStartStatePathConstraints +# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath stomp_moveit: num_timesteps: 60 diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 5f9f358d84..ad1cdf0518 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -59,7 +59,6 @@ add_subdirectory(kinematics_metrics) add_subdirectory(macros) add_subdirectory(online_signal_smoothing) add_subdirectory(planning_interface) -add_subdirectory(planning_request_adapter) add_subdirectory(planning_scene) add_subdirectory(robot_model) add_subdirectory(robot_state) @@ -115,7 +114,6 @@ install( moveit_kinematics_metrics moveit_macros moveit_planning_interface - moveit_planning_request_adapter moveit_planning_scene moveit_robot_model moveit_robot_state diff --git a/moveit_core/planning_interface/CMakeLists.txt b/moveit_core/planning_interface/CMakeLists.txt index 504a970d76..46e7ff7789 100644 --- a/moveit_core/planning_interface/CMakeLists.txt +++ b/moveit_core/planning_interface/CMakeLists.txt @@ -16,6 +16,7 @@ ament_target_dependencies(moveit_planning_interface target_link_libraries(moveit_planning_interface moveit_robot_trajectory moveit_robot_state + moveit_planning_scene moveit_utils ) diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h index ea2224a9dd..c41c8505ab 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h @@ -39,16 +39,12 @@ #include #include #include +#include #include #include #include #include -namespace planning_scene -{ -MOVEIT_CLASS_FORWARD(PlanningScene); // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc -} - /** \brief This namespace includes the base class for MoveIt planners */ namespace planning_interface { @@ -119,11 +115,11 @@ class PlanningContext /** \brief Solve the motion planning problem and store the result in \e res. This function should not clear data * structures before computing. The constructor and clear() do that. */ - virtual bool solve(MotionPlanResponse& res) = 0; + virtual void solve(MotionPlanResponse& res) = 0; /** \brief Solve the motion planning problem and store the detailed result in \e res. This function should not clear * data structures before computing. The constructor and clear() do that. */ - virtual bool solve(MotionPlanDetailedResponse& res) = 0; + virtual void solve(MotionPlanDetailedResponse& res) = 0; /** \brief If solve() is running, terminate the computation. Return false if termination not possible. No-op if * solve() is not running (returns true).*/ diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h index af7e4881bd..39200155ad 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h @@ -40,8 +40,8 @@ namespace planning_interface { -// for now this is just a typedef +// for now this is just a typedef typedef moveit_msgs::msg::MotionPlanRequest MotionPlanRequest; } // namespace planning_interface diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h new file mode 100644 index 0000000000..e7b2cdcb26 --- /dev/null +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h @@ -0,0 +1,75 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * 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 PickNik Inc. 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: Ioan Sucan, Sebastian Jahr + Description: Generic interface to adapting motion planning requests +*/ + +#pragma once + +#include +#include +#include +#include +#include + +namespace planning_interface +{ +MOVEIT_CLASS_FORWARD(PlanningRequestAdapter); // Defines PlanningRequestAdapterPtr, ConstPtr, WeakPtr... etc + +/** @brief Concept in MoveIt which can be used to modify the planning problem(pre-processing) in a planning pipeline. + * PlanningRequestAdapters enable adjusting to or validating a planning problem for a subsequent planning algorithm. + */ +class PlanningRequestAdapter +{ +public: + /** @brief Initialize parameters using the passed Node and parameter namespace. + * @param node Node instance used by the adapter + * @param parameter_namespace Parameter namespace for adapter + @details If no initialization is needed, simply implement as empty */ + virtual void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace){}; + + /** @brief Get a description of this adapter + * @return Returns a short string that identifies the planning request adapter + */ + [[nodiscard]] virtual std::string getDescription() const = 0; + + /** @brief Adapt the planning request + * @param planning_scene Representation of the environment for the planning + * @param req Motion planning request with a set of constraints + * @return True if response was generated correctly */ + [[nodiscard]] virtual moveit::core::MoveItStatus adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, + planning_interface::MotionPlanRequest& req) const = 0; +}; +} // namespace planning_interface diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h index 3b4db78f08..7f8bbadcb6 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h @@ -65,11 +65,6 @@ struct MotionPlanResponse moveit_msgs::msg::RobotState start_state; std::string planner_id; - /// Sometimes planning request adapters may add states on the solution path (e.g., add the current state of the robot - /// as prefix, when the robot started to plan only from near that state, as the current state itself appears to touch - /// obstacles). This is helpful because the added states should not be considered invalid in all situations. - std::vector added_path_index; // This won't be included into the MotionPlanResponse ROS 2 message! - // \brief Enable checking of query success or failure, for example if(response) ... explicit operator bool() const { diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h new file mode 100644 index 0000000000..8c26d62a21 --- /dev/null +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h @@ -0,0 +1,74 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * 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 PickNik Inc. 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: Sebastian Jahr */ + +#pragma once + +#include +#include +#include +#include + +namespace planning_interface +{ +MOVEIT_CLASS_FORWARD(PlanningResponseAdapter); // Defines PlanningResponseAdapterPtr, ConstPtr, WeakPtr... etc + +/** @brief Concept in MoveIt which can be used to modify the planning solution (post-processing) in a planning pipeline. + * PlanningResponseAdapters enable using for example smoothing and trajectory generation algorithms in sequence to + * produce a trajectory. + */ +class PlanningResponseAdapter +{ +public: + /** @brief Initialize parameters using the passed Node and parameter namespace. + * @param node Node instance used by the adapter + * @param parameter_namespace Parameter namespace for adapter + @details If no initialization is needed, simply implement as empty */ + virtual void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace){}; + + /** \brief Get a description of this adapter + * @return Returns a short string that identifies the planning response adapter + */ + [[nodiscard]] virtual std::string getDescription() const = 0; + + /** @brief Adapt the planning response + * @param planning_scene Representation of the environment for the planning + * @param req Motion planning request with a set of constraints + * @param res Motion planning response containing the solution that is adapted. */ + virtual void adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const = 0; +}; +} // namespace planning_interface diff --git a/moveit_core/planning_request_adapter/CMakeLists.txt b/moveit_core/planning_request_adapter/CMakeLists.txt deleted file mode 100644 index f3b589ed6c..0000000000 --- a/moveit_core/planning_request_adapter/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -add_library(moveit_planning_request_adapter SHARED src/planning_request_adapter.cpp) -target_include_directories(moveit_planning_request_adapter PUBLIC - $ - $ -) -set_target_properties(moveit_planning_request_adapter PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_planning_request_adapter - rclcpp - rmw_implementation - urdf - urdfdom - urdfdom_headers - visualization_msgs -) -target_link_libraries(moveit_planning_request_adapter - moveit_planning_scene - moveit_planning_interface -) - -install(DIRECTORY include/ DESTINATION include/moveit_core) - -if(BUILD_TESTING) - ament_add_gmock(test_planning_request_adapter_chain test/test_planning_request_adapter_chain.cpp) - target_link_libraries(test_planning_request_adapter_chain - moveit_test_utils - moveit_planning_interface - moveit_planning_request_adapter) -endif() diff --git a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h deleted file mode 100644 index 4c0a346747..0000000000 --- a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h +++ /dev/null @@ -1,127 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 Willow Garage 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: Ioan Sucan */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -/** \brief Generic interface to adapting motion planning requests */ -namespace planning_request_adapter -{ -MOVEIT_CLASS_FORWARD(PlanningRequestAdapter); // Defines PlanningRequestAdapterPtr, ConstPtr, WeakPtr... etc - -/** @brief Concept in MoveIt which can be used to modify the planning problem and resulting trajectory (pre-processing - * and/or post-processing) for a motion planner. PlanningRequestAdapter enable using multiple motion planning and - * trajectory generation algorithms in sequence to produce robust motion plans. - */ -class PlanningRequestAdapter -{ -public: - /// \brief Functional interface to call a planning function - using PlannerFn = - std::function; - - /** \brief Initialize parameters using the passed Node and parameter namespace. - * @param node Node instance used by the adapter - * @param parameter_namespace Parameter namespace for adapter - If no initialization is needed, simply implement as empty */ - virtual void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) = 0; - - /** \brief Get a description of this adapter - * @return Returns a short string that identifies the planning request adapter - */ - [[nodiscard]] virtual std::string getDescription() const = 0; - - /** \brief Adapt the planning request if needed, call the planner - function \e planner and update the planning response if - needed. If the response is changed, the index values of the - states added without planning are added to \e - added_path_index - * @param planner Pointer to the planner used to solve the passed problem - * @param planning_scene Representation of the environment for the planning - * @param req Motion planning request with a set of constraints - * @param res Reference to which the generated motion plan response is written to - * @return True if response got generated correctly */ - [[nodiscard]] bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const; - - /** \brief Adapt the planning request if needed, call the planner - function \e planner and update the planning response if - needed. If the response is changed, the index values of the - states added without planning are added to \e - added_path_index - * @param planner Pointer to the planner used to solve the passed problem - * @param planning_scene Representation of the environment for the planning - * @param req Motion planning request with a set of constraints - * @param res Reference to which the generated motion plan response is written to - * @return True if response got generated correctly */ - [[nodiscard]] virtual bool adaptAndPlan(const PlannerFn& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const = 0; -}; - -/** \brief Apply a sequence of adapters to a motion plan */ -class PlanningRequestAdapterChain -{ -public: - /** \brief Add new adapter to the back of the chain - * @param adapter Adapter to be added */ - void addAdapter(const PlanningRequestAdapterConstPtr& adapter); - - /** \brief Iterate through the chain and call all adapters and planners in the correct order - * @param planner Pointer to the planner used to solve the passed problem - * @param planning_scene Representation of the environment for the planning - * @param req Motion planning request with a set of constraints - * @param res Reference to which the generated motion plan response is written to - * @return True if response got generated correctly */ - [[nodiscard]] bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const; - -private: - std::vector adapters_; -}; -} // namespace planning_request_adapter diff --git a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp deleted file mode 100644 index 5a126ae784..0000000000 --- a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp +++ /dev/null @@ -1,159 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 Willow Garage 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: Ioan Sucan */ - -#include -#include -#include -#include -#include - -namespace planning_request_adapter -{ -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.planning_request_adapter"); - -namespace -{ -bool callPlannerInterfaceSolve(const planning_interface::PlannerManager& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) -{ - planning_interface::PlanningContextPtr context = planner.getPlanningContext(planning_scene, req, res.error_code); - if (context) - { - return context->solve(res); - } - else - { - return false; - } -} - -bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAdapter::PlannerFn& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) -{ - try - { - std::swap(res.added_path_index, added_path_index); - bool result = adapter.adaptAndPlan(planner, planning_scene, req, res); - std::swap(res.added_path_index, added_path_index); - RCLCPP_DEBUG_STREAM(LOGGER, adapter.getDescription() << ": " << moveit::core::errorCodeToString(res.error_code)); - return result; - } - catch (std::exception& ex) - { - RCLCPP_ERROR(LOGGER, "Exception caught executing adapter '%s': %s\nSkipping adapter instead.", - adapter.getDescription().c_str(), ex.what()); - return planner(planning_scene, req, res); - } -} - -} // namespace - -bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const -{ - return adaptAndPlan( - [&planner](const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) { - return callPlannerInterfaceSolve(*planner, scene, req, res); - }, - planning_scene, req, res); -} - -void PlanningRequestAdapterChain::addAdapter(const PlanningRequestAdapterConstPtr& adapter) -{ - adapters_.push_back(adapter); -} - -bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const -{ - res.added_path_index.clear(); - - // if there are no adapters, run the planner directly - if (adapters_.empty()) - { - return callPlannerInterfaceSolve(*planner, planning_scene, req, res); - } - - // if there are adapters, construct a function for each, in order, - // so that in the end we have a nested sequence of functions that calls all adapters - // and eventually the planner in the correct order. - PlanningRequestAdapter::PlannerFn fn = [&planner = *planner](const planning_scene::PlanningSceneConstPtr& scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) { - return callPlannerInterfaceSolve(planner, scene, req, res); - }; - - std::vector> added_path_index_each(adapters_.size()); - for (int i = adapters_.size() - 1; i >= 0; --i) - { - fn = [&adapter = *adapters_[i], &added_path_index = added_path_index_each[i], - fn](const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) { - return callAdapter(adapter, fn, scene, req, res, added_path_index); - }; - } - - bool result = fn(planning_scene, req, res); - - // merge the index values from each adapter (in reverse order) - assert(res.added_path_index.empty()); - for (auto added_state_by_each_adapter_it = added_path_index_each.rbegin(); - added_state_by_each_adapter_it != added_path_index_each.rend(); ++added_state_by_each_adapter_it) - { - for (std::size_t added_index : *added_state_by_each_adapter_it) - { - for (std::size_t& index_in_path : res.added_path_index) - { - if (added_index <= index_in_path) - index_in_path++; - } - res.added_path_index.push_back(added_index); - } - } - std::sort(res.added_path_index.begin(), res.added_path_index.end()); - return result; -} - -} // end of namespace planning_request_adapter diff --git a/moveit_core/planning_request_adapter/test/test_planning_request_adapter_chain.cpp b/moveit_core/planning_request_adapter/test/test_planning_request_adapter_chain.cpp deleted file mode 100644 index 277b4a43c7..0000000000 --- a/moveit_core/planning_request_adapter/test/test_planning_request_adapter_chain.cpp +++ /dev/null @@ -1,204 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2023, Willow Garage, Inc. - * 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 Willow Garage 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: Hugo Laloge */ - -#include -#include -#include - -class AppendingPlanningRequestAdapter final : public planning_request_adapter::PlanningRequestAdapter -{ -public: - void initialize(const rclcpp::Node::SharedPtr& /*node*/, const std::string& /*parameter_namespace*/) override - { - } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const override - { - bool success = planner(planning_scene, req, res); - if (success) - { - res.trajectory->addSuffixWayPoint(res.trajectory->getLastWayPoint(), 1); - res.added_path_index.push_back(res.trajectory->getWayPointCount() - 1); - } - return success; - } - std::string getDescription() const override - { - return "AppendingPlanningRequestAdapter"; - } -}; - -class PrependingPlanningRequestAdapter final : public planning_request_adapter::PlanningRequestAdapter -{ -public: - void initialize(const rclcpp::Node::SharedPtr& /*node*/, const std::string& /*parameter_namespace*/) override - { - } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const override - { - bool success = planner(planning_scene, req, res); - if (success) - { - res.trajectory->addPrefixWayPoint(res.trajectory->getFirstWayPoint(), 0); - res.added_path_index.push_back(0); - } - return success; - } - std::string getDescription() const override - { - return "PrependingPlanningRequestAdapter"; - } -}; - -class PlannerManagerStub : public planning_interface::PlannerManager -{ -public: - PlannerManagerStub(planning_interface::PlanningContextPtr planningContext) - : m_planningContext_{ std::move(planningContext) } - { - } - planning_interface::PlanningContextPtr - getPlanningContext(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, - const planning_interface::MotionPlanRequest& /*req*/, - moveit_msgs::msg::MoveItErrorCodes& /*error*/) const override - { - return m_planningContext_; - } - - bool canServiceRequest(const planning_interface::MotionPlanRequest& /*req*/) const override - { - return true; - } - -private: - planning_interface::PlanningContextPtr m_planningContext_; -}; - -class PlanningContextStub : public planning_interface::PlanningContext -{ -public: - using planning_interface::PlanningContext::PlanningContext; - - void setTrajectory(robot_trajectory::RobotTrajectoryPtr trajectory) - { - m_trajectory_ = std::move(trajectory); - } - - bool solve(planning_interface::MotionPlanResponse& res) override - { - if (!m_trajectory_) - return false; - - res.trajectory = m_trajectory_; - return true; - } - - bool solve(planning_interface::MotionPlanDetailedResponse& res) override - { - if (!m_trajectory_) - return false; - - res.trajectory.push_back(m_trajectory_); - return true; - } - - bool terminate() override - { - return true; - } - - void clear() override - { - } - -private: - robot_trajectory::RobotTrajectoryPtr m_trajectory_; -}; - -class PlanningRequestAdapterTests : public testing::Test -{ -protected: - void SetUp() override - { - robot_model_ = moveit::core::loadTestingRobotModel("panda"); - group_name_ = "panda_arm"; - planning_scene_ = std::make_shared(robot_model_); - planning_context_ = std::make_shared("stub", group_name_); - planner_manager_ = std::make_shared(planning_context_); - } - - robot_trajectory::RobotTrajectoryPtr createRandomTrajectory(std::size_t waypointCount) - { - robot_trajectory::RobotTrajectoryPtr robot_trajectory = - std::make_shared(robot_model_, group_name_); - for (std::size_t i = 0; i < waypointCount; ++i) - robot_trajectory->addSuffixWayPoint(std::make_shared(robot_model_), 1.); - return robot_trajectory; - } - -protected: - std::string group_name_; - planning_request_adapter::PlanningRequestAdapterChain sut_; - moveit::core::RobotModelPtr robot_model_; - planning_scene::PlanningScenePtr planning_scene_; - std::shared_ptr planning_context_; - std::shared_ptr planner_manager_; -}; - -TEST_F(PlanningRequestAdapterTests, testMergeAddedPathIndex) -{ - sut_.addAdapter(std::make_shared()); - sut_.addAdapter(std::make_shared()); - sut_.addAdapter(std::make_shared()); - - planning_context_->setTrajectory(createRandomTrajectory(4)); - - planning_interface::MotionPlanRequest req; - req.group_name = group_name_; - planning_interface::MotionPlanResponse res; - std::ignore = sut_.adaptAndPlan(planner_manager_, planning_scene_, req, res); - - EXPECT_THAT(res.added_path_index, testing::ElementsAre(0, 5, 6)); -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp b/moveit_core/utils/include/moveit/utils/moveit_status.h similarity index 60% rename from moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp rename to moveit_core/utils/include/moveit/utils/moveit_status.h index 577a995962..b8acacf56c 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp +++ b/moveit_core/utils/include/moveit/utils/moveit_status.h @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2023, PickNik Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -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 Willow Garage 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. * @@ -32,35 +32,28 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan - * Desc: Empty adapter that just calls the planner - */ +#pragma once -#include -#include +#include -namespace default_planner_request_adapters +namespace moveit::core { -/** @brief Empty adapter that just calls the planner. */ -class Empty : public planning_request_adapter::PlanningRequestAdapter +/** + * @brief Information about potential errors + */ +struct MoveItStatus { -public: - std::string getDescription() const override - { - return "No Op"; - } + // Error code - encodes the overall reason for failure + MoveItErrorCode error_code; + // A message to provide additional information + std::string message; + // Name of the component that created the status + std::string source; - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const override - { - return planner(planning_scene, req, res); - } - - void initialize(const rclcpp::Node::SharedPtr& /* node */, const std::string& /* parameter_namespace */) override + MoveItStatus(const int code = 0 /*UNDEFINED*/, const std::string& message = std::string(), + const std::string& source = std::string()) + : error_code{ code }, message{ message }, source{ source } { } }; -} // namespace default_planner_request_adapters - -CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::Empty, planning_request_adapter::PlanningRequestAdapter) +} // namespace moveit::core diff --git a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h index 09dfd17b11..908d315fc8 100644 --- a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h +++ b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h @@ -48,8 +48,8 @@ MOVEIT_CLASS_FORWARD(CHOMPPlanningContext); // Defines CHOMPPlanningContextPtr, class CHOMPPlanningContext : public planning_interface::PlanningContext { public: - bool solve(planning_interface::MotionPlanResponse& res) override; - bool solve(planning_interface::MotionPlanDetailedResponse& res) override; + void solve(planning_interface::MotionPlanResponse& res) override; + void solve(planning_interface::MotionPlanDetailedResponse& res) override; void clear() override; bool terminate() override; diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp index d2f9efb6fa..5f93522139 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp @@ -47,25 +47,22 @@ CHOMPPlanningContext::CHOMPPlanningContext(const std::string& name, const std::s chomp_interface_ = std::make_shared(node); } -bool CHOMPPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) +void CHOMPPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) { - return chomp_interface_->solve(planning_scene_, request_, chomp_interface_->getParams(), res); + chomp_interface_->solve(planning_scene_, request_, chomp_interface_->getParams(), res); } -bool CHOMPPlanningContext::solve(planning_interface::MotionPlanResponse& res) +void CHOMPPlanningContext::solve(planning_interface::MotionPlanResponse& res) { planning_interface::MotionPlanDetailedResponse res_detailed; - bool planning_success = solve(res_detailed); - - res.error_code = res_detailed.error_code; - - if (planning_success) + solve(res_detailed); + if (res_detailed.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { res.trajectory = res_detailed.trajectory[0]; res.planning_time = res_detailed.processing_time[0]; } - return planning_success; + res.error_code = res_detailed.error_code; } bool CHOMPPlanningContext::terminate() diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h index 85059824ce..a6bca7c57e 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h @@ -50,7 +50,7 @@ class ChompPlanner ChompPlanner() = default; virtual ~ChompPlanner() = default; - bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene, + void solve(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, const ChompParameters& params, planning_interface::MotionPlanDetailedResponse& res) const; }; diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp index 0faeacab80..75e60ba0e6 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp @@ -48,7 +48,7 @@ namespace chomp { static const rclcpp::Logger LOGGER = rclcpp::get_logger("chomp_planner"); -bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene, +void ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, const ChompParameters& params, planning_interface::MotionPlanDetailedResponse& res) const { @@ -58,7 +58,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s { RCLCPP_ERROR(LOGGER, "No planning scene initialized."); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; - return false; + return; } // get the specified start state @@ -69,7 +69,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s { RCLCPP_ERROR(LOGGER, "Start state violates joint limits"); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE; - return false; + return; } ChompTrajectory trajectory(planning_scene->getRobotModel(), 3.0, .03, req.group_name); @@ -79,7 +79,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s { RCLCPP_ERROR(LOGGER, "Expecting exactly one goal constraint, got: %zd", req.goal_constraints.size()); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; - return false; + return; } if (req.goal_constraints[0].joint_constraints.empty() || !req.goal_constraints[0].position_constraints.empty() || @@ -87,7 +87,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s { RCLCPP_ERROR(LOGGER, "Only joint-space goals are supported"); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; - return false; + return; } const size_t goal_index = trajectory.getNumPoints() - 1; @@ -98,7 +98,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s { RCLCPP_ERROR(LOGGER, "Goal state violates joint limits"); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE; - return false; + return; } robotStateToArray(goal_state, req.group_name, trajectory.getTrajectoryPoint(goal_index)); @@ -141,19 +141,19 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s if (res.trajectory.empty()) { RCLCPP_ERROR(LOGGER, "No input trajectory specified"); - return false; + return; } else if (!(trajectory.fillInFromTrajectory(*res.trajectory[0]))) { RCLCPP_ERROR(LOGGER, "Input trajectory has less than 2 points, " "trajectory must contain at least start and goal state"); - return false; + return; } } else { RCLCPP_ERROR(LOGGER, "invalid interpolation method specified in the chomp_planner file"); - return false; + return; } RCLCPP_INFO(LOGGER, "CHOMP trajectory initialized using method: %s ", @@ -197,7 +197,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s { RCLCPP_ERROR(LOGGER, "Could not initialize optimizer"); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; - return false; + return; } RCLCPP_DEBUG(LOGGER, "Optimization took %ld sec to create", @@ -273,7 +273,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s { RCLCPP_ERROR(LOGGER, "Motion plan is invalid."); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; - return false; + return; } // check that final state is within goal tolerances @@ -285,14 +285,12 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s { RCLCPP_ERROR(LOGGER, "Goal constraints are violated: %s", constraint.joint_name.c_str()); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED; - return false; + return; } } res.processing_time.resize(1); res.processing_time[0] = std::chrono::duration(std::chrono::system_clock::now() - start_time).count(); - - return true; } } // namespace chomp diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst deleted file mode 100644 index 9b0ea98d17..0000000000 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst +++ /dev/null @@ -1,164 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package moveit_chomp_optimizer_adapter -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.8.0 (2023-09-10) ------------------- -* Remove added path index from planner adapter function signature (`#2285 `_) -* Contributors: Sebastian Jahr - -2.7.4 (2023-05-18) ------------------- - -2.7.3 (2023-04-24) ------------------- -* Replace Variable PROJECT_NAME in CMakeLists.txt with the actual name (`#2020 `_) -* Contributors: Shobuj Paul - -2.7.2 (2023-04-18) ------------------- - -2.7.1 (2023-03-23) ------------------- -* remove underscore from public member in MotionPlanResponse (`#1939 `_) - * remove underscore from private members - * fix more uses of the suffix notation -* Contributors: AlexWebb - -2.7.0 (2023-01-29) ------------------- -* Fix BSD license in package.xml (`#1796 `_) - * fix BSD license in package.xml - * this must also be spdx compliant -* Use a stronger source of randomness (`#1721 `_) - * Remove use of deprecated `std::random_shuffle` - * Replace random number generators with `rsl::rng` - * Utilize `rsl::uniform_real` -* Contributors: Chris Thrasher, Christian Henkel - -2.6.0 (2022-11-10) ------------------- -* Short-circuit planning adapters (`#1694 `_) - * Revert "Planning request adapters: short-circuit if failure, return code rather than bool (`#1605 `_)" - This reverts commit 66a64b4a72b6ddef1af2329f20ed8162554d5bcb. - * Add debug message in call stack of planning_request_adapters - * Short-circuit planning request adapters - * Replace if-elseif cascade with switch - * Cleanup translation of MoveItErrorCode to string - - Move default code to moveit_core/utils - - Override defaults in existing getActionResultString() - - Provide translations for all error codes defined in moveit_msgs - * Fix comment according to review - * Add braces - Co-authored-by: Henning Kayser - * Add braces - Co-authored-by: Henning Kayser - Co-authored-by: Henning Kayser - Co-authored-by: AndyZe -* Planning request adapters: short-circuit if failure, return code rather than bool (`#1605 `_) - * Return code rather than bool - * Remove all debug prints - * Small fixup - * Minor cleanup of comment and error handling - * void return from PlannerFn - * Control reaches end of non-void function - * Use a MoveItErrorCode cast - * More efficient callAdapter() - * More MoveItErrorCode - * CI fixup attempt -* Improve CMake usage (`#1550 `_) -* Contributors: AndyZe, Robert Haschke, Sebastian Jahr - -2.5.3 (2022-07-28) ------------------- - -2.5.2 (2022-07-18) ------------------- -* Remove unnecessary rclcpp.hpp includes (`#1333 `_) -* Update plugin library paths (`#1304 `_) -* Contributors: Jafar, Sebastian Jahr - -2.5.1 (2022-05-31) ------------------- - -2.5.0 (2022-05-26) ------------------- -* Make TOTG the default time-parameterization algorithm everywhere (`#1218 `_) -* Make moveit_common a 'depend' rather than 'build_depend' (`#1226 `_) -* Remove unused parameters. (`#1018 `_) -* Contributors: AndyZe, Cory Crean, Jafar, Robert Haschke, jeoseo - -2.4.0 (2022-01-20) ------------------- -* moveit_build_options() - Declare common build options like CMAKE_CXX_STANDARD, CMAKE_BUILD_TYPE, - and compiler options (namely warning flags) once. - Each package depending on moveit_core can use these via moveit_build_options(). -* Contributors: Robert Haschke - -2.3.2 (2021-12-29) ------------------- - -2.3.1 (2021-12-23) ------------------- -* Port CHOMP Motion Planner to ROS 2 (`#809 `_) -* Contributors: Henning Kayser, andreas-botbuilt - -1.1.1 (2020-10-13) ------------------- - -1.1.0 (2020-09-04) ------------------- -* [feature] Optional cpp version setting (`#2166 `_) -* [feature] Allow ROS namespaces for planning request adapters (`#1530 `_) -* [fix] Various fixes for upcoming Noetic release (`#2180 `_) -* [fix] Fix compiler warnings (`#1773 `_) -* [fix] Fix Chomp planning adapter (`#1525 `_) -* [maint] Remove ! from MoveIt name (`#1590 `_) -* Contributors: Dave Coleman, Henning Kayser, Robert Haschke, Sean Yen, Tyler Weaver, Yu, Yan - -1.0.6 (2020-08-19) ------------------- -* [maint] Migrate to clang-format-10 -* Contributors: Robert Haschke - -1.0.5 (2020-07-08) ------------------- - -1.0.4 (2020-05-30) ------------------- - -1.0.3 (2020-04-26) ------------------- -* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) -* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) -* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) -* Contributors: Robert Haschke, Sean Yen, Yu, Yan - -1.0.2 (2019-06-28) ------------------- -* [fix] Chomp planning adapter: Fix spurious error message Fix "Found empty JointState message" -* Contributors: Robert Haschke - -1.0.1 (2019-03-08) ------------------- -* [fix] segfault in chomp adapter (`#1377 `_) -* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) -* Contributors: Robert Haschke, Yu, Yan - -1.0.0 (2019-02-24) ------------------- -* [fix] catkin_lint issues (`#1341 `_) -* Contributors: Robert Haschke - -0.10.8 (2018-12-24) -------------------- - -0.10.7 (2018-12-13) -------------------- - -0.10.6 (2018-12-09) -------------------- -* [maintenance] Moved CHOMP optimizer into own package (`#1251 `_) -* rename default_planner_request_adapters/CHOMPOptimizerAdapter -> chomp/OptimizerAdapter -* Contributors: Robert Haschke diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt b/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt deleted file mode 100644 index 5338f1fdcc..0000000000 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CMakeLists.txt +++ /dev/null @@ -1,53 +0,0 @@ -cmake_minimum_required(VERSION 3.22) -project(moveit_chomp_optimizer_adapter LANGUAGES CXX) - -# Common cmake code applied to all moveit packages -find_package(moveit_common REQUIRED) -moveit_package() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(chomp_motion_planner REQUIRED) -find_package(moveit_core REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rsl REQUIRED) - -set(THIS_PACKAGE_INCLUDE_DEPENDS - chomp_motion_planner - moveit_core - pluginlib - rclcpp - rsl -) - -add_library(moveit_chomp_optimizer_adapter SHARED src/chomp_optimizer_adapter.cpp) -set_target_properties(moveit_chomp_optimizer_adapter PROPERTIES VERSION "${moveit_chomp_optimizer_adapter_VERSION}") -ament_target_dependencies(moveit_chomp_optimizer_adapter ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -install( - TARGETS moveit_chomp_optimizer_adapter - EXPORT moveit_chomp_optimizer_adapterTargets - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin -) - -ament_export_targets(moveit_chomp_optimizer_adapterTargets HAS_LIBRARY_TARGET) - -pluginlib_export_plugin_description_file(moveit_core chomp_optimizer_adapter_plugin_description.xml) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_lint_cmake_FOUND TRUE) - - # Run all lint tests in package.xml except those listed above - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/chomp_optimizer_adapter_plugin_description.xml b/moveit_planners/chomp/chomp_optimizer_adapter/chomp_optimizer_adapter_plugin_description.xml deleted file mode 100644 index 044f77d8b6..0000000000 --- a/moveit_planners/chomp/chomp_optimizer_adapter/chomp_optimizer_adapter_plugin_description.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml deleted file mode 100644 index a0da33eaae..0000000000 --- a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - moveit_chomp_optimizer_adapter - MoveIt planning request adapter utilizing chomp for solution optimization - 2.8.0 - Raghavender Sahdev - MoveIt Release Team - - BSD-3-Clause - - Raghavender Sahdev - - ament_cmake - moveit_common - - moveit_core - chomp_motion_planner - pluginlib - rsl - - ament_lint_auto - ament_lint_common - - - ament_cmake - - - diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp deleted file mode 100644 index e8f8b8cbe6..0000000000 --- a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp +++ /dev/null @@ -1,216 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, Raghavender Sahdev. - * 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 Raghavender Sahdev 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: Raghavender Sahdev */ - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace chomp -{ -static const rclcpp::Logger LOGGER = rclcpp::get_logger("chomp_planner"); - -class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter -{ -public: - OptimizerAdapter() : planning_request_adapter::PlanningRequestAdapter() - { - } - - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& /* unused */) override - { - if (!node->get_parameter("chomp.planning_time_limit", params_.planning_time_limit_)) - { - params_.planning_time_limit_ = 10.0; - RCLCPP_DEBUG(LOGGER, "Param planning_time_limit was not set. Using default value: %f", - params_.planning_time_limit_); - } - if (!node->get_parameter("chomp.max_iterations", params_.max_iterations_)) - { - params_.max_iterations_ = 200; - RCLCPP_DEBUG(LOGGER, "Param max_iterations was not set. Using default value: %d", params_.max_iterations_); - } - if (!node->get_parameter("chomp.max_iterations_after_collision_free", params_.max_iterations_after_collision_free_)) - { - params_.max_iterations_after_collision_free_ = 5; - RCLCPP_DEBUG(LOGGER, "Param max_iterations_after_collision_free was not set. Using default value: %d", - params_.max_iterations_after_collision_free_); - } - if (!node->get_parameter("chomp.smoothness_cost_weight", params_.smoothness_cost_weight_)) - { - params_.smoothness_cost_weight_ = 0.1; - RCLCPP_DEBUG(LOGGER, "Param smoothness_cost_weight was not set. Using default value: %f", - params_.smoothness_cost_weight_); - } - if (!node->get_parameter("chomp.obstacle_cost_weight", params_.obstacle_cost_weight_)) - { - params_.obstacle_cost_weight_ = 1.0; - RCLCPP_DEBUG(LOGGER, "Param obstacle_cost_weight was not set. Using default value: %f", - params_.obstacle_cost_weight_); - } - if (!node->get_parameter("chomp.learning_rate", params_.learning_rate_)) - { - params_.learning_rate_ = 0.01; - RCLCPP_DEBUG(LOGGER, "Param learning_rate was not set. Using default value: %f", params_.learning_rate_); - } - if (!node->get_parameter("chomp.smoothness_cost_velocity", params_.smoothness_cost_velocity_)) - { - params_.smoothness_cost_velocity_ = 0.0; - RCLCPP_DEBUG(LOGGER, "Param smoothness_cost_velocity was not set. Using default value: %f", - params_.smoothness_cost_velocity_); - } - if (!node->get_parameter("chomp.smoothness_cost_acceleration", params_.smoothness_cost_acceleration_)) - { - params_.smoothness_cost_acceleration_ = 1.0; - RCLCPP_DEBUG(LOGGER, "Param smoothness_cost_acceleration was not set. Using default value: %f", - params_.smoothness_cost_acceleration_); - } - if (!node->get_parameter("chomp.smoothness_cost_jerk", params_.smoothness_cost_jerk_)) - { - params_.smoothness_cost_jerk_ = 0.0; - RCLCPP_DEBUG(LOGGER, "Param smoothness_cost_jerk_ was not set. Using default value: %f", - params_.smoothness_cost_jerk_); - } - if (!node->get_parameter("chomp.ridge_factor", params_.ridge_factor_)) - { - params_.ridge_factor_ = 0.0; - RCLCPP_DEBUG(LOGGER, "Param ridge_factor_ was not set. Using default value: %f", params_.ridge_factor_); - } - if (!node->get_parameter("chomp.use_pseudo_inverse", params_.use_pseudo_inverse_)) - { - params_.use_pseudo_inverse_ = 0.0; - RCLCPP_DEBUG(LOGGER, "Param use_pseudo_inverse_ was not set. Using default value: %d", - params_.use_pseudo_inverse_); - } - if (!node->get_parameter("chomp.pseudo_inverse_ridge_factor", params_.pseudo_inverse_ridge_factor_)) - { - params_.pseudo_inverse_ridge_factor_ = 1e-4; - RCLCPP_DEBUG(LOGGER, "Param pseudo_inverse_ridge_factor was not set. Using default value: %f", - params_.pseudo_inverse_ridge_factor_); - } - if (!node->get_parameter("chomp.joint_update_limit", params_.joint_update_limit_)) - { - params_.joint_update_limit_ = 0.1; - RCLCPP_DEBUG(LOGGER, "Param joint_update_limit was not set. Using default value: %f", params_.joint_update_limit_); - } - if (!node->get_parameter("chomp.collision_clearance", params_.min_clearance_)) - { - params_.min_clearance_ = 0.2; - RCLCPP_DEBUG(LOGGER, "Param collision_clearance was not set. Using default value: %f", params_.min_clearance_); - } - if (!node->get_parameter("chomp.collision_threshold", params_.collision_threshold_)) - { - params_.collision_threshold_ = 0.07; - RCLCPP_DEBUG(LOGGER, "Param collision_threshold_ was not set. Using default value: %f", - params_.collision_threshold_); - } - if (!node->get_parameter("chomp.use_stochastic_descent", params_.use_stochastic_descent_)) - { - params_.use_stochastic_descent_ = true; - RCLCPP_DEBUG(LOGGER, "Param use_stochastic_descent was not set. Using default value: %d", - params_.use_stochastic_descent_); - } - params_.trajectory_initialization_method_ = "quintic-spline"; - std::string method; - if (node->get_parameter("chomp.trajectory_initialization_method", method) && - !params_.setTrajectoryInitializationMethod(method)) - { - RCLCPP_ERROR(LOGGER, - "Attempted to set trajectory_initialization_method to invalid value '%s'. Using default " - "'%s' instead.", - method.c_str(), params_.trajectory_initialization_method_.c_str()); - } - } - - std::string getDescription() const override - { - return "CHOMP Optimizer"; - } - - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& ps, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const override - { - RCLCPP_DEBUG(LOGGER, "CHOMP: adaptAndPlan ..."); - - // following call to planner() calls the OMPL planner and stores the trajectory inside the MotionPlanResponse res - // variable which is then used by CHOMP for optimization of the computed trajectory - if (!planner(ps, req, res)) - return false; - - // create a hybrid collision detector to set the collision checker as hybrid - collision_detection::CollisionDetectorAllocatorPtr hybrid_cd( - collision_detection::CollisionDetectorAllocatorHybrid::create()); - - // create a writable planning scene - planning_scene::PlanningScenePtr planning_scene = ps->diff(); - RCLCPP_DEBUG(LOGGER, "Configuring Planning Scene for CHOMP ..."); - planning_scene->allocateCollisionDetector(hybrid_cd); - - chomp::ChompPlanner chomp_planner; - planning_interface::MotionPlanDetailedResponse res_detailed; - res_detailed.trajectory.push_back(res.trajectory); - - bool planning_success = chomp_planner.solve(planning_scene, req, params_, res_detailed); - - if (planning_success) - { - res.trajectory = res_detailed.trajectory[0]; - res.planning_time += res_detailed.processing_time[0]; - } - res.error_code = res_detailed.error_code; - - return planning_success; - } - -private: - chomp::ChompParameters params_; -}; -} // namespace chomp - -PLUGINLIB_EXPORT_CLASS(chomp::OptimizerAdapter, planning_request_adapter::PlanningRequestAdapter) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h index f6dd127a35..8790fdadcf 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h @@ -93,8 +93,8 @@ class ModelBasedPlanningContext : public planning_interface::PlanningContext { } - bool solve(planning_interface::MotionPlanResponse& res) override; - bool solve(planning_interface::MotionPlanDetailedResponse& res) override; + void solve(planning_interface::MotionPlanResponse& res) override; + void solve(planning_interface::MotionPlanDetailedResponse& res) override; void clear() override; bool terminate() override; diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 2a03ade24f..84c920e024 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -763,88 +763,81 @@ void ompl_interface::ModelBasedPlanningContext::postSolve() RCLCPP_DEBUG(LOGGER, "%s", rclcpp::get_c_string(debug_out.str())); } -bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& res) +void ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& res) { res.error_code = solve(request_.allowed_planning_time, request_.num_planning_attempts); - if (res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + if (res.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { - double ptime = getLastPlanTime(); - if (simplify_solutions_) - { - simplifySolution(request_.allowed_planning_time - ptime); - ptime += getLastSimplifyTime(); - } - - if (interpolate_) - { - interpolateSolution(); - } - - // fill the response - RCLCPP_DEBUG(LOGGER, "%s: Returning successful solution with %lu states", getName().c_str(), - getOMPLSimpleSetup()->getSolutionPath().getStateCount()); - - res.trajectory = std::make_shared(getRobotModel(), getGroupName()); - getSolutionPath(*res.trajectory); - res.planning_time = ptime; - return true; + RCLCPP_ERROR(LOGGER, "Unable to solve the planning problem"); + return; } - else + double ptime = getLastPlanTime(); + if (simplify_solutions_) { - RCLCPP_INFO(LOGGER, "Unable to solve the planning problem"); - return false; + simplifySolution(request_.allowed_planning_time - ptime); + ptime += getLastSimplifyTime(); } + + if (interpolate_) + { + interpolateSolution(); + } + + // fill the response + RCLCPP_DEBUG(LOGGER, "%s: Returning successful solution with %lu states", getName().c_str(), + getOMPLSimpleSetup()->getSolutionPath().getStateCount()); + + res.trajectory = std::make_shared(getRobotModel(), getGroupName()); + getSolutionPath(*res.trajectory); + res.planning_time = ptime; } -bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) +void ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) { moveit_msgs::msg::MoveItErrorCodes moveit_result = solve(request_.allowed_planning_time, request_.num_planning_attempts); - if (moveit_result.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + if (moveit_result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { - res.trajectory.reserve(3); + RCLCPP_INFO(LOGGER, "Unable to solve the planning problem"); + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; + return; + } - // add info about planned solution - double ptime = getLastPlanTime(); - res.processing_time.push_back(ptime); - res.description.emplace_back("plan"); + res.trajectory.reserve(3); + + // add info about planned solution + double ptime = getLastPlanTime(); + res.processing_time.push_back(ptime); + res.description.emplace_back("plan"); + res.trajectory.resize(res.trajectory.size() + 1); + res.trajectory.back() = std::make_shared(getRobotModel(), getGroupName()); + getSolutionPath(*res.trajectory.back()); + + // simplify solution if time remains + if (simplify_solutions_) + { + simplifySolution(request_.allowed_planning_time - ptime); + res.processing_time.push_back(getLastSimplifyTime()); + res.description.emplace_back("simplify"); res.trajectory.resize(res.trajectory.size() + 1); res.trajectory.back() = std::make_shared(getRobotModel(), getGroupName()); getSolutionPath(*res.trajectory.back()); - - // simplify solution if time remains - if (simplify_solutions_) - { - simplifySolution(request_.allowed_planning_time - ptime); - res.processing_time.push_back(getLastSimplifyTime()); - res.description.emplace_back("simplify"); - res.trajectory.resize(res.trajectory.size() + 1); - res.trajectory.back() = std::make_shared(getRobotModel(), getGroupName()); - getSolutionPath(*res.trajectory.back()); - } - - if (interpolate_) - { - ompl::time::point start_interpolate = ompl::time::now(); - interpolateSolution(); - res.processing_time.push_back(ompl::time::seconds(ompl::time::now() - start_interpolate)); - res.description.emplace_back("interpolate"); - res.trajectory.resize(res.trajectory.size() + 1); - res.trajectory.back() = std::make_shared(getRobotModel(), getGroupName()); - getSolutionPath(*res.trajectory.back()); - } - - RCLCPP_DEBUG(LOGGER, "%s: Returning successful solution with %lu states", getName().c_str(), - getOMPLSimpleSetup()->getSolutionPath().getStateCount()); - res.error_code.val = moveit_result.val; - return true; } - else + + if (interpolate_) { - RCLCPP_INFO(LOGGER, "Unable to solve the planning problem"); - res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; - return false; + ompl::time::point start_interpolate = ompl::time::now(); + interpolateSolution(); + res.processing_time.push_back(ompl::time::seconds(ompl::time::now() - start_interpolate)); + res.description.emplace_back("interpolate"); + res.trajectory.resize(res.trajectory.size() + 1); + res.trajectory.back() = std::make_shared(getRobotModel(), getGroupName()); + getSolutionPath(*res.trajectory.back()); } + + RCLCPP_DEBUG(LOGGER, "%s: Returning successful solution with %lu states", getName().c_str(), + getOMPLSimpleSetup()->getSolutionPath().getStateCount()); + res.error_code.val = moveit_result.val; } const moveit_msgs::msg::MoveItErrorCodes ompl_interface::ModelBasedPlanningContext::solve(double timeout, diff --git a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp index 45f7d1498c..c16cc909ed 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp @@ -108,7 +108,8 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public // solve the planning problem planning_interface::MotionPlanDetailedResponse res; - ASSERT_TRUE(pc->solve(res)); + pc->solve(res); + ASSERT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); } void testPathConstraints(const std::vector& start, const std::vector& goal) @@ -154,7 +155,8 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public EXPECT_NE(dynamic_cast(pc->getOMPLStateSpace().get()), nullptr); planning_interface::MotionPlanDetailedResponse response; - ASSERT_TRUE(pc->solve(response)); + pc->solve(response); + ASSERT_TRUE(response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // Are the path constraints created in the planning context? auto path_constraints = pc->getPathConstraints(); @@ -191,7 +193,8 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public // Create a new response, because the solve method does not clear the given response planning_interface::MotionPlanDetailedResponse response2; - ASSERT_TRUE(pc->solve(response2)); + pc->solve(response2); + ASSERT_TRUE(response2.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // Are the path constraints created in the planning context? path_constraints = pc->getPathConstraints(); diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h index bef598c805..449bd3dc8a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h @@ -78,7 +78,7 @@ class PlanningContextBase : public planning_interface::PlanningContext * on failure * @return true on success, false otherwise */ - bool solve(planning_interface::MotionPlanResponse& res) override; + void solve(planning_interface::MotionPlanResponse& res) override; /** * @brief Will return the same trajectory as @@ -89,7 +89,7 @@ class PlanningContextBase : public planning_interface::PlanningContext * @param res The detailed response * @return true on success, false otherwise */ - bool solve(planning_interface::MotionPlanDetailedResponse& res) override; + void solve(planning_interface::MotionPlanDetailedResponse& res) override; /** * @brief Will terminate solve() @@ -117,36 +117,32 @@ class PlanningContextBase : public planning_interface::PlanningContext }; template -bool pilz_industrial_motion_planner::PlanningContextBase::solve(planning_interface::MotionPlanResponse& res) +void pilz_industrial_motion_planner::PlanningContextBase::solve(planning_interface::MotionPlanResponse& res) { - if (!terminated_) + if (terminated_) { - // Use current state as start state if not set - if (request_.start_state.joint_state.name.empty()) - { - moveit_msgs::msg::RobotState current_state; - moveit::core::robotStateToRobotStateMsg(getPlanningScene()->getCurrentState(), current_state); - request_.start_state = current_state; - } - bool result = generator_.generate(getPlanningScene(), request_, res); - return result; - // res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; - // return false; // TODO + RCLCPP_ERROR(rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_base"), + "Using solve on a terminated planning context!"); + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; + return; } - - RCLCPP_ERROR(rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_base"), - "Using solve on a terminated planning context!"); - res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; - return false; + // Use current state as start state if not set + if (request_.start_state.joint_state.name.empty()) + { + moveit_msgs::msg::RobotState current_state; + moveit::core::robotStateToRobotStateMsg(getPlanningScene()->getCurrentState(), current_state); + request_.start_state = current_state; + } + generator_.generate(getPlanningScene(), request_, res); } template -bool pilz_industrial_motion_planner::PlanningContextBase::solve( +void pilz_industrial_motion_planner::PlanningContextBase::solve( planning_interface::MotionPlanDetailedResponse& res) { // delegate to regular response planning_interface::MotionPlanResponse undetailed_response; - bool result = solve(undetailed_response); + solve(undetailed_response); res.description.push_back("plan"); res.trajectory.push_back(undetailed_response.trajectory); @@ -161,7 +157,6 @@ bool pilz_industrial_motion_planner::PlanningContextBase::solve( res.processing_time.push_back(0); res.error_code = undetailed_response.error_code; - return result; } template diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h index 27f976bf69..48c77f53db 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h @@ -105,10 +105,8 @@ class TrajectoryGenerator * @param req: motion plan request * @param res: motion plan response * @param sampling_time: sampling time of the generate trajectory - * @return motion plan succeed/fail, detailed information in motion plan - * response */ - bool generate(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, + void generate(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, double sampling_time = 0.1); protected: diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp index 0b67c62013..24df7e4672 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -303,7 +303,7 @@ TrajectoryGenerator::cartesianTrapVelocityProfile(double max_velocity_scaling_fa return vp_trans; } -bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& scene, +void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, double sampling_time) { @@ -320,7 +320,7 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& RCLCPP_ERROR_STREAM(LOGGER, ex.what()); res.error_code.val = ex.getErrorCode(); setFailureResponse(planning_begin, res); - return false; + return; } try @@ -332,7 +332,7 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& RCLCPP_ERROR_STREAM(LOGGER, ex.what()); res.error_code.val = ex.getErrorCode(); setFailureResponse(planning_begin, res); - return false; + return; } MotionPlanInfo plan_info; @@ -345,7 +345,7 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& RCLCPP_ERROR_STREAM(LOGGER, ex.what()); res.error_code.val = ex.getErrorCode(); setFailureResponse(planning_begin, res); - return false; + return; } trajectory_msgs::msg::JointTrajectory joint_trajectory; @@ -358,13 +358,12 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& RCLCPP_ERROR_STREAM(LOGGER, ex.what()); res.error_code.val = ex.getErrorCode(); setFailureResponse(planning_begin, res); - return false; + return; } moveit::core::RobotState start_state(scene->getCurrentState()); moveit::core::robotStateMsgToRobotState(req.start_state, start_state, true); setSuccessResponse(start_state, req.group_name, joint_trajectory, planning_begin, res); - return true; } } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp index ad3a4934b0..844fe72de7 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -1076,7 +1076,8 @@ bool testutils::generateTrajFromBlendTestData( goal_state_1, goal_state_1.getRobotModel()->getJointModelGroup(group_name))); // trajectory generation - if (!tg->generate(scene, req_1, res_1, sampling_time_1)) + tg->generate(scene, req_1, res_1, sampling_time_1); + if (res_1.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { std::cout << "Failed to generate first trajectory." << '\n'; return false; @@ -1096,7 +1097,8 @@ bool testutils::generateTrajFromBlendTestData( req_2.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( goal_state_2, goal_state_2.getRobotModel()->getJointModelGroup(group_name))); // trajectory generation - if (!tg->generate(scene, req_2, res_2, sampling_time_2)) + tg->generate(scene, req_2, res_2, sampling_time_2); + if (res_2.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { std::cout << "Failed to generate second trajectory." << '\n'; return false; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp index e48b26d719..a0540e5360 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp @@ -207,9 +207,8 @@ TYPED_TEST_SUITE(PlanningContextTest, PlanningContextTestTypes, /* ... */); TYPED_TEST(PlanningContextTest, NoRequest) { planning_interface::MotionPlanResponse res; - bool result = this->planning_context_->solve(res); + this->planning_context_->solve(res); - EXPECT_FALSE(result) << testutils::demangle(typeid(TypeParam).name()); EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN, res.error_code.val) << testutils::demangle(typeid(TypeParam).name()); } @@ -225,16 +224,14 @@ TYPED_TEST(PlanningContextTest, SolveValidRequest) this->planning_context_->setMotionPlanRequest(req); // TODO Formulate valid request - bool result = this->planning_context_->solve(res); + this->planning_context_->solve(res); - EXPECT_TRUE(result) << testutils::demangle(typeid(TypeParam).name()); EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, res.error_code.val) << testutils::demangle(typeid(TypeParam).name()); planning_interface::MotionPlanDetailedResponse res_detailed; - bool result_detailed = this->planning_context_->solve(res_detailed); + this->planning_context_->solve(res_detailed); - EXPECT_TRUE(result_detailed) << testutils::demangle(typeid(TypeParam).name()); EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, res.error_code.val) << testutils::demangle(typeid(TypeParam).name()); } @@ -248,9 +245,8 @@ TYPED_TEST(PlanningContextTest, SolveValidRequestDetailedResponse) planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangle(typeid(TypeParam).name())); this->planning_context_->setMotionPlanRequest(req); - bool result = this->planning_context_->solve(res); + this->planning_context_->solve(res); - EXPECT_TRUE(result) << testutils::demangle(typeid(TypeParam).name()); EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, res.error_code.val) << testutils::demangle(typeid(TypeParam).name()); } @@ -268,8 +264,7 @@ TYPED_TEST(PlanningContextTest, SolveOnTerminated) bool result_termination = this->planning_context_->terminate(); EXPECT_TRUE(result_termination) << testutils::demangle(typeid(TypeParam).name()); - bool result = this->planning_context_->solve(res); - EXPECT_FALSE(result) << testutils::demangle(typeid(TypeParam).name()); + this->planning_context_->solve(res); EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED, res.error_code.val) << testutils::demangle(typeid(TypeParam).name()); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp index 8fd48ab9e7..3a109cddcb 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp @@ -147,7 +147,8 @@ class TrajectoryBlenderTransitionWindowTest : public testing::Test } // generate trajectory planning_interface::MotionPlanResponse resp; - if (!lin_generator_->generate(planning_scene_, req, resp, sampling_time_)) + lin_generator_->generate(planning_scene_, req, resp, sampling_time_); + if (resp.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { std::runtime_error("Failed to generate trajectory."); } @@ -325,7 +326,8 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes) } // generate trajectory planning_interface::MotionPlanResponse resp; - if (!lin_generator_->generate(planning_scene_, req, resp, sampling_time_)) + lin_generator_->generate(planning_scene_, req, resp, sampling_time_); + if (resp.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { std::runtime_error("Failed to generate trajectory."); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp index df2f5ad69f..2d043eb0fc 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp @@ -289,7 +289,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, incompleteStartState) req.start_state.joint_state.position.resize(1); // prevent failing check for equal sizes planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } @@ -303,7 +303,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, nonZeroStartVelocity) // start state has non-zero velocity req.start_state.joint_state.velocity.push_back(1.0); planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); req.start_state.joint_state.velocity.clear(); } @@ -313,7 +313,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, ValidCommand) auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; planning_interface::MotionPlanResponse res; - EXPECT_TRUE(circ_->generate(planning_scene_, circ.toRequest(), res)); + circ_->generate(planning_scene_, circ.toRequest(), res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); } @@ -326,7 +326,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, velScaleToHigh) circ.setVelocityScale(1.0); planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); + circ_->generate(planning_scene_, circ.toRequest(), res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED); } @@ -339,7 +339,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, accScaleToHigh) circ.setAccelerationScale(1.0); planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); + circ_->generate(planning_scene_, circ.toRequest(), res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED); } @@ -361,7 +361,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, samePointsWithCenter) circ.getGoalConfiguration().getPose().position.z -= 1e-8; planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); + circ_->generate(planning_scene_, circ.toRequest(), res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } @@ -384,7 +384,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, samePointsWithInterim) circ.getGoalConfiguration().getPose().position.z -= 1e-8; planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); + circ_->generate(planning_scene_, circ.toRequest(), res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } @@ -400,7 +400,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, emptyAux) req.path_constraints.position_constraints.clear(); planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } @@ -416,7 +416,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, invalidAuxName) req.path_constraints.name = ""; planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } @@ -433,7 +433,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, invalidAuxLinkName) req.path_constraints.position_constraints.front().link_name = "INVALID"; planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME); } @@ -447,7 +447,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, invalidCenter) circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y += 1; planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); + circ_->generate(planning_scene_, circ.toRequest(), res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } @@ -467,7 +467,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, colinearCenter) circ.getGoalConfiguration().getPose().position.x += 0.1; planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); + circ_->generate(planning_scene_, circ.toRequest(), res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } @@ -488,7 +488,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, colinearInterim) circ.getGoalConfiguration().getPose().position.x += 0.1; planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res)); + circ_->generate(planning_scene_, circ.toRequest(), res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } @@ -506,7 +506,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, colinearCenterDueToInterim) auto circ{ tdp_->getCircCartInterimCart("circ3_interim") }; planning_interface::MotionPlanResponse res; - ASSERT_TRUE(circ_->generate(planning_scene_, circ.toRequest(), res)); + circ_->generate(planning_scene_, circ.toRequest(), res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); } @@ -539,7 +539,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, colinearCenterAndInterim) moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); planning_interface::MotionPlanResponse res; - EXPECT_TRUE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -573,7 +573,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, interimLarger180Degree) moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); planning_interface::MotionPlanResponse res; - EXPECT_TRUE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -587,7 +587,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, centerPointJointGoal) moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); planning_interface::MotionPlanResponse res; - ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -614,7 +614,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, InvalidAdditionalPrimitivePose) req.path_constraints.position_constraints.back().constraint_region.primitive_poses.push_back(center_position); planning_interface::MotionPlanResponse res; - ASSERT_FALSE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } @@ -636,7 +636,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, InvalidExtraJointConstraint) req.goal_constraints.front().joint_constraints.push_back(joint_constraint); //<-- Additional constraint planning_interface::MotionPlanResponse res; - EXPECT_FALSE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } @@ -650,7 +650,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoal) moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); planning_interface::MotionPlanResponse res; - ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -667,7 +667,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdPositionConstraint req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame(); planning_interface::MotionPlanResponse res; - ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -683,7 +683,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdOrientationConstra req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame(); planning_interface::MotionPlanResponse res; - ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -702,7 +702,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdBothConstraints) req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame(); planning_interface::MotionPlanResponse res; - ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -717,7 +717,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, InterimPointJointGoal) moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); planning_interface::MotionPlanResponse res; - ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -738,7 +738,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, InterimPointJointGoalStartVelNearZero) req.start_state.joint_state.velocity = std::vector(req.start_state.joint_state.position.size(), 1e-16); planning_interface::MotionPlanResponse res; - ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } @@ -752,7 +752,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, InterimPointPoseGoal) moveit_msgs::msg::MotionPlanRequest req = circ.toRequest(); planning_interface::MotionPlanResponse res; - ASSERT_TRUE(circ_->generate(planning_scene_, req, res)); + circ_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); checkCircResult(req, res); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp index 0e0faa8b25..9db58f5ede 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp @@ -192,22 +192,22 @@ TYPED_TEST_SUITE(TrajectoryGeneratorCommonTestNoGripper, TrajectoryGeneratorComm TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideScalingFactor) { this->req_.max_velocity_scaling_factor = 2.0; - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); this->req_.max_velocity_scaling_factor = 1.0; this->req_.max_acceleration_scaling_factor = 0; - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); this->req_.max_velocity_scaling_factor = 0.00001; this->req_.max_acceleration_scaling_factor = 1; - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); this->req_.max_velocity_scaling_factor = 1; this->req_.max_acceleration_scaling_factor = -1; - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); } @@ -217,7 +217,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideScalingFactor) TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidGroupName) { this->req_.group_name = "foot"; - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code.val); } @@ -227,7 +227,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidGroupName) TYPED_TEST(TrajectoryGeneratorCommonTestNoGripper, GripperGroup) { this->req_.group_name = "gripper"; - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code.val); } @@ -237,7 +237,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTestNoGripper, GripperGroup) // TYPED_TEST(TrajectoryGeneratorCommonTestWithGripper, GripperGroup) // { // this->req_.group_name = "gripper"; -// EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); +// this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); // EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS, this->res_.error_code.val); // } @@ -262,7 +262,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTestNoGripper, GripperGroup) TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyJointNamesInStartState) { this->req_.start_state.joint_state.name.clear(); - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } @@ -272,7 +272,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyJointNamesInStartState) TYPED_TEST(TrajectoryGeneratorCommonTest, InconsistentStartState) { this->req_.start_state.joint_state.name.push_back("joint_7"); - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } @@ -282,7 +282,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InconsistentStartState) TYPED_TEST(TrajectoryGeneratorCommonTest, StartPostionOutOfLimit) { this->req_.start_state.joint_state.position[0] = 100; - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } @@ -296,7 +296,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, StartPostionOutOfLimit) TYPED_TEST(TrajectoryGeneratorCommonTest, StartPositionVelocityNoneZero) { this->req_.start_state.joint_state.velocity[0] = 100; - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } @@ -306,7 +306,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, StartPositionVelocityNoneZero) TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyGoalConstraints) { this->req_.goal_constraints.clear(); - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } @@ -323,7 +323,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) // two goal constraints this->req_.goal_constraints.push_back(goal_constraint); this->req_.goal_constraints.push_back(goal_constraint); - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); // one joint constraint and one orientation constraint @@ -331,7 +331,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) goal_constraint.orientation_constraints.push_back(orientation_constraint); this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); // one joint constraint and one Cartesian constraint @@ -339,7 +339,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) goal_constraint.orientation_constraints.push_back(orientation_constraint); this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); // two Cartesian constraints @@ -350,7 +350,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals) goal_constraint.orientation_constraints.push_back(orientation_constraint); this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } @@ -362,7 +362,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideJointNameInGoal) moveit_msgs::msg::JointConstraint joint_constraint; joint_constraint.joint_name = "test_joint_2"; this->req_.goal_constraints.front().joint_constraints[0] = joint_constraint; - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } @@ -374,7 +374,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MissingJointConstraint) moveit_msgs::msg::JointConstraint joint_constraint; joint_constraint.joint_name = "test_joint_2"; this->req_.goal_constraints.front().joint_constraints.pop_back(); //<-- Missing joint constraint - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } @@ -384,7 +384,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MissingJointConstraint) TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideJointPositionInGoal) { this->req_.goal_constraints.front().joint_constraints[0].position = 100; - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } @@ -401,7 +401,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidLinkNameInCartesianGoal) goal_constraint.orientation_constraints.push_back(orientation_constraint); this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); // different link names in position and orientation goals @@ -409,14 +409,14 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidLinkNameInCartesianGoal) goal_constraint.orientation_constraints.front().link_name = "test_link_2"; this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); // no solver for the link goal_constraint.orientation_constraints.front().link_name = "test_link_1"; this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); } @@ -436,7 +436,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyPrimitivePoses) goal_constraint.orientation_constraints.push_back(orientation_constraint); this->req_.goal_constraints.clear(); this->req_.goal_constraints.push_back(goal_constraint); - EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_)); + this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_); EXPECT_EQ(this->res_.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp index 4d093276e5..7d2b375b41 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp @@ -222,7 +222,7 @@ TEST_F(TrajectoryGeneratorLINTest, nonZeroStartVelocity) // try to generate the result planning_interface::MotionPlanResponse res; - EXPECT_FALSE(lin_->generate(planning_scene_, req, res)); + lin_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } @@ -235,7 +235,7 @@ TEST_F(TrajectoryGeneratorLINTest, jointSpaceGoal) // generate the LIN trajectory planning_interface::MotionPlanResponse res; - ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res)); + lin_->generate(planning_scene_, lin_joint_req, res); EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory @@ -256,7 +256,7 @@ TEST_F(TrajectoryGeneratorLINTest, jointSpaceGoalNearZeroStartVelocity) // generate the LIN trajectory planning_interface::MotionPlanResponse res; - ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res)); + lin_->generate(planning_scene_, lin_joint_req, res); EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory @@ -273,7 +273,7 @@ TEST_F(TrajectoryGeneratorLINTest, cartesianSpaceGoal) // generate lin trajectory planning_interface::MotionPlanResponse res; - ASSERT_TRUE(lin_->generate(planning_scene_, lin_cart_req, res)); + lin_->generate(planning_scene_, lin_cart_req, res); EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory @@ -297,7 +297,7 @@ TEST_F(TrajectoryGeneratorLINTest, cartesianTrapezoidProfile) /// + plan LIN trajectory + /// +++++++++++++++++++++++ planning_interface::MotionPlanResponse res; - ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res, 0.01)); + lin_->generate(planning_scene_, lin_joint_req, res, 0.01); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory, target_link_hcd_)); @@ -329,7 +329,8 @@ TEST_F(TrajectoryGeneratorLINTest, LinPlannerLimitViolation) lin.setAccelerationScale(1.01); planning_interface::MotionPlanResponse res; - ASSERT_FALSE(lin_->generate(planning_scene_, lin.toRequest(), res)); + lin_->generate(planning_scene_, lin.toRequest(), res); + ASSERT_FALSE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); } /** @@ -355,7 +356,8 @@ TEST_F(TrajectoryGeneratorLINTest, LinPlannerDiscontinuousJointTraj) lin.setAccelerationScale(1.0); planning_interface::MotionPlanResponse res; - ASSERT_FALSE(lin_->generate(planning_scene_, lin.toRequest(), res)); + lin_->generate(planning_scene_, lin.toRequest(), res); + ASSERT_FALSE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); } /** @@ -382,7 +384,7 @@ TEST_F(TrajectoryGeneratorLINTest, LinStartEqualsGoal) // generate the LIN trajectory planning_interface::MotionPlanResponse res; - ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res)); + lin_->generate(planning_scene_, lin_joint_req, res); EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory @@ -409,7 +411,7 @@ TEST_F(TrajectoryGeneratorLINTest, IncorrectJointNumber) // generate the LIN trajectory planning_interface::MotionPlanResponse res; - ASSERT_FALSE(lin_->generate(planning_scene_, lin_joint_req, res)); + lin_->generate(planning_scene_, lin_joint_req, res); EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } @@ -427,7 +429,7 @@ TEST_F(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState) // generate lin trajectory planning_interface::MotionPlanResponse res; - EXPECT_FALSE(lin_->generate(planning_scene_, lin_cart_req, res)); + lin_->generate(planning_scene_, lin_cart_req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); } @@ -445,7 +447,7 @@ TEST_F(TrajectoryGeneratorLINTest, cartGoalFrameIdBothConstraints) // generate lin trajectory planning_interface::MotionPlanResponse res; - ASSERT_TRUE(lin_->generate(planning_scene_, lin_cart_req, res)); + lin_->generate(planning_scene_, lin_cart_req, res); EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); // check the resulted trajectory diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp index f0d4e06da7..6a423b6f45 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp @@ -207,8 +207,9 @@ TEST_F(TrajectoryGeneratorPTPTest, emptyRequest) EXPECT_FALSE(res.trajectory->empty()); - EXPECT_FALSE(ptp_->generate(planning_scene_, req, res)); + ptp_->generate(planning_scene_, req, res); + EXPECT_FALSE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); EXPECT_TRUE(res.trajectory->empty()); } @@ -374,7 +375,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testCartesianGoal) //**************************************** //*** test robot model without gripper *** //**************************************** - ASSERT_TRUE(ptp_->generate(planning_scene_, req, res)); + ptp_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); moveit_msgs::msg::MotionPlanResponse res_msg; @@ -422,12 +423,12 @@ TEST_F(TrajectoryGeneratorPTPTest, testCartesianGoalMissingLinkNameConstraints) planning_interface::MotionPlanRequest req_no_position_constaint_link_name = req; req_no_position_constaint_link_name.goal_constraints.front().position_constraints.front().link_name = ""; - ASSERT_FALSE(ptp_->generate(planning_scene_, req_no_position_constaint_link_name, res)); + ptp_->generate(planning_scene_, req_no_position_constaint_link_name, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); planning_interface::MotionPlanRequest req_no_orientation_constaint_link_name = req; req_no_orientation_constaint_link_name.goal_constraints.front().orientation_constraints.front().link_name = ""; - ASSERT_FALSE(ptp_->generate(planning_scene_, req_no_orientation_constaint_link_name, res)); + ptp_->generate(planning_scene_, req_no_orientation_constaint_link_name, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } @@ -454,7 +455,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testInvalidCartesianGoal) kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle); req.goal_constraints.push_back(pose_goal); - ASSERT_FALSE(ptp_->generate(planning_scene_, req, res)); + ptp_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); EXPECT_EQ(res.trajectory, nullptr); } @@ -480,7 +481,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAlreadyReached) req.goal_constraints.push_back(gc); // TODO lin and circ has different settings - ASSERT_TRUE(ptp_->generate(planning_scene_, req, res)); + ptp_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); moveit_msgs::msg::MotionPlanResponse res_msg; @@ -552,7 +553,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testScalingFactor) req.max_velocity_scaling_factor = 0.5; req.max_acceleration_scaling_factor = 1.0 / 3.0; - ASSERT_TRUE(ptp_->generate(planning_scene_, req, res)); + ptp_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); moveit_msgs::msg::MotionPlanResponse res_msg; @@ -679,7 +680,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity) gc.joint_constraints.push_back(jc); req.goal_constraints.push_back(gc); - ASSERT_TRUE(ptp_->generate(planning_scene_, req, res)); + ptp_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); moveit_msgs::msg::MotionPlanResponse res_msg; @@ -821,7 +822,7 @@ TEST_F(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel) gc.joint_constraints.push_back(jc); req.goal_constraints.push_back(gc); - ASSERT_TRUE(ptp_->generate(planning_scene_, req, res)); + ptp_->generate(planning_scene_, req, res); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); moveit_msgs::msg::MotionPlanResponse res_msg; diff --git a/moveit_planners/stomp/CMakeLists.txt b/moveit_planners/stomp/CMakeLists.txt index ef600cf98d..941da3dd2e 100644 --- a/moveit_planners/stomp/CMakeLists.txt +++ b/moveit_planners/stomp/CMakeLists.txt @@ -23,7 +23,6 @@ include_directories(include) add_library(stomp_moveit_plugin SHARED src/stomp_moveit_planner_plugin.cpp src/stomp_moveit_planning_context.cpp - src/stomp_moveit_smoothing_adapter.cpp ) ament_target_dependencies(stomp_moveit_plugin moveit_core diff --git a/moveit_planners/stomp/include/stomp_moveit/stomp_moveit_planning_context.hpp b/moveit_planners/stomp/include/stomp_moveit/stomp_moveit_planning_context.hpp index 5b53ca77bc..e7a33c5aec 100644 --- a/moveit_planners/stomp/include/stomp_moveit/stomp_moveit_planning_context.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/stomp_moveit_planning_context.hpp @@ -56,9 +56,9 @@ class StompPlanningContext : public planning_interface::PlanningContext public: StompPlanningContext(const std::string& name, const std::string& group_name, const stomp_moveit::Params& params); - bool solve(planning_interface::MotionPlanResponse& res) override; + void solve(planning_interface::MotionPlanResponse& res) override; - bool solve(planning_interface::MotionPlanDetailedResponse& res) override; + void solve(planning_interface::MotionPlanDetailedResponse& res) override; bool terminate() override; diff --git a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp index 0b0ddb30d7..bc459a63d3 100644 --- a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp +++ b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp @@ -204,7 +204,7 @@ StompPlanningContext::StompPlanningContext(const std::string& name, const std::s { } -bool StompPlanningContext::solve(planning_interface::MotionPlanResponse& res) +void StompPlanningContext::solve(planning_interface::MotionPlanResponse& res) { // Start time auto time_start = std::chrono::steady_clock::now(); @@ -222,7 +222,7 @@ bool StompPlanningContext::solve(planning_interface::MotionPlanResponse& res) if (!goal_sampler || !goal_sampler->sample(goal_state)) { res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; - return false; // Can't plan without valid goal state + return; // Can't plan without valid goal state } // STOMP config, task, planner instance @@ -267,16 +267,14 @@ bool StompPlanningContext::solve(planning_interface::MotionPlanResponse& res) // Stop time std::chrono::duration elapsed_seconds = std::chrono::steady_clock::now() - time_start; res.planning_time = elapsed_seconds.count(); - - return res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS; } -bool StompPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& /*res*/) +void StompPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& /*res*/) { // TODO(#2168): implement this function RCLCPP_ERROR(LOGGER, "StompPlanningContext::solve(planning_interface::MotionPlanDetailedResponse&) is not implemented!"); - return false; + return; } bool StompPlanningContext::terminate() diff --git a/moveit_planners/stomp/src/stomp_moveit_smoothing_adapter.cpp b/moveit_planners/stomp/src/stomp_moveit_smoothing_adapter.cpp deleted file mode 100644 index c09d35635b..0000000000 --- a/moveit_planners/stomp/src/stomp_moveit_smoothing_adapter.cpp +++ /dev/null @@ -1,128 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2023, PickNik Inc. - * 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 PickNik Inc. 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. - *********************************************************************/ - -/** @file - * @author Henning Kayser - * @brief: A PlanningRequestAdapter plugin for optimizing already solved trajectories with STOMP. - */ - -// ROS -#include -#include - -// MoveIt -#include -#include - -// STOMP -#include - -using namespace planning_interface; - -namespace stomp_moveit -{ -/** @brief This adapter uses STOMP for optimizing pre-solved trajectories */ -class StompSmoothingAdapter : public planning_request_adapter::PlanningRequestAdapter -{ -public: - StompSmoothingAdapter() : planning_request_adapter::PlanningRequestAdapter() - { - } - - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override - { - param_listener_ = std::make_shared(node, parameter_namespace); - } - - std::string getDescription() const override - { - return "Stomp Smoothing Adapter"; - } - - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& ps, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const override - { - // Following call to planner() calls the motion planner defined for the pipeline and stores the trajectory inside - // the MotionPlanResponse res variable which is then passed to STOMP for optimization - if (!planner(ps, req, res)) - { - return false; - } - - // STOMP reads the seed trajectory from trajectory constraints so we need to convert the waypoints first - const size_t seed_waypoint_count = res.trajectory->getWayPointCount(); - const std::vector joint_names = - res.trajectory->getFirstWayPoint().getJointModelGroup(req.group_name)->getActiveJointModelNames(); - const size_t joint_count = joint_names.size(); - planning_interface::MotionPlanRequest seed_req = req; - seed_req.trajectory_constraints.constraints.clear(); - seed_req.trajectory_constraints.constraints.resize(seed_waypoint_count); - for (size_t i = 0; i < seed_waypoint_count; ++i) - { - seed_req.trajectory_constraints.constraints[i].joint_constraints.resize(joint_count); - for (size_t j = 0; j < joint_count; ++j) - { - seed_req.trajectory_constraints.constraints[i].joint_constraints[j].joint_name = joint_names[j]; - seed_req.trajectory_constraints.constraints[i].joint_constraints[j].position = - res.trajectory->getWayPoint(i).getVariablePosition(joint_names[j]); - } - } - - // Initialize STOMP Planner - PlanningContextPtr planning_context = - std::make_shared("STOMP", req.group_name, param_listener_->get_params()); - planning_context->clear(); - planning_context->setPlanningScene(ps); - planning_context->setMotionPlanRequest(seed_req); - - // Solve - RCLCPP_INFO(rclcpp::get_logger("stomp_moveit"), "Smoothing result trajectory with STOMP"); - planning_interface::MotionPlanResponse stomp_res; - bool success = planning_context->solve(stomp_res); - if (success) - { - res.trajectory = stomp_res.trajectory; - res.planning_time += stomp_res.planning_time; - } - res.error_code = stomp_res.error_code; - return success; - } - -private: - std::shared_ptr param_listener_; -}; -} // namespace stomp_moveit - -CLASS_LOADER_REGISTER_CLASS(stomp_moveit::StompSmoothingAdapter, planning_request_adapter::PlanningRequestAdapter); diff --git a/moveit_planners/stomp/stomp_moveit_plugin_description.xml b/moveit_planners/stomp/stomp_moveit_plugin_description.xml index 965ef79a23..b0ed1d37c4 100644 --- a/moveit_planners/stomp/stomp_moveit_plugin_description.xml +++ b/moveit_planners/stomp/stomp_moveit_plugin_description.xml @@ -4,10 +4,4 @@ STOMP Motion Planning Plugin for MoveIt - - - STOMP Smoothing Adapter for MoveIt - - diff --git a/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py b/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py index e1a77e5207..d9a053f1e8 100644 --- a/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py +++ b/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py @@ -83,12 +83,10 @@ def generate_launch_description(): "pilz": { "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner", "request_adapters": "", - "start_state_max_bounds_error": 0.1, }, "ompl": { "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", - "start_state_max_bounds_error": 0.1, + "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/ValidateWorkspaceBounds default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision default_planning_request_adapters/FixStartStatePathConstraints""", }, } ompl_planning_yaml = load_yaml( diff --git a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py index 4e52a46ef4..5beba7753b 100644 --- a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py +++ b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py @@ -71,8 +71,7 @@ def generate_common_hybrid_launch_description(): planning_pipelines_config = { "ompl": { "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", - "start_state_max_bounds_error": 0.1, + "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/ValidateWorkspaceBounds default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision""", } } ompl_planning_yaml = load_yaml( diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index 0367ae0360..148acbf8d3 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -52,6 +52,10 @@ using moveit::getLogger; namespace { + +/** \brief To display a motion path with RVIZ, the solution is sent to this topic (moveit_msgs::msg::DisplayTrajectory) */ +const std::string DISPLAY_PATH_TOPIC = std::string("display_planned_path"); + bool isStateValid(const planning_scene::PlanningScene* planning_scene, const kinematic_constraints::KinematicConstraintSet* constraint_set, moveit::core::RobotState* state, const moveit::core::JointModelGroup* group, const double* ik_solution) @@ -72,8 +76,8 @@ MoveGroupCartesianPathService::MoveGroupCartesianPathService() void MoveGroupCartesianPathService::initialize() { - display_path_ = context_->moveit_cpp_->getNode()->create_publisher( - planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC, 10); + display_path_ = + context_->moveit_cpp_->getNode()->create_publisher(DISPLAY_PATH_TOPIC, 10); cartesian_path_service_ = context_->moveit_cpp_->getNode()->create_service( diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index 289f69e5c9..7ab35acafa 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp @@ -50,12 +50,6 @@ using moveit::getLogger; namespace move_group { -namespace -{ -constexpr bool DISPLAY_COMPUTED_MOTION_PLANS = true; -constexpr bool CHECK_SOLUTION_PATHS = true; -} // namespace - MoveGroupMoveAction::MoveGroupMoveAction() : MoveGroupCapability("MoveAction"), move_state_(IDLE), preempt_requested_{ false } { @@ -219,8 +213,7 @@ void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const std::shared_ptrgeneratePlan(the_scene, goal->get_goal()->request, res, context_->debug_, - CHECK_SOLUTION_PATHS, DISPLAY_COMPUTED_MOTION_PLANS)) + if (!planning_pipeline->generatePlan(the_scene, goal->get_goal()->request, res, context_->debug_)) { RCLCPP_ERROR(getLogger(), "Generating a plan with planning pipeline failed."); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; @@ -256,8 +249,7 @@ bool MoveGroupMoveAction::planUsingPlanningPipeline(const planning_interface::Mo planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor); try { - solved = planning_pipeline->generatePlan(plan.planning_scene, req, res, context_->debug_, CHECK_SOLUTION_PATHS, - DISPLAY_COMPUTED_MOTION_PLANS); + solved = planning_pipeline->generatePlan(plan.planning_scene, req, res, context_->debug_); } catch (std::exception& ex) { diff --git a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp index 4627d76159..790a94c7a9 100644 --- a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp @@ -43,11 +43,6 @@ namespace move_group { -namespace -{ -constexpr bool DISPLAY_COMPUTED_MOTION_PLANS = true; -constexpr bool CHECK_SOLUTION_PATHS = true; -} // namespace MoveGroupPlanService::MoveGroupPlanService() : MoveGroupCapability("MotionPlanService") { @@ -86,8 +81,7 @@ bool MoveGroupPlanService::computePlanService(const std::shared_ptrgeneratePlan(ps, req->motion_plan_request, mp_res, context_->debug_, CHECK_SOLUTION_PATHS, - DISPLAY_COMPUTED_MOTION_PLANS)) + if (!planning_pipeline->generatePlan(ps, req->motion_plan_request, mp_res, context_->debug_)) { RCLCPP_ERROR(moveit::getLogger(), "Generating a plan with planning pipeline failed."); mp_res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index e0bf25cdc6..415f2bdc49 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -54,8 +54,10 @@ set(THIS_PACKAGE_LIBRARIES moveit_plan_execution moveit_planning_scene_monitor moveit_collision_plugin_loader - default_plan_request_adapter_parameters + default_request_adapter_parameters + default_response_adapter_parameters moveit_default_planning_request_adapter_plugins + moveit_default_planning_response_adapter_plugins moveit_cpp ) @@ -84,7 +86,6 @@ include_directories(SYSTEM add_subdirectory(collision_plugin_loader) add_subdirectory(constraint_sampler_manager_loader) -add_subdirectory(introspection) add_subdirectory(kinematics_plugin_loader) add_subdirectory(moveit_cpp) add_subdirectory(plan_execution) @@ -92,6 +93,7 @@ add_subdirectory(planning_components_tools) add_subdirectory(planning_pipeline) add_subdirectory(planning_pipeline_interfaces) add_subdirectory(planning_request_adapter_plugins) +add_subdirectory(planning_response_adapter_plugins) add_subdirectory(planning_scene_monitor) add_subdirectory(rdf_loader) add_subdirectory(robot_model_loader) @@ -109,7 +111,8 @@ install( ament_export_targets(moveit_ros_planningTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -pluginlib_export_plugin_description_file(moveit_core "planning_request_adapters_plugin_description.xml") +pluginlib_export_plugin_description_file(moveit_core "default_request_adapters_plugin_description.xml") +pluginlib_export_plugin_description_file(moveit_core "default_response_adapters_plugin_description.xml") if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/moveit_ros/planning/default_request_adapters_plugin_description.xml b/moveit_ros/planning/default_request_adapters_plugin_description.xml new file mode 100644 index 0000000000..3ddcedfd35 --- /dev/null +++ b/moveit_ros/planning/default_request_adapters_plugin_description.xml @@ -0,0 +1,33 @@ + + + + + Fixes the start state to be within the joint limits specified in the URDF. + + + + + + Specify a default workspace for planning if none is specified in the planning request. + + + + + + If start state is in collision this adapter attempts to sample a new collision-free configuration near a specified configuration (in collision) by perturbing the joint values by a small amount. + + + + + + Resolves constraints that are defined in collision objects or subframes to robot links, because the former are not known to the planner. + + + + + + Check if the motion plan request contains stacked path or goal constraints. + + + + diff --git a/moveit_ros/planning/default_response_adapters_plugin_description.xml b/moveit_ros/planning/default_response_adapters_plugin_description.xml new file mode 100644 index 0000000000..82bab654ec --- /dev/null +++ b/moveit_ros/planning/default_response_adapters_plugin_description.xml @@ -0,0 +1,27 @@ + + + + + Time parameterization using Time-Optimal Trajectory Generation for Path Following With Bounded Acceleration and Velocity (Kunz and Stilman, RSS 2008). Assumes starting and ending at rest. Not jerk limited. + + + + + + Adds jerk-limited trajectory smoothing via Ruckig. Best used after a time parameterization algorithm such as TimeOptimalParameterization. + + + + + + Response adapter to display the motion path in RVIZ by publishing as EE pose marker array via ROS topic. + + + + + + Adapter to check the request path validity (collision avoidance, feasibility and constraint satisfaction). + + + + diff --git a/moveit_ros/planning/introspection/CMakeLists.txt b/moveit_ros/planning/introspection/CMakeLists.txt deleted file mode 100644 index 23b5640b15..0000000000 --- a/moveit_ros/planning/introspection/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -add_executable(moveit_list_planning_adapter_plugins src/list_planning_adapter_plugins.cpp) -ament_target_dependencies(moveit_list_planning_adapter_plugins - moveit_core - rclcpp - pluginlib -) - -install(TARGETS moveit_list_planning_adapter_plugins - DESTINATION lib/${PROJECT_NAME} -) diff --git a/moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp b/moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp deleted file mode 100644 index 274de0ebe4..0000000000 --- a/moveit_ros/planning/introspection/src/list_planning_adapter_plugins.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 Willow Garage 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: Ioan Sucan - * Desc: Simple executable to list the loadable PlanningRequestAdapter. To use it simply run: - * `ros2 run moveit_ros_planning moveit_list_planning_adapter_plugins` - */ - -#include -#include -#include -#include -#include -#include - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - - auto node = std::make_shared("list_planning_adapter_plugins"); - moveit::setLogger(node->get_logger()); - - std::unique_ptr> loader; - try - { - loader = std::make_unique>( - "moveit_core", "planning_request_adapter::PlanningRequestAdapter"); - } - catch (pluginlib::PluginlibException& ex) - { - std::cout << "Exception while creating class loader " << ex.what() << '\n'; - } - - const std::vector& classes = loader->getDeclaredClasses(); - std::cout << "Available planning request adapter plugins:" << '\n'; - for (const std::string& adapter_plugin_name : classes) - { - std::cout << " \t " << adapter_plugin_name << '\n'; - planning_request_adapter::PlanningRequestAdapterConstPtr ad; - try - { - ad = loader->createUniqueInstance(adapter_plugin_name); - } - catch (pluginlib::PluginlibException& ex) - { - std::cout << " \t\t Exception while planning adapter plugin '" << adapter_plugin_name << "': " << ex.what() - << '\n'; - } - if (ad) - std::cout << " \t\t " << ad->getDescription() << '\n'; - std::cout << '\n' << '\n'; - } - rclcpp::shutdown(); - return 0; -} diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h index faf93daa44..99f17b42fe 100644 --- a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h @@ -32,61 +32,102 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan */ +/* Author: Ioan Sucan, Sebastian Jahr + Description: Implementation of a MoveIt planning pipeline composed of a planner plugin and request and response + adapter plugins. +*/ #pragma once #include #include -#include +#include +#include #include #include -#include -#include - +#include #include - #include +#include -/** \brief Planning pipeline */ namespace planning_pipeline { -/** \brief This class facilitates loading planning plugins and - planning request adapted plugins. and allows calling - planning_interface::PlanningContext::solve() from a loaded - planning plugin and the - planning_request_adapter::PlanningRequestAdapter plugins, in the - specified order. */ +/** + * \brief Helper function template to load a vector of plugins + * + * \tparam TPluginClass Plugin class type, PlanningRequestAdapter or PlanningResponseAdapter + * \param plugin_loader Loader to create the requested plugin + * \param plugin_vector Vector to add the loaded plugin to + * \param plugin_names Names of the plugins to be loaded + */ +template +void loadPluginVector(const std::shared_ptr& node, + const std::unique_ptr>& plugin_loader, + std::vector>& plugin_vector, + const std::vector& plugin_names, const std::string& parameter_namespace) +{ + // Try loading a plugin for each plugin name + for (const std::string& plugin_name : plugin_names) + { + RCLCPP_INFO(node->get_logger(), "Try loading adapter '%s'", plugin_name.c_str()); + std::shared_ptr adapter; + try + { + adapter = plugin_loader->createUniqueInstance(plugin_name); + } + catch (pluginlib::PluginlibException& ex) + { + RCLCPP_FATAL(node->get_logger(), "Exception while loading planning adapter plugin '%s': %s", plugin_name.c_str(), + ex.what()); + throw; + } + // If loading was successful, initialize plugin and add to vector + if (adapter) + { + adapter->initialize(node, parameter_namespace); + plugin_vector.push_back(std::move(adapter)); + RCLCPP_INFO(node->get_logger(), "Loaded adapter '%s'", plugin_name.c_str()); + } + else + { + RCLCPP_WARN(node->get_logger(), "Failed to initialize adapter '%s'. Not adding it to the chain.", + plugin_name.c_str()); + } + } +}; + +/** \brief This class facilitates loading planning plugins and planning adapter plugins. It implements functionality to + * use the loaded plugins in a motion planning pipeline consisting of PlanningRequestAdapters (Pre-processing), a + * Planner plugin and PlanningResponseAdapters (Post-processing).*/ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline { public: - /** \brief When motion plans are computed and they are supposed to be automatically displayed, they are sent to this - * topic (moveit_msgs::msg::DisplauTrajectory) */ - static inline const std::string DISPLAY_PATH_TOPIC = std::string("display_planned_path"); - /** \brief When motion planning requests are received and they are supposed to be automatically published, they are - * sent to this topic (moveit_msgs::msg::MotionPlanRequest) */ - static inline const std::string MOTION_PLAN_REQUEST_TOPIC = std::string("motion_plan_request"); - /** \brief When contacts are found in the solution path reported by a planner, they can be published as markers on - * this topic (visualization_msgs::MarkerArray) */ - static inline const std::string MOTION_CONTACTS_TOPIC = std::string("display_contacts"); - - /** \brief Given a robot model (\e model), a node handle (\e pipeline_nh), initialize the planning pipeline. - \param model The robot model for which this pipeline is initialized. - \param node The ROS node that should be used for reading parameters needed for configuration - \param parameter_namespace parameter namespace where the planner configurations are stored + /** \brief Given a robot model, a ROS node, and a parameter namespace, initialize the planning pipeline. The planner plugin + and adapters are provided in form of ROS parameters. The order of the elements in the adapter vectors corresponds + to the order in the motion planning pipeline. + \param model The robot model for which this pipeline is initialized. + \param node The ROS node that should be used for reading parameters needed for configuration + \param parameter_namespace parameter namespace where the planner configurations are stored */ PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, const std::string& parameter_namespace); - /** \brief Given a robot model (\e model), a node handle (\e pipeline_nh), initialize the planning pipeline. - \param model The robot model for which this pipeline is initialized. - \param node The ROS node that should be used for reading parameters needed for configuration - \param parameter_namespace parameter namespace where the planner configurations are stored + /** \brief Given a robot model, a ROS node, and a parameter namespace, initialize the planning pipeline with the given + planner plugin and adapters. The order of the elements in the adapter vectors corresponds to the order in the + motion planning pipeline. + \param model The robot model for which this pipeline is initialized. + \param node The ROS node that should be used for reading parameters needed for configuration + \param parameter_namespace Parameter + namespace where the planner configurations are stored + \param request_adapter_plugin_names Optional vector of + RequestAdapter plugin names + \param response_adapter_plugin_names Optional vector of ResponseAdapter plugin names */ PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, const std::string& parameter_namespace, const std::string& planning_plugin_name, - const std::vector& adapter_plugin_names); + const std::vector& request_adapter_plugin_names = std::vector(), + const std::vector& response_adapter_plugin_names = std::vector()); /* BEGIN BLOCK OF DEPRECATED FUNCTIONS: TODO(sjahr): Remove after 04/2024 (6 months from merge) @@ -106,26 +147,35 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline { return false; } + [[deprecated( + "Please use getResponseAdapterPluginNames() or getRequestAdapterPluginNames().")]] const std::vector& + getAdapterPluginNames() const + { + return pipeline_parameters_.request_adapters; + } + [[deprecated("`check_solution_paths` and `display_computed_motion_plans` are deprecated. To validate the solution " + "please add the ValidatePath response adapter and to publish the path use the DisplayMotionPath " + "response adapter to your pipeline.")]] bool + generatePlan(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, + const planning_interface::MotionPlanRequest& /*req*/, planning_interface::MotionPlanResponse& /*res*/, + const bool /*publish_received_requests*/, const bool /*check_solution_paths*/, + const bool /*display_computed_motion_plans*/) const + { + return false; + } /* END BLOCK OF DEPRECATED FUNCTIONS */ - /** \brief Call the motion planner plugin and the sequence of planning request adapters (if any). - \param planning_scene The planning scene where motion planning is to be done - \param req The request for motion planning - \param res The motion planning response - \param publish_received_requests Flag indicating whether received requests should be published just before - beginning processing (useful for debugging) - \param check_solution_paths Flag indicating whether the reported plans - should be checked once again, by the planning pipeline itself - \param display_computed_motion_plans Flag indicating - whether motion plans should be published as a moveit_msgs::msg::DisplayTrajectory + /** \brief Call the chain of planning request adapters, motion planner plugin, and planning response adapters in + sequence. \param planning_scene The planning scene where motion planning is to be done \param req The request for + motion planning \param res The motion planning response \param publish_received_requests Flag indicating whether + received requests should be published just before beginning processing (useful for debugging) */ [[nodiscard]] bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - const bool publish_received_requests = false, const bool check_solution_paths = true, - const bool display_computed_motion_plans = true) const; + const bool publish_received_requests = false) const; /** \brief Request termination, if a generatePlan() function is currently computing plans */ void terminate() const; @@ -133,13 +183,19 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline /** \brief Get the name of the planning plugin used */ [[nodiscard]] const std::string& getPlannerPluginName() const { - return planner_plugin_name_; + return pipeline_parameters_.planning_plugin; } /** \brief Get the names of the planning request adapter plugins used */ - [[nodiscard]] const std::vector& getAdapterPluginNames() const + [[nodiscard]] const std::vector& getRequestAdapterPluginNames() const { - return adapter_plugin_names_; + return pipeline_parameters_.request_adapters; + } + + /** \brief Get the names of the planning response adapter plugins in the order they are processed. */ + [[nodiscard]] const std::vector& getResponseAdapterPluginNames() const + { + return pipeline_parameters_.response_adapters; } /** \brief Get the planner manager for the loaded planning plugin */ @@ -161,31 +217,44 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline } private: + /// \brief Helper function that is called by both constructors to configure and initialize a PlanningPipeline instance void configure(); + /** + * @brief Helper function to publish the planning pipeline state during the planning process + * + * @param req Current request to publish + * @param res Current pipeline result + * @param pipeline_stage Last pipeline stage that got invoked + */ + void publishPipelineState(moveit_msgs::msg::MotionPlanRequest req, const planning_interface::MotionPlanResponse& res, + const std::string& pipeline_stage) const; + // Flag that indicates whether or not the planning pipeline is currently solving a planning problem mutable std::atomic active_; + // ROS node and parameters std::shared_ptr node_; - std::string parameter_namespace_; - /// Optionally publish motion plans as a moveit_msgs::msg::DisplayTrajectory - rclcpp::Publisher::SharedPtr display_path_publisher_; + const std::string parameter_namespace_; + planning_pipeline_parameters::Params pipeline_parameters_; - /// Optionally publish the request before beginning processing (useful for debugging) - rclcpp::Publisher::SharedPtr received_request_publisher_; - - std::unique_ptr > planner_plugin_loader_; + // Planner plugin + std::unique_ptr> planner_plugin_loader_; planning_interface::PlannerManagerPtr planner_instance_; - std::string planner_plugin_name_; - std::unique_ptr > adapter_plugin_loader_; - std::unique_ptr adapter_chain_; - std::vector adapter_plugin_names_; + // Plan request adapters + std::unique_ptr> request_adapter_plugin_loader_; + std::vector planning_request_adapter_vector_; + + // Plan response adapters + std::unique_ptr> response_adapter_plugin_loader_; + std::vector planning_response_adapter_vector_; + // Robot model moveit::core::RobotModelConstPtr robot_model_; - /// Publish contacts if the generated plans are checked again by the planning pipeline - rclcpp::Publisher::SharedPtr contacts_publisher_; + /// Publish the planning pipeline progress + rclcpp::Publisher::SharedPtr progress_publisher_; rclcpp::Logger logger_; }; diff --git a/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml b/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml index f4b582e3ad..29d81597cd 100644 --- a/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml +++ b/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml @@ -2,10 +2,23 @@ planning_pipeline_parameters: planning_plugin: { type: string, description: "Name of the planner plugin used by the pipeline", + read_only: true, default_value: "UNKNOWN", } request_adapters: { + type: string_array, + description: "Names of the planning request adapter plugins (plugin names separated by space). The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.", + read_only: true, + default_value: [], + } + response_adapters: { + type: string_array, + description: "Names of the planning request adapter plugins (plugin names separated by space). The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.", + read_only: true, + default_value: [], + } + progress_topic: { type: string, - description: "Names of the planning request adapter plugins (plugin names separated by space).", - default_value: "", + description: "For every stage of the planning pipeline a progress message is published to this topic. An empty string disables the publisher.", + default_value: "pipeline_state", } diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 10548bf919..177604fcb5 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -32,22 +32,16 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan */ +/* Author: Ioan Sucan, Sebastian Jahr */ #include -#include -#include -#include -#include #include -#include #include -#include - -planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, - const std::shared_ptr& node, - const std::string& parameter_namespace) +namespace planning_pipeline +{ +PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, + const std::shared_ptr& node, const std::string& parameter_namespace) : active_{ false } , node_(node) , parameter_namespace_(parameter_namespace) @@ -55,48 +49,46 @@ planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotM , logger_(moveit::makeChildLogger("planning_pipeline")) { auto param_listener = planning_pipeline_parameters::ParamListener(node, parameter_namespace); - const auto params = param_listener.get_params(); - planner_plugin_name_ = params.planning_plugin; - if (!params.request_adapters.empty()) - { - boost::char_separator sep(" "); - boost::tokenizer> tok(params.request_adapters, sep); - for (boost::tokenizer>::iterator beg = tok.begin(); beg != tok.end(); ++beg) - { - adapter_plugin_names_.push_back(*beg); - } - } + pipeline_parameters_ = param_listener.get_params(); configure(); } -planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, - const std::shared_ptr& node, - const std::string& parameter_namespace, - const std::string& planner_plugin_name, - const std::vector& adapter_plugin_names) +PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, + const std::shared_ptr& node, const std::string& parameter_namespace, + const std::string& planner_plugin_name, + const std::vector& request_adapter_plugin_names, + const std::vector& response_adapter_plugin_names) : active_{ false } , node_(node) , parameter_namespace_(parameter_namespace) - , planner_plugin_name_(planner_plugin_name) - , adapter_plugin_names_(adapter_plugin_names) , robot_model_(model) , logger_(moveit::makeChildLogger("planning_pipeline")) { + pipeline_parameters_.planning_plugin = planner_plugin_name; + pipeline_parameters_.request_adapters = request_adapter_plugin_names; + pipeline_parameters_.request_adapters = response_adapter_plugin_names; configure(); } -void planning_pipeline::PlanningPipeline::configure() +void PlanningPipeline::configure() { - // Optional publishers for debugging - received_request_publisher_ = node_->create_publisher( - MOTION_PLAN_REQUEST_TOPIC, rclcpp::SystemDefaultsQoS()); - contacts_publisher_ = - node_->create_publisher(MOTION_CONTACTS_TOPIC, rclcpp::SystemDefaultsQoS()); - display_path_publisher_ = - node_->create_publisher(DISPLAY_PATH_TOPIC, rclcpp::SystemDefaultsQoS()); + // Check if planning plugin name is available + if (pipeline_parameters_.planning_plugin.empty()) + { + const std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); + throw std::runtime_error("Planning plugin name is empty. Please choose one of the available plugins: " + + classes_str); + } + + // If progress topic parameter is not empty, initialize publisher + if (!pipeline_parameters_.progress_topic.empty()) + { + progress_publisher_ = node_->create_publisher(pipeline_parameters_.progress_topic, + rclcpp::SystemDefaultsQoS()); + } - // load the planning plugin + // Create planner plugin loader try { planner_plugin_loader_ = std::make_unique>( @@ -108,16 +100,16 @@ void planning_pipeline::PlanningPipeline::configure() throw; } - if (planner_plugin_name_.empty() || planner_plugin_name_ == "UNKNOWN") + if (pipeline_parameters_.planning_plugin.empty() || pipeline_parameters_.planning_plugin == "UNKNOWN") { - std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); + const std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); throw std::runtime_error("Planning plugin name is empty or not defined in namespace '" + parameter_namespace_ + "'. Please choose one of the available plugins: " + classes_str); } try { - planner_instance_ = planner_plugin_loader_->createUniqueInstance(planner_plugin_name_); + planner_instance_ = planner_plugin_loader_->createUniqueInstance(pipeline_parameters_.planning_plugin); if (!planner_instance_->initialize(robot_model_, node_, parameter_namespace_)) { throw std::runtime_error("Unable to initialize planning plugin"); @@ -126,23 +118,22 @@ void planning_pipeline::PlanningPipeline::configure() } catch (pluginlib::PluginlibException& ex) { - std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); + const std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); RCLCPP_FATAL(logger_, "Exception while loading planner '%s': %s" "Available plugins: %s", - planner_plugin_name_.c_str(), ex.what(), classes_str.c_str()); + pipeline_parameters_.planning_plugin.c_str(), ex.what(), classes_str.c_str()); throw; } - // load the planner request adapters - if (!adapter_plugin_names_.empty()) + // Load the planner request adapters + if (!pipeline_parameters_.request_adapters.empty()) { - std::vector planning_request_adapter_vector; try { - adapter_plugin_loader_ = - std::make_unique>( - "moveit_core", "planning_request_adapter::PlanningRequestAdapter"); + request_adapter_plugin_loader_ = + std::make_unique>( + "moveit_core", "planning_interface::PlanningRequestAdapter"); } catch (pluginlib::PluginlibException& ex) { @@ -150,51 +141,68 @@ void planning_pipeline::PlanningPipeline::configure() throw; } - if (adapter_plugin_loader_) + if (request_adapter_plugin_loader_) { - for (const std::string& adapter_plugin_name : adapter_plugin_names_) - { - planning_request_adapter::PlanningRequestAdapterPtr planning_request_adapter; - try - { - planning_request_adapter = adapter_plugin_loader_->createUniqueInstance(adapter_plugin_name); - } - catch (pluginlib::PluginlibException& ex) - { - RCLCPP_FATAL(logger_, "Exception while loading planning adapter plugin '%s': %s", adapter_plugin_name.c_str(), - ex.what()); - throw; - } - if (planning_request_adapter) - { - planning_request_adapter->initialize(node_, parameter_namespace_); - planning_request_adapter_vector.push_back(std::move(planning_request_adapter)); - } - } + loadPluginVector(node_, request_adapter_plugin_loader_, + planning_request_adapter_vector_, + pipeline_parameters_.request_adapters, + parameter_namespace_); } - if (!planning_request_adapter_vector.empty()) + else { - adapter_chain_ = std::make_unique(); - for (planning_request_adapter::PlanningRequestAdapterConstPtr& planning_request_adapter : - planning_request_adapter_vector) - { - RCLCPP_INFO(logger_, "Using planning request adapter '%s'", planning_request_adapter->getDescription().c_str()); - adapter_chain_->addAdapter(planning_request_adapter); - } + RCLCPP_WARN(logger_, "No planning request adapter names specified."); } } else { RCLCPP_WARN(logger_, "No planning request adapter names specified."); } + // Load the planner response adapters + if (!pipeline_parameters_.response_adapters.empty()) + { + try + { + response_adapter_plugin_loader_ = + std::make_unique>( + "moveit_core", "planning_interface::PlanningResponseAdapter"); + } + catch (pluginlib::PluginlibException& ex) + { + RCLCPP_FATAL(logger_, "Exception while creating planning plugin loader %s", ex.what()); + throw; + } + if (response_adapter_plugin_loader_) + { + loadPluginVector(node_, response_adapter_plugin_loader_, + planning_response_adapter_vector_, + pipeline_parameters_.response_adapters, + parameter_namespace_); + } + } + else + { + RCLCPP_WARN(logger_, "No planning response adapter names specified."); + } } -bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - const bool publish_received_requests, - const bool check_solution_paths, - const bool display_computed_motion_plans) const +void PlanningPipeline::publishPipelineState(moveit_msgs::msg::MotionPlanRequest req, + const planning_interface::MotionPlanResponse& res, + const std::string& pipeline_stage) const +{ + if (progress_publisher_) + { + moveit_msgs::msg::PipelineState progress; + progress.request = req; + res.getMessage(progress.response); + progress.pipeline_stage = pipeline_stage; + progress_publisher_->publish(progress); + } +} + +bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + const bool publish_received_requests) const { assert(planner_instance_ != nullptr); @@ -204,172 +212,77 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla // broadcast the request we are about to work on, if needed if (publish_received_requests) { - received_request_publisher_->publish(req); + publishPipelineState(req, res, std::string("")); } // --------------------------------- // Solve the motion planning problem // --------------------------------- - bool solved = false; + + planning_interface::MotionPlanRequest mutable_request = req; try { - if (adapter_chain_) + // Call plan request adapter chain + for (const auto& req_adapter : planning_request_adapter_vector_) { - solved = adapter_chain_->adaptAndPlan(planner_instance_, planning_scene, req, res); - if (!res.added_path_index.empty()) + assert(req_adapter); + RCLCPP_INFO(node_->get_logger(), "Calling PlanningRequestAdapter '%s'", req_adapter->getDescription().c_str()); + const auto status = req_adapter->adapt(planning_scene, mutable_request); + res.error_code = status.error_code; + // Publish progress + publishPipelineState(mutable_request, res, req_adapter->getDescription()); + // If adapter does not succeed, break chain and return false + if (!res.error_code) { - std::stringstream ss; - for (std::size_t added_index : res.added_path_index) - ss << added_index << ' '; - RCLCPP_INFO(logger_, "Planning adapters have added states at index positions: [ %s]", ss.str().c_str()); + RCLCPP_ERROR(node_->get_logger(), + "PlanningRequestAdapter '%s' failed, because '%s'. Aborting planning pipeline.", + req_adapter->getDescription().c_str(), status.message.c_str()); + break; } } - else + + // Call planner + if (res.error_code) { planning_interface::PlanningContextPtr context = - planner_instance_->getPlanningContext(planning_scene, req, res.error_code); - solved = context ? context->solve(res) : false; + planner_instance_->getPlanningContext(planning_scene, mutable_request, res.error_code); + if (context) + { + RCLCPP_ERROR(node_->get_logger(), + "Failed to create PlanningContext for planner '%s'. Aborting planning pipeline.", + planner_instance_->getDescription().c_str()); + res.error_code = moveit::core::MoveItErrorCode::PLANNING_FAILED; + } + context->solve(res); + publishPipelineState(mutable_request, res, planner_instance_->getDescription()); } - } - catch (std::exception& ex) - { - RCLCPP_ERROR(logger_, "Exception caught: '%s'", ex.what()); - // Set planning pipeline to inactive - active_ = false; - return false; - } - // ----------------- - // Validate solution - // ----------------- - if (solved && res.trajectory) - { - std::size_t state_count = res.trajectory->getWayPointCount(); - RCLCPP_DEBUG(logger_, "Motion planner reported a solution path with %ld states", state_count); - if (check_solution_paths) + // Call plan response adapter chain + if (res.error_code) { - visualization_msgs::msg::MarkerArray arr; - visualization_msgs::msg::Marker m; - m.action = visualization_msgs::msg::Marker::DELETEALL; - arr.markers.push_back(m); - - std::vector indices; - if (!planning_scene->isPathValid(*res.trajectory, req.path_constraints, req.group_name, false, &indices)) + // Call plan request adapter chain + for (const auto& res_adapter : planning_response_adapter_vector_) { - // check to see if there is any problem with the states that are found to be invalid - // they are considered ok if they were added by a planning request adapter - bool problem = false; - for (const auto& index : indices) - { - bool found = false; - for (const std::size_t& added_index : res.added_path_index) - { - if (index == added_index) - { - found = true; - break; - } - } - if (!found) - { - problem = true; - break; - } - } - if (problem) - { - if (indices.size() == 1 && indices.at(0) == 0) - { // ignore cases when the robot starts at invalid location - RCLCPP_DEBUG(logger_, "It appears the robot is starting at an invalid state, but that is ok."); - } - else - { - res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; - - // display error messages - std::stringstream ss; - for (std::size_t it : indices) - { - ss << it << ' '; - } - - RCLCPP_ERROR_STREAM(logger_, "Computed path is not valid. Invalid states at index locations: [ " - << ss.str() << "] out of " << state_count - << ". Explanations follow in command line. Contacts are published on " - << contacts_publisher_->get_topic_name()); - - // call validity checks in verbose mode for the problematic states - for (std::size_t it : indices) - { - // check validity with verbose on - const moveit::core::RobotState& robot_state = res.trajectory->getWayPoint(it); - planning_scene->isStateValid(robot_state, req.path_constraints, req.group_name, true); - - // compute the contacts if any - collision_detection::CollisionRequest c_req; - collision_detection::CollisionResult c_res; - c_req.contacts = true; - c_req.max_contacts = 10; - c_req.max_contacts_per_pair = 3; - c_req.verbose = false; - planning_scene->checkCollision(c_req, c_res, robot_state); - if (c_res.contact_count > 0) - { - visualization_msgs::msg::MarkerArray arr_i; - collision_detection::getCollisionMarkersFromContacts(arr_i, planning_scene->getPlanningFrame(), - c_res.contacts); - arr.markers.insert(arr.markers.end(), arr_i.markers.begin(), arr_i.markers.end()); - } - } - RCLCPP_ERROR(logger_, "Completed listing of explanations for invalid states."); - } - } - else + assert(res_adapter); + RCLCPP_INFO(node_->get_logger(), "Calling PlanningResponseAdapter '%s'", res_adapter->getDescription().c_str()); + res_adapter->adapt(planning_scene, mutable_request, res); + publishPipelineState(mutable_request, res, res_adapter->getDescription()); + // If adapter does not succeed, break chain and return false + if (!res.error_code) { - RCLCPP_DEBUG(logger_, - "Planned path was found to be valid, except for states that were added by planning request " - "adapters, but that is ok."); + RCLCPP_ERROR(node_->get_logger(), "PlanningResponseAdapter '%s' failed", + res_adapter->getDescription().c_str()); + break; } } - else - RCLCPP_DEBUG(logger_, "Planned path was found to be valid when rechecked"); - contacts_publisher_->publish(arr); - } - - // Optionally publish DisplayTrajectory msg - if (display_computed_motion_plans) - { - moveit_msgs::msg::DisplayTrajectory disp; - disp.model_id = robot_model_->getName(); - disp.trajectory.resize(1); - res.trajectory->getRobotTrajectoryMsg(disp.trajectory.at(0)); - moveit::core::robotStateToRobotStateMsg(res.trajectory->getFirstWayPoint(), disp.trajectory_start); - display_path_publisher_->publish(disp); } } - else // If no trajectory exists, let's see if it might be related to stacked constraints + catch (std::exception& ex) { - // This should alert the user if planning failed because of contradicting constraints. - // Could be checked more thoroughly, but it is probably not worth going to that length. - bool stacked_constraints = false; - if (req.path_constraints.position_constraints.size() > 1 || req.path_constraints.orientation_constraints.size() > 1) - { - stacked_constraints = true; - } - for (const auto& constraint : req.goal_constraints) - { - if (constraint.position_constraints.size() > 1 || constraint.orientation_constraints.size() > 1) - { - stacked_constraints = true; - } - } - if (stacked_constraints) - { - RCLCPP_WARN(logger_, "More than one constraint is set. If your move_group does not have multiple end " - "effectors/arms, this is " - "unusual. Are you using a move_group_interface and forgetting to call clearPoseTargets() or " - "equivalent?"); - } + RCLCPP_ERROR(logger_, "Exception caught: '%s'", ex.what()); + // Set planning pipeline to inactive + active_ = false; + return false; } // Make sure that planner id is set in the response @@ -384,13 +297,14 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla // Set planning pipeline to inactive active_ = false; - return solved && bool(res); + return bool(res); } -void planning_pipeline::PlanningPipeline::terminate() const +void PlanningPipeline::terminate() const { if (planner_instance_) { planner_instance_->terminate(); } } +} // namespace planning_pipeline diff --git a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt index bb046af912..fe60a67adb 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt +++ b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt @@ -1,21 +1,17 @@ -generate_parameter_library(default_plan_request_adapter_parameters res/default_plan_request_adapter_params.yaml) +generate_parameter_library(default_request_adapter_parameters res/default_request_adapter_params.yaml) add_library(moveit_default_planning_request_adapter_plugins SHARED - src/empty.cpp - src/fix_start_state_bounds.cpp - src/fix_start_state_collision.cpp - src/fix_start_state_path_constraints.cpp - src/fix_workspace_bounds.cpp - src/add_ruckig_traj_smoothing.cpp - src/add_time_optimal_parameterization.cpp + src/check_for_stacked_constraints.cpp + src/check_start_state_bounds.cpp + src/check_start_state_collision.cpp + src/validate_workspace_bounds.cpp src/resolve_constraint_frames.cpp ) -target_link_libraries(moveit_default_planning_request_adapter_plugins default_plan_request_adapter_parameters) +target_link_libraries(moveit_default_planning_request_adapter_plugins default_request_adapter_parameters) set_target_properties(moveit_default_planning_request_adapter_plugins PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(moveit_default_planning_request_adapter_plugins - Boost moveit_core rclcpp pluginlib diff --git a/moveit_ros/planning/planning_request_adapter_plugins/res/default_plan_request_adapter_params.yaml b/moveit_ros/planning/planning_request_adapter_plugins/res/default_plan_request_adapter_params.yaml deleted file mode 100644 index b3f4d79b1b..0000000000 --- a/moveit_ros/planning/planning_request_adapter_plugins/res/default_plan_request_adapter_params.yaml +++ /dev/null @@ -1,45 +0,0 @@ -# This files contains the parameters for all of MoveIt's default PlanRequestAdapters -default_plan_request_adapter_parameters: - path_tolerance: { - type: double, - description: "AddTimeOptimalParameterization: Tolerance per joint in which the time parameterization is allowed to deviate from the original path.", - default_value: 0.1, - } - resample_dt: { - type: double, - description: "AddTimeOptimalParameterization: Time steps between two adjacent waypoints of the parameterized trajectory. The trajectory is re-sampled from the original path.", - default_value: 0.1, - } - min_angle_change: { - type: double, - description: "AddTimeOptimalParameterization: Minimum joint value change to consider two waypoints unique.", - default_value: 0.001, - } - start_state_max_dt: { - type: double, - description: "FixStartStateCollision/FixStartStateBounds: Maximum temporal distance of the fixed start state from the original state.", - default_value: 0.5, - } - jiggle_fraction: { - type: double, - description: "FixStartStateCollision: Joint value perturbation as a percentage of the total range of motion for the joint.", - default_value: 0.02, - } - max_sampling_attempts: { - type: int, - description: "FixStartStateCollision: Maximum number of attempts to re-sample a valid start state.", - default_value: 100, - validation: { - gt_eq<>: [ 0 ], - } - } - start_state_max_bounds_error: { - type: double, - description: "FixStartStateBounds: Maximum allowable error outside joint bounds for the starting configuration.", - default_value: 0.05, - } - default_workspace_bounds: { - type: double, - description: "FixWorkspaceBounds: Default workspace bounds representing a cube around the robot's origin whose edge length this parameter defines.", - default_value: 10.0, - } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml b/moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml new file mode 100644 index 0000000000..859c32c1bb --- /dev/null +++ b/moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml @@ -0,0 +1,12 @@ +# This file contains the parameters for all of MoveIt's default PlanRequestAdapters +default_request_adapter_parameters: + fix_start_state: { + type: bool, + description: "CheckStartStateBounds: If set true and the start state is out of bounds, the MotionPlanRequests start will be updated to a start state with enforced bounds for revolute or continuous joints.", + default_value: false, + } + default_workspace_bounds: { + type: double, + description: "ValidateWorkspaceBounds: Default workspace bounds representing a cube around the robot's origin whose edge length this parameter defines.", + default_value: 10.0, + } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp new file mode 100644 index 0000000000..cf08ffb146 --- /dev/null +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp @@ -0,0 +1,98 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * 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 PickNik Inc. 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: Felix von Drigalski, Sebastian Jahr + * Desc: A planning request adapter that checks if the motion plan request contains stacked path or goal constraints. If + * that is the case, a warning is created but the planning process is not interrupted. + */ + +#include +#include +#include +#include +#include + +#include + +namespace default_planning_request_adapters +{ + +/** @brief Check if the motion plan request contains stacked path or goal constraints */ +class CheckForStackedConstraints : public planning_interface::PlanningRequestAdapter +{ +public: + CheckForStackedConstraints() : logger_(moveit::makeChildLogger("check_for_stacked_constraints")) + { + } + + [[nodiscard]] std::string getDescription() const override + { + return std::string("CheckForStackedConstraints"); + } + + [[nodiscard]] moveit::core::MoveItStatus adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, + planning_interface::MotionPlanRequest& req) const override + { + // This should alert the user if planning failed because of contradicting constraints. + // Could be checked more thoroughly, but it is probably not worth going to that length. + if (req.path_constraints.position_constraints.size() > 1 || req.path_constraints.orientation_constraints.size() > 1) + { + RCLCPP_WARN(logger_, "More than one PATH constraint is set. If your planning group does not have multiple end " + "effectors/arms, this is " + "unusual. Are you using a move_group_interface and forgetting to call clearPoseTargets() or " + "equivalent?"); + } + for (const auto& constraint : req.goal_constraints) + { + if (constraint.position_constraints.size() > 1 || constraint.orientation_constraints.size() > 1) + { + RCLCPP_WARN(logger_, + "More than one GOAL constraint is set. If your move_group does not have multiple end " + "effectors/arms, this is " + "unusual. Are you using a move_group_interface and forgetting to call clearPoseTargets() or " + "equivalent?"); + break; + } + } + return moveit::core::MoveItStatus(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, std::string(""), getDescription()); + } + +private: + std::unique_ptr param_listener_; + rclcpp::Logger logger_; +}; +} // namespace default_planning_request_adapters + +CLASS_LOADER_REGISTER_CLASS(default_planning_request_adapters::CheckForStackedConstraints, + planning_interface::PlanningRequestAdapter) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp new file mode 100644 index 0000000000..e3a35dd057 --- /dev/null +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp @@ -0,0 +1,202 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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 Willow Garage 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: Ioan Sucan + * Desc: The CheckStartStateBounds adapter validates if the start state is within the joint limits + * specified in the URDF. The need for this adapter arises in situations where the joint limits for the physical robot + * are not properly configured. The robot may then end up in a configuration where one or more of its joints is slightly + * outside its joint limits. In this case, the motion planner is unable to plan since it will think that the starting + * state is outside joint limits. The “CheckStartStateBounds” planning request adapter can “fix” the start state by + * moving it to the joint limit. However, this is obviously not the right solution every time - e.g. where the joint is + * really outside its joint limits by a large amount. A parameter for the adapter specifies how much the joint can be + * outside its limits for it to be “fixable”. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace default_planning_request_adapters +{ + +/** @brief The CheckStartStateBounds adapter validates if the start state is within the joint limits specified in the URDF. */ +class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter +{ +public: + CheckStartStateBounds() : logger_(moveit::makeChildLogger("check_start_state_bounds")) + { + } + + void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override + { + param_listener_ = std::make_unique(node, parameter_namespace); + } + + [[nodiscard]] std::string getDescription() const override + { + return std::string("CheckStartStateBounds"); + } + + [[nodiscard]] moveit::core::MoveItStatus adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, + planning_interface::MotionPlanRequest& req) const override + { + RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); + + // Get the specified start state + moveit::core::RobotState start_state = planning_scene->getCurrentState(); + moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); + + // Get joint models + const std::vector& jmodels = + planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ? + planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() : + planning_scene->getRobotModel()->getJointModels(); + + // Read parameters + const auto params = param_listener_->get_params(); + + bool changed_req = false; + for (const moveit::core::JointModel* jmodel : jmodels) + { + // Check if we have a revolute, continuous joint. If we do, then we only need to make sure + // it is within the model's declared bounds (usually -Pi, Pi), since the values wrap around. + // It is possible that the encoder maintains values outside the range [-Pi, Pi], to inform + // how many times the joint was wrapped. Because of this, we remember the offsets for continuous + // joints, and we un-do them when the plan comes from the planner + switch (jmodel->getType()) + { + case moveit::core::JointModel::REVOLUTE: + { + if (static_cast(jmodel)->isContinuous()) + { + double initial = start_state.getJointPositions(jmodel)[0]; + start_state.enforceBounds(jmodel); + double after = start_state.getJointPositions(jmodel)[0]; + if (fabs(initial - after) > std::numeric_limits::epsilon()) + { + changed_req = true; + } + } + break; + } + // Normalize yaw; no offset needs to be remembered + case moveit::core::JointModel::PLANAR: + { + const double* p = start_state.getJointPositions(jmodel); + double copy[3] = { p[0], p[1], p[2] }; + if (static_cast(jmodel)->normalizeRotation(copy)) + { + start_state.setJointPositions(jmodel, copy); + changed_req = true; + } + break; + } + case moveit::core::JointModel::FLOATING: + { + // Normalize quaternions + const double* p = start_state.getJointPositions(jmodel); + double copy[7] = { p[0], p[1], p[2], p[3], p[4], p[5], p[6] }; + if (static_cast(jmodel)->normalizeRotation(copy)) + { + start_state.setJointPositions(jmodel, copy); + changed_req = true; + } + break; + } + default: + { + if (!start_state.satisfiesBounds(jmodel)) + { + std::stringstream joint_values; + std::stringstream joint_bounds_low; + std::stringstream joint_bounds_hi; + const double* p = start_state.getJointPositions(jmodel); + for (std::size_t k = 0; k < jmodel->getVariableCount(); ++k) + { + joint_values << p[k] << ' '; + } + const moveit::core::JointModel::Bounds& b = jmodel->getVariableBounds(); + for (const moveit::core::VariableBounds& variable_bounds : b) + { + joint_bounds_low << variable_bounds.min_position_ << ' '; + joint_bounds_hi << variable_bounds.max_position_ << ' '; + } + RCLCPP_ERROR(logger_, + "Joint '%s' from the starting state is outside bounds by: [%s] should be in " + "the range [%s], [%s].", + jmodel->getName().c_str(), joint_values.str().c_str(), joint_bounds_low.str().c_str(), + joint_bounds_hi.str().c_str()); + } + } + } + } + + // If we made any changes, consider using them them + if (params.fix_start_state && changed_req) + { + RCLCPP_WARN(logger_, "Changing start state."); + moveit::core::robotStateToRobotStateMsg(start_state, req.start_state); + return moveit::core::MoveItStatus(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, std::string(""), getDescription()); + } + + auto status = moveit::core::MoveItStatus(); + if (!changed_req) + { + status.error_code = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + } + else + { + status.error_code = moveit_msgs::msg::MoveItErrorCodes::START_STATE_INVALID; + status.message = std::string("Start state out of bounds."); + } + status.source = getDescription(); + return status; + } + +private: + std::unique_ptr param_listener_; + rclcpp::Logger logger_; +}; +} // namespace default_planning_request_adapters + +CLASS_LOADER_REGISTER_CLASS(default_planning_request_adapters::CheckStartStateBounds, + planning_interface::PlanningRequestAdapter) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp new file mode 100644 index 0000000000..894994d6ce --- /dev/null +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp @@ -0,0 +1,101 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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 Willow Garage 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: Ioan Sucan, Sebastian Jahr + * Desc: This adapter checks if the start state is in collision. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace default_planning_request_adapters +{ + +/** @brief This adapter checks if the start state is in collision.*/ +class CheckStartStateCollision : public planning_interface::PlanningRequestAdapter +{ +public: + CheckStartStateCollision() : logger_(moveit::makeChildLogger("validate_start_state")) + { + } + + [[nodiscard]] std::string getDescription() const override + { + return std::string("CheckStartStateCollision"); + } + + [[nodiscard]] moveit::core::MoveItStatus adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, + planning_interface::MotionPlanRequest& req) const override + { + RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); + + // Get the start state TODO(sjahr): Should we check if req.start state == planning scene start state? + moveit::core::RobotState start_state = planning_scene->getCurrentState(); + moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); + + collision_detection::CollisionRequest creq; + creq.group_name = req.group_name; + collision_detection::CollisionResult cres; + // TODO(sjahr): Would verbose make sense? + planning_scene->checkCollision(creq, cres, start_state); + + auto status = moveit::core::MoveItStatus(); + if (!cres.collision) + { + status.error_code = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + } + else + { + status.error_code = moveit_msgs::msg::MoveItErrorCodes::START_STATE_IN_COLLISION; + status.message = std::string("Start state in collision."); + } + status.source = getDescription(); + return status; + } + +private: + rclcpp::Logger logger_; +}; +} // namespace default_planning_request_adapters + +CLASS_LOADER_REGISTER_CLASS(default_planning_request_adapters::CheckStartStateCollision, + planning_interface::PlanningRequestAdapter) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp deleted file mode 100644 index 40f82076fa..0000000000 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp +++ /dev/null @@ -1,221 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 Willow Garage 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: Ioan Sucan - * Desc: Plan request adapter to fix start state bounds adapter fixes the start state to be within the joint limits - * specified in the URDF. The need for this adapter arises in situations where the joint limits for the physical robot - * are not properly configured. The robot may then end up in a configuration where one or more of its joints is slightly - * outside its joint limits. In this case, the motion planner is unable to plan since it will think that the starting - * state is outside joint limits. The “FixStartStateBounds” planning request adapter will “fix” the start state by - * moving it to the joint limit. However, this is obviously not the right solution every time - e.g. where the joint is - * really outside its joint limits by a large amount. A parameter for the adapter specifies how much the joint can be - * outside its limits for it to be “fixable”. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace default_planner_request_adapters -{ - -/** @brief Fix start state bounds adapter fixes the start state to be within the joint limits specified in the URDF. */ -class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdapter -{ -public: - FixStartStateBounds() : logger_(moveit::makeChildLogger("fix_start_state_bounds")) - { - } - - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override - { - param_listener_ = - std::make_unique(node, parameter_namespace); - } - - std::string getDescription() const override - { - return "Fix Start State Bounds"; - } - - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const override - { - RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); - - // get the specified start state - moveit::core::RobotState start_state = planning_scene->getCurrentState(); - moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); - - const std::vector& jmodels = - planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ? - planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() : - planning_scene->getRobotModel()->getJointModels(); - - bool change_req = false; - for (const moveit::core::JointModel* jm : jmodels) - { - // Check if we have a revolute, continuous joint. If we do, then we only need to make sure - // it is within de model's declared bounds (usually -Pi, Pi), since the values wrap around. - // It is possible that the encoder maintains values outside the range [-Pi, Pi], to inform - // how many times the joint was wrapped. Because of this, we remember the offsets for continuous - // joints, and we un-do them when the plan comes from the planner - - if (jm->getType() == moveit::core::JointModel::REVOLUTE) - { - if (static_cast(jm)->isContinuous()) - { - double initial = start_state.getJointPositions(jm)[0]; - start_state.enforceBounds(jm); - double after = start_state.getJointPositions(jm)[0]; - if (fabs(initial - after) > std::numeric_limits::epsilon()) - change_req = true; - } - } - else - // Normalize yaw; no offset needs to be remembered - if (jm->getType() == moveit::core::JointModel::PLANAR) - { - const double* p = start_state.getJointPositions(jm); - double copy[3] = { p[0], p[1], p[2] }; - if (static_cast(jm)->normalizeRotation(copy)) - { - start_state.setJointPositions(jm, copy); - change_req = true; - } - } - else - // Normalize quaternions - if (jm->getType() == moveit::core::JointModel::FLOATING) - { - const double* p = start_state.getJointPositions(jm); - double copy[7] = { p[0], p[1], p[2], p[3], p[4], p[5], p[6] }; - if (static_cast(jm)->normalizeRotation(copy)) - { - start_state.setJointPositions(jm, copy); - change_req = true; - } - } - } - - // Read parameters - const auto params = param_listener_->get_params(); - - // pointer to a prefix state we could possibly add, if we detect we have to make changes - moveit::core::RobotStatePtr prefix_state; - for (const moveit::core::JointModel* jmodel : jmodels) - { - if (!start_state.satisfiesBounds(jmodel)) - { - if (start_state.satisfiesBounds(jmodel, params.start_state_max_bounds_error)) - { - if (!prefix_state) - prefix_state = std::make_shared(start_state); - start_state.enforceBounds(jmodel); - change_req = true; - RCLCPP_INFO(logger_, "Starting state is just outside bounds (joint '%s'). Assuming within bounds.", - jmodel->getName().c_str()); - } - else - { - std::stringstream joint_values; - std::stringstream joint_bounds_low; - std::stringstream joint_bounds_hi; - const double* p = start_state.getJointPositions(jmodel); - for (std::size_t k = 0; k < jmodel->getVariableCount(); ++k) - joint_values << p[k] << ' '; - const moveit::core::JointModel::Bounds& b = jmodel->getVariableBounds(); - for (const moveit::core::VariableBounds& variable_bounds : b) - { - joint_bounds_low << variable_bounds.min_position_ << ' '; - joint_bounds_hi << variable_bounds.max_position_ << ' '; - } - RCLCPP_WARN(logger_, - "Joint '%s' from the starting state is outside bounds by a significant margin: [%s] should be in " - "the range [%s], [%s] but the error above the 'start_state_max_bounds_error' parameter " - "(currently set to %f)", - jmodel->getName().c_str(), joint_values.str().c_str(), joint_bounds_low.str().c_str(), - joint_bounds_hi.str().c_str(), params.start_state_max_bounds_error); - } - } - } - - bool solved; - // if we made any changes, use them - if (change_req) - { - planning_interface::MotionPlanRequest req2 = req; - moveit::core::robotStateToRobotStateMsg(start_state, req2.start_state); - solved = planner(planning_scene, req2, res); - } - else - solved = planner(planning_scene, req, res); - - // re-add the prefix state, if it was constructed - if (prefix_state && res.trajectory && !res.trajectory->empty()) - { - // heuristically decide a duration offset for the trajectory (induced by the additional point added as a prefix to - // the computed trajectory) - res.trajectory->setWayPointDurationFromPrevious(0, std::min(params.start_state_max_dt, - res.trajectory->getAverageSegmentDuration())); - res.trajectory->addPrefixWayPoint(prefix_state, 0.0); - // we add a prefix point, so we need to bump any previously added index positions - for (std::size_t& added_index : res.added_path_index) - { - added_index++; - } - res.added_path_index.push_back(0); - } - - return solved; - } - -private: - std::unique_ptr param_listener_; - rclcpp::Logger logger_; -}; -} // namespace default_planner_request_adapters - -CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixStartStateBounds, - planning_request_adapter::PlanningRequestAdapter) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp deleted file mode 100644 index e554feb2a1..0000000000 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 Willow Garage 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: Ioan Sucan - * Desc: Fix start state collision adapter which will attempt to sample a new collision-free configuration near a - * specified configuration (in collision) by perturbing the joint values by a small amount. The amount that it will - * perturb the values by is specified by the jiggle_fraction parameter that controls the perturbation as a percentage of - * the total range of motion for the joint. The other parameter for this adapter specifies how many random perturbations - * the adapter will sample before giving up. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace default_planner_request_adapters -{ - -/** @brief This fix start state collision adapter will attempt to sample a new collision-free configuration near a - * specified configuration (in collision) by perturbing the joint values by a small amount.*/ -class FixStartStateCollision : public planning_request_adapter::PlanningRequestAdapter -{ -public: - FixStartStateCollision() : logger_(moveit::makeChildLogger("fix_start_state_collision")) - { - } - - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override - { - param_listener_ = - std::make_unique(node, parameter_namespace); - } - - std::string getDescription() const override - { - return "Fix Start State In Collision"; - } - - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const override - { - RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); - - // get the specified start state - moveit::core::RobotState start_state = planning_scene->getCurrentState(); - moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); - - collision_detection::CollisionRequest creq; - creq.group_name = req.group_name; - collision_detection::CollisionResult cres; - planning_scene->checkCollision(creq, cres, start_state); - if (cres.collision) - { - // Rerun in verbose mode - collision_detection::CollisionRequest vcreq = creq; - collision_detection::CollisionResult vcres; - vcreq.verbose = true; - planning_scene->checkCollision(vcreq, vcres, start_state); - - if (creq.group_name.empty()) - { - RCLCPP_INFO(logger_, "Start state appears to be in collision"); - } - else - { - RCLCPP_INFO(logger_, "Start state appears to be in collision with respect to group %s", creq.group_name.c_str()); - } - - auto prefix_state = std::make_shared(start_state); - random_numbers::RandomNumberGenerator& rng = prefix_state->getRandomNumberGenerator(); - - const std::vector& jmodels = - planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ? - planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() : - planning_scene->getRobotModel()->getJointModels(); - - bool found = false; - const auto params = param_listener_->get_params(); - - for (int c = 0; !found && c < params.max_sampling_attempts; ++c) - { - for (std::size_t i = 0; !found && i < jmodels.size(); ++i) - { - std::vector sampled_variable_values(jmodels[i]->getVariableCount()); - const double* original_values = prefix_state->getJointPositions(jmodels[i]); - jmodels[i]->getVariableRandomPositionsNearBy(rng, &sampled_variable_values[0], original_values, - jmodels[i]->getMaximumExtent() * params.jiggle_fraction); - start_state.setJointPositions(jmodels[i], sampled_variable_values); - collision_detection::CollisionResult cres; - planning_scene->checkCollision(creq, cres, start_state); - if (!cres.collision) - { - found = true; - RCLCPP_INFO(logger_, "Found a valid state near the start state at distance %lf after %d attempts", - prefix_state->distance(start_state), c); - } - } - } - - if (found) - { - planning_interface::MotionPlanRequest req2 = req; - moveit::core::robotStateToRobotStateMsg(start_state, req2.start_state); - bool solved = planner(planning_scene, req2, res); - if (solved && !res.trajectory->empty()) - { - // heuristically decide a duration offset for the trajectory (induced by the additional point added as a - // prefix to the computed trajectory) - res.trajectory->setWayPointDurationFromPrevious(0, std::min(params.start_state_max_dt, - res.trajectory->getAverageSegmentDuration())); - res.trajectory->addPrefixWayPoint(prefix_state, 0.0); - // we add a prefix point, so we need to bump any previously added index positions - for (std::size_t& added_index : res.added_path_index) - { - added_index++; - } - res.added_path_index.push_back(0); - } - return solved; - } - else - { - RCLCPP_WARN( - logger_, - "Unable to find a valid state nearby the start state (using jiggle fraction of %lf and %lu sampling " - "attempts). Passing the original planning request to the planner.", - params.jiggle_fraction, params.max_sampling_attempts); - res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_IN_COLLISION; - return false; // skip remaining adapters and/or planner - } - } - else - { - if (creq.group_name.empty()) - { - RCLCPP_DEBUG(logger_, "Start state is valid"); - } - else - { - RCLCPP_DEBUG(logger_, "Start state is valid with respect to group %s", creq.group_name.c_str()); - } - return planner(planning_scene, req, res); - } - } - -private: - std::unique_ptr param_listener_; - rclcpp::Logger logger_; -}; -} // namespace default_planner_request_adapters - -CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixStartStateCollision, - planning_request_adapter::PlanningRequestAdapter) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp deleted file mode 100644 index 956eedb496..0000000000 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp +++ /dev/null @@ -1,160 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Ioan A. Sucan - * Copyright (c) 2012, Willow Garage, Inc. - * 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 Willow Garage 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: Ioan Sucan - * Desc: Fix start state collision adapter which will attempt to sample a new collision-free configuration near a - * specified configuration (in collision) by perturbing the joint values by a small amount. The amount that it will - * perturb the values by is specified by the jiggle_fraction parameter that controls the perturbation as a percentage of - * the total range of motion for the joint. The other parameter for this adapter specifies how many random perturbations - * the adapter will sample before giving up - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace default_planner_request_adapters -{ - -/** @brief This fix start state collision adapter will attempt to sample a new collision-free configuration near a - * specified configuration (in collision) by perturbing the joint values by a small amount.*/ - -class FixStartStatePathConstraints : public planning_request_adapter::PlanningRequestAdapter -{ -public: - FixStartStatePathConstraints() : logger_(moveit::makeChildLogger("fix_start_state_path_constraints")) - { - } - - void initialize(const rclcpp::Node::SharedPtr& /* node */, const std::string& /* parameter_namespace */) override - { - } - - std::string getDescription() const override - { - return "Fix Start State Path Constraints"; - } - - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const override - { - RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); - - // get the specified start state - moveit::core::RobotState start_state = planning_scene->getCurrentState(); - moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); - - // if the start state is otherwise valid but does not meet path constraints - if (planning_scene->isStateValid(start_state, req.group_name) && - !planning_scene->isStateValid(start_state, req.path_constraints, req.group_name)) - { - RCLCPP_INFO(logger_, "Path constraints not satisfied for start state..."); - planning_scene->isStateValid(start_state, req.path_constraints, req.group_name, true); - RCLCPP_INFO(logger_, "Planning to path constraints..."); - - planning_interface::MotionPlanRequest req2 = req; - req2.goal_constraints.resize(1); - req2.goal_constraints[0] = req.path_constraints; - req2.path_constraints = moveit_msgs::msg::Constraints(); - planning_interface::MotionPlanResponse res2; - // we call the planner for this additional request, but we do not want to include potential - // index information from that call - std::vector added_path_index_temp; - added_path_index_temp.swap(res.added_path_index); - bool solved1 = planner(planning_scene, req2, res2); - added_path_index_temp.swap(res.added_path_index); - - if (solved1) - { - planning_interface::MotionPlanRequest req3 = req; - RCLCPP_INFO(logger_, "Planned to path constraints. Resuming original planning request."); - - // extract the last state of the computed motion plan and set it as the new start state - moveit::core::robotStateToRobotStateMsg(res2.trajectory->getLastWayPoint(), req3.start_state); - bool solved2 = planner(planning_scene, req3, res); - res.planning_time += res2.planning_time; - - if (solved2) - { - // since we add a prefix, we need to correct any existing index positions - for (std::size_t& added_index : res.added_path_index) - { - added_index += res2.trajectory->getWayPointCount(); - } - - // we mark the fact we insert a prefix path (we specify the index position we just added) - for (std::size_t i = 0; i < res2.trajectory->getWayPointCount(); ++i) - { - res.added_path_index.push_back(i); - } - - // we need to append the solution paths. - res2.trajectory->append(*res.trajectory, 0.0); - res2.trajectory->swap(*res.trajectory); - return true; - } - else - { - return false; - } - } - else - { - RCLCPP_WARN(logger_, "Unable to plan to path constraints. Running usual motion plan."); - res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_VIOLATES_PATH_CONSTRAINTS; - res.planning_time = res2.planning_time; - return false; // skip remaining adapters and/or planner - } - } - else - { - RCLCPP_DEBUG(logger_, "Path constraints are OK. Running usual motion plan."); - return planner(planning_scene, req, res); - } - } - -private: - rclcpp::Logger logger_; -}; -} // namespace default_planner_request_adapters - -CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixStartStatePathConstraints, - planning_request_adapter::PlanningRequestAdapter) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp index cc4ef32e08..9455e10b8f 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp @@ -32,52 +32,51 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Robert Haschke */ +/* Author: Robert Haschke + Desc: This adapter changes the link_name field of a constraint from an object's (sub-)frame name to the name of the + robot link, that object is attached to. Transforming the frame names is necessary because the frames of an attached + object are not know to a planner. +*/ -#include +#include #include #include #include -namespace default_planner_request_adapters +namespace default_planning_request_adapters { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.resolve_constraint_frames"); - -class ResolveConstraintFrames : public planning_request_adapter::PlanningRequestAdapter +/// @brief Transforms frames used in constraints to link frames in the robot model. +class ResolveConstraintFrames : public planning_interface::PlanningRequestAdapter { public: ResolveConstraintFrames() : logger_(moveit::makeChildLogger("resolve_constraint_frames")) { } - void initialize(const rclcpp::Node::SharedPtr& /* node */, const std::string& /* parameter_namespace */) override - { - } - - std::string getDescription() const override + [[nodiscard]] std::string getDescription() const override { - return "Resolve constraint frames to robot links"; + return std::string("ResolveConstraintFrames"); } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const override + [[nodiscard]] moveit::core::MoveItStatus adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, + planning_interface::MotionPlanRequest& req) const override { RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); - planning_interface::MotionPlanRequest modified = req; - kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), modified.path_constraints); - for (moveit_msgs::msg::Constraints& constraint : modified.goal_constraints) + // Resolve path constraint frames + kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), req.path_constraints); + // Resolve goal constraint frames + for (moveit_msgs::msg::Constraints& constraint : req.goal_constraints) { kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), constraint); } - return planner(planning_scene, modified, res); + return moveit::core::MoveItStatus(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, std::string(""), getDescription()); } private: rclcpp::Logger logger_; }; -} // namespace default_planner_request_adapters +} // namespace default_planning_request_adapters -CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::ResolveConstraintFrames, - planning_request_adapter::PlanningRequestAdapter); +CLASS_LOADER_REGISTER_CLASS(default_planning_request_adapters::ResolveConstraintFrames, + planning_interface::PlanningRequestAdapter); diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp similarity index 54% rename from moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp rename to moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp index 5fafe8d8c9..510f0e4064 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp @@ -33,74 +33,69 @@ *********************************************************************/ /* Author: Ioan Sucan - * Desc: A fix workspace bounds adapter which will specify a default workspace for planning: a cube of size 10 m x 10 m x - * 10 m. This workspace will only be specified if the planning request to the planner does not have these fields filled in. + * Desc: If not specified by the planning request, this request adapter will specify a default workspace for planning. + * The default workspace is a cube whose edge length is defined with a ROS 2 parameter. */ -#include +#include #include #include #include #include #include -#include +#include -namespace default_planner_request_adapters +namespace default_planning_request_adapters { -/** @brief This fix workspace bounds adapter will specify a default workspace for planning: a cube of size 10 m x 10 m x - * 10 m. This workspace will only be specified if the planning request to the planner does not have these fields filled in. */ -class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapter +/** @brief If not specified by the planning request, this request adapter will specify a default workspace for planning. */ +class ValidateWorkspaceBounds : public planning_interface::PlanningRequestAdapter { public: - FixWorkspaceBounds() : logger_(moveit::makeChildLogger("fix_workspace_bounds")) + ValidateWorkspaceBounds() : logger_(moveit::makeChildLogger("validate_workspace_bounds")) { } void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { - param_listener_ = - std::make_unique(node, parameter_namespace); + param_listener_ = std::make_unique(node, parameter_namespace); } - std::string getDescription() const override + [[nodiscard]] std::string getDescription() const override { - return "Fix Workspace Bounds"; + return std::string("ValidateWorkspaceBounds"); } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const override + [[nodiscard]] moveit::core::MoveItStatus adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, + planning_interface::MotionPlanRequest& req) const override { RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); const moveit_msgs::msg::WorkspaceParameters& wparams = req.workspace_parameters; - if (wparams.min_corner.x == wparams.max_corner.x && wparams.min_corner.x == 0.0 && - wparams.min_corner.y == wparams.max_corner.y && wparams.min_corner.y == 0.0 && - wparams.min_corner.z == wparams.max_corner.z && wparams.min_corner.z == 0.0) + if (std::abs(wparams.min_corner.x) < std::numeric_limits::epsilon() && + std::abs(wparams.min_corner.y) < std::numeric_limits::epsilon() && + std::abs(wparams.min_corner.z) < std::numeric_limits::epsilon() && + std::abs(wparams.max_corner.x) < std::numeric_limits::epsilon() && + std::abs(wparams.max_corner.y) < std::numeric_limits::epsilon() && + std::abs(wparams.max_corner.z) < std::numeric_limits::epsilon()) { - RCLCPP_DEBUG(logger_, "It looks like the planning volume was not specified. Using default values."); - planning_interface::MotionPlanRequest req2 = req; - moveit_msgs::msg::WorkspaceParameters& default_wp = req2.workspace_parameters; + RCLCPP_WARN(logger_, "It looks like the planning volume was not specified. Using default values."); + moveit_msgs::msg::WorkspaceParameters& default_wp = req.workspace_parameters; const auto params = param_listener_->get_params(); default_wp.min_corner.x = default_wp.min_corner.y = default_wp.min_corner.z = -params.default_workspace_bounds / 2.0; default_wp.max_corner.x = default_wp.max_corner.y = default_wp.max_corner.z = params.default_workspace_bounds / 2.0; - return planner(planning_scene, req2, res); - } - else - { - return planner(planning_scene, req, res); } + return moveit::core::MoveItStatus(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, std::string(""), getDescription()); } private: - std::unique_ptr param_listener_; + std::unique_ptr param_listener_; rclcpp::Logger logger_; }; -} // namespace default_planner_request_adapters +} // namespace default_planning_request_adapters -CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixWorkspaceBounds, - planning_request_adapter::PlanningRequestAdapter) +CLASS_LOADER_REGISTER_CLASS(default_planning_request_adapters::ValidateWorkspaceBounds, + planning_interface::PlanningRequestAdapter) diff --git a/moveit_ros/planning/planning_request_adapters_plugin_description.xml b/moveit_ros/planning/planning_request_adapters_plugin_description.xml deleted file mode 100644 index e7f7ffa980..0000000000 --- a/moveit_ros/planning/planning_request_adapters_plugin_description.xml +++ /dev/null @@ -1,46 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Resolves constraints that are defined in collision objects or subframes to robot links, because the former are not known to the planner. - - - - - - This is an improved time parameterization using Time-Optimal Trajectory Generation for Path Following With Bounded Acceleration and Velocity (Kunz and Stilman, RSS 2008). Assumes starting and ending at rest. Not jerk limited. - - - - - - Adds jerk-limited trajectory smoothing via ruckig. Best used after a time parameterization algorithm such as TimeOptimalParameterization. - - - - diff --git a/moveit_ros/planning/planning_response_adapter_plugins/CMakeLists.txt b/moveit_ros/planning/planning_response_adapter_plugins/CMakeLists.txt new file mode 100644 index 0000000000..ad68bf15ee --- /dev/null +++ b/moveit_ros/planning/planning_response_adapter_plugins/CMakeLists.txt @@ -0,0 +1,18 @@ +generate_parameter_library(default_response_adapter_parameters res/default_response_adapter_params.yaml) + +add_library(moveit_default_planning_response_adapter_plugins SHARED + src/add_ruckig_traj_smoothing.cpp + src/add_time_optimal_parameterization.cpp + src/display_motion_path.cpp + src/validate_path.cpp +) + +target_link_libraries(moveit_default_planning_response_adapter_plugins default_response_adapter_parameters) + +set_target_properties(moveit_default_planning_response_adapter_plugins PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_default_planning_response_adapter_plugins + Boost + moveit_core + rclcpp + pluginlib +) diff --git a/moveit_ros/planning/planning_response_adapter_plugins/res/default_response_adapter_params.yaml b/moveit_ros/planning/planning_response_adapter_plugins/res/default_response_adapter_params.yaml new file mode 100644 index 0000000000..f1b500773a --- /dev/null +++ b/moveit_ros/planning/planning_response_adapter_plugins/res/default_response_adapter_params.yaml @@ -0,0 +1,28 @@ +# This file contains the parameters for all of MoveIt's default PlanResponseAdapters +default_response_adapter_parameters: + totg: + path_tolerance: { + type: double, + description: "AddTimeOptimalParameterization: Tolerance per joint in which the time parameterization is allowed to deviate from the original path.", + default_value: 0.1, + } + resample_dt: { + type: double, + description: "AddTimeOptimalParameterization: Time steps between two adjacent waypoints of the parameterized trajectory. The trajectory is re-sampled from the original path.", + default_value: 0.1, + } + min_angle_change: { + type: double, + description: "AddTimeOptimalParameterization: Minimum joint value change to consider two waypoints unique.", + default_value: 0.001, + } + display_path_topic: { + type: string, + description: "If motion plans are computed, they can be sent to this topic by the DisplayMotionPathAdapter (moveit_msgs::msg::DisplayTrajectory).", + default_value: "display_planned_path", + } + display_contacts_topic: { + type: string, + description: "If contacts are found in the solution path, they can be published as markers to this topic (visualization_msgs::MarkerArray). An empty string disables the publisher.", + default_value: "display_planned_path", + } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/add_ruckig_traj_smoothing.cpp similarity index 59% rename from moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp rename to moveit_ros/planning/planning_response_adapter_plugins/src/add_ruckig_traj_smoothing.cpp index 9e17b0daf0..9d133021dd 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/add_ruckig_traj_smoothing.cpp @@ -37,50 +37,62 @@ * algorithm requires a fully configured trajectory as initial guess. */ -#include +#include +#include #include #include +#include -namespace default_planner_request_adapters +namespace default_planning_response_adapters { using namespace trajectory_processing; /** @brief Use ruckig (https://github.com/pantor/ruckig) to adapt the trajectory to be jerk-constrained and * time-optimal.*/ -class AddRuckigTrajectorySmoothing : public planning_request_adapter::PlanningRequestAdapter +class AddRuckigTrajectorySmoothing : public planning_interface::PlanningResponseAdapter { public: - void initialize(const rclcpp::Node::SharedPtr& /* unused */, const std::string& /* unused */) override + AddRuckigTrajectorySmoothing() : logger_(moveit::makeChildLogger("add_ruckig_trajectory_smoothing")) { } - std::string getDescription() const override + [[nodiscard]] std::string getDescription() const override { - return "Add Ruckig trajectory smoothing."; + return std::string("AddRuckigTrajectorySmoothing"); } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const override + void adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { - bool result = planner(planning_scene, req, res); - if (result && res.trajectory) + RCLCPP_DEBUG(logger_, " Running '%s'", getDescription().c_str()); + if (!res.trajectory) { - if (!smoother_.applySmoothing(*res.trajectory, req.max_velocity_scaling_factor, - req.max_acceleration_scaling_factor)) - { - result = false; - } + RCLCPP_ERROR( + logger_, + "Cannot apply response adapter '%s' because MotionPlanResponse does not contain a trajectory to smooth.", + getDescription().c_str()); + res.error_code = moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN; + return; } - return result; + if (smoother_.applySmoothing(*res.trajectory, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) + { + res.error_code = moveit::core::MoveItErrorCode::SUCCESS; + } + else + { + RCLCPP_ERROR(logger_, "Response adapter '%s' failed to smooth trajectory.", getDescription().c_str()); + res.error_code = moveit::core::MoveItErrorCode::FAILURE; + } } private: RuckigSmoothing smoother_; + rclcpp::Logger logger_; }; -} // namespace default_planner_request_adapters +} // namespace default_planning_response_adapters -CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::AddRuckigTrajectorySmoothing, - planning_request_adapter::PlanningRequestAdapter) +CLASS_LOADER_REGISTER_CLASS(default_planning_response_adapters::AddRuckigTrajectorySmoothing, + planning_interface::PlanningResponseAdapter) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/add_time_optimal_parameterization.cpp similarity index 55% rename from moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp rename to moveit_ros/planning/planning_response_adapter_plugins/src/add_time_optimal_parameterization.cpp index ad0a7701c8..072b1c610b 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/add_time_optimal_parameterization.cpp @@ -34,19 +34,19 @@ /* Author: Ioan Sucan, Michael Ferguson */ -#include +#include #include #include #include -#include +#include -namespace default_planner_request_adapters +namespace default_planning_response_adapters { using namespace trajectory_processing; /** @brief This adapter uses the time-optimal trajectory generation method */ -class AddTimeOptimalParameterization : public planning_request_adapter::PlanningRequestAdapter +class AddTimeOptimalParameterization : public planning_interface::PlanningResponseAdapter { public: AddTimeOptimalParameterization() : logger_(moveit::makeChildLogger("add_time_optimal_parameterization")) @@ -55,41 +55,46 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override { - param_listener_ = - std::make_unique(node, parameter_namespace); + param_listener_ = std::make_unique(node, parameter_namespace); } - std::string getDescription() const override + [[nodiscard]] std::string getDescription() const override { - return "Add Time Optimal Parameterization"; + return std::string("AddTimeOptimalParameterization"); } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const override + void adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override { - bool result = planner(planning_scene, req, res); - if (result && res.trajectory) + RCLCPP_DEBUG(logger_, " Running '%s'", getDescription().c_str()); + if (!res.trajectory) { - RCLCPP_DEBUG(logger_, " Running '%s'", getDescription().c_str()); - const auto params = param_listener_->get_params(); - TimeOptimalTrajectoryGeneration totg(params.path_tolerance, params.resample_dt, params.min_angle_change); - if (!totg.computeTimeStamps(*res.trajectory, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) - { - RCLCPP_WARN(logger_, " Time parametrization for the solution path failed."); - result = false; - } + RCLCPP_ERROR(logger_, "Cannot apply response adapter '%s' because MotionPlanResponse does not contain a path.", + getDescription().c_str()); + res.error_code = moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN; + return; } - return result; + const auto params = param_listener_->get_params().totg; + TimeOptimalTrajectoryGeneration totg(params.path_tolerance, params.resample_dt, params.min_angle_change); + if (totg.computeTimeStamps(*res.trajectory, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) + { + res.error_code = moveit::core::MoveItErrorCode::SUCCESS; + } + else + { + RCLCPP_ERROR(logger_, "Response adapter '%s' failed to generate a trajectory.", getDescription().c_str()); + res.error_code = moveit::core::MoveItErrorCode::FAILURE; + } } protected: - std::unique_ptr param_listener_; + std::unique_ptr param_listener_; rclcpp::Logger logger_; }; -} // namespace default_planner_request_adapters +} // namespace default_planning_response_adapters -CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::AddTimeOptimalParameterization, - planning_request_adapter::PlanningRequestAdapter) +CLASS_LOADER_REGISTER_CLASS(default_planning_response_adapters::AddTimeOptimalParameterization, + planning_interface::PlanningResponseAdapter) diff --git a/moveit_ros/planning/planning_response_adapter_plugins/src/display_motion_path.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/display_motion_path.cpp new file mode 100644 index 0000000000..f2333e1102 --- /dev/null +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/display_motion_path.cpp @@ -0,0 +1,103 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * 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 PickNik Inc. 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: Ioan Sucan, Sebastian Jahr + Desc: Response adapter to display the motion path in RVIZ by publishing as EE pose marker array via ROS topic. +*/ + +#include +#include +#include +#include +#include + +#include + +namespace default_planning_response_adapters +{ +/** + * @brief Adapter to publish the EE path as marker array via ROS topic if a path exist. Otherwise, a warning is printed + * but this adapter cannot fail. + * + */ +class DisplayMotionPath : public planning_interface::PlanningResponseAdapter +{ +public: + DisplayMotionPath() : logger_(moveit::makeChildLogger("display_motion_path")) + { + } + + void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override + { + auto param_listener = + std::make_unique(node, parameter_namespace); + // Read parameters + const auto params = param_listener->get_params(); + display_path_publisher_ = node->create_publisher(params.display_path_topic, + rclcpp::SystemDefaultsQoS()); + } + + [[nodiscard]] std::string getDescription() const override + { + return std::string("DisplayMotionPath"); + } + + void adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& /* req */, + planning_interface::MotionPlanResponse& res) const override + { + RCLCPP_DEBUG(logger_, " Running '%s'", getDescription().c_str()); + if (res.trajectory) + { + moveit_msgs::msg::DisplayTrajectory disp; + disp.model_id = planning_scene->getRobotModel()->getName(); + disp.trajectory.resize(1); + res.trajectory->getRobotTrajectoryMsg(disp.trajectory.at(0)); + moveit::core::robotStateToRobotStateMsg(res.trajectory->getFirstWayPoint(), disp.trajectory_start); + display_path_publisher_->publish(disp); + } + else + { + RCLCPP_WARN(logger_, "No motion path to display in MotionPlanResponse."); + } + } + +private: + rclcpp::Logger logger_; + rclcpp::Publisher::SharedPtr display_path_publisher_; +}; +} // namespace default_planning_response_adapters + +CLASS_LOADER_REGISTER_CLASS(default_planning_response_adapters::DisplayMotionPath, + planning_interface::PlanningResponseAdapter) diff --git a/moveit_ros/planning/planning_response_adapter_plugins/src/validate_path.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/validate_path.cpp new file mode 100644 index 0000000000..8c4db824bc --- /dev/null +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/validate_path.cpp @@ -0,0 +1,154 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * 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 PickNik Inc. 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: Ioan Sucan, Sebastian Jahr + Desc: Response adapter that checks a path for validity (collision avoidance, feasibility and constraint satisfaction) +*/ + +#include +#include +#include +#include +#include + +#include +namespace default_planning_response_adapters +{ +/** + * @brief Adapter to check the request path validity (collision avoidance, feasibility and constraint satisfaction) + * + */ +class ValidateSolution : public planning_interface::PlanningResponseAdapter +{ +public: + ValidateSolution() : logger_(moveit::makeChildLogger("validate_solution")) + { + } + + void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override + { + auto param_listener = + std::make_unique(node, parameter_namespace); + // Read parameters + const auto params = param_listener->get_params(); + + if (!params.display_contacts_topic.empty()) + { + contacts_publisher_ = node->create_publisher(params.display_contacts_topic, + rclcpp::SystemDefaultsQoS()); + } + } + + [[nodiscard]] std::string getDescription() const override + { + return std::string("ValidateSolution"); + } + + void adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override + { + RCLCPP_DEBUG(logger_, " Running '%s'", getDescription().c_str()); + if (!res.trajectory) + { + RCLCPP_ERROR(logger_, "No motion path to display in MotionPlanResponse."); + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; + return; + } + + std::size_t state_count = res.trajectory->getWayPointCount(); + RCLCPP_DEBUG(logger_, "Motion planner reported a solution path with %ld states", state_count); + visualization_msgs::msg::MarkerArray arr; + visualization_msgs::msg::Marker m; + m.action = visualization_msgs::msg::Marker::DELETEALL; + arr.markers.push_back(m); + + std::vector indices; + if (!planning_scene->isPathValid(*res.trajectory, req.path_constraints, req.group_name, false, &indices)) + { + // check to see if there is any problem with the states that are found to be invalid + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; + + // If a contact publisher exists, publish contacts + if (contacts_publisher_) + { + // display error messages + std::stringstream ss; + for (std::size_t it : indices) + { + ss << it << ' '; + } + + RCLCPP_ERROR_STREAM(logger_, "Computed path is not valid. Invalid states at index locations: [ " + << ss.str() << "] out of " << state_count + << ". Explanations follow in command line. Contacts are published on " + << contacts_publisher_->get_topic_name()); + + // call validity checks in verbose mode for the problematic states + for (std::size_t it : indices) + { + // check validity with verbose on + const moveit::core::RobotState& robot_state = res.trajectory->getWayPoint(it); + planning_scene->isStateValid(robot_state, req.path_constraints, req.group_name, true); + + // compute the contacts if any + collision_detection::CollisionRequest c_req; + collision_detection::CollisionResult c_res; + c_req.contacts = true; + c_req.max_contacts = 10; + c_req.max_contacts_per_pair = 3; + c_req.verbose = false; + planning_scene->checkCollision(c_req, c_res, robot_state); + if (c_res.contact_count > 0) + { + visualization_msgs::msg::MarkerArray arr_i; + collision_detection::getCollisionMarkersFromContacts(arr_i, planning_scene->getPlanningFrame(), + c_res.contacts); + arr.markers.insert(arr.markers.end(), arr_i.markers.begin(), arr_i.markers.end()); + } + } + RCLCPP_ERROR(logger_, "Completed listing of explanations for invalid states."); + contacts_publisher_->publish(arr); + } + } + } + +private: + rclcpp::Logger logger_; + rclcpp::Publisher::SharedPtr contacts_publisher_; +}; +} // namespace default_planning_response_adapters + +CLASS_LOADER_REGISTER_CLASS(default_planning_response_adapters::ValidateSolution, + planning_interface::PlanningResponseAdapter) diff --git a/moveit_setup_assistant/unported_templates/ompl-chomp_planning_pipeline.launch.xml b/moveit_setup_assistant/unported_templates/ompl-chomp_planning_pipeline.launch.xml index 07e984b6f1..c70502fca8 100644 --- a/moveit_setup_assistant/unported_templates/ompl-chomp_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/unported_templates/ompl-chomp_planning_pipeline.launch.xml @@ -2,11 +2,10 @@ From 315e29c7eedccbf338449957b6852843fd3eeea8 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 13 Nov 2023 19:40:36 +0100 Subject: [PATCH 02/11] Clang-tidy --- moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 177604fcb5..451e811de9 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -192,7 +192,7 @@ void PlanningPipeline::publishPipelineState(moveit_msgs::msg::MotionPlanRequest if (progress_publisher_) { moveit_msgs::msg::PipelineState progress; - progress.request = req; + progress.request = std::move(req); res.getMessage(progress.response); progress.pipeline_stage = pipeline_stage; progress_publisher_->publish(progress); From 3332826f48cefb9c265a64e1949b076f7322fee4 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 14 Nov 2023 17:06:27 +0100 Subject: [PATCH 03/11] Fix wrong if statement --- .../planning/planning_pipeline/src/planning_pipeline.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 451e811de9..8d874821d3 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -247,14 +247,17 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_interface::PlanningContextPtr context = planner_instance_->getPlanningContext(planning_scene, mutable_request, res.error_code); if (context) + { + context->solve(res); + publishPipelineState(mutable_request, res, planner_instance_->getDescription()); + } + else { RCLCPP_ERROR(node_->get_logger(), "Failed to create PlanningContext for planner '%s'. Aborting planning pipeline.", planner_instance_->getDescription().c_str()); res.error_code = moveit::core::MoveItErrorCode::PLANNING_FAILED; } - context->solve(res); - publishPipelineState(mutable_request, res, planner_instance_->getDescription()); } // Call plan response adapter chain From f03c6ac79b7628ba64a8e68d5acf5c31f22d42a1 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 14 Nov 2023 17:07:10 +0100 Subject: [PATCH 04/11] Remove moveit status in favor of the error code --- .../planning_request_adapter.h | 5 +- .../include/moveit/utils/moveit_error_code.h | 9 ++- .../include/moveit/utils/moveit_status.h | 59 ------------------- .../src/planning_pipeline.cpp | 2 +- .../src/check_for_stacked_constraints.cpp | 6 +- .../src/check_start_state_bounds.cpp | 13 ++-- .../src/check_start_state_collision.cpp | 10 ++-- .../src/resolve_constraint_frames.cpp | 6 +- .../src/validate_workspace_bounds.cpp | 6 +- 9 files changed, 32 insertions(+), 84 deletions(-) delete mode 100644 moveit_core/utils/include/moveit/utils/moveit_status.h diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h index e7b2cdcb26..23910a6da3 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h @@ -40,7 +40,6 @@ #include #include -#include #include #include @@ -69,7 +68,7 @@ class PlanningRequestAdapter * @param planning_scene Representation of the environment for the planning * @param req Motion planning request with a set of constraints * @return True if response was generated correctly */ - [[nodiscard]] virtual moveit::core::MoveItStatus adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, - planning_interface::MotionPlanRequest& req) const = 0; + [[nodiscard]] virtual moveit::core::MoveItErrorCode adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, + planning_interface::MotionPlanRequest& req) const = 0; }; } // namespace planning_interface diff --git a/moveit_core/utils/include/moveit/utils/moveit_error_code.h b/moveit_core/utils/include/moveit/utils/moveit_error_code.h index 5f920744d0..3c55c6b927 100644 --- a/moveit_core/utils/include/moveit/utils/moveit_error_code.h +++ b/moveit_core/utils/include/moveit/utils/moveit_error_code.h @@ -47,13 +47,18 @@ namespace core class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes { public: - MoveItErrorCode(int code = 0) + MoveItErrorCode(const int code = moveit_msgs::msg::MoveItErrorCodes::UNDEFINED, const std::string& error_message = "", + const std::string& error_source = "") { val = code; + message = error_message; + source = error_source; } MoveItErrorCode(const moveit_msgs::msg::MoveItErrorCodes& code) { val = code.val; + message = code.message; + source = code.source; } explicit operator bool() const { @@ -80,6 +85,8 @@ inline std::string errorCodeToString(MoveItErrorCode error_code) { case moveit::core::MoveItErrorCode::SUCCESS: return std::string("SUCCESS"); + case moveit::core::MoveItErrorCode::UNDEFINED: + return std::string("UNDEFINED"); case moveit::core::MoveItErrorCode::FAILURE: return std::string("FAILURE"); case moveit::core::MoveItErrorCode::PLANNING_FAILED: diff --git a/moveit_core/utils/include/moveit/utils/moveit_status.h b/moveit_core/utils/include/moveit/utils/moveit_status.h deleted file mode 100644 index b8acacf56c..0000000000 --- a/moveit_core/utils/include/moveit/utils/moveit_status.h +++ /dev/null @@ -1,59 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2023, PickNik Inc. - * 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. - *********************************************************************/ - -#pragma once - -#include - -namespace moveit::core -{ -/** - * @brief Information about potential errors - */ -struct MoveItStatus -{ - // Error code - encodes the overall reason for failure - MoveItErrorCode error_code; - // A message to provide additional information - std::string message; - // Name of the component that created the status - std::string source; - - MoveItStatus(const int code = 0 /*UNDEFINED*/, const std::string& message = std::string(), - const std::string& source = std::string()) - : error_code{ code }, message{ message }, source{ source } - { - } -}; -} // namespace moveit::core diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 8d874821d3..0365a77922 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -228,7 +228,7 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& assert(req_adapter); RCLCPP_INFO(node_->get_logger(), "Calling PlanningRequestAdapter '%s'", req_adapter->getDescription().c_str()); const auto status = req_adapter->adapt(planning_scene, mutable_request); - res.error_code = status.error_code; + res.error_code = status.val; // Publish progress publishPipelineState(mutable_request, res, req_adapter->getDescription()); // If adapter does not succeed, break chain and return false diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp index cf08ffb146..e33680723f 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp @@ -61,8 +61,8 @@ class CheckForStackedConstraints : public planning_interface::PlanningRequestAda return std::string("CheckForStackedConstraints"); } - [[nodiscard]] moveit::core::MoveItStatus adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, - planning_interface::MotionPlanRequest& req) const override + [[nodiscard]] moveit::core::MoveItErrorCode adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, + planning_interface::MotionPlanRequest& req) const override { // This should alert the user if planning failed because of contradicting constraints. // Could be checked more thoroughly, but it is probably not worth going to that length. @@ -85,7 +85,7 @@ class CheckForStackedConstraints : public planning_interface::PlanningRequestAda break; } } - return moveit::core::MoveItStatus(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, std::string(""), getDescription()); + return moveit::core::MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, std::string(""), getDescription()); } private: diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp index e3a35dd057..c84cbcab74 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp @@ -76,8 +76,8 @@ class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter return std::string("CheckStartStateBounds"); } - [[nodiscard]] moveit::core::MoveItStatus adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, - planning_interface::MotionPlanRequest& req) const override + [[nodiscard]] moveit::core::MoveItErrorCode adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, + planning_interface::MotionPlanRequest& req) const override { RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); @@ -175,17 +175,18 @@ class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter { RCLCPP_WARN(logger_, "Changing start state."); moveit::core::robotStateToRobotStateMsg(start_state, req.start_state); - return moveit::core::MoveItStatus(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, std::string(""), getDescription()); + return moveit::core::MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, std::string(""), + getDescription()); } - auto status = moveit::core::MoveItStatus(); + auto status = moveit::core::MoveItErrorCode(); if (!changed_req) { - status.error_code = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + status.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; } else { - status.error_code = moveit_msgs::msg::MoveItErrorCodes::START_STATE_INVALID; + status.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_INVALID; status.message = std::string("Start state out of bounds."); } status.source = getDescription(); diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp index 894994d6ce..c2cd549504 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp @@ -63,8 +63,8 @@ class CheckStartStateCollision : public planning_interface::PlanningRequestAdapt return std::string("CheckStartStateCollision"); } - [[nodiscard]] moveit::core::MoveItStatus adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, - planning_interface::MotionPlanRequest& req) const override + [[nodiscard]] moveit::core::MoveItErrorCode adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, + planning_interface::MotionPlanRequest& req) const override { RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); @@ -78,14 +78,14 @@ class CheckStartStateCollision : public planning_interface::PlanningRequestAdapt // TODO(sjahr): Would verbose make sense? planning_scene->checkCollision(creq, cres, start_state); - auto status = moveit::core::MoveItStatus(); + auto status = moveit::core::MoveItErrorCode(); if (!cres.collision) { - status.error_code = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + status.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; } else { - status.error_code = moveit_msgs::msg::MoveItErrorCodes::START_STATE_IN_COLLISION; + status.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_IN_COLLISION; status.message = std::string("Start state in collision."); } status.source = getDescription(); diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp index 9455e10b8f..236138486d 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp @@ -58,8 +58,8 @@ class ResolveConstraintFrames : public planning_interface::PlanningRequestAdapte return std::string("ResolveConstraintFrames"); } - [[nodiscard]] moveit::core::MoveItStatus adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, - planning_interface::MotionPlanRequest& req) const override + [[nodiscard]] moveit::core::MoveItErrorCode adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, + planning_interface::MotionPlanRequest& req) const override { RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); // Resolve path constraint frames @@ -69,7 +69,7 @@ class ResolveConstraintFrames : public planning_interface::PlanningRequestAdapte { kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), constraint); } - return moveit::core::MoveItStatus(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, std::string(""), getDescription()); + return moveit::core::MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, std::string(""), getDescription()); } private: diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp index 510f0e4064..ce74da5456 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp @@ -67,8 +67,8 @@ class ValidateWorkspaceBounds : public planning_interface::PlanningRequestAdapte return std::string("ValidateWorkspaceBounds"); } - [[nodiscard]] moveit::core::MoveItStatus adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, - planning_interface::MotionPlanRequest& req) const override + [[nodiscard]] moveit::core::MoveItErrorCode adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, + planning_interface::MotionPlanRequest& req) const override { RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); const moveit_msgs::msg::WorkspaceParameters& wparams = req.workspace_parameters; @@ -88,7 +88,7 @@ class ValidateWorkspaceBounds : public planning_interface::PlanningRequestAdapte default_wp.max_corner.x = default_wp.max_corner.y = default_wp.max_corner.z = params.default_workspace_bounds / 2.0; } - return moveit::core::MoveItStatus(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, std::string(""), getDescription()); + return moveit::core::MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, std::string(""), getDescription()); } private: From e191169c8487c24d08e86974cd5130eedf5be637 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 14 Nov 2023 17:31:41 +0100 Subject: [PATCH 05/11] Apply suggestions from code review Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- .../src/resolve_constraint_frames.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp index 236138486d..6b81f2c2f4 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp @@ -34,8 +34,8 @@ /* Author: Robert Haschke Desc: This adapter changes the link_name field of a constraint from an object's (sub-)frame name to the name of the - robot link, that object is attached to. Transforming the frame names is necessary because the frames of an attached - object are not know to a planner. + robot link, that the object is attached to. Transforming the frame names is necessary because the frames of an attached + object are not known to a planner. */ #include From 0d6c0088dff36b9e42c302b520bf04d4b22ee56f Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 14 Nov 2023 17:32:32 +0100 Subject: [PATCH 06/11] Update documentation --- .../moveit/planning_interface/planning_request_adapter.h | 2 +- .../moveit/planning_interface/planning_response_adapter.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h index 23910a6da3..1a25e49f5c 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h @@ -56,7 +56,7 @@ class PlanningRequestAdapter /** @brief Initialize parameters using the passed Node and parameter namespace. * @param node Node instance used by the adapter * @param parameter_namespace Parameter namespace for adapter - @details If no initialization is needed, simply implement as empty */ + * @details The default implementation is empty */ virtual void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace){}; /** @brief Get a description of this adapter diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h index 8c26d62a21..ddade4c5d4 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h @@ -55,7 +55,7 @@ class PlanningResponseAdapter /** @brief Initialize parameters using the passed Node and parameter namespace. * @param node Node instance used by the adapter * @param parameter_namespace Parameter namespace for adapter - @details If no initialization is needed, simply implement as empty */ + * @details The default implementation is empty */ virtual void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace){}; /** \brief Get a description of this adapter From 15fbf0469c708b6f05cb76b562a329dc20aaa78c Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 15 Nov 2023 09:20:01 +0100 Subject: [PATCH 07/11] Format --- .../src/resolve_constraint_frames.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp index 6b81f2c2f4..1374c86c3b 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp @@ -34,8 +34,8 @@ /* Author: Robert Haschke Desc: This adapter changes the link_name field of a constraint from an object's (sub-)frame name to the name of the - robot link, that the object is attached to. Transforming the frame names is necessary because the frames of an attached - object are not known to a planner. + robot link, that the object is attached to. Transforming the frame names is necessary because the frames of an + attached object are not known to a planner. */ #include From 910eecc6f3f645c5b10ec53f0f5a66fbf00c3ac7 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 31 Oct 2023 12:19:36 +0100 Subject: [PATCH 08/11] Add unittest --- .../test/planning_pipeline_test_plugins.cpp | 119 ++++++++++++++++++ .../test/planning_pipeline_tests.cpp | 85 +++++++++++++ 2 files changed, 204 insertions(+) create mode 100644 moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp create mode 100644 moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp diff --git a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp new file mode 100644 index 0000000000..df9162d20d --- /dev/null +++ b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp @@ -0,0 +1,119 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * 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 PickNik Inc. 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: Sebastian Jahr */ + +#include +#include +#include +#include +#include + +namespace planning_pipeline_test_plugins +{ +/// @brief A dummy request adapter that does nothing to test the planning pipeline +class DummyRequestAdapter : public planning_interface::PlanningRequestAdapter +{ + void initialize(const rclcpp::Node::SharedPtr& /*node*/, const std::string& /*parameter_namespace*/) override{}; + std::string getDescription() const override{ return "" }; + bool adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, + planning_interface::MotionPlanRequest& /*req*/, + const planning_interface::MotionPlanResponse& /*res*/) override + { + // Mock light computations + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + return true; + }; +}; + +/// @brief A dummy response adapter that does nothing to test the planning pipeline +class DummyResponseAdapter : public planning_interface::PlanningResponseAdapter +{ + void initialize(const rclcpp::Node::SharedPtr& /*node*/, const std::string& /*parameter_namespace*/) override{}; + std::string getDescription() const override{ return "" }; + bool adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, + const planning_interface::MotionPlanRequest& /*req*/, + planning_interface::MotionPlanResponse& /*res*/) override + { + // Mock light computations + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + return true; + }; +}; + +/// @brief A dummy planning context that does nothing +class DummyPlanningContext : public class planning_interface::PlanningContext +{ + DummyPlannerPlugin(){}; + ~DummyPlannerPlugin() override{}; + bool solve(MotionPlanResponse& res) override + { + // Mock heavy computations + std::this_thread::sleep_for(std::chrono::seconds(1)); + // Planning succeeded + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + return true; + }; + bool solve(MotionPlanDetailedResponse& res) override + { + // Mock heavy computations + std::this_thread::sleep_for(std::chrono::seconds(1)); + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + return true; + }; + bool terminate() override{}; + void clear() override{}; +}; + +/// @brief A dummy planning manager that does nothing +class DummyPlannerManager : public planning_interface::PlannerManager +{ + ~PlannerManager() override{}; + PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, + const MotionPlanRequest& /*req*/, + moveit_msgs::msg::MoveItErrorCodes& /*error_code*/) const override + { + return DummyPlanningContext(); + }; + bool canServiceRequest(const MotionPlanRequest& req) const override + { + return true; + }; +}; +} // namespace planning_pipeline_test_plugins + +CLASS_LOADER_REGISTER_CLASS(planning_pipeline_test_plugins::DummyRequestAdapter, + planning_interface::PlanningRequestAdapter); +CLASS_LOADER_REGISTER_CLASS(planning_pipeline_test_plugins::DummyResponseAdapter, + planning_interface::PlanningResponseAdapter); diff --git a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp new file mode 100644 index 0000000000..331073c6f1 --- /dev/null +++ b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp @@ -0,0 +1,85 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * 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 PickNik Inc. 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: Sebastian Jahr */ + +#include + +#include +#include + +class TestPlanningPipeline : public testing::Test +{ +protected: + void SetUp() override + { + robot_model_ = moveit::core::RobotModelBuilder("empty_robot", "base_link").build(); + node_ = rclcpp::Node::make_shared("planning_pipeline_test"); + }; + void TearDown() override + { + } + + std::shared_ptr robot_model_; +} + +TEST_F(TestPlanningPipeline, HappyPath) +{ + // GIVEN a valid configuration and a valid motion plan request + // WHEN the planning pipeline is configured + // THEN it is successful + // WHEN generatePlan is called + // THEN The plugins are called in the correct order + // THEN A successful response is created +} + +TEST_F(TestPlanningPipeline, HappyPathTerminate) +{ + // GIVEN a valid config and request + // WHEN the pipeline is started and terminate is called + // THEN the pipeline terminates the running planner and returns a result +} + +TEST_F(TestPlanningPipeline, NoPlannerPluginConfigured) +{ + // GIVEN a configuration without planner plugin + // WHEN the pipeline is configured + // THEN an exception is thrown +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From e8c2d54e2682e91b49ecef374a30bcc32b3d956c Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 15 Nov 2023 14:49:07 +0100 Subject: [PATCH 09/11] Fix typo --- moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 0365a77922..e17d075489 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -67,7 +67,7 @@ PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model { pipeline_parameters_.planning_plugin = planner_plugin_name; pipeline_parameters_.request_adapters = request_adapter_plugin_names; - pipeline_parameters_.request_adapters = response_adapter_plugin_names; + pipeline_parameters_.response_adapters = response_adapter_plugin_names; configure(); } From 232b479c7f90116d8922d412026f02fdf5530cc0 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 15 Nov 2023 15:49:31 +0100 Subject: [PATCH 10/11] Finish unittests --- moveit_ros/planning/CMakeLists.txt | 4 +- .../planning/planning_pipeline/CMakeLists.txt | 36 ++++++- .../test/planning_pipeline_test_plugins.cpp | 99 +++++++++++-------- .../test/planning_pipeline_tests.cpp | 50 ++++++++-- ...ning_pipeline_test_plugins_description.xml | 21 ++++ 5 files changed, 154 insertions(+), 56 deletions(-) create mode 100644 moveit_ros/planning/planning_pipeline_test_plugins_description.xml diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index 415f2bdc49..4a4edb8476 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -113,10 +113,10 @@ ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) pluginlib_export_plugin_description_file(moveit_core "default_request_adapters_plugin_description.xml") pluginlib_export_plugin_description_file(moveit_core "default_response_adapters_plugin_description.xml") - if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) + pluginlib_export_plugin_description_file(moveit_core "planning_pipeline_test_plugins_description.xml") + find_package(ament_lint_auto REQUIRED) # These don't pass yet, disable them for now set(ament_cmake_copyright_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) diff --git a/moveit_ros/planning/planning_pipeline/CMakeLists.txt b/moveit_ros/planning/planning_pipeline/CMakeLists.txt index 76c43223e4..44116520ac 100644 --- a/moveit_ros/planning/planning_pipeline/CMakeLists.txt +++ b/moveit_ros/planning/planning_pipeline/CMakeLists.txt @@ -11,9 +11,43 @@ ament_target_dependencies(moveit_planning_pipeline moveit_core moveit_msgs rclcpp - Boost pluginlib ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + add_library(moveit_pipeline_test_plugins SHARED + test/planning_pipeline_test_plugins.cpp + ) + target_include_directories(moveit_pipeline_test_plugins PUBLIC $) + ament_target_dependencies(moveit_pipeline_test_plugins + moveit_core + rclcpp + pluginlib + ) + set_target_properties(moveit_pipeline_test_plugins PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + + ament_add_gtest(moveit_planning_pipeline_test test/planning_pipeline_tests.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + target_include_directories(moveit_planning_pipeline_test PUBLIC $) + ament_target_dependencies(moveit_planning_pipeline_test + moveit_core + moveit_msgs + rclcpp + pluginlib + ) + target_link_libraries(moveit_planning_pipeline_test + moveit_planning_pipeline + ) + + install(TARGETS moveit_pipeline_test_plugins + EXPORT moveit_pipeline_test_pluginsTargets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + ) +endif() + install(DIRECTORY include/ DESTINATION include/moveit_ros_planning) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_planning_pipeline_export.h DESTINATION include/moveit_ros_planning) diff --git a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp index df9162d20d..1b2940f981 100644 --- a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp +++ b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp @@ -40,80 +40,93 @@ #include #include -namespace planning_pipeline_test_plugins +namespace planning_pipeline_test { -/// @brief A dummy request adapter that does nothing to test the planning pipeline -class DummyRequestAdapter : public planning_interface::PlanningRequestAdapter +/// @brief A dummy request adapter that does nothing and is always successful +class AlwaysSuccessRequestAdapter : public planning_interface::PlanningRequestAdapter { - void initialize(const rclcpp::Node::SharedPtr& /*node*/, const std::string& /*parameter_namespace*/) override{}; - std::string getDescription() const override{ return "" }; - bool adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, - planning_interface::MotionPlanRequest& /*req*/, - const planning_interface::MotionPlanResponse& /*res*/) override +public: + std::string getDescription() const override + { + return "AlwaysSuccessRequestAdapter"; + } + moveit::core::MoveItErrorCode adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, + planning_interface::MotionPlanRequest& /*req*/) const override { // Mock light computations - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - return true; - }; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + return moveit::core::MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, std::string(""), getDescription()); + } }; -/// @brief A dummy response adapter that does nothing to test the planning pipeline -class DummyResponseAdapter : public planning_interface::PlanningResponseAdapter +/// @brief A dummy response adapter that does nothing and is always successful +class AlwaysSuccessResponseAdapter : public planning_interface::PlanningResponseAdapter { - void initialize(const rclcpp::Node::SharedPtr& /*node*/, const std::string& /*parameter_namespace*/) override{}; - std::string getDescription() const override{ return "" }; - bool adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, +public: + std::string getDescription() const override + { + return "AlwaysSuccessResponseAdapter"; + } + void adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, const planning_interface::MotionPlanRequest& /*req*/, - planning_interface::MotionPlanResponse& /*res*/) override + planning_interface::MotionPlanResponse& res) const override { // Mock light computations - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - return true; - }; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + } }; -/// @brief A dummy planning context that does nothing -class DummyPlanningContext : public class planning_interface::PlanningContext +/// @brief A dummy planning context that does nothing and is always successful +class DummyPlanningContext : public planning_interface::PlanningContext { - DummyPlannerPlugin(){}; - ~DummyPlannerPlugin() override{}; - bool solve(MotionPlanResponse& res) override +public: + DummyPlanningContext() : planning_interface::PlanningContext("DummyPlanningContext", "empty_group") + { + } + void solve(planning_interface::MotionPlanResponse& res) override { // Mock heavy computations std::this_thread::sleep_for(std::chrono::seconds(1)); // Planning succeeded res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - return true; - }; - bool solve(MotionPlanDetailedResponse& res) override + } + void solve(planning_interface::MotionPlanDetailedResponse& res) override { // Mock heavy computations std::this_thread::sleep_for(std::chrono::seconds(1)); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + } + bool terminate() override + { return true; - }; - bool terminate() override{}; + } void clear() override{}; }; /// @brief A dummy planning manager that does nothing class DummyPlannerManager : public planning_interface::PlannerManager { - ~PlannerManager() override{}; - PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, - const MotionPlanRequest& /*req*/, - moveit_msgs::msg::MoveItErrorCodes& /*error_code*/) const override +public: + ~DummyPlannerManager() override + { + } + planning_interface::PlanningContextPtr + getPlanningContext(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, + const planning_interface::MotionPlanRequest& /*req*/, + moveit_msgs::msg::MoveItErrorCodes& /*error_code*/) const override { - return DummyPlanningContext(); - }; - bool canServiceRequest(const MotionPlanRequest& req) const override + return std::make_shared(); + } + bool canServiceRequest(const planning_interface::MotionPlanRequest& /*req*/) const override { return true; - }; + } }; -} // namespace planning_pipeline_test_plugins +} // namespace planning_pipeline_test -CLASS_LOADER_REGISTER_CLASS(planning_pipeline_test_plugins::DummyRequestAdapter, - planning_interface::PlanningRequestAdapter); -CLASS_LOADER_REGISTER_CLASS(planning_pipeline_test_plugins::DummyResponseAdapter, - planning_interface::PlanningResponseAdapter); +CLASS_LOADER_REGISTER_CLASS(planning_pipeline_test::DummyPlannerManager, planning_interface::PlannerManager) +CLASS_LOADER_REGISTER_CLASS(planning_pipeline_test::AlwaysSuccessRequestAdapter, + planning_interface::PlanningRequestAdapter) +CLASS_LOADER_REGISTER_CLASS(planning_pipeline_test::AlwaysSuccessResponseAdapter, + planning_interface::PlanningResponseAdapter) diff --git a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp index 331073c6f1..010058554c 100644 --- a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp +++ b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp @@ -39,6 +39,14 @@ #include #include +namespace +{ +const std::vector REQUEST_ADAPTERS{ "planning_pipeline_test/AlwaysSuccessRequestAdapter", + "planning_pipeline_test/AlwaysSuccessRequestAdapter" }; +const std::vector RESPONSE_ADAPTERS{ "planning_pipeline_test/AlwaysSuccessResponseAdapter", + "planning_pipeline_test/AlwaysSuccessResponseAdapter" }; +const std::string PLANNER_PLUGIN = std::string("planning_pipeline_test/DummyPlannerManager"); +} // namespace class TestPlanningPipeline : public testing::Test { protected: @@ -51,24 +59,42 @@ class TestPlanningPipeline : public testing::Test { } + std::shared_ptr node_; std::shared_ptr robot_model_; -} + planning_pipeline::PlanningPipelinePtr pipeline_ptr_{ nullptr }; +}; TEST_F(TestPlanningPipeline, HappyPath) { - // GIVEN a valid configuration and a valid motion plan request + // GIVEN a valid configuration // WHEN the planning pipeline is configured // THEN it is successful + EXPECT_NO_THROW(pipeline_ptr_ = std::make_shared( + robot_model_, node_, "", PLANNER_PLUGIN, REQUEST_ADAPTERS, RESPONSE_ADAPTERS)); + // THEN a planning pipeline exists + EXPECT_NE(pipeline_ptr_, nullptr); + // THEN the pipeline is inactive + EXPECT_FALSE(pipeline_ptr_->isActive()); + // THEN the pipeline contains a valid robot model + EXPECT_NE(pipeline_ptr_->getRobotModel(), nullptr); + // THEN a planner plugin is loaded + EXPECT_NE(pipeline_ptr_->getPlannerManager(), nullptr); + // THEN the loaded request adapter names equal the adapter names input vector + const auto loaded_req_adapters = pipeline_ptr_->getRequestAdapterPluginNames(); + EXPECT_TRUE(std::equal(loaded_req_adapters.begin(), loaded_req_adapters.end(), REQUEST_ADAPTERS.begin())); + // THEN the loaded request adapter names equal the adapter names input vector + const auto loaded_res_adapters = pipeline_ptr_->getResponseAdapterPluginNames(); + EXPECT_TRUE(std::equal(loaded_res_adapters.begin(), loaded_res_adapters.end(), RESPONSE_ADAPTERS.begin())); + // THEN the loaded planner plugin name equals the planner name input argument + EXPECT_EQ(pipeline_ptr_->getPlannerPluginName(), PLANNER_PLUGIN); + // WHEN generatePlan is called - // THEN The plugins are called in the correct order // THEN A successful response is created -} - -TEST_F(TestPlanningPipeline, HappyPathTerminate) -{ - // GIVEN a valid config and request - // WHEN the pipeline is started and terminate is called - // THEN the pipeline terminates the running planner and returns a result + planning_interface::MotionPlanResponse motion_plan_response; + planning_interface::MotionPlanRequest motion_plan_request; + const auto planning_scene_ptr = std::make_shared(robot_model_); + EXPECT_TRUE(pipeline_ptr_->generatePlan(planning_scene_ptr, motion_plan_request, motion_plan_response)); + EXPECT_TRUE(motion_plan_response.error_code); } TEST_F(TestPlanningPipeline, NoPlannerPluginConfigured) @@ -76,10 +102,14 @@ TEST_F(TestPlanningPipeline, NoPlannerPluginConfigured) // GIVEN a configuration without planner plugin // WHEN the pipeline is configured // THEN an exception is thrown + EXPECT_THROW(pipeline_ptr_ = std::make_shared( + robot_model_, node_, "", "NoExistingPlannerPlugin", REQUEST_ADAPTERS, RESPONSE_ADAPTERS), + std::runtime_error); } int main(int argc, char** argv) { + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/moveit_ros/planning/planning_pipeline_test_plugins_description.xml b/moveit_ros/planning/planning_pipeline_test_plugins_description.xml new file mode 100644 index 0000000000..a1eb796bd6 --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_test_plugins_description.xml @@ -0,0 +1,21 @@ + + + + + A dummy request adapter that does nothing and is always successful + + + + + + A dummy request adapter that does nothing and is always successful + + + + + + A dummy request adapter that does nothing and is always successful + + + + \ No newline at end of file From a4ae7d7d24c6d60c456a33ed628a083b616ea58e Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 15 Nov 2023 15:51:07 +0100 Subject: [PATCH 11/11] Format .. --- .../planning/planning_pipeline_test_plugins_description.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning/planning_pipeline_test_plugins_description.xml b/moveit_ros/planning/planning_pipeline_test_plugins_description.xml index a1eb796bd6..f22987059e 100644 --- a/moveit_ros/planning/planning_pipeline_test_plugins_description.xml +++ b/moveit_ros/planning/planning_pipeline_test_plugins_description.xml @@ -18,4 +18,4 @@ - \ No newline at end of file +