From 2843459b341e020a0d94ab3773ae57f8f9b9a082 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 3 Feb 2023 11:37:18 +0100 Subject: [PATCH 01/52] Fix buildfarm errors - Add missing package dependencies - Add author tags to package.xml --- core/package.xml | 1 + demo/package.xml | 5 +++-- rviz_marker_tools/CMakeLists.txt | 1 + rviz_marker_tools/package.xml | 2 ++ visualization/package.xml | 1 + 5 files changed, 8 insertions(+), 2 deletions(-) diff --git a/core/package.xml b/core/package.xml index 0f8386382..143cdd035 100644 --- a/core/package.xml +++ b/core/package.xml @@ -31,6 +31,7 @@ rosunit rostest moveit_resources_fanuc_moveit_config + moveit_planners diff --git a/demo/package.xml b/demo/package.xml index 265813b01..cb8376123 100644 --- a/demo/package.xml +++ b/demo/package.xml @@ -4,9 +4,10 @@ 0.1.0 demo tasks illustrating various capabilities of MTC. - simon Goldstein + Robert Haschke + Simon Goldstein Henning Kayser - Henning Kayser + Robert Haschke BSD diff --git a/rviz_marker_tools/CMakeLists.txt b/rviz_marker_tools/CMakeLists.txt index 7312cb713..c3e17a576 100644 --- a/rviz_marker_tools/CMakeLists.txt +++ b/rviz_marker_tools/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS visualization_msgs roscpp rviz + tf2_eigen ) catkin_package( diff --git a/rviz_marker_tools/package.xml b/rviz_marker_tools/package.xml index daab32f0c..263b86c9f 100644 --- a/rviz_marker_tools/package.xml +++ b/rviz_marker_tools/package.xml @@ -4,6 +4,7 @@ Tools for marker creation / handling BSD + Robert Haschke Robert Haschke catkin @@ -12,4 +13,5 @@ geometry_msgs roscpp rviz + tf2_eigen diff --git a/visualization/package.xml b/visualization/package.xml index 8a9caefc1..e6d00dd95 100644 --- a/visualization/package.xml +++ b/visualization/package.xml @@ -4,6 +4,7 @@ Visualization tools for MoveIt Task Pipeline BSD + Robert Haschke Robert Haschke Michael Goerner From f921553fa4b5ffb448ac7497f79ff2a9342c761c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Mon, 6 Feb 2023 14:35:13 +0100 Subject: [PATCH 02/52] revert to upstream pybind repository (#424) Robert's fork is not required anymore. However, we can't forward to the current branch head because it breaks python2 support. --- .gitmodules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index c4dea34be..9339bbb20 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,5 +1,5 @@ [submodule "core/python/pybind11"] path = core/python/pybind11 - url = https://github.com/rhaschke/pybind11 + url = https://github.com/pybind/pybind11 branch = smart_holder shallow = true From f2d97e524df6059b763e146da0163b6548902754 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 11 Feb 2023 13:31:50 +0100 Subject: [PATCH 03/52] Fix odr compiler warning on build farm https://build.ros.org/job/Ndev_db__moveit_task_constructor__debian_buster_amd64/3 --- core/include/moveit/python/task_constructor/properties.h | 3 +++ core/python/bindings/src/core.h | 3 --- core/python/bindings/src/properties.cpp | 3 --- 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/core/include/moveit/python/task_constructor/properties.h b/core/include/moveit/python/task_constructor/properties.h index 5dea8d619..c52e1a802 100644 --- a/core/include/moveit/python/task_constructor/properties.h +++ b/core/include/moveit/python/task_constructor/properties.h @@ -6,6 +6,9 @@ #include #include +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropertyMap) + namespace moveit { namespace python { diff --git a/core/python/bindings/src/core.h b/core/python/bindings/src/core.h index f69e8bd2e..9be7a4de6 100644 --- a/core/python/bindings/src/core.h +++ b/core/python/bindings/src/core.h @@ -109,9 +109,6 @@ class PyPropagatingEitherWay : public PyStage } // namespace task_constructor } // namespace moveit -PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property) -PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropertyMap) - PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::solvers::PlannerInterface) PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::SolutionBase) diff --git a/core/python/bindings/src/properties.cpp b/core/python/bindings/src/properties.cpp index 7f26293e4..e53d7a293 100644 --- a/core/python/bindings/src/properties.cpp +++ b/core/python/bindings/src/properties.cpp @@ -39,9 +39,6 @@ namespace py = pybind11; using namespace py::literals; using namespace moveit::task_constructor; -PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property) -PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropertyMap) - namespace moveit { namespace python { namespace { From 4b5de15e1b48b2af5edd4c847cbb7eabc9c66bdc Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 11 Feb 2023 13:47:35 +0100 Subject: [PATCH 04/52] Remove unused eigen_conversions includes --- core/src/marker_tools.cpp | 1 - core/src/stages/passthrough.cpp | 1 - demo/include/moveit_task_constructor_demo/pick_place_task.h | 2 -- visualization/visualization_tools/src/marker_visualization.cpp | 1 - 4 files changed, 5 deletions(-) diff --git a/core/src/marker_tools.cpp b/core/src/marker_tools.cpp index 6dbb5e5d0..24f3aa849 100644 --- a/core/src/marker_tools.cpp +++ b/core/src/marker_tools.cpp @@ -1,7 +1,6 @@ #include #include #include -#include namespace vm = visualization_msgs; diff --git a/core/src/stages/passthrough.cpp b/core/src/stages/passthrough.cpp index 786221e1f..4ca934827 100644 --- a/core/src/stages/passthrough.cpp +++ b/core/src/stages/passthrough.cpp @@ -44,7 +44,6 @@ #include #include -#include #include #include #include diff --git a/demo/include/moveit_task_constructor_demo/pick_place_task.h b/demo/include/moveit_task_constructor_demo/pick_place_task.h index 5301fb6dd..41e9f9211 100644 --- a/demo/include/moveit_task_constructor_demo/pick_place_task.h +++ b/demo/include/moveit_task_constructor_demo/pick_place_task.h @@ -58,8 +58,6 @@ #include #include -#include - #pragma once namespace moveit_task_constructor_demo { diff --git a/visualization/visualization_tools/src/marker_visualization.cpp b/visualization/visualization_tools/src/marker_visualization.cpp index 131cd7c71..9e3ceaed4 100644 --- a/visualization/visualization_tools/src/marker_visualization.cpp +++ b/visualization/visualization_tools/src/marker_visualization.cpp @@ -12,7 +12,6 @@ #endif #include #include -#include namespace moveit_rviz_plugin { From 3b05949be92401081e5b038439413af34b86deb3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 13 Feb 2023 15:05:35 +0100 Subject: [PATCH 05/52] pick_place_task: monitor last state before Connect ... to prune solutions as much as possible --- demo/src/pick_place_task.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 207ac3384..e4232ec3e 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -179,7 +179,6 @@ bool PickPlaceTask::init() { * Current State * * * ***************************************************/ - Stage* current_state_ptr = nullptr; // Forward current_state on to grasp pose generator { auto current_state = std::make_unique("current state"); @@ -193,8 +192,6 @@ bool PickPlaceTask::init() { } return true; }); - - current_state_ptr = applicability_filter.get(); t.add(std::move(applicability_filter)); } @@ -203,10 +200,12 @@ bool PickPlaceTask::init() { * Open Hand * * * ***************************************************/ + Stage* initial_state_ptr = nullptr; { // Open Hand auto stage = std::make_unique("open hand", sampling_planner); stage->setGroup(hand_group_name_); stage->setGoal(hand_open_pose_); + initial_state_ptr = stage.get(); // remember start state for monitoring grasp pose generator t.add(std::move(stage)); } @@ -228,7 +227,7 @@ bool PickPlaceTask::init() { * Pick Object * * * ***************************************************/ - Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator + Stage* pick_stage_ptr = nullptr; { auto grasp = std::make_unique("pick object"); t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); @@ -263,7 +262,7 @@ bool PickPlaceTask::init() { stage->setPreGraspPose(hand_open_pose_); stage->setObject(object); stage->setAngleDelta(M_PI / 12); - stage->setMonitoredStage(current_state_ptr); // Hook into current state + stage->setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions // Compute IK auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); @@ -302,7 +301,6 @@ bool PickPlaceTask::init() { { auto stage = std::make_unique("attach object"); stage->attachObject(object, hand_frame_); - attach_object_stage = stage.get(); grasp->insert(std::move(stage)); } @@ -337,11 +335,13 @@ bool PickPlaceTask::init() { .... * Forbid collision (object support) * ***************************************************/ { - auto stage = std::make_unique("forbid collision (object,surface)"); + auto stage = std::make_unique("forbid collision (object,support)"); stage->allowCollisions({ object }, support_surfaces_, false); grasp->insert(std::move(stage)); } + pick_stage_ptr = grasp.get(); // remember for monitoring place pose generator + // Add grasp container to task t.add(std::move(grasp)); } @@ -403,7 +403,7 @@ bool PickPlaceTask::init() { p.pose = place_pose_; p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_; stage->setPose(p); - stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage + stage->setMonitoredStage(pick_stage_ptr); // hook into successful pick solutions // Compute IK auto wrapper = std::make_unique("place pose IK", std::move(stage)); From ca5716a5c38b39a0a0b8bb8e5b7558d80d95e483 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 13 Feb 2023 15:09:24 +0100 Subject: [PATCH 06/52] CI: Add prerelease workflow Add missing test dependency --- .github/workflows/prerelease.yaml | 41 +++++++++++++++++++++++++++++++ demo/package.xml | 1 + 2 files changed, 42 insertions(+) create mode 100644 .github/workflows/prerelease.yaml diff --git a/.github/workflows/prerelease.yaml b/.github/workflows/prerelease.yaml new file mode 100644 index 000000000..347a23daa --- /dev/null +++ b/.github/workflows/prerelease.yaml @@ -0,0 +1,41 @@ +# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). +# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) + +name: pre-release + +on: + workflow_dispatch: + +permissions: + contents: read # to fetch code (actions/checkout) + +jobs: + default: + strategy: + matrix: + distro: [noetic] + + env: + # https://github.com/ros-industrial/industrial_ci/issues/666 + BUILDER: catkin_make_isolated + ROS_DISTRO: ${{ matrix.distro }} + PRERELEASE: true + BASEDIR: ${{ github.workspace }}/.work + + name: "${{ matrix.distro }}" + runs-on: ubuntu-latest + steps: + - name: "Free up disk space" + run: | + sudo apt-get -qq purge build-essential "ghc*" + sudo apt-get clean + # cleanup docker images not used by us + docker system prune -af + # free up a lot of stuff from /usr/local + sudo rm -rf /usr/local + df -h + - uses: actions/checkout@v3 + with: + submodules: recursive + - name: industrial_ci + uses: ros-industrial/industrial_ci@master diff --git a/demo/package.xml b/demo/package.xml index cb8376123..202a47352 100644 --- a/demo/package.xml +++ b/demo/package.xml @@ -25,4 +25,5 @@ moveit_resources_panda_moveit_config rostest + moveit_fake_controller_manager From d95a2fc787dafa2dfa21bf49c77ffd0a68e7e485 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 15 Feb 2023 14:30:14 +0100 Subject: [PATCH 07/52] Use MoveIt's libmoveit_python_tools.so Drop our own version of that lib and thus resolve a deploy conflict. --- core/CMakeLists.txt | 1 - .../python/python_tools/geometry_msg_types.h | 27 ------ .../moveit/python/python_tools/ros_init.h | 31 ------ .../moveit/python/python_tools/ros_types.h | 94 ------------------- .../python/task_constructor/properties.h | 4 +- core/python/bindings/CMakeLists.txt | 27 +----- core/python/bindings/src/python_tools.cpp | 53 ----------- core/python/bindings/src/ros_init.cpp | 68 -------------- core/python/bindings/src/ros_types.cpp | 61 ------------ core/python/bindings/src/stages.cpp | 1 + .../src/moveit/python_tools/__init__.py | 1 - core/python/test/rostest_mtc.py | 2 +- core/python/test/rostest_trampoline.py | 2 +- demo/scripts/cartesian.py | 2 +- 14 files changed, 7 insertions(+), 367 deletions(-) delete mode 100644 core/include/moveit/python/python_tools/geometry_msg_types.h delete mode 100644 core/include/moveit/python/python_tools/ros_init.h delete mode 100644 core/include/moveit/python/python_tools/ros_types.h delete mode 100644 core/python/bindings/src/python_tools.cpp delete mode 100644 core/python/bindings/src/ros_init.cpp delete mode 100644 core/python/bindings/src/ros_types.cpp delete mode 100644 core/python/src/moveit/python_tools/__init__.py diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 74e44e1f5..8568162a3 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -25,7 +25,6 @@ catkin_package( ${PROJECT_NAME} ${PROJECT_NAME}_stages ${PROJECT_NAME}_stage_plugins - moveit_python_tools INCLUDE_DIRS include CATKIN_DEPENDS diff --git a/core/include/moveit/python/python_tools/geometry_msg_types.h b/core/include/moveit/python/python_tools/geometry_msg_types.h deleted file mode 100644 index 711738d38..000000000 --- a/core/include/moveit/python/python_tools/geometry_msg_types.h +++ /dev/null @@ -1,27 +0,0 @@ -#pragma once - -#include "ros_types.h" -#include - -/** Convienency type casters, also allowing to initialize Stamped geometry msgs from a string */ - -namespace pybind11 { -namespace detail { - -template <> -struct type_caster : type_caster_ros_msg -{ - // Python -> C++ - bool load(handle src, bool convert) { - type_caster str_caster; - if (convert && str_caster.load(src, false)) { // string creates identity pose with given frame - value.header.frame_id = static_cast(str_caster); - value.pose.orientation.w = 1.0; - return true; - } - return type_caster_ros_msg::load(src, convert); - } -}; - -} // namespace detail -} // namespace pybind11 diff --git a/core/include/moveit/python/python_tools/ros_init.h b/core/include/moveit/python/python_tools/ros_init.h deleted file mode 100644 index 5bc05052e..000000000 --- a/core/include/moveit/python/python_tools/ros_init.h +++ /dev/null @@ -1,31 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace moveit { -namespace python { - -/// singleton class to initialize ROS C++ (once) from Python -class InitProxy -{ -public: - static void init(const std::string& node_name = "moveit_python_wrapper", - const std::map& remappings = std::map(), - uint32_t options = 0); - static void shutdown(); - - ~InitProxy(); - -private: - InitProxy(const std::string& node_name, const std::map& remappings, uint32_t options); - - static boost::mutex lock_; - static std::unique_ptr singleton_instance_; - -private: - std::unique_ptr spinner; -}; -} // namespace python -} // namespace moveit diff --git a/core/include/moveit/python/python_tools/ros_types.h b/core/include/moveit/python/python_tools/ros_types.h deleted file mode 100644 index 437e4fe46..000000000 --- a/core/include/moveit/python/python_tools/ros_types.h +++ /dev/null @@ -1,94 +0,0 @@ -#pragma once - -#include -#include -#include - -/** Provide pybind11 type converters for ROS types */ - -namespace moveit { -namespace python { - -PYBIND11_EXPORT pybind11::object createMessage(const std::string& ros_msg_name); -PYBIND11_EXPORT bool convertible(const pybind11::handle& h, const char* ros_msg_name); - -} // namespace python -} // namespace moveit - -namespace pybind11 { -namespace detail { - -/// Convert ros::Duration / ros::WallDuration into a float -template -struct DurationCaster -{ - // C++ -> Python - static handle cast(T&& src, return_value_policy /* policy */, handle /* parent */) { - return PyFloat_FromDouble(src.toSec()); - } - - // Python -> C++ - bool load(handle src, bool convert) { - if (hasattr(src, "to_sec")) { - value = T(src.attr("to_sec")().cast()); - } else if (convert) { - value = T(src.cast()); - } else - return false; - return true; - } - - PYBIND11_TYPE_CASTER(T, _("Duration")); -}; - -template <> -struct type_caster : DurationCaster -{}; - -template <> -struct type_caster : DurationCaster -{}; - -/// Convert ROS message types (C++ <-> python) -template ::value>> -struct type_caster_ros_msg -{ - // C++ -> Python - static handle cast(const T& src, return_value_policy /* policy */, handle /* parent */) { - // serialize src into (python) buffer - std::size_t size = ros::serialization::serializationLength(src); - object pbuffer = reinterpret_steal(PyBytes_FromStringAndSize(nullptr, size)); - ros::serialization::OStream stream(reinterpret_cast(PyBytes_AsString(pbuffer.ptr())), size); - ros::serialization::serialize(stream, src); - // deserialize python type from buffer - object msg = moveit::python::createMessage(ros::message_traits::DataType::value()); - msg.attr("deserialize")(pbuffer); - return msg.release(); - } - - // Python -> C++ - bool load(handle src, bool /*convert*/) { - if (!moveit::python::convertible(src, ros::message_traits::DataType::value())) - return false; - // serialize src into (python) buffer - object pstream = module::import("io").attr("BytesIO")(); - src.attr("serialize")(pstream); - object pbuffer = pstream.attr("getvalue")(); - // deserialize C++ type from buffer - char* cbuffer = nullptr; - Py_ssize_t size; - PyBytes_AsStringAndSize(pbuffer.ptr(), &cbuffer, &size); - ros::serialization::IStream cstream(const_cast(reinterpret_cast(cbuffer)), size); - ros::serialization::deserialize(cstream, value); - return true; - } - - PYBIND11_TYPE_CASTER(T, _()); -}; - -template -struct type_caster::value>> : type_caster_ros_msg -{}; - -} // namespace detail -} // namespace pybind11 diff --git a/core/include/moveit/python/task_constructor/properties.h b/core/include/moveit/python/task_constructor/properties.h index c52e1a802..a164f5530 100644 --- a/core/include/moveit/python/task_constructor/properties.h +++ b/core/include/moveit/python/task_constructor/properties.h @@ -1,7 +1,7 @@ #pragma once -#include -#include +#include +#include #include #include #include diff --git a/core/python/bindings/CMakeLists.txt b/core/python/bindings/CMakeLists.txt index b0809ee47..d83a10b48 100644 --- a/core/python/bindings/CMakeLists.txt +++ b/core/python/bindings/CMakeLists.txt @@ -1,31 +1,6 @@ -# python tools support lib -set(TOOLS_LIB_NAME moveit_python_tools) -set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/python_tools) -add_library(${TOOLS_LIB_NAME} SHARED - ${INCLUDES}/ros_init.h - ${INCLUDES}/ros_types.h - src/ros_init.cpp - src/ros_types.cpp -) -target_include_directories(${TOOLS_LIB_NAME} - PUBLIC $ - PUBLIC ${catkin_INCLUDE_DIRS} -) -target_link_libraries(${TOOLS_LIB_NAME} PUBLIC pybind11::pybind11 ${Boost_LIBRARIES} ${roscpp_LIBRARIES}) - -install(TARGETS ${TOOLS_LIB_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -) - # catkin_lint cannot detect target declarations in functions, here in pybind11_add_module #catkin_lint: ignore undefined_target -# moveit.python_tools -pybind11_add_module(pymoveit_python_tools src/python_tools.cpp) -target_link_libraries(pymoveit_python_tools PRIVATE ${TOOLS_LIB_NAME}) -set_target_properties(pymoveit_python_tools PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}) - # moveit.task_constructor set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/task_constructor) pybind11_add_module(pymoveit_mtc @@ -42,6 +17,6 @@ target_link_libraries(pymoveit_mtc PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_stages set_target_properties(pymoveit_mtc PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}) # install python libs -install(TARGETS pymoveit_python_tools pymoveit_mtc +install(TARGETS pymoveit_mtc LIBRARY DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION} ) diff --git a/core/python/bindings/src/python_tools.cpp b/core/python/bindings/src/python_tools.cpp deleted file mode 100644 index d10e5665e..000000000 --- a/core/python/bindings/src/python_tools.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, Bielefeld University - * 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 Bielefeld University 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. - *********************************************************************/ - -#include -#include -#include -#include - -namespace py = pybind11; -using namespace moveit::python; - -PYBIND11_MODULE(pymoveit_python_tools, m) { - m.doc() = "MoveIt python tools"; - - m.def("roscpp_init", &InitProxy::init, "Initialize C++ ROS", py::arg("node_name") = "moveit_python_wrapper", - py::arg("remappings") = std::map(), py::arg("options") = 0); - m.def("roscpp_shutdown", &InitProxy::shutdown, "Shutdown C++ ROS"); - - py::enum_(m, "InitOption") - .value("AnonymousName", ros::init_options::AnonymousName) - .value("NoRosout", ros::init_options::NoRosout); -} diff --git a/core/python/bindings/src/ros_init.cpp b/core/python/bindings/src/ros_init.cpp deleted file mode 100644 index 30fdd38c1..000000000 --- a/core/python/bindings/src/ros_init.cpp +++ /dev/null @@ -1,68 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, Bielefeld University - * 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 Bielefeld University 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. - *********************************************************************/ - -#include -#include - -namespace moveit { -namespace python { - -boost::mutex InitProxy::lock_; -std::unique_ptr InitProxy::singleton_instance_; - -void InitProxy::init(const std::string& node_name, const std::map& remappings, - uint32_t options) { - boost::mutex::scoped_lock slock(lock_); - if (!singleton_instance_ && !ros::isInitialized()) - singleton_instance_.reset(new InitProxy(node_name, remappings, options)); -} - -void InitProxy::shutdown() { - boost::mutex::scoped_lock slock(lock_); - singleton_instance_.reset(); -} - -InitProxy::InitProxy(const std::string& node_name, const std::map& remappings, - uint32_t options) { - ros::init(remappings, node_name, options | ros::init_options::NoSigintHandler); - spinner.reset(new ros::AsyncSpinner(1)); - spinner->start(); -} - -InitProxy::~InitProxy() { - spinner->stop(); - spinner.reset(); -} -} // namespace python -} // namespace moveit diff --git a/core/python/bindings/src/ros_types.cpp b/core/python/bindings/src/ros_types.cpp deleted file mode 100644 index ebab88a81..000000000 --- a/core/python/bindings/src/ros_types.cpp +++ /dev/null @@ -1,61 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, Bielefeld University - * 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 Bielefeld University 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. - *********************************************************************/ - -#include - -namespace py = pybind11; -namespace moveit { -namespace python { - -py::object createMessage(const std::string& ros_msg_name) { - // find delimiting '/' in ros msg name - std::size_t pos = ros_msg_name.find('/'); - // import module - py::module m = py::module::import((ros_msg_name.substr(0, pos) + ".msg").c_str()); - // retrieve type instance - py::object cls = m.attr(ros_msg_name.substr(pos + 1).c_str()); - // create message instance - return cls(); -} - -bool convertible(const pybind11::handle& h, const char* ros_msg_name) { - try { - PyObject* o = h.attr("_type").ptr(); - return py::cast(o) == ros_msg_name; - } catch (const std::exception& e) { - return false; - } -} -} // namespace python -} // namespace moveit diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index df5cf0e06..99fce8922 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -40,6 +40,7 @@ #include #include #include +#include namespace py = pybind11; using namespace py::literals; diff --git a/core/python/src/moveit/python_tools/__init__.py b/core/python/src/moveit/python_tools/__init__.py deleted file mode 100644 index c6b08468f..000000000 --- a/core/python/src/moveit/python_tools/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from pymoveit_python_tools import * diff --git a/core/python/test/rostest_mtc.py b/core/python/test/rostest_mtc.py index 5a0f5bbc7..8592b0b28 100755 --- a/core/python/test/rostest_mtc.py +++ b/core/python/test/rostest_mtc.py @@ -3,7 +3,7 @@ from __future__ import print_function import unittest import rostest -from moveit.python_tools import roscpp_init +from moveit.tools import roscpp_init from moveit.task_constructor import core, stages from geometry_msgs.msg import PoseStamped, Pose from geometry_msgs.msg import Vector3Stamped, Vector3 diff --git a/core/python/test/rostest_trampoline.py b/core/python/test/rostest_trampoline.py index 277c99b42..61c6bc5e9 100755 --- a/core/python/test/rostest_trampoline.py +++ b/core/python/test/rostest_trampoline.py @@ -4,7 +4,7 @@ from __future__ import print_function import unittest import rostest -from moveit.python_tools import roscpp_init +from moveit.tools import roscpp_init from moveit.task_constructor import core, stages from moveit.core.planning_scene import PlanningScene from geometry_msgs.msg import Vector3Stamped, Vector3 diff --git a/demo/scripts/cartesian.py b/demo/scripts/cartesian.py index f54c524f0..4ea92c12f 100755 --- a/demo/scripts/cartesian.py +++ b/demo/scripts/cartesian.py @@ -7,7 +7,7 @@ from math import pi import time -from moveit.python_tools import roscpp_init +from moveit.tools import roscpp_init roscpp_init("mtc_tutorial") From 885ac49ffb30e8e17e475f38d516ae6a45434e38 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 15 Feb 2023 14:32:20 +0100 Subject: [PATCH 08/52] Use MoveIt's roscpp_initialize --- core/package.xml | 1 + core/python/test/rostest_mtc.py | 4 ++-- core/python/test/rostest_trampoline.py | 4 ++-- demo/scripts/alternatives.py | 4 ++-- demo/scripts/cartesian.py | 4 ++-- demo/scripts/compute_ik.py | 4 ++-- demo/scripts/current_state.py | 4 ++-- demo/scripts/fallbacks.py | 4 ++-- demo/scripts/fix_collision_objects.py | 4 ++-- demo/scripts/fixed_state.py | 4 ++-- demo/scripts/generate_pose.py | 4 ++-- demo/scripts/merger.py | 4 ++-- demo/scripts/modify_planning_scene.py | 4 ++-- demo/scripts/pickplace.py | 4 ++-- demo/scripts/properties.py | 4 ++-- 15 files changed, 29 insertions(+), 28 deletions(-) diff --git a/core/package.xml b/core/package.xml index 143cdd035..2dcb3b793 100644 --- a/core/package.xml +++ b/core/package.xml @@ -32,6 +32,7 @@ rostest moveit_resources_fanuc_moveit_config moveit_planners + moveit_commander diff --git a/core/python/test/rostest_mtc.py b/core/python/test/rostest_mtc.py index 8592b0b28..54e38e645 100755 --- a/core/python/test/rostest_mtc.py +++ b/core/python/test/rostest_mtc.py @@ -3,7 +3,7 @@ from __future__ import print_function import unittest import rostest -from moveit.tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize from moveit.task_constructor import core, stages from geometry_msgs.msg import PoseStamped, Pose from geometry_msgs.msg import Vector3Stamped, Vector3 @@ -65,5 +65,5 @@ def createDisplacement(group, displacement): if __name__ == "__main__": - roscpp_init("test_mtc") + roscpp_initialize("test_mtc") rostest.rosrun("mtc", "base", Test) diff --git a/core/python/test/rostest_trampoline.py b/core/python/test/rostest_trampoline.py index 61c6bc5e9..eb0e8cf12 100755 --- a/core/python/test/rostest_trampoline.py +++ b/core/python/test/rostest_trampoline.py @@ -4,7 +4,7 @@ from __future__ import print_function import unittest import rostest -from moveit.tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize from moveit.task_constructor import core, stages from moveit.core.planning_scene import PlanningScene from geometry_msgs.msg import Vector3Stamped, Vector3 @@ -131,5 +131,5 @@ def test_propagator(self): if __name__ == "__main__": - roscpp_init("test_mtc") + roscpp_initialize("test_mtc") rostest.rosrun("mtc", "trampoline", TestTrampolines) diff --git a/demo/scripts/alternatives.py b/demo/scripts/alternatives.py index e32367a53..7bdfa20e4 100755 --- a/demo/scripts/alternatives.py +++ b/demo/scripts/alternatives.py @@ -2,10 +2,10 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize import time -roscpp_init("mtc_tutorial_alternatives") +roscpp_initialize("mtc_tutorial_alternatives") # Use the joint interpolation planner jointPlanner = core.JointInterpolationPlanner() diff --git a/demo/scripts/cartesian.py b/demo/scripts/cartesian.py index 4ea92c12f..bd304de59 100755 --- a/demo/scripts/cartesian.py +++ b/demo/scripts/cartesian.py @@ -7,9 +7,9 @@ from math import pi import time -from moveit.tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize -roscpp_init("mtc_tutorial") +roscpp_initialize("mtc_tutorial") # [cartesianTut1] group = "panda_arm" diff --git a/demo/scripts/compute_ik.py b/demo/scripts/compute_ik.py index 0a8b6e8ae..7258af9a3 100755 --- a/demo/scripts/compute_ik.py +++ b/demo/scripts/compute_ik.py @@ -6,9 +6,9 @@ from std_msgs.msg import Header import time -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize -roscpp_init("mtc_tutorial_compute_ik") +roscpp_initialize("mtc_tutorial_compute_ik") # Specify the planning group group = "panda_arm" diff --git a/demo/scripts/current_state.py b/demo/scripts/current_state.py index 35e19ec63..e8011dbe4 100755 --- a/demo/scripts/current_state.py +++ b/demo/scripts/current_state.py @@ -2,10 +2,10 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize import time -roscpp_init("mtc_tutorial_current_state") +roscpp_initialize("mtc_tutorial_current_state") # Create a task task = core.Task() diff --git a/demo/scripts/fallbacks.py b/demo/scripts/fallbacks.py index 8538d224c..f99016c2c 100755 --- a/demo/scripts/fallbacks.py +++ b/demo/scripts/fallbacks.py @@ -2,10 +2,10 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize import time -roscpp_init("mtc_tutorial_fallbacks") +roscpp_initialize("mtc_tutorial_fallbacks") # use cartesian and joint interpolation planners cartesianPlanner = core.CartesianPath() diff --git a/demo/scripts/fix_collision_objects.py b/demo/scripts/fix_collision_objects.py index a26e70361..3fdd14e11 100755 --- a/demo/scripts/fix_collision_objects.py +++ b/demo/scripts/fix_collision_objects.py @@ -2,10 +2,10 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize import time -roscpp_init("mtc_tutorial_current_state") +roscpp_initialize("mtc_tutorial_current_state") # Create a task task = core.Task() diff --git a/demo/scripts/fixed_state.py b/demo/scripts/fixed_state.py index 1df7db7aa..7ada93eb8 100755 --- a/demo/scripts/fixed_state.py +++ b/demo/scripts/fixed_state.py @@ -3,11 +3,11 @@ from moveit.core import planning_scene from moveit.task_constructor import core, stages -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize from moveit.core.planning_scene import PlanningScene import time -roscpp_init("mtc_tutorial_current_state") +roscpp_initialize("mtc_tutorial_current_state") # Create a task diff --git a/demo/scripts/generate_pose.py b/demo/scripts/generate_pose.py index 5b89a0022..ce8aa705c 100755 --- a/demo/scripts/generate_pose.py +++ b/demo/scripts/generate_pose.py @@ -3,10 +3,10 @@ from moveit.task_constructor import core, stages from geometry_msgs.msg import PoseStamped -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize import time -roscpp_init("mtc_tutorial_compute_ik") +roscpp_initialize("mtc_tutorial_compute_ik") # Specify the planning group group = "panda_arm" diff --git a/demo/scripts/merger.py b/demo/scripts/merger.py index f9a2bb0b1..f2d261391 100755 --- a/demo/scripts/merger.py +++ b/demo/scripts/merger.py @@ -2,10 +2,10 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize import time -roscpp_init("mtc_tutorial_merger") +roscpp_initialize("mtc_tutorial_merger") # use the joint interpolation planner planner = core.JointInterpolationPlanner() diff --git a/demo/scripts/modify_planning_scene.py b/demo/scripts/modify_planning_scene.py index 49bc06283..ef5385da3 100755 --- a/demo/scripts/modify_planning_scene.py +++ b/demo/scripts/modify_planning_scene.py @@ -5,10 +5,10 @@ from moveit_msgs.msg import CollisionObject from shape_msgs.msg import SolidPrimitive from geometry_msgs.msg import PoseStamped -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize import time -roscpp_init("mtc_tutorial_modify_planning_scene") +roscpp_initialize("mtc_tutorial_modify_planning_scene") # Create a task task = core.Task() diff --git a/demo/scripts/pickplace.py b/demo/scripts/pickplace.py index 3e769c612..e71af604e 100755 --- a/demo/scripts/pickplace.py +++ b/demo/scripts/pickplace.py @@ -1,13 +1,13 @@ #! /usr/bin/env python # -*- coding: utf-8 -*- -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize from moveit.task_constructor import core, stages from moveit_commander import PlanningSceneInterface from geometry_msgs.msg import PoseStamped, TwistStamped import time -roscpp_init("pickplace") +roscpp_initialize("pickplace") # [pickAndPlaceTut1] # Specify robot parameters diff --git a/demo/scripts/properties.py b/demo/scripts/properties.py index 1256732dc..cffc0e0fc 100644 --- a/demo/scripts/properties.py +++ b/demo/scripts/properties.py @@ -5,9 +5,9 @@ from geometry_msgs.msg import PoseStamped import time -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize -roscpp_init("mtc_tutorial") +roscpp_initialize("mtc_tutorial") # Create a task container task = core.Task() From 7926f69e97c58b1a345a115d727ec543f48d6eef Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 15 Feb 2023 16:06:39 +0100 Subject: [PATCH 09/52] Provide ComputeIK.ik_frame as full PoseStamped Released MoveIt doesn't (yet) provide a conversion from string to PoseStamped. --- core/python/test/rostest_trampoline.py | 4 ++-- demo/scripts/compute_ik.py | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/core/python/test/rostest_trampoline.py b/core/python/test/rostest_trampoline.py index eb0e8cf12..562adf7b7 100755 --- a/core/python/test/rostest_trampoline.py +++ b/core/python/test/rostest_trampoline.py @@ -7,7 +7,7 @@ from moveit_commander.roscpp_initializer import roscpp_initialize from moveit.task_constructor import core, stages from moveit.core.planning_scene import PlanningScene -from geometry_msgs.msg import Vector3Stamped, Vector3 +from geometry_msgs.msg import Vector3Stamped, Vector3, PoseStamped from std_msgs.msg import Header PLANNING_GROUP = "manipulator" @@ -75,7 +75,7 @@ class PyMoveRelX(stages.MoveRelative): def __init__(self, x, planner, name="Move ±x"): stages.MoveRelative.__init__(self, name, planner) self.group = PLANNING_GROUP - self.ik_frame = "tool0" + self.ik_frame = PoseStamped(header=Header(frame_id="tool0")) self.setDirection( Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(x, 0, 0)) ) diff --git a/demo/scripts/compute_ik.py b/demo/scripts/compute_ik.py index 7258af9a3..e910991e5 100755 --- a/demo/scripts/compute_ik.py +++ b/demo/scripts/compute_ik.py @@ -40,7 +40,8 @@ # Wrap Cartesian generator into a ComputeIK stage to yield a joint pose computeIK = stages.ComputeIK("compute IK", generator) computeIK.group = group # Use the group-specific IK solver -computeIK.ik_frame = "panda_link8" # Which end-effector frame should reach the target? +# Which end-effector frame should reach the target? +computeIK.ik_frame = PoseStamped(header=Header(frame_id="panda_link8")) computeIK.max_ik_solutions = 4 # Limit the number of IK solutions # [propertyTut14] props = computeIK.properties From eae0bdc27fa193077d559566cff9c94813c98bb5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 15 Feb 2023 23:20:32 +0100 Subject: [PATCH 10/52] ros1-0.1.1 --- capabilities/CHANGELOG.rst | 3 +++ capabilities/package.xml | 2 +- core/CHANGELOG.rst | 10 ++++++++++ core/package.xml | 2 +- demo/CHANGELOG.rst | 10 ++++++++++ demo/package.xml | 2 +- msgs/CHANGELOG.rst | 3 +++ msgs/package.xml | 2 +- rviz_marker_tools/CHANGELOG.rst | 3 +++ rviz_marker_tools/package.xml | 2 +- visualization/CHANGELOG.rst | 5 +++++ visualization/package.xml | 2 +- 12 files changed, 40 insertions(+), 6 deletions(-) diff --git a/capabilities/CHANGELOG.rst b/capabilities/CHANGELOG.rst index ad574bdb4..d07938e4c 100644 --- a/capabilities/CHANGELOG.rst +++ b/capabilities/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_task_constructor_capabilities ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.1 (2023-02-15) +------------------ + 0.1.0 (2023-02-02) ------------------ * Initial release diff --git a/capabilities/package.xml b/capabilities/package.xml index e4a99dacc..b38ccc446 100644 --- a/capabilities/package.xml +++ b/capabilities/package.xml @@ -1,6 +1,6 @@ moveit_task_constructor_capabilities - 0.1.0 + 0.1.1 MoveGroupCapabilites to interact with MoveIt diff --git a/core/CHANGELOG.rst b/core/CHANGELOG.rst index 2d9e93900..75afb61d0 100644 --- a/core/CHANGELOG.rst +++ b/core/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package moveit_task_constructor_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.1 (2023-02-15) +------------------ +* Resort to MoveIt's python tools + * Provide ComputeIK.ik_frame as full PoseStamped +* Fixed build farm issues + * Removed unused eigen_conversions includes + * Fixed odr compiler warning on Buster + * Fixed missing dependency declarations +* Contributors: Robert Haschke + 0.1.0 (2023-02-02) ------------------ * Initial release diff --git a/core/package.xml b/core/package.xml index 2dcb3b793..9edd4a2f5 100644 --- a/core/package.xml +++ b/core/package.xml @@ -1,6 +1,6 @@ moveit_task_constructor_core - 0.1.0 + 0.1.1 MoveIt Task Pipeline https://github.com/ros-planning/moveit_task_constructor diff --git a/demo/CHANGELOG.rst b/demo/CHANGELOG.rst index a7bb7ea3e..3e65c6385 100644 --- a/demo/CHANGELOG.rst +++ b/demo/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package moveit_task_constructor_demo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.1 (2023-02-15) +------------------ +* Resort to MoveIt's python tools + * Provide ComputeIK.ik_frame as full PoseStamped +* Fixed build farm issues + * Fixed missing dependency declarations +* pick_place_task: monitor last state before Connect + ... to prune solutions as much as possible +* Contributors: Robert Haschke + 0.1.0 (2023-02-02) ------------------ * Initial release diff --git a/demo/package.xml b/demo/package.xml index 202a47352..ac68b783c 100644 --- a/demo/package.xml +++ b/demo/package.xml @@ -1,7 +1,7 @@ moveit_task_constructor_demo - 0.1.0 + 0.1.1 demo tasks illustrating various capabilities of MTC. Robert Haschke diff --git a/msgs/CHANGELOG.rst b/msgs/CHANGELOG.rst index 7897285ca..bb5f44752 100644 --- a/msgs/CHANGELOG.rst +++ b/msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_task_constructor_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.1 (2023-02-15) +------------------ + 0.1.0 (2023-02-02) ------------------ * Initial release diff --git a/msgs/package.xml b/msgs/package.xml index 00cef36a5..b59e0747e 100644 --- a/msgs/package.xml +++ b/msgs/package.xml @@ -1,6 +1,6 @@ moveit_task_constructor_msgs - 0.1.0 + 0.1.1 Messages for MoveIt Task Pipeline BSD diff --git a/rviz_marker_tools/CHANGELOG.rst b/rviz_marker_tools/CHANGELOG.rst index 7d7a83ced..5d90c5678 100644 --- a/rviz_marker_tools/CHANGELOG.rst +++ b/rviz_marker_tools/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rviz_marker_tools ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.1 (2023-02-15) +------------------ + 0.1.0 (2023-02-02) ------------------ * Initial release diff --git a/rviz_marker_tools/package.xml b/rviz_marker_tools/package.xml index 263b86c9f..ce114edea 100644 --- a/rviz_marker_tools/package.xml +++ b/rviz_marker_tools/package.xml @@ -1,6 +1,6 @@ rviz_marker_tools - 0.1.0 + 0.1.1 Tools for marker creation / handling BSD diff --git a/visualization/CHANGELOG.rst b/visualization/CHANGELOG.rst index 959674a85..7c595fcd3 100644 --- a/visualization/CHANGELOG.rst +++ b/visualization/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_task_constructor_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.1 (2023-02-15) +------------------ +* Remove unused eigen_conversions includes +* Contributors: Robert Haschke + 0.1.0 (2023-02-02) ------------------ * Initial release diff --git a/visualization/package.xml b/visualization/package.xml index e6d00dd95..94cd06dae 100644 --- a/visualization/package.xml +++ b/visualization/package.xml @@ -1,6 +1,6 @@ moveit_task_constructor_visualization - 0.1.0 + 0.1.1 Visualization tools for MoveIt Task Pipeline BSD From 3d3236575d0b41f1080b31b78532e7e078a52189 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 17 Feb 2023 11:41:32 +0100 Subject: [PATCH 11/52] JointInterpolation: fix timeout handling The timeout parameter was essentially ignored and the check was always true. --- core/src/solvers/joint_interpolation.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index 600fd6f02..dfac9182d 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -103,7 +103,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) { - const auto start_time = std::chrono::steady_clock::now(); + const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration>(timeout); auto to{ from->diff() }; @@ -125,8 +125,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr } to->getCurrentStateNonConst().update(); - timeout = std::chrono::duration(std::chrono::steady_clock::now() - start_time).count(); - if (timeout <= 0.0) + if (std::chrono::steady_clock::now() >= deadline) return false; return plan(from, to, jmg, timeout, result, path_constraints); From 052a56a333862b5ee0ef8fc400007be48b68ce60 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 17 Feb 2023 12:17:33 +0100 Subject: [PATCH 12/52] Add MultiPlanner solvers a planner that tries multiple planners in sequence --- .../task_constructor/solvers/multi_planner.h | 74 ++++++++++++++ core/src/CMakeLists.txt | 4 +- core/src/solvers/multi_planner.cpp | 98 +++++++++++++++++++ 3 files changed, 175 insertions(+), 1 deletion(-) create mode 100644 core/include/moveit/task_constructor/solvers/multi_planner.h create mode 100644 core/src/solvers/multi_planner.cpp diff --git a/core/include/moveit/task_constructor/solvers/multi_planner.h b/core/include/moveit/task_constructor/solvers/multi_planner.h new file mode 100644 index 000000000..dd824fe9d --- /dev/null +++ b/core/include/moveit/task_constructor/solvers/multi_planner.h @@ -0,0 +1,74 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Bielefeld University + * 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 Bielefeld University 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. + *********************************************************************/ + +/* Authors: Robert Haschke + Desc: meta planner, running multiple planners in parallel or sequence +*/ + +#pragma once + +#include +#include + +namespace moveit { +namespace task_constructor { +namespace solvers { + +MOVEIT_CLASS_FORWARD(MultiPlanner); + +/** A meta planner that runs multiple alternative planners in sequence and returns the first found solution. + * + * This is useful to sequence different planning strategies of increasing complexity, + * e.g. Cartesian or joint-space interpolation first, then OMPL, ... + */ +class MultiPlanner : public PlannerInterface, public std::vector +{ +public: + using PlannerList = std::vector; + using PlannerList::PlannerList; // inherit all std::vector constructors + + void init(const moveit::core::RobotModelConstPtr& robot_model) override; + + bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + + bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; +}; +} // namespace solvers +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index e25a036c5..45cefe57c 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -18,6 +18,7 @@ add_library(${PROJECT_NAME} ${PROJECT_INCLUDE}/solvers/cartesian_path.h ${PROJECT_INCLUDE}/solvers/joint_interpolation.h ${PROJECT_INCLUDE}/solvers/pipeline_planner.h + ${PROJECT_INCLUDE}/solvers/multi_planner.h container.cpp cost_terms.cpp @@ -32,8 +33,9 @@ add_library(${PROJECT_NAME} solvers/planner_interface.cpp solvers/cartesian_path.cpp - solvers/pipeline_planner.cpp solvers/joint_interpolation.cpp + solvers/pipeline_planner.cpp + solvers/multi_planner.cpp ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) target_include_directories(${PROJECT_NAME} diff --git a/core/src/solvers/multi_planner.cpp b/core/src/solvers/multi_planner.cpp new file mode 100644 index 000000000..f91553110 --- /dev/null +++ b/core/src/solvers/multi_planner.cpp @@ -0,0 +1,98 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Bielefeld University + * 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 Bielefeld University 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. + *********************************************************************/ + +/* Authors: Michael Goerner, Robert Haschke + Desc: generate and validate a straight-line Cartesian path +*/ + +#include +#include +#include + +namespace moveit { +namespace task_constructor { +namespace solvers { + +void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) { + for (const auto& p : *this) + p->init(robot_model); +} + +bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) { + double remaining_time = timeout; + auto start_time = std::chrono::steady_clock::now(); + + for (const auto& p : *this) { + if (remaining_time < 0) + return false; // timeout + if (result) + result->clear(); + if (p->plan(from, to, jmg, remaining_time, result, path_constraints)) + return true; + + auto now = std::chrono::steady_clock::now(); + remaining_time -= std::chrono::duration(now - start_time).count(); + start_time = now; + } + return false; +} + +bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) { + double remaining_time = timeout; + auto start_time = std::chrono::steady_clock::now(); + + for (const auto& p : *this) { + if (remaining_time < 0) + return false; // timeout + if (result) + result->clear(); + if (p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints)) + return true; + + auto now = std::chrono::steady_clock::now(); + remaining_time -= std::chrono::duration(now - start_time).count(); + start_time = now; + } + return false; +} +} // namespace solvers +} // namespace task_constructor +} // namespace moveit From 573858e51ad7c64e85e76adf854f822aa3589cad Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 17 Feb 2023 13:59:36 +0100 Subject: [PATCH 13/52] PlannerInterface: provide setters for properties --- .../moveit/task_constructor/solvers/planner_interface.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index 60139b3bd..5c2bc889b 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -49,6 +49,9 @@ MOVEIT_CLASS_FORWARD(PlanningScene); namespace robot_trajectory { MOVEIT_CLASS_FORWARD(RobotTrajectory); } +namespace trajectory_processing { +MOVEIT_CLASS_FORWARD(TimeParameterization); +} namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(LinkModel); @@ -75,6 +78,11 @@ class PlannerInterface const PropertyMap& properties() const { return properties_; } void setProperty(const std::string& name, const boost::any& value) { properties_.set(name, value); } + void setMaxVelocityScalingFactor(double factor) { properties_.set("max_velocity_scaling_factor", factor); } + void setMaxAccelerationScalingFactor(double factor) { properties_.set("max_acceleration_scaling_factor", factor); } + void setTimeParameterization(const trajectory_processing::TimeParameterizationPtr& tp) { + properties_.set("time_parameterization", tp); + } virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0; From 6dc70b1d497ff68126e68aa3ea9a260cf07f4386 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 17 Feb 2023 14:01:58 +0100 Subject: [PATCH 14/52] PlannerInterface: provide "timeout" property The MultiPlanner requires to set individual timeouts for its planners. --- core/include/moveit/task_constructor/solvers/multi_planner.h | 3 +++ .../moveit/task_constructor/solvers/planner_interface.h | 1 + core/src/solvers/cartesian_path.cpp | 2 +- core/src/solvers/joint_interpolation.cpp | 1 + core/src/solvers/multi_planner.cpp | 4 ++-- core/src/solvers/pipeline_planner.cpp | 2 +- core/src/solvers/planner_interface.cpp | 1 + 7 files changed, 10 insertions(+), 4 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/multi_planner.h b/core/include/moveit/task_constructor/solvers/multi_planner.h index dd824fe9d..2487dadff 100644 --- a/core/include/moveit/task_constructor/solvers/multi_planner.h +++ b/core/include/moveit/task_constructor/solvers/multi_planner.h @@ -51,6 +51,9 @@ MOVEIT_CLASS_FORWARD(MultiPlanner); * * This is useful to sequence different planning strategies of increasing complexity, * e.g. Cartesian or joint-space interpolation first, then OMPL, ... + * This is (slightly) different from the Fallbacks container, as the MultiPlanner directly applies its planners to each + * individual planning job. In contrast, the Fallbacks container first runs the active child to exhaustion before + * switching to the next child, which possibly applies a different planning strategy. */ class MultiPlanner : public PlannerInterface, public std::vector { diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index 5c2bc889b..67a876ee2 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -78,6 +78,7 @@ class PlannerInterface const PropertyMap& properties() const { return properties_; } void setProperty(const std::string& name, const boost::any& value) { properties_.set(name, value); } + void setTimeout(double timeout) { properties_.set("timeout", timeout); } void setMaxVelocityScalingFactor(double factor) { properties_.set("max_velocity_scaling_factor", factor); } void setMaxAccelerationScalingFactor(double factor) { properties_.set("max_acceleration_scaling_factor", factor); } void setTimeParameterization(const trajectory_processing::TimeParameterizationPtr& tp) { diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 9852470ff..4e9df5dc3 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -71,7 +71,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, // reach pose of forward kinematics return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg, - timeout, result, path_constraints); + std::min(timeout, properties().get("timeout")), result, path_constraints); } bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index dfac9182d..e9727529d 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -103,6 +103,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) { + timeout = std::min(timeout, properties().get("timeout")); const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration>(timeout); auto to{ from->diff() }; diff --git a/core/src/solvers/multi_planner.cpp b/core/src/solvers/multi_planner.cpp index f91553110..7603f7640 100644 --- a/core/src/solvers/multi_planner.cpp +++ b/core/src/solvers/multi_planner.cpp @@ -53,7 +53,7 @@ bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) { - double remaining_time = timeout; + double remaining_time = std::min(timeout, properties().get("timeout")); auto start_time = std::chrono::steady_clock::now(); for (const auto& p : *this) { @@ -76,7 +76,7 @@ bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) { - double remaining_time = timeout; + double remaining_time = std::min(timeout, properties().get("timeout")); auto start_time = std::chrono::steady_clock::now(); for (const auto& p : *this) { diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 66d12d5c1..333759084 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -143,7 +143,7 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa const moveit::core::JointModelGroup* jmg, double timeout) { req.group_name = jmg->getName(); req.planner_id = p.get("planner"); - req.allowed_planning_time = timeout; + req.allowed_planning_time = std::min(timeout, p.get("timeout")); req.start_state.is_diff = true; // we don't specify an extra start state req.num_planning_attempts = p.get("num_planning_attempts"); diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp index 617fb476a..a58b110da 100644 --- a/core/src/solvers/planner_interface.cpp +++ b/core/src/solvers/planner_interface.cpp @@ -47,6 +47,7 @@ namespace solvers { PlannerInterface::PlannerInterface() { auto& p = properties(); + p.declare("timeout", std::numeric_limits::infinity(), "timeout for planner (s)"); p.declare("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor"); p.declare("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor"); p.declare("time_parameterization", std::make_shared()); From db6d90ab699bac8219a754d0da35c36105028da3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 17 Feb 2023 22:35:40 +0100 Subject: [PATCH 15/52] CartesianPath: Deprecate redundant property setters --- .../moveit/task_constructor/solvers/cartesian_path.h | 6 ++++-- demo/src/pick_place_task.cpp | 4 ++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 732111cb9..7cb53ee38 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -56,8 +56,10 @@ class CartesianPath : public PlannerInterface void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); } void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); } - void setMaxVelocityScaling(double factor) { setProperty("max_velocity_scaling_factor", factor); } - void setMaxAccelerationScaling(double factor) { setProperty("max_acceleration_scaling_factor", factor); } + [[deprecated("Replace with setMaxVelocityScalingFactor")]] // clang-format off + void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); } + [[deprecated("Replace with setMaxAccelerationScaling")]] // clang-format off + void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); } void init(const moveit::core::RobotModelConstPtr& robot_model) override; diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index e4232ec3e..5eb72ec8a 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -163,8 +163,8 @@ bool PickPlaceTask::init() { // Cartesian planner auto cartesian_planner = std::make_shared(); - cartesian_planner->setMaxVelocityScaling(1.0); - cartesian_planner->setMaxAccelerationScaling(1.0); + cartesian_planner->setMaxVelocityScalingFactor(1.0); + cartesian_planner->setMaxAccelerationScalingFactor(1.0); cartesian_planner->setStepSize(.01); // Set task properties From ea776e35f6b71ee40d03809804cd16a6ed57eb6e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 23 Feb 2023 20:42:23 +0100 Subject: [PATCH 16/52] Fix marker creation: allow zero scale for geometric shapes (#430) Boxes, spheres, and cylinders might explicitly have a zero size. Don't reset their scaling to (1,1,1) --- core/src/marker_tools.cpp | 2 ++ rviz_marker_tools/src/marker_creation.cpp | 9 +++------ 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/core/src/marker_tools.cpp b/core/src/marker_tools.cpp index 24f3aa849..5f738e3a5 100644 --- a/core/src/marker_tools.cpp +++ b/core/src/marker_tools.cpp @@ -122,6 +122,8 @@ void generateMarkers(const moveit::core::RobotState& robot_state, const MarkerCa auto element_handler = [&](const T& element) { if (element && element->geometry) { createGeometryMarker(m, *element->geometry, element->origin, materialColor(*model, materialName(*element))); + if (m.scale.x == 0 && m.scale.y == 0 && m.scale.z == 0) + return; // skip zero-size marker m.pose = rviz_marker_tools::composePoses(robot_state.getGlobalLinkTransform(name), m.pose); callback(m, name); valid_found = true; diff --git a/rviz_marker_tools/src/marker_creation.cpp b/rviz_marker_tools/src/marker_creation.cpp index 8d68b2ffd..eae1913e6 100644 --- a/rviz_marker_tools/src/marker_creation.cpp +++ b/rviz_marker_tools/src/marker_creation.cpp @@ -158,12 +158,6 @@ void prepareMarker(vm::Marker& m, int marker_type) { m.points.clear(); m.colors.clear(); - // ensure valid scale - if (m.scale.x == 0 && m.scale.y == 0 && m.scale.z == 0) { - m.scale.x = 1.0; - m.scale.y = 1.0; - m.scale.z = 1.0; - } // ensure non-null orientation if (m.pose.orientation.w == 0 && m.pose.orientation.x == 0 && m.pose.orientation.y == 0 && m.pose.orientation.z == 0) m.pose.orientation.w = 1.0; @@ -188,6 +182,7 @@ vm::Marker& makeXYPlane(vm::Marker& m) { p[3].y = -1.0; p[3].z = 0.0; + m.scale.x = m.scale.y = m.scale.z = 1.0; prepareMarker(m, vm::Marker::TRIANGLE_LIST); m.points.push_back(p[0]); m.points.push_back(p[1]); @@ -217,6 +212,7 @@ vm::Marker& makeYZPlane(vm::Marker& m) { /// create a cone of given angle along the x-axis vm::Marker makeCone(double angle, vm::Marker& m) { + m.scale.x = m.scale.y = m.scale.z = 1.0; prepareMarker(m, vm::Marker::TRIANGLE_LIST); geometry_msgs::Point p[3]; p[0].x = p[0].y = p[0].z = 0.0; @@ -296,6 +292,7 @@ vm::Marker& makeArrow(vm::Marker& m, double scale, bool tip_at_origin) { } vm::Marker& makeText(vm::Marker& m, const std::string& text) { + m.scale.x = m.scale.y = m.scale.z = 1.0; prepareMarker(m, vm::Marker::TEXT_VIEW_FACING); m.text = text; return m; From 6671aedeb722f20ddf18a3b95394d1086b51d951 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 23 Feb 2023 23:54:17 +0100 Subject: [PATCH 17/52] Remove moveit/__init__.py during .deb builds --- core/setup.py | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/core/setup.py b/core/setup.py index 9d20d30e4..b3582ddaf 100644 --- a/core/setup.py +++ b/core/setup.py @@ -9,4 +9,19 @@ # specify location of root ("") package dir package_dir={"": "python/src"}, ) -setup(**d) +dist = setup(**d) + +# Remove moveit/__init__.py when building .deb packages +# Otherwise, the installation procedure will complain about conflicting files (with moveit_core) +try: + libdir = dist.command_obj[ + "install_lib" + ].install_dir # installation dir (.../lib/python3/dist-packages) + if libdir.startswith("/opt/ros"): + import os + import shutil + + os.remove(os.path.join(libdir, "moveit", "__init__.py")) + shutil.rmtree(os.path.join(libdir, "moveit", "__pycache__")) +except AttributeError as e: + pass From d07aece163b0dfaa9b84e2aff387ac12d343c74a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 24 Feb 2023 00:02:47 +0100 Subject: [PATCH 18/52] ros1-0.1.2 --- capabilities/CHANGELOG.rst | 3 +++ capabilities/package.xml | 2 +- core/CHANGELOG.rst | 14 +++++++++++++- core/package.xml | 2 +- demo/CHANGELOG.rst | 5 +++++ demo/package.xml | 2 +- msgs/CHANGELOG.rst | 3 +++ msgs/package.xml | 2 +- rviz_marker_tools/CHANGELOG.rst | 5 +++++ rviz_marker_tools/package.xml | 2 +- visualization/CHANGELOG.rst | 3 +++ visualization/package.xml | 2 +- 12 files changed, 38 insertions(+), 7 deletions(-) diff --git a/capabilities/CHANGELOG.rst b/capabilities/CHANGELOG.rst index d07938e4c..70c3ce154 100644 --- a/capabilities/CHANGELOG.rst +++ b/capabilities/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_task_constructor_capabilities ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.2 (2023-02-24) +------------------ + 0.1.1 (2023-02-15) ------------------ diff --git a/capabilities/package.xml b/capabilities/package.xml index b38ccc446..7db2754ef 100644 --- a/capabilities/package.xml +++ b/capabilities/package.xml @@ -1,6 +1,6 @@ moveit_task_constructor_capabilities - 0.1.1 + 0.1.2 MoveGroupCapabilites to interact with MoveIt diff --git a/core/CHANGELOG.rst b/core/CHANGELOG.rst index 75afb61d0..5fe14dc4d 100644 --- a/core/CHANGELOG.rst +++ b/core/CHANGELOG.rst @@ -2,11 +2,23 @@ Changelog for package moveit_task_constructor_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.2 (2023-02-24) +------------------ +* Remove moveit/__init__.py during .deb builds to avoid installation conflicts +* MultiPlanner solver (`#429 `_): a planner that tries multiple planners in sequence + + * CartesianPath: Deprecate redundant property setters + * PlannerInterface: provide "timeout" property + * PlannerInterface: provide setters for properties +* JointInterpolation: fix timeout handling +* Contributors: Robert Haschke + 0.1.1 (2023-02-15) ------------------ * Resort to MoveIt's python tools - * Provide ComputeIK.ik_frame as full PoseStamped +* Provide ComputeIK.ik_frame as full PoseStamped * Fixed build farm issues + * Removed unused eigen_conversions includes * Fixed odr compiler warning on Buster * Fixed missing dependency declarations diff --git a/core/package.xml b/core/package.xml index 9edd4a2f5..de0b18716 100644 --- a/core/package.xml +++ b/core/package.xml @@ -1,6 +1,6 @@ moveit_task_constructor_core - 0.1.1 + 0.1.2 MoveIt Task Pipeline https://github.com/ros-planning/moveit_task_constructor diff --git a/demo/CHANGELOG.rst b/demo/CHANGELOG.rst index 3e65c6385..344261076 100644 --- a/demo/CHANGELOG.rst +++ b/demo/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_task_constructor_demo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.2 (2023-02-24) +------------------ +* CartesianPath: Deprecate redundant property setters (`#429 `_) +* Contributors: Robert Haschke + 0.1.1 (2023-02-15) ------------------ * Resort to MoveIt's python tools diff --git a/demo/package.xml b/demo/package.xml index ac68b783c..5e69295a0 100644 --- a/demo/package.xml +++ b/demo/package.xml @@ -1,7 +1,7 @@ moveit_task_constructor_demo - 0.1.1 + 0.1.2 demo tasks illustrating various capabilities of MTC. Robert Haschke diff --git a/msgs/CHANGELOG.rst b/msgs/CHANGELOG.rst index bb5f44752..fb883eb2a 100644 --- a/msgs/CHANGELOG.rst +++ b/msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_task_constructor_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.2 (2023-02-24) +------------------ + 0.1.1 (2023-02-15) ------------------ diff --git a/msgs/package.xml b/msgs/package.xml index b59e0747e..fd200537d 100644 --- a/msgs/package.xml +++ b/msgs/package.xml @@ -1,6 +1,6 @@ moveit_task_constructor_msgs - 0.1.1 + 0.1.2 Messages for MoveIt Task Pipeline BSD diff --git a/rviz_marker_tools/CHANGELOG.rst b/rviz_marker_tools/CHANGELOG.rst index 5d90c5678..dac5227e6 100644 --- a/rviz_marker_tools/CHANGELOG.rst +++ b/rviz_marker_tools/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rviz_marker_tools ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.2 (2023-02-24) +------------------ +* Fix marker creation: allow zero scale for geometric shapes (`#430 `_) +* Contributors: Robert Haschke + 0.1.1 (2023-02-15) ------------------ diff --git a/rviz_marker_tools/package.xml b/rviz_marker_tools/package.xml index ce114edea..3bbdd9c7e 100644 --- a/rviz_marker_tools/package.xml +++ b/rviz_marker_tools/package.xml @@ -1,6 +1,6 @@ rviz_marker_tools - 0.1.1 + 0.1.2 Tools for marker creation / handling BSD diff --git a/visualization/CHANGELOG.rst b/visualization/CHANGELOG.rst index 7c595fcd3..8b669596b 100644 --- a/visualization/CHANGELOG.rst +++ b/visualization/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_task_constructor_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.2 (2023-02-24) +------------------ + 0.1.1 (2023-02-15) ------------------ * Remove unused eigen_conversions includes diff --git a/visualization/package.xml b/visualization/package.xml index 94cd06dae..32e01e212 100644 --- a/visualization/package.xml +++ b/visualization/package.xml @@ -1,6 +1,6 @@ moveit_task_constructor_visualization - 0.1.1 + 0.1.2 Visualization tools for MoveIt Task Pipeline BSD From ca1c7c7e2409c44f4f172aa834ce550a2117a6dc Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 28 Feb 2023 11:13:24 +0100 Subject: [PATCH 19/52] Fix typo --- core/include/moveit/task_constructor/solvers/cartesian_path.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 7cb53ee38..d2c2c0699 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -58,7 +58,7 @@ class CartesianPath : public PlannerInterface [[deprecated("Replace with setMaxVelocityScalingFactor")]] // clang-format off void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); } - [[deprecated("Replace with setMaxAccelerationScaling")]] // clang-format off + [[deprecated("Replace with setMaxAccelerationScalingFactor")]] // clang-format off void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); } void init(const moveit::core::RobotModelConstPtr& robot_model) override; From a3cb8c6584d6cc0c2b8fc6aa9dc802aecb8647a1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 27 Feb 2023 22:42:15 +0100 Subject: [PATCH 20/52] Fix: Fetch pybind11 submodule if not yet present cmake's execute_process pipes COMMANDs together. Thus, `git submodule update` received the output of `git submodule init` as input and didn't do anything. --- core/python/CMakeLists.txt | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/core/python/CMakeLists.txt b/core/python/CMakeLists.txt index cf877722f..48bb60fa5 100644 --- a/core/python/CMakeLists.txt +++ b/core/python/CMakeLists.txt @@ -21,9 +21,8 @@ set(PYBIND11_CMAKECONFIG_INSTALL_DIR ${CATKIN_PACKAGE_SHARE_DESTINATION}/cmake if(NOT EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/pybind11/CMakeLists.txt") message("Missing content of submodule pybind11: Use 'git clone --recurse-submodule' in future.\n" "Checking out content automatically") - execute_process(COMMAND git submodule init - COMMAND git submodule update - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) + execute_process(COMMAND git submodule init WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) + execute_process(COMMAND git submodule update WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) endif() #catkin_lint: ignore_once subproject duplicate_cmd add_subdirectory(pybind11) From 61bb2fdc58e3d7e550c57617937a3d5286e421ef Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 28 Feb 2023 13:18:53 +0100 Subject: [PATCH 21/52] ComputeIK: Limit collision checking to JMG (#428) That's what MoveIt is doing as well. --- core/src/stages/compute_ik.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 1f793f77d..5ce320b49 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -99,7 +99,7 @@ namespace { // TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene, robot_state::RobotState& robot_state, Eigen::Isometry3d pose, - const robot_model::LinkModel* link, + const robot_model::LinkModel* link, const robot_model::JointModelGroup* jmg = nullptr, collision_detection::CollisionResult* collision_result = nullptr) { // consider all rigidly connected parent links as well const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link); @@ -130,6 +130,8 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce collision_detection::CollisionRequest req; collision_detection::CollisionResult result; req.contacts = (collision_result != nullptr); + if (jmg) + req.group_name = jmg->getName(); collision_detection::CollisionResult& res = collision_result ? *collision_result : result; scene->checkCollision(req, res, robot_state, acm); return res.collision; @@ -311,7 +313,7 @@ void ComputeIK::compute() { collision_detection::CollisionResult collisions; robot_state::RobotState sandbox_state{ scene->getCurrentState() }; bool colliding = - !ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, &collisions); + !ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, jmg, &collisions); // frames at target pose and ik frame std::deque frame_markers; @@ -371,6 +373,7 @@ void ComputeIK::compute() { collision_detection::CollisionResult res; req.contacts = true; req.max_contacts = 1; + req.group_name = jmg->getName(); scene->checkCollision(req, res, *state); solution.feasible = ignore_collisions || !res.collision; if (!res.contacts.empty()) { From 08dc34c5b3f4df5e05ba892e0029ed189c96370c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 6 Mar 2023 16:26:41 +0100 Subject: [PATCH 22/52] Use const reference instead of reference for ros::NodeHandle (#437) --- demo/src/pick_place_task.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 5eb72ec8a..f9732dd27 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -47,7 +47,7 @@ void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const throw std::runtime_error("Failed to spawn object: " + object.id); } -moveit_msgs::CollisionObject createTable(ros::NodeHandle& pnh) { +moveit_msgs::CollisionObject createTable(const ros::NodeHandle& pnh) { std::string table_name, table_reference_frame; std::vector table_dimensions; geometry_msgs::Pose pose; @@ -69,7 +69,7 @@ moveit_msgs::CollisionObject createTable(ros::NodeHandle& pnh) { return object; } -moveit_msgs::CollisionObject createObject(ros::NodeHandle& pnh) { +moveit_msgs::CollisionObject createObject(const ros::NodeHandle& pnh) { std::string object_name, object_reference_frame; std::vector object_dimensions; geometry_msgs::Pose pose; From 75e4260e2a258676e842266d287e0e97776ebf71 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 6 Mar 2023 16:27:09 +0100 Subject: [PATCH 23/52] MoveRelative: Allow backwards operation for joint-space delta (#436) --- core/src/stages/move_relative.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 2e9099de1..930385149 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -79,11 +79,12 @@ void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model) { planner_->init(robot_model); } -static bool getJointStateFromOffset(const boost::any& direction, const moveit::core::JointModelGroup* jmg, - moveit::core::RobotState& robot_state) { +static bool getJointStateFromOffset(const boost::any& direction, const Interface::Direction dir, + const moveit::core::JointModelGroup* jmg, moveit::core::RobotState& robot_state) { try { const auto& accepted = jmg->getActiveJointModels(); const auto& joints = boost::any_cast>(direction); + double sign = dir == Interface::Direction::FORWARD ? +1.0 : -1.0; for (const auto& j : joints) { auto jm = robot_state.getRobotModel()->getJointModel(j.first); if (!jm || std::find(accepted.begin(), accepted.end(), jm) == accepted.end()) @@ -92,7 +93,7 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c if (jm->getVariableCount() != 1) throw std::runtime_error("Cannot plan for multi-variable joint '" + j.first + "'"); auto index = jm->getFirstVariableIndex(); - robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second); + robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + sign * j.second); robot_state.enforceBounds(jm); } robot_state.update(); @@ -189,7 +190,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning robot_trajectory::RobotTrajectoryPtr robot_trajectory; bool success = false; - if (getJointStateFromOffset(direction, jmg, scene->getCurrentStateNonConst())) { + if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) { // plan to joint-space target success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); } else { From c57a0bcc1295aa8eaad7de9447185cb7c1b3d68b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 6 Mar 2023 16:29:26 +0100 Subject: [PATCH 24/52] Fixup: Remove moveit/__init__.py during .deb build Fix install path recognition (6671aedeb722f20ddf18a3b95394d1086b51d951). Debian install process targets: /tmp/binarydeb/ros-noetic-moveit-task-constructor-core-/debian/ros-noetic-moveit-task-constructor-core/opt/ros/noetic/lib/python3/dist-packages/moveit --- core/setup.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/core/setup.py b/core/setup.py index b3582ddaf..cd88b3320 100644 --- a/core/setup.py +++ b/core/setup.py @@ -14,10 +14,9 @@ # Remove moveit/__init__.py when building .deb packages # Otherwise, the installation procedure will complain about conflicting files (with moveit_core) try: - libdir = dist.command_obj[ - "install_lib" - ].install_dir # installation dir (.../lib/python3/dist-packages) - if libdir.startswith("/opt/ros"): + # installation dir (.../lib/python3/dist-packages) + libdir = dist.command_obj["install_lib"].install_dir + if "/debian/ros-" in libdir and "moveit-task-constructor-core/opt/ros/" in libdir: import os import shutil From 4781ed563657cb7ecfcca857649bdf9f1c8d95b9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 6 Mar 2023 16:35:31 +0100 Subject: [PATCH 25/52] ros1-0.1.3 --- capabilities/CHANGELOG.rst | 3 +++ capabilities/package.xml | 2 +- core/CHANGELOG.rst | 7 +++++++ core/package.xml | 2 +- demo/CHANGELOG.rst | 5 +++++ demo/package.xml | 2 +- msgs/CHANGELOG.rst | 3 +++ msgs/package.xml | 2 +- rviz_marker_tools/CHANGELOG.rst | 3 +++ rviz_marker_tools/package.xml | 2 +- visualization/CHANGELOG.rst | 3 +++ visualization/package.xml | 2 +- 12 files changed, 30 insertions(+), 6 deletions(-) diff --git a/capabilities/CHANGELOG.rst b/capabilities/CHANGELOG.rst index 70c3ce154..1d1f57f56 100644 --- a/capabilities/CHANGELOG.rst +++ b/capabilities/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_task_constructor_capabilities ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.3 (2023-03-06) +------------------ + 0.1.2 (2023-02-24) ------------------ diff --git a/capabilities/package.xml b/capabilities/package.xml index 7db2754ef..98487b18b 100644 --- a/capabilities/package.xml +++ b/capabilities/package.xml @@ -1,6 +1,6 @@ moveit_task_constructor_capabilities - 0.1.2 + 0.1.3 MoveGroupCapabilites to interact with MoveIt diff --git a/core/CHANGELOG.rst b/core/CHANGELOG.rst index 5fe14dc4d..ae0c53b37 100644 --- a/core/CHANGELOG.rst +++ b/core/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_task_constructor_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.3 (2023-03-06) +------------------ +* MoveRelative: Allow backwards operation for joint-space delta (`#436 `_) +* ComputeIK: Limit collision checking to JMG (`#428 `_) +* Fix: Fetch pybind11 submodule if not yet present +* Contributors: Robert Haschke + 0.1.2 (2023-02-24) ------------------ * Remove moveit/__init__.py during .deb builds to avoid installation conflicts diff --git a/core/package.xml b/core/package.xml index de0b18716..80d3a7062 100644 --- a/core/package.xml +++ b/core/package.xml @@ -1,6 +1,6 @@ moveit_task_constructor_core - 0.1.2 + 0.1.3 MoveIt Task Pipeline https://github.com/ros-planning/moveit_task_constructor diff --git a/demo/CHANGELOG.rst b/demo/CHANGELOG.rst index 344261076..56da3339c 100644 --- a/demo/CHANGELOG.rst +++ b/demo/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_task_constructor_demo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.3 (2023-03-06) +------------------ +* Use const reference instead of reference for ros::NodeHandle (`#437 `_) +* Contributors: Robert Haschke + 0.1.2 (2023-02-24) ------------------ * CartesianPath: Deprecate redundant property setters (`#429 `_) diff --git a/demo/package.xml b/demo/package.xml index 5e69295a0..14dc16db5 100644 --- a/demo/package.xml +++ b/demo/package.xml @@ -1,7 +1,7 @@ moveit_task_constructor_demo - 0.1.2 + 0.1.3 demo tasks illustrating various capabilities of MTC. Robert Haschke diff --git a/msgs/CHANGELOG.rst b/msgs/CHANGELOG.rst index fb883eb2a..945dd4b93 100644 --- a/msgs/CHANGELOG.rst +++ b/msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_task_constructor_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.3 (2023-03-06) +------------------ + 0.1.2 (2023-02-24) ------------------ diff --git a/msgs/package.xml b/msgs/package.xml index fd200537d..41f74146b 100644 --- a/msgs/package.xml +++ b/msgs/package.xml @@ -1,6 +1,6 @@ moveit_task_constructor_msgs - 0.1.2 + 0.1.3 Messages for MoveIt Task Pipeline BSD diff --git a/rviz_marker_tools/CHANGELOG.rst b/rviz_marker_tools/CHANGELOG.rst index dac5227e6..584c508a2 100644 --- a/rviz_marker_tools/CHANGELOG.rst +++ b/rviz_marker_tools/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rviz_marker_tools ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.3 (2023-03-06) +------------------ + 0.1.2 (2023-02-24) ------------------ * Fix marker creation: allow zero scale for geometric shapes (`#430 `_) diff --git a/rviz_marker_tools/package.xml b/rviz_marker_tools/package.xml index 3bbdd9c7e..60f69ca4f 100644 --- a/rviz_marker_tools/package.xml +++ b/rviz_marker_tools/package.xml @@ -1,6 +1,6 @@ rviz_marker_tools - 0.1.2 + 0.1.3 Tools for marker creation / handling BSD diff --git a/visualization/CHANGELOG.rst b/visualization/CHANGELOG.rst index 8b669596b..5c492add0 100644 --- a/visualization/CHANGELOG.rst +++ b/visualization/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_task_constructor_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.3 (2023-03-06) +------------------ + 0.1.2 (2023-02-24) ------------------ diff --git a/visualization/package.xml b/visualization/package.xml index 32e01e212..12c1304a6 100644 --- a/visualization/package.xml +++ b/visualization/package.xml @@ -1,6 +1,6 @@ moveit_task_constructor_visualization - 0.1.2 + 0.1.3 Visualization tools for MoveIt Task Pipeline BSD From f07b81fcd02d318de90a342f2b9cb3f0ad12c7ef Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 24 Apr 2023 11:56:13 +0200 Subject: [PATCH 26/52] cleanup --- core/include/moveit/task_constructor/stages/move_to.h | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h index 5968017b8..bf37e5660 100644 --- a/core/include/moveit/task_constructor/stages/move_to.h +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -66,10 +66,8 @@ class MoveTo : public PropagatingEitherWay void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); } void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link); template - void setIKFrame(const T& p, const std::string& link) { - Eigen::Isometry3d pose; - pose = p; - setIKFrame(pose, link); + void setIKFrame(const T& pose, const std::string& link) { + setIKFrame(Eigen::Isometry3d(pose), link); } void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); } From b346e7eb7816ed5d53ea83bbb61faa6bf3f15405 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 May 2023 15:23:23 +0200 Subject: [PATCH 27/52] Task: findChild() and operator[] should directly operate on stages() (#435) Considering the (fixed) name of the top-level container is meaningless. --- core/include/moveit/task_constructor/task.h | 3 +++ core/test/test_container.cpp | 29 +++++++++++++-------- 2 files changed, 21 insertions(+), 11 deletions(-) diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 36ff5f942..4ede86088 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -85,6 +85,9 @@ class Task : protected WrapperBase const std::string& name() const { return stages()->name(); } void setName(const std::string& name) { stages()->setName(name); } + Stage* findChild(const std::string& name) const { return stages()->findChild(name); } + Stage* operator[](int index) const { return stages()->operator[](index); } + const moveit::core::RobotModelConstPtr& getRobotModel() const; /// setting the robot model also resets the task void setRobotModel(const moveit::core::RobotModelConstPtr& robot_model); diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 7d736bd0b..1a7a9de4c 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -79,20 +79,27 @@ TEST(ContainerBase, positionForInsert) { /* TODO: remove interface as it returns raw pointers */ TEST(ContainerBase, findChild) { - SerialContainer s; + auto s = std::make_unique(); Stage *a, *b, *c1, *d; - s.add(Stage::pointer(a = new NamedStage("a"))); - s.add(Stage::pointer(b = new NamedStage("b"))); - s.add(Stage::pointer(c1 = new NamedStage("c"))); + s->add(Stage::pointer(a = new NamedStage("a"))); + s->add(Stage::pointer(b = new NamedStage("b"))); + s->add(Stage::pointer(c1 = new NamedStage("c"))); auto sub = ContainerBase::pointer(new SerialContainer("c")); sub->add(Stage::pointer(d = new NamedStage("d"))); - s.add(std::move(sub)); - - EXPECT_EQ(s.findChild("a"), a); - EXPECT_EQ(s.findChild("b"), b); - EXPECT_EQ(s.findChild("c"), c1); - EXPECT_EQ(s.findChild("d"), nullptr); - EXPECT_EQ(s.findChild("c/d"), d); + s->add(std::move(sub)); + + EXPECT_EQ(s->findChild("a"), a); + EXPECT_EQ(s->findChild("b"), b); + EXPECT_EQ(s->findChild("c"), c1); + EXPECT_EQ(s->findChild("d"), nullptr); + EXPECT_EQ(s->findChild("c/d"), d); + + Task t("", false, std::move(s)); + EXPECT_EQ(t.findChild("a"), a); + EXPECT_EQ(t.findChild("b"), b); + EXPECT_EQ(t.findChild("c"), c1); + EXPECT_EQ(t.findChild("d"), nullptr); + EXPECT_EQ(t.findChild("c/d"), d); } template From 7f10292ab349da7f1c611d31ca38f4bf8dd16112 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 May 2023 15:29:53 +0200 Subject: [PATCH 28/52] JointInterpolationPlanner: pass optional max_effort property along to GripperCommand (#458) MoveIt passes the effort field of the last trajectory point as the max_effort for a GripperCommand. Thus we pass the max_effort property to the effort field of the trajectory's last waypoint. --- core/src/solvers/joint_interpolation.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index e9727529d..705510cd3 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -51,6 +51,8 @@ using namespace trajectory_processing; JointInterpolationPlanner::JointInterpolationPlanner() { auto& p = properties(); p.declare("max_step", 0.1, "max joint step"); + // allow passing max_effort to GripperCommand actions via + p.declare("max_effort", "max_effort for GripperCommand actions"); } void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {} @@ -95,6 +97,20 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), props.get("max_acceleration_scaling_factor")); + // set max_effort on first and last waypoint (first, because we might reverse the trajectory) + const auto& max_effort = properties().get("max_effort"); + if (!max_effort.empty()) { + double effort = boost::any_cast(max_effort); + for (const auto* jm : jmg->getActiveJointModels()) { + if (jm->getVariableCount() != 1) + continue; + result->getFirstWayPointPtr()->dropAccelerations(); + result->getFirstWayPointPtr()->setJointEfforts(jm, &effort); + result->getLastWayPointPtr()->dropAccelerations(); + result->getLastWayPointPtr()->setJointEfforts(jm, &effort); + } + } + return true; } From ede7cb71cc302932b6fe427c714b84fa725aeae2 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 May 2023 17:02:00 +0200 Subject: [PATCH 29/52] Improve documentation (#431) * Improve general description of stage and container types * Clarify purpose of `CurrentState` stage * Add troubleshooting section --- core/doc/basics.rst | 35 +++++++++++++++++++++-------- core/doc/index.rst | 1 + core/doc/troubleshooting.rst | 31 +++++++++++++++++++++++++ core/python/bindings/src/stages.cpp | 3 ++- 4 files changed, 60 insertions(+), 10 deletions(-) create mode 100644 core/doc/troubleshooting.rst diff --git a/core/doc/basics.rst b/core/doc/basics.rst index edcff535f..cd1e87297 100644 --- a/core/doc/basics.rst +++ b/core/doc/basics.rst @@ -1,30 +1,47 @@ Basic Concepts ============== -The fundamental idea of MTC is that complex motion planning problems can be composed into a set of simpler subproblems. The top-level planning problem is specified as a Task while all subproblems are specified by Stages. Stages can be arranged in any arbitrary order and hierarchy only limited by the individual stages types. The order in which stages can be arranged is restricted by the direction in which results are passed. There are three possible stages relating to the result flow: generator, propagator, and connector stages: +The fundamental idea of MTC is that complex motion planning problems can be composed into a set of simpler subproblems. The top-level planning problem is specified as a Task while all subproblems are specified by Stages. Stages can be arranged in any arbitrary order and hierarchy only limited by the individual stages types. The order in which stages can be arranged is restricted by the direction in which results are passed. There are three possible stage types w.r.t. their data flow: generators, propagators, and connector stages: .. glossary:: Generators - compute their results independently of their neighbor stages and pass them in both directions, backwards and forwards. An example is an IK sampler for geometric poses where approaching and departing motions (neighbor stages) depend on the solution. + compute their results independently of their neighboring stages and pass them in both directions, backwards and forwards. Examples include pose generators, e.g. for grasping or placing, as well as ``ComputeIK``, which computes IK solutions for Cartesian targets. neighboring stages can continue processing from the generated states. The most important generator stage, however, is ``CurrentState``, which provides the current robot state as the starting point for a planning pipeline. Propagators - receive the result of one neighbor stage, solve a subproblem and then propagate their result to the neighbor on the opposite site. Depending on the implementation, propagating stages can pass solutions forward, backward or in both directions separately. An example is a stage that computes a Cartesian path based on either a start or a goal state. + receive the result of *one* neighboring stage as input, plan towards a goal state, and then propagate their result to the opposite interface site. Propagating stages can receive their input on either interface, begin or end. The flow of information (forwards or backwards) is determined by the input interface of the stage. An example is a stage that computes a Cartesian path based on either a start or a goal state. Connectors - do not propagate any results, but rather attempt to bridge the gap between the resulting states of both neighbors. An example is the computation of a free-motion plan from one given state to another. + do not propagate any results, but rather attempt to bridge the gap between the resulting states of both its neighboring stages. It receives input states from both, the begin and end interface and attempts to connect them via a suitable motion plan. Obviously, any pair of input states needs to be *compatible*, i.e. their states (including collision and attached objects as well as joint poses) need to match except for those joints that are part of the given planning group. -Additional to the order types, there are different hierarchy types allowing to encapsulate subordinate stages. Stages without subordinate stages are called primitive stages, higher-level stages are called container stages. There are three container types: +Processing starts from generator stages, expands via propagators, and finally connects partial solution sequences via connector stages. +Obviously, there exist limitations on how stages can be connected to each other. For example, two generator stages cannot occur in sequence as they would both attempt to *write* into their interfaces, but non of them is actually *reading*. Same applies for two connectors in a row: they would both attempt to read, while no stage is actually writing. +The compatibility of stages is automatically checked once before planning by ``Task::init()``. + +To compose a planning pipeline from multiple seemingly independent parts, for example grasping an object and placing it at a new location, one needs to break the typical linear pipeline structure: the place pose is another generator stage (additionally to the ``CurrentState`` stage we are starting from) serving as a seed for the placing sub solution. However, this stage is not completely independent from the grasping sub solution: it should continue where grasping left off, namely with the grasped object attached to the gripper. To convey this state information, the place pose generator should inherit from ``MonitoringGenerator``, which monitors the solutions of another stage in the pipeline in fast-forwards them for further processing in the dependent stage. + +In order to hierarchically organize planning pipelines and to allow for reuse of sub pipelines, e.g. for grasping or placing, one can encapsulate stages within various containers. +Stages without children are called primitive stages. We provide three types of containers: .. glossary:: Wrappers - encapsulate a single subordinate stage and modify or filter the results. For example, a filter stage that only accepts solutions of its child stage that satisfy a certain constraint can be realized as a wrapper. Another standard use of this type includes the IK wrapper stage, which generates inverse kinematics solutions based on planning scenes annotated with a pose target property. + encapsulate a single subordinate stage and modify or filter its results. For example, a filter stage that only accepts solutions of its child stage that satisfy a certain constraint can be realized as a wrapper. Another standard use of this type includes the IK wrapper stage, which generates inverse kinematics solutions based on planning scenes annotated with a pose target property. - Serial Containers + Serial containers hold a sequence of subordinate stages and only consider end-to-end solutions as results. An example is a picking motion that consists of a sequence of coherent steps. - Parallel Containers - combine set of subordinate stages and can be used for passing the best of alternative results, running fallback solvers or for merging multiple independent solutions. Examples are running alternative planners for a free-motion plan, picking objects with the right hand or with the left hand as a fallback, or moving the arm and opening the gripper at the same time. + Parallel containers + consider the solutions of all their children as feasible. Different sub types are available, namely: + + ``Alternative`` container + processes all children simultaneously (currently in a round-robin fashion). For example, one could plan a grasping sequence for a left and right arm in parallel if the actual choice of the arm doesn't matter for the task. + + ``Fallback`` container + processes their children sequentially: only if the current child has exhausted all its solution candidates (and didn't produce any feasible solution), the next child is considered. + Use this container, e.g. if you prefer grasping with the right arm, but allow falling back to the left if really needed. + + ``Merger`` container + processes their children simultaneously and combine their solutions into an joint solution. It is assumed that children operate on disjoint joint model groups, e.g. the arm and the hand, such that their solution trajectories can be executed in parallel after being merged. Stages not only support solving motion planning problems. They can also be used for all kinds of state transitions, as for instance modifying the planning scene. Combined with the possibility of using class inheritance it is possible to construct very complex behavior while only relying on a well-structured set of primitive stages. diff --git a/core/doc/index.rst b/core/doc/index.rst index c80cedc52..75f0c94af 100644 --- a/core/doc/index.rst +++ b/core/doc/index.rst @@ -28,3 +28,4 @@ Organization of the documentation concepts howto api + troubleshooting diff --git a/core/doc/troubleshooting.rst b/core/doc/troubleshooting.rst new file mode 100644 index 000000000..8ee9f0b90 --- /dev/null +++ b/core/doc/troubleshooting.rst @@ -0,0 +1,31 @@ +.. _sec-troubleshooting: + +Troubleshooting +=============== + +``Task::init()`` reports mismatching interfaces +----------------------------------------------- + +Before planning, the planning pipeline is checked for consistency. Each stage type has a specific flow interface, e.g. generator-type stages write into both, their begin and end interface, propagator-type stages read from one and write to the opposite, and connector-type stages read from both sides. Obviously these interfaces need to match. If they don't, an ``InitStageException`` is thrown. Per default, this is not very verbose, but you can use the following code snippet to get some more info: + + .. code-block:: c + + try { + task.plan(); + } catch (const InitStageException& e) { + std::cerr << e << std::endl; + throw; + } + +For example, the following pipeline: + +- ↕ current +- ⛓ connect +- ↑ moveTo + +throws the error: ``task pipeline: end interface (←) of 'moveto' does not match mine (→)``. + +The validation process runs sequentially through a ``SerialContainer``. Here, ``current``, as a generator stage is compatible to ``connect``, writing to the interface read by ``connect``. +``moveTo`` as a propagator can operate, in principle, forwards and backwards. By default, the operation direction is inferred automatically. Here, as ``connect`` requires a reading end-interface, moveTo should seemingly operate backwards. However, now the whole pipeline is incompatible to the enclosing container's interface: a task requires a generator-type interface as it generates solutions - there is no input interface to read from. Hence, the *reading* end interface (←) of ``moveto`` conflicts with a *writing* end interface of the task. + +Obviously, in a ``ParallelContainer`` all (direct) children need to share a common interface. diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index 99fce8922..01d3e9f6f 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -117,7 +117,8 @@ void export_stages(pybind11::module& m) { )", "collision_object"_a); properties::class_(m, "CurrentState", R"( - Fetch the current PlanningScene state via the ``get_planning_scene`` service. + Fetch the current PlanningScene / real robot state via the ``get_planning_scene`` service. + Usually, this stage should be used *once* at the beginning of your pipeline to start from the current state. .. literalinclude:: ./../../../demo/scripts/current_state.py :language: python From d1a6916206067980aa154def8f2d45a30f70225e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 May 2023 17:07:11 +0200 Subject: [PATCH 30/52] Stage::explainFailure() (#445) ... to facilitate spotting the stage causing a task to fail --- core/include/moveit/task_constructor/container.h | 1 + core/include/moveit/task_constructor/stage.h | 2 ++ core/include/moveit/task_constructor/task.h | 3 +++ core/src/container.cpp | 15 +++++++++++++++ core/src/stage.cpp | 5 +++++ core/src/task.cpp | 12 ++++++++++-- 6 files changed, 36 insertions(+), 2 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 6f142e9ac..2dcd325f4 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -76,6 +76,7 @@ class ContainerBase : public Stage virtual bool canCompute() const = 0; virtual void compute() = 0; + void explainFailure(std::ostream& os) const override; /// called by a (direct) child when a new solution becomes available virtual void onNewSolution(const SolutionBase& s) = 0; diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index be2d446a5..4d1a6706f 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -232,6 +232,8 @@ class Stage /// Should we generate failure solutions? Note: Always report a failure! bool storeFailures() const; + virtual void explainFailure(std::ostream& os) const; + /// Get the stage's property map PropertyMap& properties(); const PropertyMap& properties() const { return const_cast(this)->properties(); } diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 4ede86088..91d49e733 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -132,6 +132,9 @@ class Task : protected WrapperBase /// print current task state (number of found solutions and propagated states) to std::cout void printState(std::ostream& os = std::cout) const; + /// print an explanation for a planning failure to os + void explainFailure(std::ostream& os = std::cout) const override; + size_t numSolutions() const { return solutions().size(); } const ordered& solutions() const { return stages()->solutions(); } const std::list& failures() const { return stages()->failures(); } diff --git a/core/src/container.cpp b/core/src/container.cpp index b9043ebfe..cac8dd665 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -446,6 +446,20 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) { throw errors; } +void ContainerBase::explainFailure(std::ostream& os) const { + for (const auto& stage : pimpl()->children()) { + if (!stage->solutions().empty()) + continue; // skip deeper traversal, this stage produced solutions + if (stage->numFailures()) { + os << stage->name() << " (0/" << stage->numFailures() << ")"; + stage->explainFailure(os); + os << std::endl; + break; + } + stage->explainFailure(os); // recursively process children + } +} + std::ostream& operator<<(std::ostream& os, const ContainerBase& container) { ContainerBase::StageCallback processor = [&os](const Stage& stage, unsigned int depth) -> bool { os << std::string(2 * depth, ' ') << *stage.pimpl() << std::endl; @@ -690,6 +704,7 @@ void SerialContainer::compute() { } } + ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name) : ContainerBasePrivate(me, name) {} diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 6e031aef2..3298b3ad5 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -428,6 +428,11 @@ bool Stage::storeFailures() const { return pimpl()->storeFailures(); } +void Stage::explainFailure(std::ostream& os) const { + if (!failures().empty()) + os << ": " << failures().front()->comment(); +} + PropertyMap& Stage::properties() { return pimpl()->properties_; } diff --git a/core/src/task.cpp b/core/src/task.cpp index 243205262..4dc94e26a 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -237,9 +237,12 @@ moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) { init(); // Print state and return success if there are solutions otherwise the input error_code - const auto success_or = [this](const int32_t error_code) { + const auto success_or = [this](const int32_t error_code) -> int32_t { + if (numSolutions() > 0) + return moveit::core::MoveItErrorCode::SUCCESS; printState(); - return numSolutions() > 0 ? moveit::core::MoveItErrorCode::SUCCESS : error_code; + explainFailure(); + return error_code; }; impl->preempt_requested_ = false; const double available_time = timeout(); @@ -313,5 +316,10 @@ const core::RobotModelConstPtr& Task::getRobotModel() const { void Task::printState(std::ostream& os) const { os << *stages(); } + +void Task::explainFailure(std::ostream& os) const { + os << "Failing stage(s):\n"; + stages()->explainFailure(os); +} } // namespace task_constructor } // namespace moveit From 1002d8494648f7680738fd0efe9ffd8cb2f6f185 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 May 2023 23:22:49 +0200 Subject: [PATCH 31/52] Add python binding for Task::insert --- core/python/bindings/src/core.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp index d216e268f..c2dad580d 100644 --- a/core/python/bindings/src/core.cpp +++ b/core/python/bindings/src/core.cpp @@ -440,6 +440,7 @@ void export_core(pybind11::module& m) { t.add(it->cast()); }, "Append stage(s) to the task's top-level container") + .def("insert", &Task::insert, "stage"_a, "before"_a = -1, "Insert stage before given index") .def("__len__", [](const Task& t) { t.stages()->numChildren(); }) .def( "__getitem__", From 84cb880a9d8eceb102fce8d46dda7671ac0bf724 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 5 May 2023 00:14:06 +0200 Subject: [PATCH 32/52] Add python binding for ModifyPlanningScene::allowCollisions(std::string, bool) --- core/python/bindings/src/stages.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index 01d3e9f6f..fa134efa2 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -105,6 +105,8 @@ void export_stages(pybind11::module& m) { const std::string& attach_link) { self.attachObjects(elementOrList(names), attach_link, false); }, "Detach multiple objects from a robot link", "names"_a, "attach_link"_a) + .def("allowCollisions", [](ModifyPlanningScene& self, const std::string& object, bool allow) {self.allowCollisions(object, allow);}, + "Allow or disable all collisions involving the given object", "object"_a, "enable_collision"_a = true) .def("allowCollisions", [](ModifyPlanningScene& self, const py::object& first, const py::object& second, bool enable_collision) { self.allowCollisions(elementOrList(first), elementOrList(second), enable_collision); From b318c3cae91daffb0ae6e679d5dd01e5157da3ef Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 4 May 2023 23:23:20 +0200 Subject: [PATCH 33/52] Basic Move test: MoveRelative + MoveTo --- core/python/test/rostest_mtc.py | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/core/python/test/rostest_mtc.py b/core/python/test/rostest_mtc.py index 54e38e645..02d697e7f 100755 --- a/core/python/test/rostest_mtc.py +++ b/core/python/test/rostest_mtc.py @@ -22,23 +22,21 @@ def setUpClass(self): def tearDown(self): pass - def test_MoveRelative(self): - task = core.Task() - task.add(stages.CurrentState("current")) - move = stages.MoveRelative("move", core.JointInterpolationPlanner()) - move.group = self.PLANNING_GROUP - move.setDirection({"joint_1": 0.2, "joint_2": 0.4}) - task.add(move) + def test_MoveAndExecute(self): + moveRel = stages.MoveRelative("moveRel", core.JointInterpolationPlanner()) + moveRel.group = self.PLANNING_GROUP + moveRel.setDirection({"joint_1": 0.2, "joint_2": 0.4}) - task.enableIntrospection() - task.init() - task.plan() + moveTo = stages.MoveTo("moveTo", core.JointInterpolationPlanner()) + moveTo.group = self.PLANNING_GROUP + moveTo.setGoal("all-zeros") + + task = core.Task() + task.add(stages.CurrentState("current"), moveRel, moveTo) + self.assertTrue(task.plan()) self.assertEqual(len(task.solutions), 1) - for s in task.solutions: - print(s) - s = task.solutions[0] - task.execute(s) + task.execute(task.solutions[0]) def test_Merger(self): cartesian = core.CartesianPath() From 45ca1a67a98eefe97b81dd47ca16df96b5e2d550 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 5 May 2023 00:24:45 +0200 Subject: [PATCH 34/52] TestModifyPlanningScene --- core/python/test/rostest_mps.py | 67 +++++++++++++++++++++++++++++++ core/python/test/rostest_mtc.py | 8 ---- core/python/test/rostest_mtc.test | 1 + 3 files changed, 68 insertions(+), 8 deletions(-) create mode 100755 core/python/test/rostest_mps.py diff --git a/core/python/test/rostest_mps.py b/core/python/test/rostest_mps.py new file mode 100755 index 000000000..3d6b741a7 --- /dev/null +++ b/core/python/test/rostest_mps.py @@ -0,0 +1,67 @@ +#! /usr/bin/env python + +from __future__ import print_function +import unittest +import rostest +from moveit_commander.roscpp_initializer import roscpp_initialize +from moveit_commander import PlanningSceneInterface +from moveit.task_constructor import core, stages +from geometry_msgs.msg import PoseStamped + + +def make_pose(x, y, z): + pose = PoseStamped() + pose.header.frame_id = "base_link" + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = z + pose.pose.orientation.w = 1.0 + return pose + + +class TestModifyPlanningScene(unittest.TestCase): + PLANNING_GROUP = "manipulator" + + def setUp(self): + super(TestModifyPlanningScene, self).setUp() + self.psi = PlanningSceneInterface(synchronous=True) + # insert a box to collide with + self.psi.add_box("box", make_pose(0.8, 0.25, 1.25), [0.2, 0.2, 0.2]) + + # colliding motion + move = stages.MoveRelative("move", core.JointInterpolationPlanner()) + move.group = self.PLANNING_GROUP + move.setDirection({"joint_1": 0.3}) + + self.task = task = core.Task() + task.add(stages.CurrentState("current"), move) + + def test_collision(self): + self.assertFalse(self.task.plan()) + + def test_allow_collision_list(self): + mps = stages.ModifyPlanningScene("mps") + mps.allowCollisions("box", ["link_4", "link_5", "link_6"], True) + self.task.insert(mps, 1) + self.assertTrue(self.task.plan()) + + def test_allow_collision_all(self): + # insert an extra collision object that is unknown to ACM + self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]) + # attach box to end effector + self.psi.attach_box("link_6", "box") + mps = stages.ModifyPlanningScene("mps") + self.assertFalse(self.task.plan()) + + # allow all collisions for attached "box" object + mps.allowCollisions("box", True) + self.task.insert(mps, 1) + self.assertTrue(self.task.plan()) + # restore original state + self.psi.remove_attached_object("link_6", "box") + self.psi.remove_world_object("block") + + +if __name__ == "__main__": + roscpp_initialize("test_mtc") + rostest.rosrun("mtc", "mps", TestModifyPlanningScene) diff --git a/core/python/test/rostest_mtc.py b/core/python/test/rostest_mtc.py index 02d697e7f..958066ad6 100755 --- a/core/python/test/rostest_mtc.py +++ b/core/python/test/rostest_mtc.py @@ -14,14 +14,6 @@ class Test(unittest.TestCase): PLANNING_GROUP = "manipulator" - @classmethod - def setUpClass(self): - pass - - @classmethod - def tearDown(self): - pass - def test_MoveAndExecute(self): moveRel = stages.MoveRelative("moveRel", core.JointInterpolationPlanner()) moveRel.group = self.PLANNING_GROUP diff --git a/core/python/test/rostest_mtc.test b/core/python/test/rostest_mtc.test index b028af75c..0c34d9a51 100644 --- a/core/python/test/rostest_mtc.test +++ b/core/python/test/rostest_mtc.test @@ -1,5 +1,6 @@ + From d59acdb96913e63f64ffee94295fa27821e35fea Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 6 May 2023 20:08:15 +0200 Subject: [PATCH 35/52] Fix allowCollisions(object, enable_collision) Call ACM::setDefaultEntry to also cover objects that are not yet known to the ACM. --- core/src/stages/modify_planning_scene.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index e222f07a2..d26fe0294 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -111,8 +111,10 @@ void ModifyPlanningScene::allowCollisions(planning_scene::PlanningScene& scene, collision_detection::AllowedCollisionMatrix& acm = scene.getAllowedCollisionMatrixNonConst(); bool allow = invert ? !pairs.allow : pairs.allow; if (pairs.second.empty()) { - for (const auto& name : pairs.first) + for (const auto& name : pairs.first) { + acm.setDefaultEntry(name, allow); acm.setEntry(name, allow); + } } else acm.setEntry(pairs.first, pairs.second, allow); } From dee73b2ddefd365fe173a83c48bcf8625684b349 Mon Sep 17 00:00:00 2001 From: VideoSystemsTech <61198558+VideoSystemsTech@users.noreply.github.com> Date: Tue, 16 May 2023 14:32:17 +0200 Subject: [PATCH 36/52] Expose argument of PipelinePlanner's constructor to Python (#462) --- core/python/bindings/src/solvers.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index e56c03ebf..a1d3b405f 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -39,6 +39,7 @@ #include namespace py = pybind11; +using namespace py::literals; using namespace moveit::task_constructor; using namespace moveit::task_constructor::solvers; @@ -79,7 +80,7 @@ void export_solvers(py::module& m) { .property("goal_orientation_tolerance", "float: Tolerance for reaching orientation goals") .property("display_motion_plans", "bool: Publish generated solutions via a topic") .property("publish_planning_requests", "bool: Publish motion planning requests via a topic") - .def(py::init<>()); + .def(py::init(), "pipeline"_a = std::string("ompl")); properties::class_( m, "JointInterpolationPlanner", From 4d2a5714fabf9eaa6fa57ce15ab3d525119a97b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Wed, 17 May 2023 19:55:18 +0200 Subject: [PATCH 37/52] Use pluginlib consistently (#463) --- capabilities/src/execute_task_solution_capability.cpp | 4 ++-- core/src/stages/plugins.cpp | 2 +- visualization/motion_planning_tasks/src/plugin_init.cpp | 2 +- visualization/motion_planning_tasks/src/pluginlib_factory.h | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index af0e83562..e6207dfe2 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -191,5 +191,5 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr } // namespace move_group -#include -CLASS_LOADER_REGISTER_CLASS(move_group::ExecuteTaskSolutionCapability, move_group::MoveGroupCapability) +#include +PLUGINLIB_EXPORT_CLASS(move_group::ExecuteTaskSolutionCapability, move_group::MoveGroupCapability) diff --git a/core/src/stages/plugins.cpp b/core/src/stages/plugins.cpp index 9b9864f8f..2c977174c 100644 --- a/core/src/stages/plugins.cpp +++ b/core/src/stages/plugins.cpp @@ -1,6 +1,6 @@ #include -#include +#include /// register plugins to use with ClassLoader diff --git a/visualization/motion_planning_tasks/src/plugin_init.cpp b/visualization/motion_planning_tasks/src/plugin_init.cpp index 1bf297082..375a93510 100644 --- a/visualization/motion_planning_tasks/src/plugin_init.cpp +++ b/visualization/motion_planning_tasks/src/plugin_init.cpp @@ -34,7 +34,7 @@ /* Author: Robert Haschke */ -#include +#include #include "task_display.h" #include "task_panel.h" diff --git a/visualization/motion_planning_tasks/src/pluginlib_factory.h b/visualization/motion_planning_tasks/src/pluginlib_factory.h index 77b91206a..457c40408 100644 --- a/visualization/motion_planning_tasks/src/pluginlib_factory.h +++ b/visualization/motion_planning_tasks/src/pluginlib_factory.h @@ -45,7 +45,7 @@ #include #ifndef Q_MOC_RUN -#include +#include #include #include #endif From 6a01550e8df6408ae7df18909dea1efa88dc79a9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 17 May 2023 21:01:57 +0200 Subject: [PATCH 38/52] ComputeIK: update RobotState before calling setFromIK() This became necessary due to https://github.com/ros-planning/moveit/issues/3388. --- core/src/stages/compute_ik.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 5ce320b49..86307d0c0 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -388,8 +388,10 @@ void ComputeIK::compute() { double remaining_time = timeout(); auto start_time = std::chrono::steady_clock::now(); while (ik_solutions.size() < max_ik_solutions && remaining_time > 0) { - if (tried_current_state_as_seed) + if (tried_current_state_as_seed) { sandbox_state.setToRandomPositions(jmg); + sandbox_state.update(); + } tried_current_state_as_seed = true; size_t previous = ik_solutions.size(); From 1daef934ee9f8a8433c989988a5b1b480261cb04 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 15 May 2023 21:13:15 +0200 Subject: [PATCH 39/52] Add python binding for ModifyPlanningScene::removeObject --- core/python/bindings/src/stages.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index fa134efa2..191e13d96 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -116,7 +116,10 @@ void export_stages(pybind11::module& m) { .. _CollisionObject: https://docs.ros.org/en/melodic/api/moveit_msgs/html/msg/CollisionObject.html - )", "collision_object"_a); + )", "collision_object"_a) + .def("removeObject", &ModifyPlanningScene::removeObject, + "Remove a CollisionObject_ from the planning scene", "name"_a) + ; properties::class_(m, "CurrentState", R"( Fetch the current PlanningScene / real robot state via the ``get_planning_scene`` service. From bd400de184909579ab13cc8886c30e506208b48f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 16 May 2023 01:15:26 +0200 Subject: [PATCH 40/52] Fix add/remove object in backward operation - addObject() will actually remove the object from scene - removeObject() is not supported (we would need to know which object to add) --- .../stages/modify_planning_scene.h | 4 +- core/python/test/rostest_mps.py | 57 +++++++++++++++++-- core/src/stages/modify_planning_scene.cpp | 57 ++++++++++++------- 3 files changed, 89 insertions(+), 29 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h index 80ec9d4a3..c5570922e 100644 --- a/core/include/moveit/task_constructor/stages/modify_planning_scene.h +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -149,8 +149,8 @@ class ModifyPlanningScene : public PropagatingEitherWay protected: // apply stored modifications to scene - InterfaceState apply(const InterfaceState& from, bool invert); - void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::CollisionObject& object); + std::pair apply(const InterfaceState& from, bool invert); + void processCollisionObject(planning_scene::PlanningScene& scene, moveit_msgs::CollisionObject& object, bool invert); void attachObjects(planning_scene::PlanningScene& scene, const std::pair>& pair, bool invert); void allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs, bool invert); diff --git a/core/python/test/rostest_mps.py b/core/python/test/rostest_mps.py index 3d6b741a7..1a9675031 100755 --- a/core/python/test/rostest_mps.py +++ b/core/python/test/rostest_mps.py @@ -25,24 +25,28 @@ class TestModifyPlanningScene(unittest.TestCase): def setUp(self): super(TestModifyPlanningScene, self).setUp() self.psi = PlanningSceneInterface(synchronous=True) + self.make_box = self.psi._PlanningSceneInterface__make_box # insert a box to collide with self.psi.add_box("box", make_pose(0.8, 0.25, 1.25), [0.2, 0.2, 0.2]) + self.task = task = core.Task() + task.add(stages.CurrentState("current")) + def insertMove(self, position=-1): # colliding motion move = stages.MoveRelative("move", core.JointInterpolationPlanner()) move.group = self.PLANNING_GROUP move.setDirection({"joint_1": 0.3}) - - self.task = task = core.Task() - task.add(stages.CurrentState("current"), move) + self.task.insert(move, position) def test_collision(self): + self.insertMove() self.assertFalse(self.task.plan()) def test_allow_collision_list(self): - mps = stages.ModifyPlanningScene("mps") + mps = stages.ModifyPlanningScene("allow specific collisions for box") mps.allowCollisions("box", ["link_4", "link_5", "link_6"], True) - self.task.insert(mps, 1) + self.task.add(mps) + self.insertMove() self.assertTrue(self.task.plan()) def test_allow_collision_all(self): @@ -50,10 +54,11 @@ def test_allow_collision_all(self): self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]) # attach box to end effector self.psi.attach_box("link_6", "box") - mps = stages.ModifyPlanningScene("mps") + self.insertMove() self.assertFalse(self.task.plan()) # allow all collisions for attached "box" object + mps = stages.ModifyPlanningScene("allow all collisions for box") mps.allowCollisions("box", True) self.task.insert(mps, 1) self.assertTrue(self.task.plan()) @@ -61,6 +66,46 @@ def test_allow_collision_all(self): self.psi.remove_attached_object("link_6", "box") self.psi.remove_world_object("block") + def test_fw_add_object(self): + mps = stages.ModifyPlanningScene("addObject(block)") + mps.addObject(self.make_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])) + self.task.add(mps) + self.insertMove() + self.assertFalse(self.task.plan()) + + def test_fw_remove_object(self): + mps = stages.ModifyPlanningScene("removeObject(box)") + mps.removeObject("box") + self.task.insert(mps, 1) + self.assertTrue(self.task.plan()) + s = self.task.solutions[0].toMsg() + self.assertEqual(s.sub_trajectory[1].scene_diff.world.collision_objects[0].id, "box") + + def test_bw_add_object(self): + # add object to move_group's planning scene + self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]) + + # backward operation will actually remove the object + mps = stages.ModifyPlanningScene("addObject(block) backwards") + mps.addObject(self.make_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])) + self.task.insert(mps, 0) + self.insertMove(0) + self.assertTrue(self.task.plan()) + + self.psi.remove_world_object("block") # restore original state + + s = self.task.solutions[0].toMsg() + for ps in [s.start_scene] + [s.sub_trajectory[i].scene_diff for i in [0, 1]]: + objects = [o.id for o in ps.world.collision_objects] + self.assertTrue(objects == ["block", "box"]) + + def test_bw_remove_object(self): + mps = stages.ModifyPlanningScene("removeObject(box) backwards") + mps.removeObject("box") + self.task.insert(mps, 0) + self.insertMove(0) + self.assertFalse(self.task.plan()) + if __name__ == "__main__": roscpp_initialize("test_mtc") diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index d26fe0294..319dba851 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -84,11 +84,13 @@ void ModifyPlanningScene::allowCollisions(const std::string& first, const moveit } void ModifyPlanningScene::computeForward(const InterfaceState& from) { - sendForward(from, apply(from, false), SubTrajectory()); + auto result = apply(from, false); + sendForward(from, std::move(result.first), std::move(result.second)); } void ModifyPlanningScene::computeBackward(const InterfaceState& to) { - sendBackward(apply(to, true), to, SubTrajectory()); + auto result = apply(to, true); + sendBackward(std::move(result.first), to, std::move(result.second)); } void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene& scene, @@ -121,30 +123,43 @@ void ModifyPlanningScene::allowCollisions(planning_scene::PlanningScene& scene, // invert indicates, whether to detach instead of attach (and vice versa) // as well as to forbid instead of allow collision (and vice versa) -InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool invert) { +std::pair ModifyPlanningScene::apply(const InterfaceState& from, bool invert) { planning_scene::PlanningScenePtr scene = from.scene()->diff(); - InterfaceState result(scene); - // add/remove objects - for (const auto& collision_object : collision_objects_) - processCollisionObject(*scene, collision_object); - - // attach/detach objects - for (const auto& pair : attach_objects_) - attachObjects(*scene, pair, invert); - - // allow/forbid collisions - for (const auto& pairs : collision_matrix_edits_) - allowCollisions(*scene, pairs, invert); - - if (callback_) - callback_(scene, properties()); - - return result; + InterfaceState state(scene); + SubTrajectory traj; + try { + // add/remove objects + for (auto& collision_object : collision_objects_) + processCollisionObject(*scene, collision_object, invert); + + // attach/detach objects + for (const auto& pair : attach_objects_) + attachObjects(*scene, pair, invert); + + // allow/forbid collisions + for (const auto& pairs : collision_matrix_edits_) + allowCollisions(*scene, pairs, invert); + + if (callback_) + callback_(scene, properties()); + } catch (const std::exception& e) { + traj.markAsFailure(e.what()); + } + return std::make_pair(state, traj); } void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene& scene, - const moveit_msgs::CollisionObject& object) { + moveit_msgs::CollisionObject& object, bool invert) { + auto op = object.operation; + if (invert) { + if (op == moveit_msgs::CollisionObject::ADD) + op = moveit_msgs::CollisionObject::REMOVE; + else if (op == moveit_msgs::CollisionObject::REMOVE) + throw std::runtime_error("cannot apply removeObject() backwards"); + } + scene.processCollisionObjectMsg(object); + object.operation = op; } } // namespace stages } // namespace task_constructor From 397fc070ea87594c0fc9a01b1196dd65b3064a4d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 16 May 2023 09:01:07 +0200 Subject: [PATCH 41/52] Fix SolutionBase::fillMessage(): also write start_scene This method was only doing half of the job, namely adding subsolutions to the message fields. However, the start_scene was not yet written. This was handled manually in some but not all callers. To avoid this inconsistency, the new method toMsg() takes care of both actions now, while the old fillMessage() method was renamed to appendTo(). --- core/include/moveit/task_constructor/storage.h | 14 ++++++++------ core/python/bindings/src/core.cpp | 6 +++--- core/src/introspection.cpp | 4 +--- core/src/storage.cpp | 16 ++++++++++------ core/src/task.cpp | 3 +-- demo/src/pick_place_task.cpp | 2 +- 6 files changed, 24 insertions(+), 21 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 23d561783..0b7dd54cb 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -311,9 +311,11 @@ class SolutionBase auto& markers() { return markers_; } const auto& markers() const { return markers_; } + /// convert solution to message + void toMsg(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection = nullptr) const; /// append this solution to Solution msg - virtual void fillMessage(moveit_task_constructor_msgs::Solution& solution, - Introspection* introspection = nullptr) const = 0; + virtual void appendTo(moveit_task_constructor_msgs::Solution& solution, + Introspection* introspection = nullptr) const = 0; void fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection = nullptr) const; /// required to dispatch to type-specific CostTerm methods via vtable @@ -354,7 +356,7 @@ class SubTrajectory : public SolutionBase robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; } void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; } - void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override; + void appendTo(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override; double computeCost(const CostTerm& cost, std::string& comment) const override; @@ -387,7 +389,7 @@ class SolutionSequence : public SolutionBase void push_back(const SolutionBase& solution); /// append all subsolutions to solution - void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; + void appendTo(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; double computeCost(const CostTerm& cost, std::string& comment) const override; @@ -418,8 +420,8 @@ class WrappedSolution : public SolutionBase : SolutionBase(creator, cost), wrapped_(wrapped) {} explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped) : WrappedSolution(creator, wrapped, wrapped->cost()) {} - void fillMessage(moveit_task_constructor_msgs::Solution& solution, - Introspection* introspection = nullptr) const override; + void appendTo(moveit_task_constructor_msgs::Solution& solution, + Introspection* introspection = nullptr) const override; double computeCost(const CostTerm& cost, std::string& comment) const override; diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp index c2dad580d..d3f953aeb 100644 --- a/core/python/bindings/src/core.cpp +++ b/core/python/bindings/src/core.cpp @@ -124,9 +124,9 @@ void export_core(pybind11::module& m) { ":visualization_msgs:`Marker`: Markers to visualize important aspects of the trajectory (read-only)") .def( "toMsg", - [](const SolutionBasePtr& s) { + [](const SolutionBase& self) { moveit_task_constructor_msgs::Solution msg; - s->fillMessage(msg); + self.toMsg(msg); return msg; }, "Convert to the ROS message ``Solution``"); @@ -495,7 +495,7 @@ void export_core(pybind11::module& m) { MoveGroupInterface::Plan plan; moveit_task_constructor_msgs::Solution serialized; - solution->fillMessage(serialized); + solution->toMsg(serialized); for (const moveit_task_constructor_msgs::SubTrajectory& traj : serialized.sub_trajectory) { if (!traj.trajectory.joint_trajectory.points.empty()) { diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 588b9e99d..7dec53554 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -146,9 +146,7 @@ void Introspection::registerSolution(const SolutionBase& s) { } void Introspection::fillSolution(moveit_task_constructor_msgs::Solution& msg, const SolutionBase& s) { - s.fillMessage(msg, this); - s.start()->scene()->getPlanningSceneMsg(msg.start_scene); - + s.toMsg(msg, this); msg.task_id = impl->task_id_; } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index e59fda669..6804997d2 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -204,6 +204,11 @@ void SolutionBase::markAsFailure(const std::string& msg) { } } +void SolutionBase::toMsg(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { + appendTo(msg, introspection); + start()->scene()->getPlanningSceneMsg(msg.start_scene); +} + void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection) const { info.id = introspection ? introspection->solutionId(*this) : 0; info.cost = this->cost(); @@ -216,7 +221,7 @@ void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, In std::copy(markers.begin(), markers.end(), info.markers.begin()); } -void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { +void SubTrajectory::appendTo(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { msg.sub_trajectory.emplace_back(); moveit_task_constructor_msgs::SubTrajectory& t = msg.sub_trajectory.back(); SolutionBase::fillInfo(t.info, introspection); @@ -235,7 +240,7 @@ void SolutionSequence::push_back(const SolutionBase& solution) { subsolutions_.push_back(&solution); } -void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { +void SolutionSequence::appendTo(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { moveit_task_constructor_msgs::SubSolution sub_msg; SolutionBase::fillInfo(sub_msg.info, introspection); @@ -257,7 +262,7 @@ void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, msg.sub_trajectory.reserve(msg.sub_trajectory.size() + subsolutions_.size()); for (const SolutionBase* s : subsolutions_) { size_t current = msg.sub_trajectory.size(); - s->fillMessage(msg, introspection); + s->appendTo(msg, introspection); // zero IDs of sub solutions with same creator as this if (s->creator() == this->creator()) { @@ -274,9 +279,8 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co return f(*this, comment); } -void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution, - Introspection* introspection) const { - wrapped_->fillMessage(solution, introspection); +void WrappedSolution::appendTo(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection) const { + wrapped_->appendTo(solution, introspection); // prepend this solutions info as a SubSolution msg moveit_task_constructor_msgs::SubSolution sub_msg; diff --git a/core/src/task.cpp b/core/src/task.cpp index 4dc94e26a..d316fde33 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -270,8 +270,7 @@ moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) { ac.waitForServer(); moveit_task_constructor_msgs::ExecuteTaskSolutionGoal goal; - s.fillMessage(goal.solution, pimpl()->introspection_.get()); - s.start()->scene()->getPlanningSceneMsg(goal.solution.start_scene); + s.toMsg(goal.solution, pimpl()->introspection_.get()); ac.sendGoal(goal); ac.waitForResult(); diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index f9732dd27..1428eacc4 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -502,7 +502,7 @@ bool PickPlaceTask::execute() { // actionlib::SimpleActionClient // execute("execute_task_solution", true); execute.waitForServer(); // moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; - // task_->solutions().front()->fillMessage(execute_goal.solution); + // task_->solutions().front()->toMsg(execute_goal.solution); // execute.sendGoalAndWait(execute_goal); // execute_result = execute.getResult()->error_code; From 76d293863bec6091fed252889600f21b706e35b2 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 May 2023 08:48:28 +0200 Subject: [PATCH 42/52] introspection: remove any invalid ROS-name chars from hostname (#465) --- core/src/introspection.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 588b9e99d..0148edbcd 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -49,6 +49,12 @@ #include #include +namespace ros { +namespace names { +bool isValidCharInName(char c); // unfortunately this is not declared in ros/names.h +} // namespace names +} // namespace ros + namespace moveit { namespace task_constructor { @@ -57,8 +63,10 @@ std::string getTaskId(const TaskPrivate* task) { std::ostringstream oss; char our_hostname[256] = { 0 }; gethostname(our_hostname, sizeof(our_hostname) - 1); - // Hostname could have `-` as a character but this is an invalid character in ROS so we replace it with `_` - std::replace(std::begin(our_hostname), std::end(our_hostname), '-', '_'); + // Replace all invalid ROS-name chars with an underscore + std::replace_if( + our_hostname, our_hostname + strlen(our_hostname), + [](const char ch) { return !ros::names::isValidCharInName(ch); }, '_'); oss << our_hostname << "_" << getpid() << "_" << reinterpret_cast(task); return oss.str(); } From 45ff3ea437b9b2785a22ed8c44f5a1ba6d683970 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 8 Jul 2022 11:02:26 +0000 Subject: [PATCH 43/52] Improve cmake - Modernize - Add include folder for INSTALL_INTERFACE --- core/CMakeLists.txt | 3 +-- core/src/CMakeLists.txt | 5 +++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 7db5914ff..d10d2f5ba 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -34,8 +34,7 @@ install(DIRECTORY include/ DESTINATION include pluginlib_export_plugin_description_file(${PROJECT_NAME} motion_planning_stages_plugin_description.xml) -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME} ${PROJECT_NAME}_stages) +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) ament_export_dependencies(Boost) ament_export_dependencies(geometry_msgs) ament_export_dependencies(moveit_core) diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index f844f89e4..5c7f5be91 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -47,12 +47,13 @@ ament_target_dependencies(${PROJECT_NAME} ) target_include_directories(${PROJECT_NAME} PUBLIC $ - PUBLIC $ + $ + $ ) add_subdirectory(stages) install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}Targets ARCHIVE DESTINATION lib LIBRARY DESTINATION lib) From 78da3e46e6de845849f89f8b4ef8ef172dad9adb Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 25 May 2023 17:49:23 +0200 Subject: [PATCH 44/52] Gracefully handle NULL robot_trajectory (#469) --- core/src/stages/move_relative.cpp | 58 ++++++++++++++++--------------- 1 file changed, 30 insertions(+), 28 deletions(-) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 930385149..2f5acb746 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -281,35 +281,37 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning success = planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints); - robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); - reached_state->updateLinkTransforms(); - const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset; - - double distance = 0.0; - if (use_rotation_distance) { - Eigen::AngleAxisd rotation(reached_pose.linear() * ik_pose_world.linear().transpose()); - distance = rotation.angle(); - } else - distance = (reached_pose.translation() - ik_pose_world.translation()).norm(); - - // min_distance reached? - if (min_distance > 0.0) { - success = distance >= min_distance; - if (!success) { - char msg[100]; - snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance); - solution.setComment(msg); + if (robot_trajectory) { // the following requires a robot_trajectory returned from planning + robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); + reached_state->updateLinkTransforms(); + const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset; + + double distance = 0.0; + if (use_rotation_distance) { + Eigen::AngleAxisd rotation(reached_pose.linear() * ik_pose_world.linear().transpose()); + distance = rotation.angle(); + } else + distance = (reached_pose.translation() - ik_pose_world.translation()).norm(); + + // min_distance reached? + if (min_distance > 0.0) { + success = distance >= min_distance; + if (!success) { + char msg[100]; + snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance); + solution.setComment(msg); + } + } else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case + success = true; + } else if (!success) + solution.setComment("failed to move full distance"); + + // visualize plan + auto ns = props.get("marker_ns"); + if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance + visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose, + linear, distance); } - } else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case - success = true; - } else if (!success) - solution.setComment("failed to move full distance"); - - // visualize plan - auto ns = props.get("marker_ns"); - if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance - visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose, - linear, distance); } } From 538233893328cd567ed1ccfd16bb89e5b086094f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 25 May 2023 21:52:07 +0200 Subject: [PATCH 45/52] Disable MPS tests, failing due to #432 --- core/python/test/rostest_mps.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/python/test/rostest_mps.py b/core/python/test/rostest_mps.py index 1a9675031..883d0830f 100755 --- a/core/python/test/rostest_mps.py +++ b/core/python/test/rostest_mps.py @@ -81,7 +81,7 @@ def test_fw_remove_object(self): s = self.task.solutions[0].toMsg() self.assertEqual(s.sub_trajectory[1].scene_diff.world.collision_objects[0].id, "box") - def test_bw_add_object(self): + def DISABLED_test_bw_add_object(self): # add object to move_group's planning scene self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]) @@ -99,7 +99,7 @@ def test_bw_add_object(self): objects = [o.id for o in ps.world.collision_objects] self.assertTrue(objects == ["block", "box"]) - def test_bw_remove_object(self): + def DISABLED_test_bw_remove_object(self): mps = stages.ModifyPlanningScene("removeObject(box) backwards") mps.removeObject("box") self.task.insert(mps, 0) From 2728b3c94c11dd455d27876528211558d815d8b9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 25 May 2023 22:07:35 +0200 Subject: [PATCH 46/52] Remove downstream package mtc_pour ... as fillMessage() was renamed to toMsg() --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 462c32a14..ceede99da 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -43,7 +43,7 @@ jobs: CLANG_TIDY_ARGS: -quiet -export-fixes ${{ github.workspace }}/.work/clang-tidy-fixes.yaml DOCKER_IMAGE: moveit/moveit:${{ matrix.env.IMAGE }} UNDERLAY: /root/ws_moveit/install - DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master" + DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master" CCACHE_DIR: ${{ github.workspace }}/.ccache BASEDIR: ${{ github.workspace }}/.work CACHE_PREFIX: "${{ matrix.env.IMAGE }}${{ contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && '-ccov' || '' }}" From 6f7282423d4e1defd840e4adcfa6df7b0e338b3a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 26 May 2023 00:16:00 +0200 Subject: [PATCH 47/52] MPS: fixup processCollisionObject - Declare CollisionObject argument as constant: Internally the argument is temporarily modified, but for a caller it is effectively const. - Correctly restore the old operation mode - Fixup check in unit test --- .../task_constructor/stages/modify_planning_scene.h | 3 ++- core/python/test/rostest_mps.py | 11 ++++++++--- core/src/stages/modify_planning_scene.cpp | 10 ++++++---- 3 files changed, 16 insertions(+), 8 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h index c5570922e..802daed33 100644 --- a/core/include/moveit/task_constructor/stages/modify_planning_scene.h +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -150,7 +150,8 @@ class ModifyPlanningScene : public PropagatingEitherWay protected: // apply stored modifications to scene std::pair apply(const InterfaceState& from, bool invert); - void processCollisionObject(planning_scene::PlanningScene& scene, moveit_msgs::CollisionObject& object, bool invert); + void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::CollisionObject& object, + bool invert); void attachObjects(planning_scene::PlanningScene& scene, const std::pair>& pair, bool invert); void allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs, bool invert); diff --git a/core/python/test/rostest_mps.py b/core/python/test/rostest_mps.py index 883d0830f..2c2551248 100755 --- a/core/python/test/rostest_mps.py +++ b/core/python/test/rostest_mps.py @@ -95,9 +95,14 @@ def DISABLED_test_bw_add_object(self): self.psi.remove_world_object("block") # restore original state s = self.task.solutions[0].toMsg() - for ps in [s.start_scene] + [s.sub_trajectory[i].scene_diff for i in [0, 1]]: - objects = [o.id for o in ps.world.collision_objects] - self.assertTrue(objects == ["block", "box"]) + + # block shouldn't be in start scene + objects = [o.id for o in s.start_scene.world.collision_objects] + self.assertTrue(objects == ["box"]) + + # only addObject(block) should add it + objects = [o.id for o in s.sub_trajectory[1].scene_diff.world.collision_objects] + self.assertTrue(objects == ["block", "box"]) def DISABLED_test_bw_remove_object(self): mps = stages.ModifyPlanningScene("removeObject(box) backwards") diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index 319dba851..6b1e7fc84 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -149,17 +149,19 @@ std::pair ModifyPlanningScene::apply(const Interf } void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene& scene, - moveit_msgs::CollisionObject& object, bool invert) { - auto op = object.operation; + const moveit_msgs::CollisionObject& object, bool invert) { + const auto op = object.operation; if (invert) { if (op == moveit_msgs::CollisionObject::ADD) - op = moveit_msgs::CollisionObject::REMOVE; + // (temporarily) change operation to REMOVE to revert adding the object + const_cast(object).operation = moveit_msgs::CollisionObject::REMOVE; else if (op == moveit_msgs::CollisionObject::REMOVE) throw std::runtime_error("cannot apply removeObject() backwards"); } scene.processCollisionObjectMsg(object); - object.operation = op; + // restore previous operation (for next call) + const_cast(object).operation = op; } } // namespace stages } // namespace task_constructor From c605a0059a3030bd1095202d0662c6f8e43e345e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 26 May 2023 00:09:21 +0200 Subject: [PATCH 48/52] Replace namespace robot_[model|state] with moveit::core --- .../src/execute_task_solution_capability.cpp | 4 ++-- core/src/merge.cpp | 8 ++++---- core/src/solvers/cartesian_path.cpp | 2 +- core/src/solvers/pipeline_planner.cpp | 4 ++-- core/src/stages/compute_ik.cpp | 20 +++++++++---------- core/src/stages/connect.cpp | 2 +- core/src/stages/generate_grasp_pose.cpp | 2 +- core/src/stages/move_relative.cpp | 4 ++-- core/src/stages/move_to.cpp | 2 +- .../src/task_display.cpp | 2 +- .../src/display_solution.cpp | 2 +- .../src/task_solution_visualization.cpp | 2 +- 12 files changed, 27 insertions(+), 27 deletions(-) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index e6207dfe2..984829da7 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -130,9 +130,9 @@ void ExecuteTaskSolutionCapability::preemptCallback() { bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution, plan_execution::ExecutableMotionPlan& plan) { - robot_model::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel(); + moveit::core::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel(); - robot_state::RobotState state(model); + moveit::core::RobotState state(model); { planning_scene_monitor::LockedPlanningSceneRO scene(context_->planning_scene_monitor_); state = scene->getCurrentState(); diff --git a/core/src/merge.cpp b/core/src/merge.cpp index 3bae7a327..e02acb56f 100644 --- a/core/src/merge.cpp +++ b/core/src/merge.cpp @@ -105,7 +105,7 @@ moveit::core::JointModelGroup* merge(const std::vector& sub_trajectories, - const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group, + const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group, const trajectory_processing::TimeParameterization& time_parameterization) { if (sub_trajectories.size() <= 1) throw std::runtime_error("Expected multiple sub solutions"); @@ -141,7 +141,7 @@ merge(const std::vector& sub_trajecto std::vector values; values.reserve(max_num_vars); - auto merged_state = std::make_shared(base_state); + auto merged_state = std::make_shared(base_state); while (true) { bool finished = true; size_t index = merged_traj->getWayPointCount(); @@ -151,7 +151,7 @@ merge(const std::vector& sub_trajecto continue; // no more waypoints in this sub solution finished = false; // there was a waypoint, continue while loop - const robot_state::RobotState& sub_state = sub->getWayPoint(index); + const moveit::core::RobotState& sub_state = sub->getWayPoint(index); sub_state.copyJointGroupPositions(sub->getGroup(), values); merged_state->setJointGroupPositions(sub->getGroup(), values); } @@ -162,7 +162,7 @@ merge(const std::vector& sub_trajecto // add waypoint without timing merged_traj->addSuffixWayPoint(merged_state, 0.0); // create new RobotState for next waypoint - merged_state = std::make_shared(*merged_state); + merged_state = std::make_shared(*merged_state); } // add timing diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 4e9df5dc3..36c8bd891 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -89,7 +89,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons const double* joint_positions) { state->setJointGroupPositions(jmg, joint_positions); state->update(); - return !sandbox_scene->isStateColliding(const_cast(*state), jmg->getName()) && + return !sandbox_scene->isStateColliding(const_cast(*state), jmg->getName()) && kcs.decide(*state).satisfied; }; diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 333759084..57fd495da 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -52,10 +52,10 @@ struct PlannerCache { using PlannerID = std::tuple; using PlannerMap = std::map >; - using ModelList = std::list, PlannerMap> >; + using ModelList = std::list, PlannerMap> >; ModelList cache_; - PlannerMap::mapped_type& retrieve(const robot_model::RobotModelConstPtr& model, const PlannerID& id) { + PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& id) { // find model in cache_ and remove expired entries while doing so ModelList::iterator model_it = cache_.begin(); while (model_it != cache_.end()) { diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 86307d0c0..738a66178 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -98,11 +98,11 @@ namespace { // ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though... // TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene, - robot_state::RobotState& robot_state, Eigen::Isometry3d pose, - const robot_model::LinkModel* link, const robot_model::JointModelGroup* jmg = nullptr, + moveit::core::RobotState& robot_state, Eigen::Isometry3d pose, + const moveit::core::LinkModel* link, const moveit::core::JointModelGroup* jmg = nullptr, collision_detection::CollisionResult* collision_result = nullptr) { // consider all rigidly connected parent links as well - const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link); + const moveit::core::LinkModel* parent = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link); if (parent != link) // transform pose into pose suitable to place parent pose = pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent); @@ -116,10 +116,10 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce while (parent) { pending_links.push_back(&parent->getName()); link = parent; - const robot_model::JointModel* joint = link->getParentJointModel(); + const moveit::core::JointModel* joint = link->getParentJointModel(); parent = joint->getParentLinkModel(); - if (joint->getType() != robot_model::JointModel::FIXED) { + if (joint->getType() != moveit::core::JointModel::FIXED) { for (const std::string* name : pending_links) acm.setDefaultEntry(*name, true); pending_links.clear(); @@ -280,7 +280,7 @@ void ComputeIK::compute() { } // determine IK link from ik_frame - const robot_model::LinkModel* link = nullptr; + const moveit::core::LinkModel* link = nullptr; geometry_msgs::PoseStamped ik_pose_msg; const boost::any& value = props.get("ik_frame"); if (value.empty()) { // property undefined @@ -311,7 +311,7 @@ void ComputeIK::compute() { // validate placed link for collisions collision_detection::CollisionResult collisions; - robot_state::RobotState sandbox_state{ scene->getCurrentState() }; + moveit::core::RobotState sandbox_state{ scene->getCurrentState() }; bool colliding = !ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, jmg, &collisions); @@ -349,7 +349,7 @@ void ComputeIK::compute() { std::vector compare_pose; const std::string& compare_pose_name = props.get("default_pose"); if (!compare_pose_name.empty()) { - robot_state::RobotState compare_state(robot_model); + moveit::core::RobotState compare_state(robot_model); compare_state.setToDefaultValues(jmg, compare_pose_name); compare_state.copyJointGroupPositions(jmg, compare_pose); } else @@ -359,7 +359,7 @@ void ComputeIK::compute() { IKSolutions ik_solutions; auto is_valid = [scene, ignore_collisions, min_solution_distance, - &ik_solutions](robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, + &ik_solutions](moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg, const double* joint_positions) { for (const auto& sol : ik_solutions) { if (jmg->distance(joint_positions, sol.joint_positions.data()) < min_solution_distance) @@ -419,7 +419,7 @@ void ComputeIK::compute() { solution.markAsFailure(ss.str()); } // set scene's robot state - robot_state::RobotState& solution_state = solution_scene->getCurrentStateNonConst(); + moveit::core::RobotState& solution_state = solution_scene->getCurrentStateNonConst(); solution_state.setJointGroupPositions(jmg, ik_solutions[i].joint_positions.data()); solution_state.update(); diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 16e2ea8ff..5f71defb9 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -152,7 +152,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { planning_scene::PlanningScenePtr end = start->diff(); const moveit::core::JointModelGroup* jmg = final_goal_state.getJointModelGroup(pair.first); final_goal_state.copyJointGroupPositions(jmg, positions); - robot_state::RobotState& goal_state = end->getCurrentStateNonConst(); + moveit::core::RobotState& goal_state = end->getCurrentStateNonConst(); goal_state.setJointGroupPositions(jmg, positions); goal_state.update(); intermediate_scenes.push_back(end); diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 1d53ca4d2..dd820e9b8 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -150,7 +150,7 @@ void GenerateGraspPose::compute() { const std::string& eef = props.get("eef"); const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef); - robot_state::RobotState& robot_state = scene->getCurrentStateNonConst(); + moveit::core::RobotState& robot_state = scene->getCurrentStateNonConst(); try { applyPreGrasp(robot_state, jmg, props.property("pregrasp")); } catch (const moveit::Exception& e) { diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 2f5acb746..7af3d58c0 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -166,7 +166,7 @@ static void visualizePlan(std::deque& markers, Inter bool MoveRelative::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& solution, Interface::Direction dir) { scene = state.scene()->diff(); - const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel(); + const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel(); assert(robot_model); const auto& props = properties(); @@ -282,7 +282,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints); if (robot_trajectory) { // the following requires a robot_trajectory returned from planning - robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); + moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); reached_state->updateLinkTransforms(); const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset; diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 0ce8ea116..c2721eedd 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -177,7 +177,7 @@ bool MoveTo::getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_po bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& solution, Interface::Direction dir) { scene = state.scene()->diff(); - const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel(); + const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel(); assert(robot_model); const auto& props = properties(); diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index 8a3e875a9..c405499fc 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -120,7 +120,7 @@ void TaskDisplay::loadRobotModel() { const srdf::ModelSharedPtr& srdf = rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); - robot_model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf)); + robot_model_.reset(new moveit::core::RobotModel(rdf_loader_->getURDF(), srdf)); // Send to child class trajectory_visual_->onRobotModelLoaded(robot_model_); diff --git a/visualization/visualization_tools/src/display_solution.cpp b/visualization/visualization_tools/src/display_solution.cpp index 3d5dee7a7..bfb704c3b 100644 --- a/visualization/visualization_tools/src/display_solution.cpp +++ b/visualization/visualization_tools/src/display_solution.cpp @@ -65,7 +65,7 @@ float DisplaySolution::getWayPointDurationFromPrevious(const IndexPair& idx_pair return data_[idx_pair.first].trajectory_->getWayPointDurationFromPrevious(idx_pair.second); } -const robot_state::RobotStatePtr& DisplaySolution::getWayPointPtr(const IndexPair& idx_pair) const { +const moveit::core::RobotStatePtr& DisplaySolution::getWayPointPtr(const IndexPair& idx_pair) const { return data_[idx_pair.first].trajectory_->getWayPointPtr(idx_pair.second); } diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index f522f3967..b62c7e287 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -209,7 +209,7 @@ void TaskSolutionVisualization::setName(const QString& name) { slider_dock_panel_->setWindowTitle(name + " - Slider"); } -void TaskSolutionVisualization::onRobotModelLoaded(const robot_model::RobotModelConstPtr& robot_model) { +void TaskSolutionVisualization::onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model) { // Error check if (!robot_model) { ROS_ERROR_STREAM_NAMED("task_solution_visualization", "No robot model found"); From 0ba9796fe85db47be0d1130a27b08070271ccc96 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 26 Jul 2023 07:52:43 +0200 Subject: [PATCH 49/52] Revert "Silent googletest warnings" This reverts commit 16af904e05c8f5334ca6ef9dc927e691f1fa79d1. The corresponding upstream fixes were merged into ament packages. --- .github/workflows/ci.yaml | 3 --- 1 file changed, 3 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index e336a093b..8ce5174e8 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -42,9 +42,6 @@ jobs: UNDERLAY: /root/ws_moveit/install # TODO: Port to ROS2 # DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master" - # Silent googletest warnings: https://github.com/ament/ament_cmake/pull/408#issuecomment-1306004975 - UPSTREAM_WORKSPACE: github:ament/googletest#clalancette/update-to-gtest-1.11 - AFTER_SETUP_UPSTREAM_WORKSPACE: dpkg --purge --force-all ros-rolling-gtest-vendor ros-rolling-gmock-vendor TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release CCACHE_DIR: ${{ github.workspace }}/.ccache BASEDIR: ${{ github.workspace }}/.work From f4cd7d5b85de9f9d209ef21cbf9f582b52ab6550 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 4 Oct 2023 15:20:10 +0200 Subject: [PATCH 50/52] Enable parallel planning with PipelinePlanner (#450) * Refactor pipeline planner Make code readable Re-order plan functions Make usable with parallel planning Enable configuring multiple pipelines Add callbacks Cleanup and documentation Add API to set parallel planning callbacks and deprecate functions Pass pipeline map by reference Small clang-tidy fix Update core/src/solvers/pipeline_planner.cpp Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Update core/src/solvers/pipeline_planner.cpp Format Refactor to avoid calling .at(0) twice Use no default stopping criteria Update fallbacks_move demo * Cleanup + address deprecation warnings * Enabling optionally using a property defined pipeline planner map * Address review * Disable humble CI for ros2 branch * Add pipeline planner unittests + some checks * Add short comment --- .github/workflows/ci.yaml | 1 - .../solvers/pipeline_planner.h | 148 +++++++-- core/src/solvers/pipeline_planner.cpp | 281 ++++++++++-------- core/test/CMakeLists.txt | 1 + core/test/pick_pa10.cpp | 3 +- core/test/pick_pr2.cpp | 3 +- core/test/pick_ur5.cpp | 3 +- core/test/test_pipeline_planner.cpp | 63 ++++ demo/src/fallbacks_move_to.cpp | 6 +- 9 files changed, 340 insertions(+), 169 deletions(-) create mode 100644 core/test/test_pipeline_planner.cpp diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 8ce5174e8..b5552d85e 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -19,7 +19,6 @@ jobs: fail-fast: false matrix: env: - - IMAGE: humble-source - IMAGE: rolling-source NAME: ccov TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage" diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index 68f10cd8e..4415123c5 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -32,13 +32,16 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Robert Haschke - Desc: plan using MoveIt's PlanningPipeline +/* Authors: Robert Haschke, Sebastian Jahr + Description: Solver that uses a set of MoveIt PlanningPipelines to solve a given planning problem. */ #pragma once #include +#include +#include +#include #include #include @@ -56,48 +59,131 @@ MOVEIT_CLASS_FORWARD(PipelinePlanner); class PipelinePlanner : public PlannerInterface { public: - struct Specification - { - moveit::core::RobotModelConstPtr model; - std::string ns{ "ompl" }; - std::string pipeline{ "ompl" }; - std::string adapter_param{ "request_adapters" }; - }; - - static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, - const moveit::core::RobotModelConstPtr& model) { - Specification spec; - spec.model = model; - return create(node, spec); + /** Simple Constructor to use only a single pipeline + * \param [in] node ROS 2 node + * \param [in] pipeline_name Name of the planning pipeline to be used. This is also the assumed namespace where the + * parameters of this pipeline can be found \param [in] planner_id Planner id to be used for planning + */ + PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl", + const std::string& planner_id = ""); + + [[deprecated("Deprecated: Use new constructor implementations.")]] // clang-format off + PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& /*planning_pipeline*/){}; + + /** \brief Constructor + * \param [in] node ROS 2 node + * \param [in] pipeline_id_planner_id_map A map containing pairs of planning pipeline name and planner plugin name + * for the planning requests + * \param [in] planning_pipelines Optional: A map with the pipeline names and initialized corresponding planning + * pipelines + * \param [in] stopping_criterion_callback Callback function to stop parallel planning pipeline according to a user defined criteria + * \param [in] solution_selection_function Callback function to choose the best solution when multiple pipelines are used + */ + PipelinePlanner(const rclcpp::Node::SharedPtr& node, + const std::unordered_map& pipeline_id_planner_id_map, + const std::unordered_map& planning_pipelines = + std::unordered_map(), + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback = nullptr, + const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function = + &moveit::planning_pipeline_interfaces::getShortestSolution); + + [[deprecated("Replaced with setPlannerId(pipeline_name, planner_id)")]] // clang-format off + void setPlannerId(const std::string& /*planner*/) { /* Do nothing */ } - static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, const Specification& spec); - - /** - * - * @param node used to load the parameters for the planning pipeline + /** \brief Set the planner id for a specific planning pipeline for the planning requests + * \param [in] pipeline_name Name of the planning pipeline for which the planner id is set + * \param [in] planner_id Name of the planner ID that should be used by the planning pipeline + * \return true if the pipeline exists and the corresponding ID is set successfully + */ + bool setPlannerId(const std::string& pipeline_name, const std::string& planner_id); + + /** \brief Set stopping criterion function for parallel planning + * \param [in] stopping_criterion_callback New stopping criterion function that will be used + */ + void setStoppingCriterionFunction(const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback); + + /** \brief Set solution selection function for parallel planning + * \param [in] solution_selection_function New solution selection that will be used + */ + void setSolutionSelectionFunction(const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function); + + /** \brief This function is called when an MTC task that uses this solver is initialized. If no pipelines are + * configured when this function is invoked, the planning pipelines named in the 'pipeline_id_planner_id_map' are + * initialized with the given robot model. + * \param [in] robot_model A robot model that is used to initialize the + * planning pipelines of this solver */ - PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline = "ompl"); - - PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline); - - void setPlannerId(const std::string& planner) { setProperty("planner", planner); } - void init(const moveit::core::RobotModelConstPtr& robot_model) override; + /** + * \brief Plan a trajectory from a planning scene 'from' to scene 'to' + * \param [in] from Start planning scene + * \param [in] to Goal planning scene (used to create goal constraints) + * \param [in] joint_model_group Group of joints for which this trajectory is created + * \param [in] timeout Maximum planning timeout for an individual stage that is using the pipeline planner in seconds + * \param [in] result Reference to the location where the created trajectory is stored if planning is successful + * \param [in] path_constraints Path contraints for the planning problem + * \return true If the solver found a successful solution for the given planning problem + */ bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, - const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const core::JointModelGroup* joint_model_group, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + /** \brief Plan a trajectory from a planning scene 'from' to a target pose with an offset + * \param [in] from Start planning scene + * \param [in] link Link for which a target pose is given + * \param [in] offset Offset to be applied to a given target pose + * \param [in] target Target pose + * \param [in] joint_model_group Group of joints for which this trajectory is created + * \param [in] timeout Maximum planning timeout for an individual stage that is using the pipeline planner in seconds + * \param [in] result Reference to the location where the created trajectory is stored if planning is successful + * \param [in] path_constraints Path contraints for the planning problem + * \return true If the solver found a successful solution for the given planning problem + */ bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* joint_model_group, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; protected: - std::string pipeline_name_; - planning_pipeline::PlanningPipelinePtr planner_; + /** \brief Function that actually uses the planning pipelines to solve the given planning problem. It is called by + * the public plan function after the goal constraints are generated. This function uses a predefined number of + * planning pipelines in parallel to solve the planning problem and choose the best (user-defined) solution. + * \param [in] planning_scene Scene for which the planning should be solved + * \param [in] joint_model_group + * Group of joints for which this trajectory is created + * \param [in] goal_constraints Set of constraints that need to + * be satisfied by the solution + * \param [in] timeout Maximum planning timeout for an individual stage that is using the pipeline planner in seconds + * \param [in] result Reference to the location where the created + * trajectory is stored if planning is successful + * \param [in] path_constraints Path contraints for the planning + * problem + * \return true If the solver found a successful solution for the given planning problem + */ + bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene, + const moveit::core::JointModelGroup* joint_model_group, + const moveit_msgs::msg::Constraints& goal_constraints, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()); + rclcpp::Node::SharedPtr node_; + + /** \brief Map of pipeline names (ids) and their corresponding planner ids. The planning problem is solved for every + * pipeline-planner pair in this map. If no pipelines are passed via constructor argument, the pipeline names are + * used to initialize a set of planning pipelines. */ + std::unordered_map pipeline_id_planner_id_map_; + + /** \brief Map of pipelines names and planning pipelines. This map is used to quickly search for a requested motion + * planning pipeline when during plan(..) */ + std::unordered_map planning_pipelines_; + + moveit::planning_pipeline_interfaces::StoppingCriterionFunction stopping_criterion_callback_; + moveit::planning_pipeline_interfaces::SolutionSelectionFunction solution_selection_function_; + }; } // namespace solvers } // namespace task_constructor diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index bd469022e..702cd44cd 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -53,160 +53,187 @@ namespace moveit { namespace task_constructor { namespace solvers { -struct PlannerCache -{ - using PlannerID = std::tuple; - using PlannerMap = std::map >; - using ModelList = std::list, PlannerMap> >; - ModelList cache_; - - PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& id) { - // find model in cache_ and remove expired entries while doing so - ModelList::iterator model_it = cache_.begin(); - while (model_it != cache_.end()) { - if (model_it->first.expired()) { - model_it = cache_.erase(model_it); - continue; - } - if (model_it->first.lock() == model) - break; - ++model_it; - } - if (model_it == cache_.end()) // if not found, create a new PlannerMap for this model - model_it = cache_.insert(cache_.begin(), std::make_pair(model, PlannerMap())); - - return model_it->second.insert(std::make_pair(id, PlannerMap::mapped_type())).first->second; - } -}; - -planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const rclcpp::Node::SharedPtr& node, - const PipelinePlanner::Specification& spec) { - static PlannerCache cache; - - static constexpr char const* PLUGIN_PARAMETER_NAME = "planning_plugin"; - - std::string pipeline_ns = spec.ns; - const std::string parameter_name = pipeline_ns + "." + PLUGIN_PARAMETER_NAME; - // fallback to old structure for pipeline parameters in MoveIt - if (!node->has_parameter(parameter_name)) { - node->declare_parameter(parameter_name, rclcpp::ParameterType::PARAMETER_STRING); - } - if (std::string parameter; !node->get_parameter(parameter_name, parameter)) { - RCLCPP_WARN(node->get_logger(), "Failed to find '%s.%s'. %s", pipeline_ns.c_str(), PLUGIN_PARAMETER_NAME, - "Attempting to load pipeline from old parameter structure. Please update your MoveIt config."); - pipeline_ns = "move_group"; +PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name, + const std::string& planner_id) + : PipelinePlanner(node, [&]() { + std::unordered_map pipeline_id_planner_id_map; + pipeline_id_planner_id_map[pipeline_name] = planner_id; + return pipeline_id_planner_id_map; + }()) {} + +PipelinePlanner::PipelinePlanner( + const rclcpp::Node::SharedPtr& node, const std::unordered_map& pipeline_id_planner_id_map, + const std::unordered_map& planning_pipelines, + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback, + const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function) + : node_(node) + , pipeline_id_planner_id_map_(pipeline_id_planner_id_map) + , stopping_criterion_callback_(stopping_criterion_callback) + , solution_selection_function_(solution_selection_function) { + // If the pipeline name - pipeline map is passed as constructor argument, use it. Otherwise, it will be created in + // the init(..) function + if (!planning_pipelines.empty()) { + planning_pipelines_ = planning_pipelines; } + // Declare properties of the MotionPlanRequest + properties().declare("num_planning_attempts", 1u, "number of planning attempts"); + properties().declare( + "workspace_parameters", moveit_msgs::msg::WorkspaceParameters(), "allowed workspace of mobile base?"); + + properties().declare("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals"); + properties().declare("goal_position_tolerance", 1e-4, "tolerance for reaching position goals"); + properties().declare("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals"); + // Declare properties that configure the planning pipeline + properties().declare("display_motion_plans", false, + "publish generated solutions on topic " + + planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC); + properties().declare("publish_planning_requests", false, + "publish motion planning requests on topic " + + planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC); + properties().declare>( + "pipeline_id_planner_id_map", std::unordered_map(), + "Set of pipelines and planners used for planning"); +} - PlannerCache::PlannerID id(pipeline_ns, spec.adapter_param); - - std::weak_ptr& entry = cache.retrieve(spec.model, id); - planning_pipeline::PlanningPipelinePtr planner = entry.lock(); - if (!planner) { - // create new entry - planner = std::make_shared(spec.model, node, pipeline_ns, - PLUGIN_PARAMETER_NAME, spec.adapter_param); - // store in cache - entry = planner; +bool PipelinePlanner::setPlannerId(const std::string& pipeline_name, const std::string& planner_id) { + // Only set ID if pipeline exists. It is not possible to create new pipelines with this command. + if (pipeline_id_planner_id_map_.count(pipeline_name) == 0) { + RCLCPP_ERROR(node_->get_logger(), + "PipelinePlanner does not have a pipeline called '%s'. Cannot set pipeline ID '%s'", + pipeline_name.c_str(), planner_id.c_str()); + return false; } - return planner; + pipeline_id_planner_id_map_[pipeline_name] = planner_id; + return true; } -PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name) - : pipeline_name_{ pipeline_name }, node_(node) { - auto& p = properties(); - p.declare("planner", "", "planner id"); - - p.declare("num_planning_attempts", 1u, "number of planning attempts"); - p.declare("workspace_parameters", moveit_msgs::msg::WorkspaceParameters(), - "allowed workspace of mobile base?"); - - p.declare("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals"); - p.declare("goal_position_tolerance", 1e-4, "tolerance for reaching position goals"); - p.declare("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals"); - - p.declare("display_motion_plans", false, - "publish generated solutions on topic " + planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC); - p.declare("publish_planning_requests", false, - "publish motion planning requests on topic " + - planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC); +void PipelinePlanner::setStoppingCriterionFunction( + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_function) { + stopping_criterion_callback_ = stopping_criterion_function; } - -PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline) - : PipelinePlanner(rclcpp::Node::SharedPtr()) { - planner_ = planning_pipeline; +void PipelinePlanner::setSolutionSelectionFunction( + const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function) { + solution_selection_function_ = solution_selection_function; } void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) { - if (!planner_) { - Specification spec; - spec.model = robot_model; - spec.pipeline = pipeline_name_; - spec.ns = pipeline_name_; - planner_ = create(node_, spec); - } else if (robot_model != planner_->getRobotModel()) { + // If no planning pipelines exist, create them based on the pipeline names provided in pipeline_id_planner_id_map_. + // The assumption here is that all parameters required by the planning pipeline can be found in a namespace that + // equals the pipeline name. + if (planning_pipelines_.empty()) { + planning_pipelines_ = moveit::planning_pipeline_interfaces::createPlanningPipelineMap( + [&]() { + // Create pipeline name vector from the keys of pipeline_id_planner_id_map_ + if (pipeline_id_planner_id_map_.empty()) { + throw std::runtime_error("Cannot initialize PipelinePlanner: No planning pipeline was provided and " + "pipeline_id_planner_id_map_ is empty!"); + } + + std::vector pipeline_names; + for (const auto& pipeline_name_planner_id_pair : pipeline_id_planner_id_map_) { + pipeline_names.push_back(pipeline_name_planner_id_pair.first); + } + return pipeline_names; + }(), + robot_model, node_); + } + + // Check if it is still empty + if (planning_pipelines_.empty()) { throw std::runtime_error( - "The robot model of the planning pipeline isn't the same as the task's robot model -- " - "use Task::setRobotModel for setting the robot model when using custom planning pipeline"); + "Cannot initialize PipelinePlanner: Could not create any valid entries for planning pipeline maps!"); } - planner_->displayComputedMotionPlans(properties().get("display_motion_plans")); - planner_->publishReceivedRequests(properties().get("publish_planning_requests")); -} -void initMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& req, const PropertyMap& p, - const moveit::core::JointModelGroup* jmg, double timeout) { - req.group_name = jmg->getName(); - req.planner_id = p.get("planner"); - req.allowed_planning_time = std::min(timeout, p.get("timeout")); - req.start_state.is_diff = true; // we don't specify an extra start state - - req.num_planning_attempts = p.get("num_planning_attempts"); - req.max_velocity_scaling_factor = p.get("max_velocity_scaling_factor"); - req.max_acceleration_scaling_factor = p.get("max_acceleration_scaling_factor"); - req.workspace_parameters = p.get("workspace_parameters"); + // Configure all pipelines according to the configuration in properties + for (auto const& name_pipeline_pair : planning_pipelines_) { + name_pipeline_pair.second->displayComputedMotionPlans(properties().get("display_motion_plans")); + name_pipeline_pair.second->publishReceivedRequests(properties().get("publish_planning_requests")); + } } bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, - const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* joint_model_group, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints) { - const auto& props = properties(); - moveit_msgs::msg::MotionPlanRequest req; - initMotionPlanRequest(req, props, jmg, timeout); - - req.goal_constraints.resize(1); - req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(to->getCurrentState(), jmg, - props.get("goal_joint_tolerance")); - req.path_constraints = path_constraints; - - ::planning_interface::MotionPlanResponse res; - bool success = planner_->generatePlan(from, req, res); - result = res.trajectory; - return success; + // Construct goal constraints from the goal planning scene + const auto goal_constraints = kinematic_constraints::constructGoalConstraints( + to->getCurrentState(), joint_model_group, properties().get("goal_joint_tolerance")); + return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints); } bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen, - const moveit::core::JointModelGroup* jmg, double timeout, + const moveit::core::JointModelGroup* joint_model_group, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints) { - const auto& props = properties(); - moveit_msgs::msg::MotionPlanRequest req; - initMotionPlanRequest(req, props, jmg, timeout); - + // Construct a Cartesian target pose from the given target transform and offset geometry_msgs::msg::PoseStamped target; target.header.frame_id = from->getPlanningFrame(); target.pose = tf2::toMsg(target_eigen * offset.inverse()); - req.goal_constraints.resize(1); - req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( - link.getName(), target, props.get("goal_position_tolerance"), - props.get("goal_orientation_tolerance")); - req.path_constraints = path_constraints; + const auto goal_constraints = kinematic_constraints::constructGoalConstraints( + link.getName(), target, properties().get("goal_position_tolerance"), + properties().get("goal_orientation_tolerance")); + + return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints); +} + +bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, + const moveit::core::JointModelGroup* joint_model_group, + const moveit_msgs::msg::Constraints& goal_constraints, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints) { + // Create a request for every planning pipeline that should run in parallel + std::vector requests; + requests.reserve(pipeline_id_planner_id_map_.size()); + + auto const property_pipeline_id_planner_id_map = + properties().get>("pipeline_id_planner_id_map"); + for (auto const& pipeline_id_planner_id_pair : + (!property_pipeline_id_planner_id_map.empty() ? property_pipeline_id_planner_id_map : + pipeline_id_planner_id_map_)) { + // Check that requested pipeline exists and skip it if it doesn't exist + if (planning_pipelines_.find(pipeline_id_planner_id_pair.first) == planning_pipelines_.end()) { + RCLCPP_WARN( + node_->get_logger(), + "Pipeline '%s' is not available of this PipelineSolver instance. Skipping a request for this pipeline.", + pipeline_id_planner_id_pair.first.c_str()); + continue; + } + // Create MotionPlanRequest for pipeline + moveit_msgs::msg::MotionPlanRequest request; + request.pipeline_id = pipeline_id_planner_id_pair.first; + request.group_name = joint_model_group->getName(); + request.planner_id = pipeline_id_planner_id_pair.second; + request.allowed_planning_time = timeout; + request.start_state.is_diff = true; // we don't specify an extra start state + request.num_planning_attempts = properties().get("num_planning_attempts"); + request.max_velocity_scaling_factor = properties().get("max_velocity_scaling_factor"); + request.max_acceleration_scaling_factor = properties().get("max_acceleration_scaling_factor"); + request.workspace_parameters = properties().get("workspace_parameters"); + request.goal_constraints.resize(1); + request.goal_constraints.at(0) = goal_constraints; + request.path_constraints = path_constraints; + requests.push_back(request); + } - ::planning_interface::MotionPlanResponse res; - bool success = planner_->generatePlan(from, req, res); - result = res.trajectory; - return success; + // Run planning pipelines in parallel to create a vector of responses. If a solution selection function is provided, + // planWithParallelPipelines will return a vector with the single best solution + std::vector<::planning_interface::MotionPlanResponse> responses = + moveit::planning_pipeline_interfaces::planWithParallelPipelines( + requests, planning_scene, planning_pipelines_, stopping_criterion_callback_, solution_selection_function_); + + // If solutions exist and the first one is successful + if (!responses.empty()) { + auto const solution = responses.at(0); + if (solution) { + // Choose the first solution trajectory as response + result = solution.trajectory; + return bool(result); + } + } + return false; } } // namespace solvers } // namespace task_constructor diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 1d8ab42f0..69fe81160 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -54,6 +54,7 @@ if (BUILD_TESTING) mtc_add_gtest(test_move_to.cpp move_to.launch.py) mtc_add_gtest(test_move_relative.cpp move_to.launch.py) + mtc_add_gtest(test_pipeline_planner.cpp) # building these integration tests works without moveit config packages ament_add_gtest_executable(pick_ur5 pick_ur5.cpp) diff --git a/core/test/pick_pa10.cpp b/core/test/pick_pa10.cpp index 367372400..4ee7b9336 100644 --- a/core/test/pick_pa10.cpp +++ b/core/test/pick_pa10.cpp @@ -47,8 +47,7 @@ TEST(PA10, pick) { t.setProperty("eef", std::string("la_tool_mount")); t.setProperty("gripper", std::string("left_hand")); - auto pipeline = std::make_shared(node); - pipeline->setPlannerId("RRTConnectkConfigDefault"); + auto pipeline = std::make_shared(node, "ompl", "RRTConnectkConfigDefault"); auto cartesian = std::make_shared(); Stage* initial_stage = nullptr; diff --git a/core/test/pick_pr2.cpp b/core/test/pick_pr2.cpp index 2554097b5..c8a31c792 100644 --- a/core/test/pick_pr2.cpp +++ b/core/test/pick_pr2.cpp @@ -40,8 +40,7 @@ TEST(PR2, pick) { auto node = rclcpp::Node::make_shared("pr2"); // planner used for connect - auto pipeline = std::make_shared(node); - pipeline->setPlannerId("RRTConnectkConfigDefault"); + auto pipeline = std::make_shared(node, "ompl", "RRTConnectkConfigDefault"); // connect to pick stages::Connect::GroupPlannerVector planners = { { "left_arm", pipeline }, { "left_gripper", pipeline } }; auto connect = std::make_unique("connect", planners); diff --git a/core/test/pick_ur5.cpp b/core/test/pick_ur5.cpp index 87d17e87e..d268bdc15 100644 --- a/core/test/pick_ur5.cpp +++ b/core/test/pick_ur5.cpp @@ -42,8 +42,7 @@ TEST(UR5, pick) { auto node = rclcpp::Node::make_shared("ur5"); // planner used for connect - auto pipeline = std::make_shared(node); - pipeline->setPlannerId("RRTConnectkConfigDefault"); + auto pipeline = std::make_shared(node, "ompl", "RRTConnectkConfigDefault"); // connect to pick stages::Connect::GroupPlannerVector planners = { { "arm", pipeline }, { "gripper", pipeline } }; auto connect = std::make_unique("connect", planners); diff --git a/core/test/test_pipeline_planner.cpp b/core/test/test_pipeline_planner.cpp new file mode 100644 index 000000000..347d0b572 --- /dev/null +++ b/core/test/test_pipeline_planner.cpp @@ -0,0 +1,63 @@ +#include "models.h" + +#include +#include + +#include + +using namespace moveit::task_constructor; + +// Test fixture for PipelinePlanner +struct PipelinePlannerTest : public testing::Test +{ + PipelinePlannerTest() { + node->declare_parameter("STOMP.planning_plugin", "stomp_moveit/StompPlanner"); + }; + const rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("test_pipeline_planner"); + const moveit::core::RobotModelPtr robot_model = getModel(); +}; + +TEST_F(PipelinePlannerTest, testInitialization) { + // GIVEN a valid robot model, ROS node and PipelinePlanner + auto pipeline_planner = solvers::PipelinePlanner(node, "STOMP", "stomp"); + // WHEN a PipelinePlanner instance is initialized + // THEN it does not throw + EXPECT_NO_THROW(pipeline_planner.init(robot_model)); +} + +TEST_F(PipelinePlannerTest, testWithoutPlanningPipelines) { + // GIVEN a PipelinePlanner instance without planning pipelines + std::unordered_map empty_pipeline_id_planner_id_map; + auto pipeline_planner = solvers::PipelinePlanner(node, empty_pipeline_id_planner_id_map); + // WHEN a PipelinePlanner instance is initialized + // THEN it does not throw + EXPECT_THROW(pipeline_planner.init(robot_model), std::runtime_error); +} + +TEST_F(PipelinePlannerTest, testValidPlan) { + // GIVEN an initialized PipelinePlanner + auto pipeline_planner = solvers::PipelinePlanner(node, "STOMP", "stomp"); + pipeline_planner.init(robot_model); + // WHEN a solution for a valid request is requested + auto scene = std::make_shared(robot_model); + robot_trajectory::RobotTrajectoryPtr result = + std::make_shared(robot_model, robot_model->getJointModelGroup("group")); + // THEN it returns true + EXPECT_TRUE(pipeline_planner.plan(scene, scene, robot_model->getJointModelGroup("group"), 1.0, result)); +} + +TEST_F(PipelinePlannerTest, testInvalidPipelineID) { + // GIVEN a valid initialized PipelinePlanner instance + auto pipeline_planner = solvers::PipelinePlanner(node, "STOMP", "stomp"); + pipeline_planner.init(robot_model); + // WHEN the planner ID for a non-existing planning pipeline is set + // THEN setPlannerID returns false + EXPECT_FALSE(pipeline_planner.setPlannerId("CHOMP", "stomp")); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/demo/src/fallbacks_move_to.cpp b/demo/src/fallbacks_move_to.cpp index 7bcdccef7..a95ef42ab 100644 --- a/demo/src/fallbacks_move_to.cpp +++ b/demo/src/fallbacks_move_to.cpp @@ -31,14 +31,12 @@ int main(int argc, char** argv) { cartesian->setJumpThreshold(2.0); const auto ptp = [&node]() { - auto pp{ std::make_shared(node, "pilz_industrial_motion_planner") }; - pp->setPlannerId("PTP"); + auto pp{ std::make_shared(node, "pilz_industrial_motion_planner", "PTP") }; return pp; }(); const auto rrtconnect = [&node]() { - auto pp{ std::make_shared(node, "ompl") }; - pp->setPlannerId("RRTConnect"); + auto pp{ std::make_shared(node, "ompl", "RRTConnectkConfigDefault") }; return pp; }(); From 39eeae4a1bd9b69f047b8eb3df0a0291fffac13d Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 6 Oct 2023 18:04:25 +0200 Subject: [PATCH 51/52] set a non-infinite default timeout in CurrentState stage (#491) Co-authored-by: Mario Prats --- core/src/stages/current_state.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/core/src/stages/current_state.cpp b/core/src/stages/current_state.cpp index ca48fb1b6..f03b37adb 100644 --- a/core/src/stages/current_state.cpp +++ b/core/src/stages/current_state.cpp @@ -35,6 +35,8 @@ /* Authors: Michael Goerner, Luca Lach, Robert Haschke */ +#include + #include #include #include @@ -47,13 +49,16 @@ namespace moveit { namespace task_constructor { namespace stages { +using namespace std::chrono_literals; +constexpr std::chrono::duration DEFAULT_TIMEOUT = 3s; + static const rclcpp::Logger LOGGER = rclcpp::get_logger("CurrentState"); CurrentState::CurrentState(const std::string& name) : Generator(name) { auto& p = properties(); Property& timeout = p.property("timeout"); timeout.setDescription("max time to wait for get_planning_scene service"); - timeout.setValue(-1.0); + timeout.setValue(DEFAULT_TIMEOUT.count()); } void CurrentState::init(const moveit::core::RobotModelConstPtr& robot_model) { From 5c4ef60525dd1402ed04064960694ccd8bc9d768 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 9 Oct 2023 15:12:58 +0200 Subject: [PATCH 52/52] Add planner name to trajectory info (#490) * Add planner name to trajectory info * Extend unittest --- .../task_constructor/solvers/cartesian_path.h | 2 + .../solvers/joint_interpolation.h | 2 + .../solvers/pipeline_planner.h | 8 +++ .../solvers/planner_interface.h | 3 + .../moveit/task_constructor/stages/connect.h | 12 +++- .../include/moveit/task_constructor/storage.h | 28 +++++--- core/src/solvers/pipeline_planner.cpp | 7 ++ core/src/stages/connect.cpp | 68 +++++++++++++------ core/src/stages/move_relative.cpp | 2 + core/src/stages/move_to.cpp | 2 + core/src/storage.cpp | 1 + core/test/test_pipeline_planner.cpp | 2 + msgs/msg/SolutionInfo.msg | 3 + .../src/task_list_model.cpp | 2 +- 14 files changed, 106 insertions(+), 36 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 230cb7b53..8dfa3ea2b 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -71,6 +71,8 @@ class CartesianPath : public PlannerInterface const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + + std::string getPlannerId() const override { return std::string("CartesianPath"); } }; } // namespace solvers } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/solvers/joint_interpolation.h b/core/include/moveit/task_constructor/solvers/joint_interpolation.h index 4e080c56f..66df1e510 100644 --- a/core/include/moveit/task_constructor/solvers/joint_interpolation.h +++ b/core/include/moveit/task_constructor/solvers/joint_interpolation.h @@ -66,6 +66,8 @@ class JointInterpolationPlanner : public PlannerInterface const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + + std::string getPlannerId() const override { return std::string("JointInterpolationPlanner"); } }; } // namespace solvers } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index 4415123c5..f024da201 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -148,6 +148,12 @@ class PipelinePlanner : public PlannerInterface robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + /** + * \brief Get planner name + * \return Name of the last successful planner + */ + std::string getPlannerId() const override; + protected: /** \brief Function that actually uses the planning pipelines to solve the given planning problem. It is called by * the public plan function after the goal constraints are generated. This function uses a predefined number of @@ -172,6 +178,8 @@ class PipelinePlanner : public PlannerInterface rclcpp::Node::SharedPtr node_; + std::string last_successful_planner_; + /** \brief Map of pipeline names (ids) and their corresponding planner ids. The planning problem is solved for every * pipeline-planner pair in this map. If no pipelines are passed via constructor argument, the pipeline names are * used to initialize a set of planning pipelines. */ diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index 760d1155e..ff9948414 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -99,6 +99,9 @@ class PlannerInterface const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0; + + // get name of the planner + virtual std::string getPlannerId() const = 0; }; } // namespace solvers } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index c8cf883d5..94dfde045 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -73,7 +73,13 @@ class Connect : public Connecting WAYPOINTS = 1 }; - using GroupPlannerVector = std::vector >; + struct PlannerIdTrajectoryPair + { + std::string planner_name; + robot_trajectory::RobotTrajectoryConstPtr robot_trajectory_ptr; + }; + + using GroupPlannerVector = std::vector>; Connect(const std::string& name = "connect", const GroupPlannerVector& planners = {}); void setPathConstraints(moveit_msgs::msg::Constraints path_constraints) { @@ -85,10 +91,10 @@ class Connect : public Connecting void compute(const InterfaceState& from, const InterfaceState& to) override; protected: - SolutionSequencePtr makeSequential(const std::vector& sub_trajectories, + SolutionSequencePtr makeSequential(const std::vector& trajectory_planner_vector, const std::vector& intermediate_scenes, const InterfaceState& from, const InterfaceState& to); - SubTrajectoryPtr merge(const std::vector& sub_trajectories, + SubTrajectoryPtr merge(const std::vector& trajectory_planner_vector, const std::vector& intermediate_scenes, const moveit::core::RobotState& state); diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index bc75bcc07..5bff53d74 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -275,8 +275,8 @@ class SolutionBase public: virtual ~SolutionBase() = default; - inline const InterfaceState* start() const { return start_; } - inline const InterfaceState* end() const { return end_; } + [[nodiscard]] inline const InterfaceState* start() const { return start_; } + [[nodiscard]] inline const InterfaceState* end() const { return end_; } /** Set the solution's start_state_ * @@ -298,18 +298,21 @@ class SolutionBase const_cast(state).addIncoming(this); } - inline const Stage* creator() const { return creator_; } + [[nodiscard]] inline const Stage* creator() const { return creator_; } void setCreator(Stage* creator); - inline double cost() const { return cost_; } + [[nodiscard]] inline double cost() const { return cost_; } void setCost(double cost); void markAsFailure(const std::string& msg = std::string()); - inline bool isFailure() const { return !std::isfinite(cost_); } + [[nodiscard]] inline bool isFailure() const { return !std::isfinite(cost_); } - const std::string& comment() const { return comment_; } + [[nodiscard]] const std::string& plannerId() const { return planner_id_; } + void setPlannerId(const std::string& planner_id) { planner_id_ = planner_id; } + + [[nodiscard]] const std::string& comment() const { return comment_; } void setComment(const std::string& comment) { comment_ = comment; } - auto& markers() { return markers_; } + [[nodiscard]] auto& markers() { return markers_; } const auto& markers() const { return markers_; } /// convert solution to message @@ -326,14 +329,17 @@ class SolutionBase bool operator<(const SolutionBase& other) const { return this->cost_ < other.cost_; } protected: - SolutionBase(Stage* creator = nullptr, double cost = 0.0, std::string comment = "") - : creator_(creator), cost_(cost), comment_(std::move(comment)) {} + SolutionBase(Stage* creator = nullptr, double cost = 0.0, std::string comment = std::string(""), + std::string planner_id = std::string("")) + : creator_(creator), cost_(cost), planner_id_(std::move(planner_id)), comment_(std::move(comment)) {} private: // back-pointer to creating stage, allows to access sub-solutions Stage* creator_; // associated cost double cost_; + // name of the planner used to create this solution + std::string planner_id_; // comment for this solution, e.g. explanation of failure std::string comment_; // markers for this solution, e.g. target frame or collision indicators @@ -351,8 +357,8 @@ class SubTrajectory : public SolutionBase public: SubTrajectory( const robot_trajectory::RobotTrajectoryConstPtr& trajectory = robot_trajectory::RobotTrajectoryConstPtr(), - double cost = 0.0, std::string comment = "") - : SolutionBase(nullptr, cost, std::move(comment)), trajectory_(trajectory) {} + double cost = 0.0, std::string comment = std::string(""), std::string planner_id = std::string("")) + : SolutionBase(nullptr, cost, std::move(comment), std::move(planner_id)), trajectory_(trajectory) {} robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; } void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; } diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 702cd44cd..0c2435320 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -67,6 +67,7 @@ PipelinePlanner::PipelinePlanner( const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback, const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function) : node_(node) + , last_successful_planner_("") , pipeline_id_planner_id_map_(pipeline_id_planner_id_map) , stopping_criterion_callback_(stopping_criterion_callback) , solution_selection_function_(solution_selection_function) { @@ -167,6 +168,8 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co const moveit::core::JointModelGroup* joint_model_group, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints) { + last_successful_planner_.clear(); + // Construct a Cartesian target pose from the given target transform and offset geometry_msgs::msg::PoseStamped target; target.header.frame_id = from->getPlanningFrame(); @@ -230,11 +233,15 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning if (solution) { // Choose the first solution trajectory as response result = solution.trajectory; + last_successful_planner_ = solution.planner_id; return bool(result); } } return false; } +std::string PipelinePlanner::getPlannerId() const { + return last_successful_planner_.empty() ? std::string("Unknown") : last_successful_planner_; +} } // namespace solvers } // namespace task_constructor } // namespace moveit diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index e013a180a..d4591f5c6 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -141,7 +141,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { const auto& path_constraints = props.get("path_constraints"); const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState(); - std::vector sub_trajectories; + std::vector trajectory_planner_vector; std::vector intermediate_scenes; planning_scene::PlanningSceneConstPtr start = from.scene(); @@ -161,7 +161,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { robot_trajectory::RobotTrajectoryPtr trajectory; success = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints); - sub_trajectories.push_back(trajectory); // include failed trajectory + trajectory_planner_vector.push_back(PlannerIdTrajectoryPair({ pair.second->getPlannerId(), trajectory })); if (!success) break; @@ -172,20 +172,21 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { SolutionBasePtr solution; if (success && mode != SEQUENTIAL) // try to merge - solution = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState()); + solution = merge(trajectory_planner_vector, intermediate_scenes, from.scene()->getCurrentState()); if (!solution) // success == false or merging failed: store sequentially - solution = makeSequential(sub_trajectories, intermediate_scenes, from, to); + solution = makeSequential(trajectory_planner_vector, intermediate_scenes, from, to); if (!success) // error during sequential planning solution->markAsFailure(); + connect(from, to, solution); } SolutionSequencePtr -Connect::makeSequential(const std::vector& sub_trajectories, +Connect::makeSequential(const std::vector& trajectory_planner_vector, const std::vector& intermediate_scenes, const InterfaceState& from, const InterfaceState& to) { - assert(!sub_trajectories.empty()); - assert(sub_trajectories.size() + 1 == intermediate_scenes.size()); + assert(!trajectory_planner_vector.empty()); + assert(trajectory_planner_vector.size() + 1 == intermediate_scenes.size()); /* We need to decouple the sequence of subsolutions, created here, from the external from and to states. Hence, we create new interface states for all subsolutions. */ @@ -193,18 +194,19 @@ Connect::makeSequential(const std::vectorsetCreator(this); - if (!sub) // a null RobotTrajectoryPtr indicates a failure + if (!pair.robot_trajectory_ptr) // a null RobotTrajectoryPtr indicates a failure inserted->markAsFailure(); // push back solution pointer sub_solutions.push_back(&*inserted); // create a new end state, either from intermediate or final planning scene planning_scene::PlanningSceneConstPtr end_ps = - (sub_solutions.size() < sub_trajectories.size()) ? *++scene_it : to.scene(); + (sub_solutions.size() < trajectory_planner_vector.size()) ? *++scene_it : to.scene(); const InterfaceState* end = &*states_.insert(states_.end(), InterfaceState(end_ps)); // provide newly created start/end states @@ -217,26 +219,50 @@ Connect::makeSequential(const std::vector(std::move(sub_solutions)); } -SubTrajectoryPtr Connect::merge(const std::vector& sub_trajectories, +SubTrajectoryPtr Connect::merge(const std::vector& trajectory_planner_vector, const std::vector& intermediate_scenes, const moveit::core::RobotState& state) { // no need to merge if there is only a single sub trajectory - if (sub_trajectories.size() == 1) - return std::make_shared(sub_trajectories[0]); + if (trajectory_planner_vector.size() == 1) + return std::make_shared(trajectory_planner_vector.at(0).robot_trajectory_ptr, 0.0, std::string(""), + trajectory_planner_vector.at(0).planner_name); auto jmg = merged_jmg_.get(); assert(jmg); auto timing = properties().get("merge_time_parameterization"); - robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg, *timing); - if (!trajectory) + robot_trajectory::RobotTrajectoryPtr merged_trajectory = task_constructor::merge( + [&]() { + // Move trajectories into single vector + std::vector robot_traj_vector; + robot_traj_vector.reserve(trajectory_planner_vector.size()); + + // Copy second elements of robot planner vector into separate vector + std::transform(begin(trajectory_planner_vector), end(trajectory_planner_vector), + std::back_inserter(robot_traj_vector), + [](auto const& pair) { return pair.robot_trajectory_ptr; }); + return robot_traj_vector; + }(), + state, jmg, *timing); + + // check merged trajectory is empty or has collisions + if (!merged_trajectory || + !intermediate_scenes.front()->isPathValid(*merged_trajectory, + properties().get("path_constraints"))) { return SubTrajectoryPtr(); + } - // check merged trajectory for collisions - if (!intermediate_scenes.front()->isPathValid(*trajectory, - properties().get("path_constraints"))) - return SubTrajectoryPtr(); + // Copy first elements of robot planner vector into separate vector + std::vector planner_names; + planner_names.reserve(trajectory_planner_vector.size()); + std::transform(begin(trajectory_planner_vector), end(trajectory_planner_vector), std::back_inserter(planner_names), + [](auto const& pair) { return pair.planner_name; }); - return std::make_shared(trajectory); + // Create a sequence of planner names + std::string planner_name_sequence; + for (auto const& name : planner_names) { + planner_name_sequence += std::string(", " + name); + } + return std::make_shared(merged_trajectory, 0.0, std::string(""), planner_name_sequence); } } // namespace stages } // namespace task_constructor diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 7052d6c4c..62450fa77 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -199,6 +199,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) { // plan to joint-space target success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + solution.setPlannerId(planner_->getPlannerId()); } else { // Cartesian targets require an IK reference frame const moveit::core::LinkModel* link; @@ -287,6 +288,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning success = planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints); + solution.setPlannerId(planner_->getPlannerId()); if (robot_trajectory) { // the following requires a robot_trajectory returned from planning moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index e7257d447..1a44a197a 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -209,6 +209,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) { // plan to joint-space target success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + solution.setPlannerId(planner_->getPlannerId()); } else { // Cartesian goal // Where to go? Eigen::Isometry3d target; @@ -241,6 +242,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP // plan to Cartesian target success = planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints); + solution.setPlannerId(planner_->getPlannerId()); } // store result diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 16485371c..f08859baf 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -215,6 +215,7 @@ void SolutionBase::fillInfo(moveit_task_constructor_msgs::msg::SolutionInfo& inf info.id = introspection ? introspection->solutionId(*this) : 0; info.cost = this->cost(); info.comment = this->comment(); + info.planner_id = this->plannerId(); const Introspection* ci = introspection; info.stage_id = ci ? ci->stageId(this->creator()) : 0; diff --git a/core/test/test_pipeline_planner.cpp b/core/test/test_pipeline_planner.cpp index 347d0b572..6f19a18b8 100644 --- a/core/test/test_pipeline_planner.cpp +++ b/core/test/test_pipeline_planner.cpp @@ -23,6 +23,7 @@ TEST_F(PipelinePlannerTest, testInitialization) { // WHEN a PipelinePlanner instance is initialized // THEN it does not throw EXPECT_NO_THROW(pipeline_planner.init(robot_model)); + EXPECT_EQ(pipeline_planner.getPlannerId(), "Unknown"); } TEST_F(PipelinePlannerTest, testWithoutPlanningPipelines) { @@ -44,6 +45,7 @@ TEST_F(PipelinePlannerTest, testValidPlan) { std::make_shared(robot_model, robot_model->getJointModelGroup("group")); // THEN it returns true EXPECT_TRUE(pipeline_planner.plan(scene, scene, robot_model->getJointModelGroup("group"), 1.0, result)); + EXPECT_EQ(pipeline_planner.getPlannerId(), "stomp"); } TEST_F(PipelinePlannerTest, testInvalidPipelineID) { diff --git a/msgs/msg/SolutionInfo.msg b/msgs/msg/SolutionInfo.msg index c3346ab53..5689205c2 100644 --- a/msgs/msg/SolutionInfo.msg +++ b/msgs/msg/SolutionInfo.msg @@ -10,5 +10,8 @@ string comment # id of stage that created this trajectory uint32 stage_id +# name of the planner that created this solution +string planner_id + # markers, e.g. providing additional hints or illustrating failure visualization_msgs/Marker[] markers diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index 21a542612..3cd646690 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -295,7 +295,7 @@ void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_m DisplaySolutionPtr TaskListModel::processSolutionMessage(const moveit_task_constructor_msgs::msg::Solution& msg) { auto it = remote_tasks_.find(msg.task_id); if (it == remote_tasks_.cend()) - return DisplaySolutionPtr(); // unkown task + return DisplaySolutionPtr(); // unknown task RemoteTaskModel* remote_task = it->second; if (!remote_task)