diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 6134243c5..845e3266b 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -19,18 +19,19 @@ jobs: fail-fast: false matrix: env: + # - IMAGE: rolling-source + # NAME: ccov + # TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage" - IMAGE: rolling-source - NAME: ccov - TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage" - IMAGE: rolling-source CLANG_TIDY: pedantic - - IMAGE: rolling-source + - IMAGE: jazzy-source NAME: asan # Add fast_unwind_on_malloc=0 to fix stacktraces being too short or do not make sense # see https://github.com/google/sanitizers/wiki/AddressSanitizer # Disable alloc/dealloc mismatch warnings: https://github.com/ros2/rclcpp/pull/1324 DOCKER_RUN_OPTS: >- - -e PRELOAD=libasan.so.5 + -e PRELOAD=libasan.so.8 -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions,fast_unwind_on_malloc=0" -e ASAN_OPTIONS="new_delete_type_mismatch=0,alloc_dealloc_mismatch=0" TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g" @@ -38,7 +39,7 @@ jobs: env: CLANG_TIDY_ARGS: -quiet -export-fixes ${{ github.workspace }}/.work/clang-tidy-fixes.yaml DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }} - UNDERLAY: /root/ws_moveit/install + UNDERLAY: ${{ endsWith(matrix.env.IMAGE, '-source') && '/root/ws_moveit/install' || ''}} # TODO: Port to ROS2 # DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master" TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release diff --git a/.github/workflows/prerelease.yaml b/.github/workflows/prerelease.yaml index e82d28df3..18c6c0a13 100644 --- a/.github/workflows/prerelease.yaml +++ b/.github/workflows/prerelease.yaml @@ -5,24 +5,24 @@ name: pre-release on: workflow_dispatch: + inputs: + ROS_DISTRO: + type: string + required: true + description: 'ROS distribution codename:' + default: noetic 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 }} + ROS_DISTRO: ${{ inputs.ROS_DISTRO }} PRERELEASE: true BASEDIR: ${{ github.workspace }}/.work - name: "${{ matrix.distro }}" + name: "${{ inputs.ROS_DISTRO }}" runs-on: ubuntu-latest steps: - name: "Free up disk space" diff --git a/.gitmodules b/.gitmodules index 9339bbb20..d7508c721 100644 --- a/.gitmodules +++ b/.gitmodules @@ -3,3 +3,6 @@ url = https://github.com/pybind/pybind11 branch = smart_holder shallow = true +[submodule "core/src/scope_guard"] + path = core/src/scope_guard + url = https://github.com/ricab/scope_guard diff --git a/README.md b/README.md index 7063027ef..39cda41bd 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,7 @@ This repository provides the following branches: ## Tutorial -We provide a tutorial for a pick-and-place pipeline without bells & whistles [as part of the MoveIt tutorials](https://ros-planning.github.io/moveit_tutorials/doc/moveit_task_constructor/moveit_task_constructor_tutorial.html). +We provide a tutorial for a pick-and-place pipeline without bells & whistles [as part of the MoveIt tutorials](https://moveit.github.io/moveit_tutorials/doc/moveit_task_constructor/moveit_task_constructor_tutorial.html). ## Roadmap diff --git a/capabilities/CMakeLists.txt b/capabilities/CMakeLists.txt index f07b78e46..e3d4d0c86 100644 --- a/capabilities/CMakeLists.txt +++ b/capabilities/CMakeLists.txt @@ -3,11 +3,14 @@ project(moveit_task_constructor_capabilities) find_package(ament_cmake REQUIRED) +find_package(fmt REQUIRED) find_package(Boost REQUIRED) find_package(moveit_common REQUIRED) moveit_package() find_package(moveit_core REQUIRED) find_package(moveit_ros_move_group REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(moveit_task_constructor_core REQUIRED) find_package(moveit_task_constructor_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp_action REQUIRED) @@ -18,9 +21,12 @@ add_library(${PROJECT_NAME} SHARED ) ament_target_dependencies(${PROJECT_NAME} Boost + fmt rclcpp_action moveit_core moveit_ros_move_group + moveit_ros_planning + moveit_task_constructor_core moveit_task_constructor_msgs ) @@ -31,6 +37,8 @@ install(TARGETS ${PROJECT_NAME} pluginlib_export_plugin_description_file(moveit_ros_move_group capabilities_plugin_description.xml) ament_export_dependencies(moveit_core) ament_export_dependencies(moveit_ros_move_group) +ament_export_dependencies(moveit_ros_planning) +ament_export_dependencies(moveit_task_constructor_core) ament_export_dependencies(moveit_task_constructor_msgs) ament_export_dependencies(pluginlib) ament_export_dependencies(rclcpp_action) diff --git a/capabilities/package.xml b/capabilities/package.xml index 4f61ceffc..518b37010 100644 --- a/capabilities/package.xml +++ b/capabilities/package.xml @@ -12,6 +12,7 @@ ament_cmake + fmt moveit_core moveit_ros_move_group moveit_ros_planning diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 27bdee5a4..badb430d7 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -43,9 +43,7 @@ #include #include #include -#include - -#include +#include namespace { @@ -164,8 +162,8 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr if (!joint_names.empty()) { group = findJointModelGroup(*model, joint_names); if (!group) { - RCLCPP_ERROR_STREAM(LOGGER, "Could not find JointModelGroup that actuates {" - << boost::algorithm::join(joint_names, ", ") << "}"); + RCLCPP_ERROR_STREAM(LOGGER, fmt::format("Could not find JointModelGroup that actuates {{{}}}", + fmt::join(joint_names, ", "))); return false; } RCLCPP_DEBUG(LOGGER, "Using JointModelGroup '%s' for execution", group->getName().c_str()); @@ -184,11 +182,12 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr exec_traj.controller_name = sub_traj.execution_info.controller_names; /* TODO add action feedback and markers */ - exec_traj.effect_on_success = [this, - &scene_diff = const_cast<::moveit_msgs::msg::PlanningScene&>(sub_traj.scene_diff), + exec_traj.effect_on_success = [this, &scene_diff = const_cast<::moveit_msgs::msg::PlanningScene&>(sub_traj.scene_diff), description](const plan_execution::ExecutableMotionPlan* /*plan*/) { + // Never modify joint state directly (only via robot trajectories) scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState(); scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState(); + scene_diff.robot_state.is_diff = true; // silent empty JointState msg error if (!moveit::core::isEmpty(scene_diff)) { RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description); diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index d10d2f5ba..48cfd1b9d 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -4,6 +4,7 @@ project(moveit_task_constructor_core) find_package(ament_cmake REQUIRED) find_package(Boost REQUIRED) +find_package(fmt REQUIRED) find_package(geometry_msgs REQUIRED) find_package(moveit_common REQUIRED) moveit_package() @@ -11,6 +12,7 @@ find_package(moveit_core REQUIRED) find_package(moveit_ros_planning REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(moveit_task_constructor_msgs REQUIRED) +find_package(py_binding_tools REQUIRED) find_package(rclcpp REQUIRED) find_package(rviz_marker_tools REQUIRED) find_package(tf2_eigen REQUIRED) @@ -25,8 +27,8 @@ add_compile_options(-fvisibility-inlines-hidden) set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor) add_subdirectory(src) -# TODO: enable python wrapper -#add_subdirectory(python) +ament_python_install_package(moveit PACKAGE_DIR python/moveit) +add_subdirectory(python) add_subdirectory(test) install(DIRECTORY include/ DESTINATION include @@ -41,6 +43,7 @@ ament_export_dependencies(moveit_core) ament_export_dependencies(moveit_ros_planning) ament_export_dependencies(moveit_ros_planning_interface) ament_export_dependencies(moveit_task_constructor_msgs) +ament_export_dependencies(py_binding_tools) ament_export_dependencies(rclcpp) ament_export_dependencies(rviz_marker_tools) ament_export_dependencies(tf2_eigen) diff --git a/core/doc/tutorials/properties.rst b/core/doc/tutorials/properties.rst index d9b260ec2..58dc19599 100644 --- a/core/doc/tutorials/properties.rst +++ b/core/doc/tutorials/properties.rst @@ -97,11 +97,6 @@ You can obtain a reference to the the PropertyMap of a stage like so :start-after: [propertyTut10] :end-before: [propertyTut10] -.. literalinclude:: ../../../demo/scripts/properties.py - :language: python - :start-after: [propertyTut11] - :end-before: [propertyTut11] - As mentioned, each stage contains a PropertyMap. Stages communicate to each other via their interfaces. 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 e058ea8bf..000000000 --- a/core/include/moveit/python/python_tools/ros_types.h +++ /dev/null @@ -1,90 +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 -{}; - -/// 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 a164f5530..e661fa354 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 @@ -35,12 +35,12 @@ class PropertyConverter : protected PropertyConverterBase static boost::any fromPython(const pybind11::object& po) { return pybind11::cast(po); } template - typename std::enable_if::value, std::string>::type rosMsgName() { - return ros::message_traits::DataType::value(); + typename std::enable_if::value, std::string>::type rosMsgName() { + return rosidl_generator_traits::name(); } template - typename std::enable_if::value, std::string>::type rosMsgName() { + typename std::enable_if::value, std::string>::type rosMsgName() { return std::string(); } }; diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 203a08e02..81c82e709 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -51,6 +51,10 @@ class ContainerBase : public Stage PRIVATE_CLASS(ContainerBase) using pointer = std::unique_ptr; + /// Explicitly enable/disable pruning + void setPruning(bool pruning) { setProperty("pruning", pruning); } + bool pruning() const { return properties().get("pruning"); } + size_t numChildren() const; Stage* findChild(const std::string& name) const; Stage* operator[](int index) const; diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 02320b4c3..e52864dc2 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -95,6 +95,7 @@ class LambdaCostTerm : public TrajectoryCostTerm LambdaCostTerm(const SubTrajectorySignature& term); LambdaCostTerm(const SubTrajectoryShortSignature& term); + using TrajectoryCostTerm::operator(); double operator()(const SubTrajectory& s, std::string& comment) const override; protected: @@ -132,6 +133,8 @@ class PathLength : public TrajectoryCostTerm PathLength(std::vector joints); /// Limit measurements to given joints and use given weighting PathLength(std::map j) : joints(std::move(j)) {} + + using TrajectoryCostTerm::operator(); double operator()(const SubTrajectory& s, std::string& comment) const override; std::map joints; //< joint weights @@ -145,6 +148,8 @@ class DistanceToReference : public TrajectoryCostTerm std::map w = std::map()); DistanceToReference(const std::map& ref, Mode m = Mode::AUTO, std::map w = std::map()); + + using TrajectoryCostTerm::operator(); double operator()(const SubTrajectory& s, std::string& comment) const override; moveit_msgs::msg::RobotState reference; @@ -156,6 +161,7 @@ class DistanceToReference : public TrajectoryCostTerm class TrajectoryDuration : public TrajectoryCostTerm { public: + using TrajectoryCostTerm::operator(); double operator()(const SubTrajectory& s, std::string& comment) const override; }; @@ -167,6 +173,7 @@ class LinkMotion : public TrajectoryCostTerm std::string link_name; + using TrajectoryCostTerm::operator(); double operator()(const SubTrajectory& s, std::string& comment) const override; }; @@ -191,6 +198,7 @@ class Clearance : public TrajectoryCostTerm std::function distance_to_cost; + using TrajectoryCostTerm::operator(); double operator()(const SubTrajectory& s, std::string& comment) const override; }; diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index dcd46ba2a..1d3c37ba8 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -52,6 +52,10 @@ class CartesianPath : public PlannerInterface public: CartesianPath(); + void setIKFrame(const geometry_msgs::msg::PoseStamped& pose) { setProperty("ik_frame", pose); } + void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link); + void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); } + void setStepSize(double step_size) { setProperty("step_size", step_size); } void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); } void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); } diff --git a/core/include/moveit/task_constructor/solvers/multi_planner.h b/core/include/moveit/task_constructor/solvers/multi_planner.h index 6f27badb2..3d502eade 100644 --- a/core/include/moveit/task_constructor/solvers/multi_planner.h +++ b/core/include/moveit/task_constructor/solvers/multi_planner.h @@ -71,6 +71,8 @@ class MultiPlanner : public PlannerInterface, public std::vector #include +#include #include #include @@ -136,6 +137,7 @@ class StagePrivate void sendBackward(InterfaceState&& from, const InterfaceState& to, const SolutionBasePtr& solution); template inline void send(const InterfaceState& start, InterfaceState&& end, const SolutionBasePtr& solution); + void spawn(InterfaceState&& from, InterfaceState&& to, const SolutionBasePtr& solution); void spawn(InterfaceState&& state, const SolutionBasePtr& solution); void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution); @@ -143,7 +145,7 @@ class StagePrivate void newSolution(const SolutionBasePtr& solution); bool storeFailures() const { return introspection_ != nullptr; } void runCompute() { - RCLCPP_DEBUG_STREAM(LOGGER, "Computing stage '" << name() << "'"); + RCLCPP_DEBUG_STREAM(LOGGER, fmt::format("Computing stage '{}'", name())); auto compute_start_time = std::chrono::steady_clock::now(); try { compute(); diff --git a/core/include/moveit/task_constructor/stages/noop.h b/core/include/moveit/task_constructor/stages/noop.h new file mode 100644 index 000000000..02f41994f --- /dev/null +++ b/core/include/moveit/task_constructor/stages/noop.h @@ -0,0 +1,64 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Sherbrooke 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: Captain Yoshi */ + +#pragma once + +#include +#include + +namespace moveit { +namespace task_constructor { +namespace stages { + +/** no-op stage, which doesn't modify the interface state nor adds a trajectory. + * However, it can be used to store custom stage properties, + * which in turn can be queried post-planning to steer the execution. + */ + +class NoOp : public PropagatingEitherWay +{ +public: + NoOp(const std::string& name = "no-op") : PropagatingEitherWay(name){}; + +private: + bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& /*trajectory*/, + Interface::Direction /*dir*/) override { + scene = state.scene()->diff(); + return true; + }; +}; +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/include/moveit/task_constructor/stages/passthrough.h b/core/include/moveit/task_constructor/stages/passthrough.h index a43139486..e6f081e87 100644 --- a/core/include/moveit/task_constructor/stages/passthrough.h +++ b/core/include/moveit/task_constructor/stages/passthrough.h @@ -36,9 +36,6 @@ #pragma once #include -#include -#include -#include namespace moveit { namespace task_constructor { diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 0b7f5b6f3..0d8e350a0 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -95,10 +95,9 @@ class InterfaceState */ struct Priority : std::tuple { - Priority(unsigned int depth, double cost, Status status = ENABLED) - : std::tuple(status, depth, cost) { - assert(std::isfinite(cost)); - } + Priority(unsigned int depth, double cost, Status status) + : std::tuple(status, depth, cost) {} + Priority(unsigned int depth, double cost) : Priority(depth, cost, std::isfinite(cost) ? ENABLED : PRUNED) {} // Constructor copying depth and cost, but modifying its status Priority(const Priority& other, Status status) : Priority(other.depth(), other.cost(), status) {} diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index ad1327749..fae0f7212 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -119,6 +119,9 @@ class Task : protected WrapperBase using WrapperBase::setTimeout; using WrapperBase::timeout; + using WrapperBase::pruning; + using WrapperBase::setPruning; + /// reset all stages void reset() final; /// initialize all stages with given scene @@ -126,8 +129,9 @@ class Task : protected WrapperBase /// reset, init scene (if not yet done), and init all stages, then start planning moveit::core::MoveItErrorCode plan(size_t max_solutions = 0); - /// interrupt current planning (or execution) + /// interrupt current planning void preempt(); + void resetPreemptRequest(); /// execute solution, return the result moveit::core::MoveItErrorCode execute(const SolutionBase& s); diff --git a/core/include/moveit/task_constructor/task_p.h b/core/include/moveit/task_constructor/task_p.h index f0e6dc0a6..ef42dd346 100644 --- a/core/include/moveit/task_constructor/task_p.h +++ b/core/include/moveit/task_constructor/task_p.h @@ -63,7 +63,7 @@ class TaskPrivate : public WrapperBasePrivate std::string ns_; robot_model_loader::RobotModelLoaderPtr robot_model_loader_; moveit::core::RobotModelConstPtr robot_model_; - bool preempt_requested_; + std::atomic preempt_requested_; // introspection and monitoring std::unique_ptr introspection_; diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h index fcc6f591c..a2b30ec6c 100644 --- a/core/include/moveit/task_constructor/utils.h +++ b/core/include/moveit/task_constructor/utils.h @@ -60,7 +60,6 @@ MOVEIT_CLASS_FORWARD(RobotState); namespace task_constructor { MOVEIT_CLASS_FORWARD(Property); -MOVEIT_CLASS_FORWARD(SolutionBase); namespace utils { @@ -141,7 +140,7 @@ class Flags }; bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene, - const moveit::core::JointModelGroup* jmg, SolutionBase& solution, + const moveit::core::JointModelGroup* jmg, std::string& error_msg, const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame); } // namespace utils } // namespace task_constructor diff --git a/core/package.xml b/core/package.xml index 3cb44239b..6720f64cb 100644 --- a/core/package.xml +++ b/core/package.xml @@ -3,9 +3,9 @@ 0.1.3 MoveIt Task Pipeline - https://github.com/ros-planning/moveit_task_constructor - https://github.com/ros-planning/moveit_task_constructor - https://github.com/ros-planning/moveit_task_constructor/issues + https://github.com/moveit/moveit_task_constructor + https://github.com/moveit/moveit_task_constructor + https://github.com/moveit/moveit_task_constructor/issues BSD Michael Goerner @@ -14,11 +14,13 @@ ament_cmake ament_cmake_python + fmt geometry_msgs moveit_core moveit_ros_planning moveit_ros_planning_interface moveit_task_constructor_msgs + py_binding_tools rclcpp rviz_marker_tools tf2_eigen @@ -33,7 +35,7 @@ launch_testing_ament_cmake - + moveit_resources_fanuc_moveit_config moveit_planners diff --git a/core/python/CMakeLists.txt b/core/python/CMakeLists.txt index dcb988585..8b0616f13 100644 --- a/core/python/CMakeLists.txt +++ b/core/python/CMakeLists.txt @@ -24,14 +24,10 @@ add_subdirectory(pybind11) # C++ wrapper code add_subdirectory(bindings) -ament_python_install_package("src") - -if(BUILD_TESTING) +if(BUILD_TESTING AND NOT DEFINED ENV{PRELOAD}) find_package(ament_cmake_pytest REQUIRED) - set(_pytest_tests - test/test_mtc.py - test/rostest_mtc.py - ) + find_package(launch_testing_ament_cmake) + set(_pytest_tests test/test_mtc.py) foreach(_test_path ${_pytest_tests}) get_filename_component(_test_name ${_test_path} NAME_WE) ament_add_pytest_test(${_test_name} ${_test_path} @@ -40,4 +36,8 @@ if(BUILD_TESTING) WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} ) endforeach() + + # add_launch_test(test/test.launch.py TARGET rostest_mtc ARGS "exe:=${CMAKE_CURRENT_SOURCE_DIR}/test/rostest_mtc.py") + # add_launch_test(test/test.launch.py TARGET rostest_mps ARGS "exe:=${CMAKE_CURRENT_SOURCE_DIR}/test/rostest_mps.py") + # add_launch_test(test/test.launch.py TARGET rostest_trampoline ARGS "exe:=${CMAKE_CURRENT_SOURCE_DIR}/test/rostest_trampoline.py") endif() diff --git a/core/python/bindings/CMakeLists.txt b/core/python/bindings/CMakeLists.txt index d92cb8f5e..1bd444f46 100644 --- a/core/python/bindings/CMakeLists.txt +++ b/core/python/bindings/CMakeLists.txt @@ -1,27 +1,3 @@ -# 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 $ -) -target_link_libraries(${TOOLS_LIB_NAME} PUBLIC pybind11::pybind11 ${Boost_LIBRARIES} rclcpp::rclcpp) - -install(TARGETS ${TOOLS_LIB_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -# moveit.python_tools -pybind11_add_module(pymoveit_python_tools src/python_tools.cpp) -target_link_libraries(pymoveit_python_tools PRIVATE ${TOOLS_LIB_NAME}) - # moveit.task_constructor set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/task_constructor) pybind11_add_module(pymoveit_mtc @@ -34,9 +10,9 @@ pybind11_add_module(pymoveit_mtc src/module.cpp ) target_include_directories(pymoveit_mtc PUBLIC $) -target_link_libraries(pymoveit_mtc PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_stages ${TOOLS_LIB_NAME}) +target_link_libraries(pymoveit_mtc PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_stages py_binding_tools::py_binding_tools) # install python libs -install(TARGETS pymoveit_python_tools pymoveit_mtc - LIBRARY DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" +install(TARGETS pymoveit_mtc + LIBRARY DESTINATION "${PYTHON_INSTALL_DIR}" ) diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp index 0ba74bab8..090e0b628 100644 --- a/core/python/bindings/src/core.cpp +++ b/core/python/bindings/src/core.cpp @@ -33,6 +33,7 @@ *********************************************************************/ #include "core.h" +#include "utils.h" #include #include #include @@ -52,23 +53,6 @@ namespace python { namespace { -// utility function to normalize index: negative indeces reference from the end -size_t normalize_index(size_t size, long index) { - if (index < 0) - index += size; - if (index >= long(size) || index < 0) - throw pybind11::index_error("Index out of range"); - return index; -} - -// implement operator[](index) -template -typename T::value_type get_item(const T& container, long index) { - auto it = container.begin(); - std::advance(it, normalize_index(container.size(), index)); - return *it; -} - py::list getForwardedProperties(const Stage& self) { py::list l; for (const std::string& value : self.forwardedProperties()) @@ -86,9 +70,8 @@ void setForwardedProperties(Stage& self, const py::object& names) { for (auto item : names) s.emplace(item.cast()); } catch (const py::cast_error& e) { - // manually translate cast_error to type error - PyErr_SetString(PyExc_TypeError, e.what()); - throw py::error_already_set(); + // translate cast_error to type_error with an informative message + throw py::type_error("Expecting a string or a list of strings"); } self.setForwardedProperties(s); } @@ -97,7 +80,9 @@ void setForwardedProperties(Stage& self, const py::object& names) { void export_core(pybind11::module& m) { /// translate InitStageException into InitStageError - static py::exception init_stage_error(m, "InitStageError"); + PYBIND11_CONSTINIT static py::gil_safe_call_once_and_store exc_storage; + exc_storage.call_once_and_store_result([&]() { return py::exception(m, "InitStageError"); }); + /// provide extended error description for InitStageException py::register_exception_translator([](std::exception_ptr p) { // NOLINT(performance-unnecessary-value-param) try { @@ -106,7 +91,7 @@ void export_core(pybind11::module& m) { } catch (const InitStageException& e) { std::stringstream message; message << e; - init_stage_error(message.str().c_str()); + py::set_error(exc_storage.get_stored(), message.str().c_str()); } }); @@ -427,7 +412,7 @@ void export_core(pybind11::module& m) { .def_property_readonly("failures", &Task::failures, "Solutions: Failed Solutions of the stage (read-only)") .def_property("name", &Task::name, &Task::setName, "str: name of the task displayed e.g. in rviz") - .def("loadRobotModel", &Task::loadRobotModel, "robot_description"_a = "robot_description", + .def("loadRobotModel", &Task::loadRobotModel, "node"_a, "robot_description"_a = "robot_description", "Load robot model from given ROS parameter") .def("getRobotModel", &Task::getRobotModel) .def("enableIntrospection", &Task::enableIntrospection, "enabled"_a = true, @@ -486,30 +471,7 @@ void export_core(pybind11::module& m) { "publish", [](Task& self, const SolutionBasePtr& solution) { self.introspection().publishSolution(*solution); }, "solution"_a, "Publish the given solution to the ROS topic ``solution``") - .def_static( - "execute", - [](const SolutionBasePtr& solution) { - using namespace moveit::planning_interface; - PlanningSceneInterface psi; - MoveGroupInterface mgi(solution->start()->scene()->getRobotModel()->getJointModelGroupNames()[0]); - - MoveGroupInterface::Plan plan; - moveit_task_constructor_msgs::msg::Solution serialized; - solution->toMsg(serialized); - - for (const moveit_task_constructor_msgs::msg::SubTrajectory& traj : serialized.sub_trajectory) { - if (!traj.trajectory.joint_trajectory.points.empty()) { - plan.trajectory_ = traj.trajectory; - if (!mgi.execute(plan)) { - ROS_ERROR("Execution failed! Aborting!"); - return; - } - } - psi.applyPlanningScene(traj.scene_diff); - } - ROS_INFO("Executed successfully"); - }, - "solution"_a, "Send given solution to ``move_group`` node for execution"); + .def("execute", &Task::execute, "solution"_a, "Send given solution to ``move_group`` node for execution"); } } // namespace python } // namespace moveit diff --git a/core/python/bindings/src/properties.cpp b/core/python/bindings/src/properties.cpp index e53d7a293..4330d0f80 100644 --- a/core/python/bindings/src/properties.cpp +++ b/core/python/bindings/src/properties.cpp @@ -43,6 +43,14 @@ namespace moveit { namespace python { namespace { +/** In order to assign new property values in Python, we need to convert the Python object + * to a boost::any instance of the correct type. As the C++ type cannot be inferred from + * the Python type, we can support this assignment only for a few basic types (see fromPython()) + * as well as ROS message types. For other types, a generic assignment via + * stage.properties["property"] = value + * is not possible. Instead, use the .property declaration on the stage to allow for + * direct assignment like this: stage.property = value + **/ class PropertyConverterRegistry { struct Entry @@ -102,10 +110,8 @@ py::object PropertyConverterRegistry::toPython(const boost::any& value) { auto it = REGISTRY_SINGLETON.types_.find(value.type()); if (it == REGISTRY_SINGLETON.types_.end()) { - std::string msg("No Python -> C++ conversion for: "); - msg += boost::core::demangle(value.type().name()); - PyErr_SetString(PyExc_TypeError, msg.c_str()); - throw py::error_already_set(); + std::string name = boost::core::demangle(value.type().name()); + throw py::type_error("No Python -> C++ conversion for: " + name); } return it->second.to_(value); @@ -113,15 +119,15 @@ py::object PropertyConverterRegistry::toPython(const boost::any& value) { std::string rosMsgName(PyObject* object) { py::object o = py::reinterpret_borrow(object); - try { - return o.attr("_type").cast(); - } catch (const py::error_already_set&) { - // change error to TypeError - std::string msg = o.attr("__class__").attr("__name__").cast(); - msg += " is not a ROS message type"; - PyErr_SetString(PyExc_TypeError, msg.c_str()); - throw py::error_already_set(); - } + auto cls = o.attr("__class__"); + auto name = cls.attr("__name__").cast(); + auto module = cls.attr("__module__").cast(); + auto pos = module.find(".msg"); + if (pos == std::string::npos) + // object is not a ROS message type, return it's class name instead + return module + "." + name; + else + return module.substr(0, pos) + "/msg/" + name; } boost::any PropertyConverterRegistry::fromPython(const py::object& po) { @@ -129,30 +135,17 @@ boost::any PropertyConverterRegistry::fromPython(const py::object& po) { if (PyBool_Check(o)) return (o == Py_True); -#if PY_MAJOR_VERSION >= 3 if (PyLong_Check(o)) return PyLong_AS_LONG(o); -#else - if (PyInt_Check(o)) - return PyInt_AS_LONG(o); -#endif if (PyFloat_Check(o)) return PyFloat_AS_DOUBLE(o); -#if PY_MAJOR_VERSION >= 3 if (PyUnicode_Check(o)) -#else - if (PyString_Check(o)) -#endif return py::cast(o); const std::string& ros_msg_name = rosMsgName(o); auto it = REGISTRY_SINGLETON.msg_names_.find(ros_msg_name); - if (it == REGISTRY_SINGLETON.msg_names_.end()) { - std::string msg("No Python -> C++ conversion for: "); - msg += ros_msg_name; - PyErr_SetString(PyExc_TypeError, msg.c_str()); - throw py::error_already_set(); - } + if (it == REGISTRY_SINGLETON.msg_names_.end()) + throw py::type_error("No C++ conversion available for (property) type: " + ros_msg_name); return it->second->second.from_(po); } diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index 8c9b086d1..b9635a808 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -36,7 +36,9 @@ #include #include #include -#include +#include +#include +#include "utils.h" namespace py = pybind11; using namespace py::literals; @@ -47,6 +49,7 @@ PYBIND11_SMART_HOLDER_TYPE_CASTERS(PlannerInterface) PYBIND11_SMART_HOLDER_TYPE_CASTERS(PipelinePlanner) PYBIND11_SMART_HOLDER_TYPE_CASTERS(JointInterpolationPlanner) PYBIND11_SMART_HOLDER_TYPE_CASTERS(CartesianPath) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(MultiPlanner) namespace moveit { namespace python { @@ -66,11 +69,9 @@ void export_solvers(py::module& m) { from moveit.task_constructor import core # Create and configure a planner instance - pipelinePlanner = core.PipelinePlanner() - pipelinePlanner.planner = 'PRMkConfigDefault' + pipelinePlanner = core.PipelinePlanner(node, 'ompl', 'PRMkConfigDefault') pipelinePlanner.num_planning_attempts = 10 )") - .property("planner", "str: Planner ID") .property("num_planning_attempts", "int: Number of planning attempts") .property( "workspace_parameters", @@ -78,7 +79,8 @@ void export_solvers(py::module& m) { .property("goal_joint_tolerance", "float: Tolerance for reaching joint goals") .property("goal_position_tolerance", "float: Tolerance for reaching position goals") .property("goal_orientation_tolerance", "float: Tolerance for reaching orientation goals") - .def(py::init(), "pipeline"_a = std::string("ompl")); + .def(py::init(), "node"_a, + "pipeline"_a = std::string("ompl"), "planner_id"_a = std::string("")); properties::class_( m, "JointInterpolationPlanner", @@ -114,6 +116,27 @@ void export_solvers(py::module& m) { "This values specifies the fraction of mean acceptable joint motion per step.") .property("min_fraction", "float: Fraction of overall distance required to succeed.") .def(py::init<>()); + + properties::class_(m, "MultiPlanner", R"( + A meta planner that runs multiple alternative planners in sequence and returns the first found solution. :: + + from moveit.task_constructor import core + + # Instantiate MultiPlanner + multiPlanner = core.MultiPlanner() + )") + .def("__len__", &MultiPlanner::size) + .def("__getitem__", &get_item) + .def( + "add", + [](MultiPlanner& self, const py::args& args) { + for (auto it = args.begin(), end = args.end(); it != end; ++it) + self.push_back(it->cast()); + }, + "Insert one or more planners") + .def( + "clear", [](MultiPlanner& self) { self.clear(); }, "Remove all planners") + .def(py::init<>()); } } // namespace python } // namespace moveit diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index 082e78865..e86ef4de6 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -38,9 +38,9 @@ #include #include #include -#include +#include #include -#include +#include namespace py = pybind11; using namespace py::literals; @@ -114,7 +114,7 @@ void export_stages(pybind11::module& m) { .def("addObject", &ModifyPlanningScene::addObject, R"( Add a CollisionObject_ to the planning scene - .. _CollisionObject: https://docs.ros.org/en/melodic/api/moveit_msgs/html/msg/CollisionObject.html + .. _CollisionObject: https://docs.ros.org/en/noetic/api/moveit_msgs/html/msg/CollisionObject.html )", "collision_object"_a) .def("removeObject", &ModifyPlanningScene::removeObject, @@ -191,10 +191,13 @@ void export_stages(pybind11::module& m) { int: Set the maximum number of inverse kinematic solutions thats should be generated. )") + .property("max_ik_solutions", "uint: max number of solutions to return") .property("ignore_collisions", R"( bool: Specify if collisions with other members of the planning scene are allowed. )") + .property("min_solution_distance", "reject solution that are closer than this to previously found solutions") + .property("constraints", "additional constraints to obey") .property("ik_frame", R"( PoseStamped_: Specify the frame with respect to which the inverse kinematics @@ -323,6 +326,8 @@ void export_stages(pybind11::module& m) { For an example, see :ref:`How-To-Guides `. )") + .property("merge_mode", "Defines the merge strategy to use") + .property("max_distance", "maximally accepted distance between end and goal sate") .def(py::init(), "name"_a = std::string("connect"), "planners"_a); @@ -352,7 +357,7 @@ void export_stages(pybind11::module& m) { str: Name of the object in the planning scene, attached to the robot which should be placed )") .property("eef", "str: Name of the end effector that should be used for grasping") - .property("pose", R"( + .property("pose", R"( PoseStamped_: The pose where the object should be placed, i.e. states should be sampled .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html @@ -388,7 +393,7 @@ void export_stages(pybind11::module& m) { for an implementation of a task hierarchy that makes use of the ``GeneratePose`` stage. )") - .property("pose", R"( + .property("pose", R"( PoseStamped_: Set the pose, which should be spawned on each new solution of the monitored stage. .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html diff --git a/core/python/bindings/src/utils.h b/core/python/bindings/src/utils.h new file mode 100644 index 000000000..14786d885 --- /dev/null +++ b/core/python/bindings/src/utils.h @@ -0,0 +1,26 @@ +#include + +namespace moveit { +namespace python { +namespace { + +// utility function to normalize index: negative indeces reference from the end +size_t normalize_index(size_t size, long index) { + if (index < 0) + index += size; + if (index >= long(size) || index < 0) + throw pybind11::index_error("Index out of range"); + return index; +} + +// implement operator[](index) +template +typename T::value_type get_item(const T& container, long index) { + auto it = container.begin(); + std::advance(it, normalize_index(container.size(), index)); + return *it; +} + +} // namespace +} // namespace python +} // namespace moveit diff --git a/core/python/src/__init__.py b/core/python/moveit/__init__.py similarity index 100% rename from core/python/src/__init__.py rename to core/python/moveit/__init__.py diff --git a/core/python/src/task_constructor/__init__.py b/core/python/moveit/task_constructor/__init__.py similarity index 100% rename from core/python/src/task_constructor/__init__.py rename to core/python/moveit/task_constructor/__init__.py diff --git a/core/python/src/task_constructor/core.py b/core/python/moveit/task_constructor/core.py similarity index 100% rename from core/python/src/task_constructor/core.py rename to core/python/moveit/task_constructor/core.py diff --git a/core/python/src/task_constructor/stages.py b/core/python/moveit/task_constructor/stages.py similarity index 100% rename from core/python/src/task_constructor/stages.py rename to core/python/moveit/task_constructor/stages.py diff --git a/core/python/pybind11 b/core/python/pybind11 index 4070a64f8..f4bc71f98 160000 --- a/core/python/pybind11 +++ b/core/python/pybind11 @@ -1 +1 @@ -Subproject commit 4070a64f867c60842476f74a2212110ea5e05230 +Subproject commit f4bc71f981d4eb2dd780215fd3c5a7420f1f03aa diff --git a/core/python/src/python_tools/__init__.py b/core/python/src/python_tools/__init__.py deleted file mode 100644 index c6b08468f..000000000 --- a/core/python/src/python_tools/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from pymoveit_python_tools import * diff --git a/core/python/test/rostest_mps.py b/core/python/test/rostest_mps.py index f0c66fa5c..94e735e86 100755 --- a/core/python/test/rostest_mps.py +++ b/core/python/test/rostest_mps.py @@ -1,14 +1,15 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 -from __future__ import print_function import unittest -import rostest -from moveit_commander.roscpp_initializer import roscpp_initialize -from moveit_commander import PlanningSceneInterface +import rclcpp from moveit.task_constructor import core, stages from geometry_msgs.msg import PoseStamped +def setUpModule(): + rclcpp.init() + + def make_pose(x, y, z): pose = PoseStamped() pose.header.frame_id = "base_link" @@ -28,7 +29,9 @@ def setUp(self): 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.node = rclcpp.Node("test_mtc") self.task = task = core.Task() + self.task.loadRobotModel(self.node) task.add(stages.CurrentState("current")) def insertMove(self, position=-1): @@ -113,5 +116,4 @@ def test_bw_remove_object(self): if __name__ == "__main__": - roscpp_initialize("test_mtc") - rostest.rosrun("mtc", "mps", TestModifyPlanningScene) + unittest.main() diff --git a/core/python/test/rostest_mtc.py b/core/python/test/rostest_mtc.py index 958066ad6..fe0432429 100755 --- a/core/python/test/rostest_mtc.py +++ b/core/python/test/rostest_mtc.py @@ -1,19 +1,23 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 -from __future__ import print_function import unittest -import rostest -from moveit_commander.roscpp_initializer import roscpp_initialize +import rclcpp from moveit.task_constructor import core, stages -from geometry_msgs.msg import PoseStamped, Pose +from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import Vector3Stamped, Vector3 from std_msgs.msg import Header -import rospy + + +def setUpModule(): + rclcpp.init() class Test(unittest.TestCase): PLANNING_GROUP = "manipulator" + def setUp(self): + self.node = rclcpp.Node("test_mtc") + def test_MoveAndExecute(self): moveRel = stages.MoveRelative("moveRel", core.JointInterpolationPlanner()) moveRel.group = self.PLANNING_GROUP @@ -24,6 +28,8 @@ def test_MoveAndExecute(self): moveTo.setGoal("all-zeros") task = core.Task() + task.loadRobotModel(self.node) + task.add(stages.CurrentState("current"), moveRel, moveTo) self.assertTrue(task.plan()) @@ -43,6 +49,8 @@ def createDisplacement(group, displacement): return move task = core.Task() + task.loadRobotModel(self.node) + task.add(stages.CurrentState("current")) merger = core.Merger("merger") merger.insert(createDisplacement(self.PLANNING_GROUP, [-0.2, 0, 0])) @@ -55,5 +63,4 @@ def createDisplacement(group, displacement): if __name__ == "__main__": - roscpp_initialize("test_mtc") - rostest.rosrun("mtc", "base", Test) + unittest.main() diff --git a/core/python/test/rostest_mtc.test b/core/python/test/rostest_mtc.test deleted file mode 100644 index 0c34d9a51..000000000 --- a/core/python/test/rostest_mtc.test +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/core/python/test/rostest_trampoline.py b/core/python/test/rostest_trampoline.py index 562adf7b7..af5f2ebd2 100755 --- a/core/python/test/rostest_trampoline.py +++ b/core/python/test/rostest_trampoline.py @@ -1,10 +1,8 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- -from __future__ import print_function import unittest -import rostest -from moveit_commander.roscpp_initializer import roscpp_initialize +import rclcpp from moveit.task_constructor import core, stages from moveit.core.planning_scene import PlanningScene from geometry_msgs.msg import Vector3Stamped, Vector3, PoseStamped @@ -12,11 +10,21 @@ PLANNING_GROUP = "manipulator" -pybind11_versions = [ - k for k in __builtins__.__dict__.keys() if k.startswith("__pybind11_internals_v") -] + +def setUpModule(): + rclcpp.init() + + +def pybind11_versions(): + try: + keys = __builtins__.keys() # for use with pytest + except AttributeError: + keys = __builtins__.__dict__.keys() # use from cmdline + return [k for k in keys if k.startswith("__pybind11_internals_v")] + + incompatible_pybind11_msg = "MoveIt and MTC use incompatible pybind11 versions: " + "\n- ".join( - pybind11_versions + pybind11_versions() ) @@ -91,9 +99,11 @@ class TestTrampolines(unittest.TestCase): def setUp(self): self.cartesian = core.CartesianPath() self.jointspace = core.JointInterpolationPlanner() + self.node = rclcpp.Node("test_mtc") def create(self, *stages): task = core.Task() + task.loadRobotModel(self.node) task.enableIntrospection() for stage in stages: task.add(stage) @@ -106,12 +116,12 @@ def plan(self, task, expected_solutions=None, wait=False): if wait: input("Waiting for any key (allows inspection in rviz)") - @unittest.skipIf(len(pybind11_versions) > 1, incompatible_pybind11_msg) + @unittest.skipIf(len(pybind11_versions()) > 1, incompatible_pybind11_msg) def test_generator(self): task = self.create(PyGenerator()) self.plan(task, expected_solutions=PyGenerator.max_calls) - @unittest.skipIf(len(pybind11_versions) > 1, incompatible_pybind11_msg) + @unittest.skipIf(len(pybind11_versions()) > 1, incompatible_pybind11_msg) def test_monitoring_generator(self): task = self.create( stages.CurrentState("current"), @@ -131,5 +141,4 @@ def test_propagator(self): if __name__ == "__main__": - roscpp_initialize("test_mtc") - rostest.rosrun("mtc", "trampoline", TestTrampolines) + unittest.main() diff --git a/core/python/test/test.launch.py b/core/python/test/test.launch.py new file mode 100644 index 000000000..c9f9a11a1 --- /dev/null +++ b/core/python/test/test.launch.py @@ -0,0 +1,51 @@ +import unittest + +import launch_testing +import pytest +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc +from moveit_configs_utils import MoveItConfigsBuilder + + +@pytest.mark.launch_test +def generate_test_description(): + moveit_config = MoveItConfigsBuilder("moveit_resources_fanuc").to_moveit_configs() + + test_exec = Node( + executable=[LaunchConfiguration("exe")], + output="screen", + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, + ], + ) + + return ( + LaunchDescription( + [ + DeclareLaunchArgument(name="exe"), + test_exec, + KeepAliveProc(), + ReadyToTest(), + ] + ), + {"test_exec": test_exec}, + ) + + +class TestTerminatingProcessStops(unittest.TestCase): + def test_gtest_run_complete(self, proc_info, test_exec): + proc_info.assertWaitForShutdown(process=test_exec, timeout=4000.0) + + +@launch_testing.post_shutdown_test() +class TaskModelTestAfterShutdown(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that all processes in the launch exit with code 0 + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/core/python/test/test_mtc.py b/core/python/test/test_mtc.py index 19cd843b0..77a57645d 100644 --- a/core/python/test/test_mtc.py +++ b/core/python/test/test_mtc.py @@ -1,16 +1,40 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- -from __future__ import print_function -import unittest, sys +import sys +import pytest +import unittest +import rclcpp from geometry_msgs.msg import Pose, PoseStamped, PointStamped, TwistStamped, Vector3Stamped from moveit_msgs.msg import RobotState, Constraints, MotionPlanRequest from moveit.task_constructor import core, stages +def setUpModule(): + rclcpp.init() + + +# When py_binding_tools and MTC are compiled with different pybind11 versions, +# the corresponding classes are not interoperable. +def check_pybind11_incompatibility(): + rclcpp.init([]) + node = rclcpp.Node("dummy") + try: + core.PipelinePlanner(node) + except TypeError: + return True + finally: + rclcpp.shutdown() + return False + + +incompatible_pybind11 = check_pybind11_incompatibility() +incompatible_pybind11_msg = "MoveIt and MTC use incompatible pybind11 versions" + + class TestPropertyMap(unittest.TestCase): - def __init__(self, *args, **kwargs): - super(TestPropertyMap, self).__init__(*args, **kwargs) + def setUp(self): + self.node = rclcpp.Node("test_mtc_props") self.props = core.PropertyMap() def _check(self, name, value): @@ -28,8 +52,9 @@ def test_assign(self): # MotionPlanRequest is not registered as property type and should raise self.assertRaises(TypeError, self._check, "request", MotionPlanRequest()) + @unittest.skipIf(incompatible_pybind11, incompatible_pybind11_msg) def test_assign_in_reference(self): - planner = core.PipelinePlanner() + planner = core.PipelinePlanner(self.node) props = planner.properties props["goal_joint_tolerance"] = 3.14 @@ -41,21 +66,15 @@ def test_assign_in_reference(self): props["planner"] = "planner" self.assertEqual(props["planner"], "planner") - self.assertEqual(planner.planner, "planner") props["double"] = 3.14 a = props props["double"] = 2.71 self.assertEqual(a["double"], 2.71) - planner.planner = "other" - self.assertEqual(props["planner"], "other") - self.assertEqual(planner.planner, "other") - del planner # We can still access props, because actual destruction of planner is delayed self.assertEqual(props["goal_joint_tolerance"], 2.71) - self.assertEqual(props["planner"], "other") def test_iter(self): # assign values so we can iterate over them @@ -85,8 +104,7 @@ def test_expose(self): class TestModifyPlanningScene(unittest.TestCase): - def __init__(self, *args, **kwargs): - super(TestModifyPlanningScene, self).__init__(*args, **kwargs) + def setUp(self): self.mps = stages.ModifyPlanningScene("mps") def test_attach_objects_invalid_args(self): @@ -117,9 +135,10 @@ def test_allow_collisions(self): class TestStages(unittest.TestCase): - def __init__(self, *args, **kwargs): - super(TestStages, self).__init__(*args, **kwargs) - self.planner = core.PipelinePlanner() + @unittest.skipIf(incompatible_pybind11, incompatible_pybind11_msg) + def setUp(self): + self.node = rclcpp.Node("test_mtc_stages") + self.planner = core.PipelinePlanner(self.node) def _check(self, stage, name, value): self._check_assign(stage, name, value) @@ -201,8 +220,7 @@ def test_MoveRelative(self): stage.setDirection({"joint": 0.1}) def test_Connect(self): - planner = core.PipelinePlanner() - stage = stages.Connect("connect", [("group1", planner), ("group2", planner)]) + stage = stages.Connect("connect", [("group1", self.planner), ("group2", self.planner)]) def test_FixCollisionObjects(self): stage = stages.FixCollisionObjects("collision") diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index 6f65c0bb9..bbd1c279e 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -38,6 +38,7 @@ add_library(${PROJECT_NAME} SHARED solvers/multi_planner.cpp ) ament_target_dependencies(${PROJECT_NAME} + fmt moveit_core geometry_msgs moveit_ros_planning diff --git a/core/src/container.cpp b/core/src/container.cpp index fe9f71629..afa0bc9bf 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -47,7 +47,7 @@ #include #include #include -#include +#include #include using namespace std::placeholders; @@ -212,7 +212,7 @@ void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState* // we mark the first state with ARMED and all other states down the tree with PRUNED. // This allows us to re-enable the ARMED state, but not the PRUNED states, // when new states arrive in a Connecting stage. - // For details, https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202 + // For details, https://github.com/moveit/moveit_task_constructor/pull/309#issuecomment-974636202 if (status == InterfaceState::Status::ARMED) status = InterfaceState::Status::PRUNED; // only the first state is marked as ARMED @@ -233,7 +233,10 @@ inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Prio } void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "'" << child.name() << "' generated a failure"); + if (!static_cast(me_)->pruning()) + return; + + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), fmt::format("'{}' generated a failure", child.name())); switch (child.pimpl()->interfaceFlags()) { case GENERATE: // just ignore: the pair of (new) states isn't known to us anyway @@ -323,7 +326,10 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I newSolution(solution); } -ContainerBase::ContainerBase(ContainerBasePrivate* impl) : Stage(impl) {} +ContainerBase::ContainerBase(ContainerBasePrivate* impl) : Stage(impl) { + auto& p = properties(); + p.declare("pruning", std::string("enable pruning?")).configureInitFrom(Stage::PARENT, "pruning"); +} size_t ContainerBase::numChildren() const { return pimpl()->children().size(); @@ -504,9 +510,8 @@ struct SolutionCollector }; void SerialContainer::onNewSolution(const SolutionBase& current) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("SerialContainer"), "'" << this->name() - << "' received solution of child stage '" - << current.creator()->name() << "'"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("SerialContainer"), fmt::format("'{}' received solution of child stage '{}'", + this->name(), current.creator()->name())); // failures should never trigger this callback assert(!current.isFailure()); @@ -575,10 +580,10 @@ void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) else if ((flags1 & READS_END) && (flags2 & WRITES_PREV_END)) stage2.setPrevEnds(stage1.ends()); else { - boost::format desc("cannot connect end interface of '%1%' (%2%) to start interface of '%3%' (%4%)"); - desc % stage1.name() % flowSymbol(flags1); - desc % stage2.name() % flowSymbol(flags2); - throw InitStageException(*me(), desc.str()); + throw InitStageException( + *me(), fmt::format("cannot connect end interface of '{}' ({}) to start interface of '{}' ({})", // + stage1.name(), flowSymbol(flags1), // + stage2.name(), flowSymbol(flags2))); } } @@ -588,12 +593,10 @@ void SerialContainerPrivate::validateInterface(const StagePrivate& child, Interf if (required == UNKNOWN) return; // cannot yet validate InterfaceFlags child_interface = child.interfaceFlags() & mask; - if (required != child_interface) { - boost::format desc("%1% interface (%3%) of '%2%' does not match mine (%4%)"); - desc % (mask == START_IF_MASK ? "start" : "end") % child.name(); - desc % flowSymbol(child_interface) % flowSymbol(required); - throw InitStageException(*me_, desc.str()); - } + if (required != child_interface) + throw InitStageException(*me_, fmt::format("{0} interface ({2}) of '{1}' does not match mine ({3})", + (mask == START_IF_MASK ? "start" : "end"), child.name(), + flowSymbol(child_interface), flowSymbol(required))); } // called by parent asking for pruning of this' interface @@ -651,8 +654,8 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) { exceptions.append(e); } - required_interface_ = (first.pimpl()->interfaceFlags() & START_IF_MASK) | // clang-format off - (last.pimpl()->interfaceFlags() & END_IF_MASK); // clang-format off + required_interface_ = (first.pimpl()->interfaceFlags() & START_IF_MASK) | // + (last.pimpl()->interfaceFlags() & END_IF_MASK); if (exceptions) throw exceptions; @@ -707,7 +710,6 @@ void SerialContainer::compute() { } } - ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name) : ContainerBasePrivate(me, name) {} @@ -746,11 +748,11 @@ void ParallelContainerBasePrivate::initializeExternalInterfaces() { // States received by the container need to be copied to all children's pull interfaces. if (requiredInterface() & READS_START) starts() = std::make_shared([this](Interface::iterator external, Interface::UpdateFlags updated) { - this->propagateStateToAllChildren(external, updated); + this->propagateStateToAllChildren(external, updated); }); if (requiredInterface() & READS_END) ends() = std::make_shared([this](Interface::iterator external, Interface::UpdateFlags updated) { - this->propagateStateToAllChildren(external, updated); + this->propagateStateToAllChildren(external, updated); }); } @@ -765,14 +767,12 @@ void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, valid = valid & ((external & mask) == (child_interface & mask)); } - if (!valid) { - boost::format desc("interface of '%1%' (%3% %4%) does not match %2% (%5% %6%)."); - desc % child.name(); - desc % (first ? "external one" : "other children's"); - desc % flowSymbol(child_interface) % flowSymbol(child_interface); - desc % flowSymbol(external) % flowSymbol(external); - throw InitStageException(*me_, desc.str()); - } + if (!valid) + throw InitStageException( + *me_, fmt::format("interface of '{0}' ({2} {3}) does not match {1} ({4} {5}).", child.name(), + (first ? "external one" : "other children's"), // + flowSymbol(child_interface), flowSymbol(child_interface), + flowSymbol(external), flowSymbol(external))); } void ParallelContainerBasePrivate::validateConnectivity() const { @@ -786,7 +786,8 @@ void ParallelContainerBasePrivate::validateConnectivity() const { } template -void ParallelContainerBasePrivate::propagateStateToAllChildren(Interface::iterator external, Interface::UpdateFlags updated) { +void ParallelContainerBasePrivate::propagateStateToAllChildren(Interface::iterator external, + Interface::UpdateFlags updated) { for (const Stage::pointer& stage : children()) copyState(external, stage->pimpl()->pullInterface(), updated); } @@ -796,8 +797,8 @@ ParallelContainerBase::ParallelContainerBase(const std::string& name) : ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {} void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) { - pimpl()->liftSolution(std::make_shared(this, &solution, cost, std::move(comment)), - solution.start(), solution.end()); + pimpl()->liftSolution(std::make_shared(this, &solution, cost, std::move(comment)), solution.start(), + solution.end()); } void ParallelContainerBase::spawn(InterfaceState&& state, SubTrajectory&& t) { @@ -878,7 +879,7 @@ void Fallbacks::onNewSolution(const SolutionBase& s) { } inline void Fallbacks::replaceImpl() { - FallbacksPrivate *impl = pimpl(); + FallbacksPrivate* impl = pimpl(); if (pimpl()->interfaceFlags() == pimpl()->requiredInterface()) return; switch (pimpl()->requiredInterface()) { @@ -901,11 +902,10 @@ inline void Fallbacks::replaceImpl() { pimpl_ = impl; } -FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name) - : ParallelContainerBasePrivate(me, name) {} +FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} FallbacksPrivate::FallbacksPrivate(FallbacksPrivate&& other) -: ParallelContainerBasePrivate(static_cast(other.me()), "") { + : ParallelContainerBasePrivate(static_cast(other.me()), "") { // move contents of other this->ParallelContainerBasePrivate::operator=(std::move(other)); } @@ -935,8 +935,8 @@ void FallbacksPrivateCommon::reset() { } bool FallbacksPrivateCommon::canCompute() const { - while(current_ != children().end() && // not completely exhausted - !(*current_)->pimpl()->canCompute()) // but current child cannot compute + while (current_ != children().end() && // not completely exhausted + !(*current_)->pimpl()->canCompute()) // but current child cannot compute return const_cast(this)->nextJob(); // advance to next job // return value: current child is well defined and thus can compute? @@ -949,12 +949,14 @@ void FallbacksPrivateCommon::compute() { inline void FallbacksPrivateCommon::nextChild() { if (std::next(current_) != children().end()) - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Fallbacks"), "Child '" << (*current_)->name() << "' failed, trying next one."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Fallbacks"), + fmt::format("Child '{}' failed, trying next one.", (*current_)->name())); ++current_; // advance to next child } -FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old) - : FallbacksPrivateCommon(std::move(old)) { FallbacksPrivateCommon::reset(); } +FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old) : FallbacksPrivateCommon(std::move(old)) { + FallbacksPrivateCommon::reset(); +} bool FallbacksPrivateGenerator::nextJob() { assert(current_ != children().end() && !(*current_)->pimpl()->canCompute()); @@ -965,27 +967,27 @@ bool FallbacksPrivateGenerator::nextJob() { return false; } - do { nextChild(); } - while (current_ != children().end() && !(*current_)->pimpl()->canCompute()); + do { + nextChild(); + } while (current_ != children().end() && !(*current_)->pimpl()->canCompute()); // return value shall indicate current_->canCompute() return current_ != children().end(); } - FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old) - : FallbacksPrivateCommon(std::move(old)) { + : FallbacksPrivateCommon(std::move(old)) { switch (requiredInterface()) { - case PROPAGATE_FORWARDS: - dir_ = Interface::FORWARD; - starts() = std::make_shared(); - break; - case PROPAGATE_BACKWARDS: - dir_ = Interface::BACKWARD; - ends() = std::make_shared(); - break; - default: - assert(false); + case PROPAGATE_FORWARDS: + dir_ = Interface::FORWARD; + starts() = std::make_shared(); + break; + case PROPAGATE_BACKWARDS: + dir_ = Interface::BACKWARD; + ends() = std::make_shared(); + break; + default: + assert(false); } FallbacksPrivatePropagator::reset(); } @@ -1005,8 +1007,8 @@ bool FallbacksPrivatePropagator::nextJob() { assert(current_ != children().end() && !(*current_)->pimpl()->canCompute()); const auto jobs = pullInterface(dir_); - if (job_ != jobs->end()) { // current job exists, but is exhausted on current child - if (!job_has_solutions_) // job didn't produce solutions -> feed to next child + if (job_ != jobs->end()) { // current job exists, but is exhausted on current child + if (!job_has_solutions_) // job didn't produce solutions -> feed to next child nextChild(); else current_ = children().end(); // indicate that this job is exhausted on all children @@ -1024,9 +1026,9 @@ bool FallbacksPrivatePropagator::nextJob() { // pick next job if needed and possible if (job_ == jobs->end()) { // need to pick next job if (!jobs->empty() && jobs->front()->priority().enabled()) - job_ = jobs->begin(); + job_ = jobs->begin(); else - return false; // no more jobs available + return false; // no more jobs available } // When arriving here, we have a valid job_ and a current_ child to feed it. Let's do that. @@ -1034,13 +1036,11 @@ bool FallbacksPrivatePropagator::nextJob() { return true; } - -FallbacksPrivateConnect::FallbacksPrivateConnect(FallbacksPrivate&& old) - : FallbacksPrivate(std::move(old)) { - starts_ = std::make_shared( - std::bind(&FallbacksPrivateConnect::propagateStateUpdate, this, std::placeholders::_1, std::placeholders::_2)); - ends_ = std::make_shared( - std::bind(&FallbacksPrivateConnect::propagateStateUpdate, this, std::placeholders::_1, std::placeholders::_2)); +FallbacksPrivateConnect::FallbacksPrivateConnect(FallbacksPrivate&& old) : FallbacksPrivate(std::move(old)) { + starts_ = std::make_shared(std::bind(&FallbacksPrivateConnect::propagateStateUpdate, + this, std::placeholders::_1, std::placeholders::_2)); + ends_ = std::make_shared(std::bind(&FallbacksPrivateConnect::propagateStateUpdate, + this, std::placeholders::_1, std::placeholders::_2)); FallbacksPrivateConnect::reset(); } @@ -1057,7 +1057,7 @@ void FallbacksPrivateConnect::propagateStateUpdate(Interface::iterator external, } bool FallbacksPrivateConnect::canCompute() const { - for (auto it=children().begin(), end=children().end(); it!=end; ++it) + for (auto it = children().begin(), end = children().end(); it != end; ++it) if ((*it)->pimpl()->canCompute()) { active_ = it; return true; @@ -1074,7 +1074,8 @@ void FallbacksPrivateConnect::compute() { void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { // expect failure to be reported from active child - assert(active_ != children().end() && active_->get() == &child); (void)child; + assert(active_ != children().end() && active_->get() == &child); + (void)child; // ... thus we can use std::next(active_) to find the next child auto next = std::next(active_); @@ -1095,7 +1096,6 @@ void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceSt parent()->pimpl()->onNewFailure(*me(), from, to); } - MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} void MergerPrivate::resolveInterface(InterfaceFlags expected) { @@ -1117,7 +1117,7 @@ void MergerPrivate::resolveInterface(InterfaceFlags expected) { Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) { properties().declare("time_parameterization", - std::make_shared()); + std::make_shared()); } void Merger::reset() { @@ -1167,7 +1167,7 @@ void Merger::onNewSolution(const SolutionBase& s) { void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) { const SubTrajectory* trajectory = dynamic_cast(&s); if (!trajectory || !trajectory->trajectory()) { - RCLCPP_ERROR(rclcpp::get_logger("Merger"), "Only simple, valid trajectories are supported"); + RCLCPP_ERROR(rclcpp::get_logger("Merger"), "Only simple, valid trajectories are supported"); return; } @@ -1294,8 +1294,9 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions, oss << "Invalid waypoint(s): "; if (invalid_index.size() == merged->getWayPointCount()) oss << "all"; - else for (size_t i : invalid_index) - oss << i << ", "; + else + for (size_t i : invalid_index) + oss << i << ", "; t.setComment(oss.str()); } else { // accumulate costs and markers diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index edbcf747b..04dc6b8d8 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -44,7 +44,7 @@ #include -#include +#include #include namespace moveit { @@ -207,9 +207,7 @@ double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) cons return 0.0; if (!traj->getWayPoint(0).knowsFrameTransform(link_name)) { - boost::format desc("LinkMotionCost: frame '%1%' unknown in trajectory"); - desc % link_name; - comment = desc.str(); + comment = fmt::format("LinkMotionCost: frame '{}' unknown in trajectory", link_name); return std::numeric_limits::infinity(); } @@ -275,11 +273,10 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const return result.minimum_distance; } }; - auto collision_comment{ [=](const auto& distance) { - boost::format desc{ PREFIX + "allegedly valid solution collides between '%1%' and '%2%'" }; - desc % distance.link_names[0] % distance.link_names[1]; - return desc.str(); - } }; + auto collision_comment = [=](const auto& distance) { + return fmt::format(PREFIX + "allegedly valid solution collides between '{}' and '{}'", distance.link_names[0], + distance.link_names[1]); + }; double distance{ 0.0 }; @@ -291,13 +288,11 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const return std::numeric_limits::infinity(); } distance = distance_data.distance; - if (!cumulative) { - boost::format desc{ PREFIX + "distance %1% between '%2%' and '%3%'" }; - desc % distance % distance_data.link_names[0] % distance_data.link_names[1]; - comment = desc.str(); - } else { - comment = PREFIX + "cumulative distance " + std::to_string(distance); - } + if (!cumulative) + comment = fmt::format(PREFIX + "distance {} between '{}' and '{}'", distance, distance_data.link_names[0], + distance_data.link_names[1]); + else + comment = fmt::format(PREFIX + "cumulative distance {}", distance); } else { // check trajectory for (size_t i = 0; i < s.trajectory()->getWayPointCount(); ++i) { auto distance_data = check_distance(state, s.trajectory()->getWayPoint(i)); @@ -308,10 +303,7 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const distance += distance_data.distance; } distance /= s.trajectory()->getWayPointCount(); - - boost::format desc(PREFIX + "average%1% distance: %2%"); - desc % (cumulative ? " cumulative" : "") % distance; - comment = desc.str(); + comment = fmt::format(PREFIX + "average{} distance: {}", (cumulative ? " cumulative" : ""), distance); } return distance_to_cost(distance); diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index d8e938483..15a2509a7 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -79,8 +79,9 @@ class IntrospectionExecutor { public: void add_node(const rclcpp::Node::SharedPtr& node) { - std::call_once(once_flag_, [this] { executor_ = rclcpp::executors::SingleThreadedExecutor::make_unique(); }); std::lock_guard lock(mutex_); + if (!executor_) + executor_ = rclcpp::executors::SingleThreadedExecutor::make_unique(); executor_->add_node(node); if (nodes_count_++ == 0) executor_thread_ = std::thread([this] { executor_->spin(); }); @@ -93,6 +94,7 @@ class IntrospectionExecutor executor_->cancel(); if (executor_thread_.joinable()) executor_thread_.join(); + executor_.reset(); } } @@ -101,16 +103,13 @@ class IntrospectionExecutor std::thread executor_thread_; size_t nodes_count_ = 0; std::mutex mutex_; - std::once_flag once_flag_; }; class IntrospectionPrivate { public: IntrospectionPrivate(const TaskPrivate* task, Introspection* self) : task_(task), task_id_(getTaskId(task)) { - rclcpp::NodeOptions options; - options.arguments({ "--ros-args", "-r", "__node:=introspection_" + task_id_ }); - node_ = rclcpp::Node::make_shared("_", task_->ns(), options); + node_ = rclcpp::Node::make_shared("introspection_" + task_id_, task_->ns()); executor_.add_node(node_); task_description_publisher_ = node_->create_publisher( DESCRIPTION_TOPIC, rclcpp::QoS(2).transient_local()); @@ -159,7 +158,7 @@ class IntrospectionPrivate /// services to provide an individual Solution rclcpp::Service::SharedPtr get_solution_service_; rclcpp::Node::SharedPtr node_; - inline static IntrospectionExecutor executor_; + IntrospectionExecutor executor_; /// mapping from stages to their id std::map stage_to_id_map_; diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 4eea5278b..2c504bf55 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -37,7 +37,8 @@ */ #include -#include +#include +#include #include #include #include @@ -292,8 +293,8 @@ void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMa } catch (const Property::undefined&) { } - RCLCPP_DEBUG_STREAM(LOGGER, pair.first << ": " << p.initialized_from_ << " -> " << source << ": " - << Property::serialize(value)); + RCLCPP_DEBUG_STREAM( + LOGGER, fmt::format("{}: {} -> {}: {}", pair.first, p.initialized_from_, source, Property::serialize(value))); p.setCurrentValue(value); p.initialized_from_ = source; } @@ -319,9 +320,8 @@ Property::undefined::undefined(const std::string& name, const std::string& msg) setName(name); } -static boost::format TYPE_ERROR_FMT("type (%1%) doesn't match property's declared type (%2%)"); Property::type_error::type_error(const std::string& current_type, const std::string& declared_type) - : Property::error(boost::str(TYPE_ERROR_FMT % current_type % declared_type)) {} + : Property::error(fmt::format("type {} doesn't match property's declared type {}", current_type, declared_type)) {} } // namespace task_constructor } // namespace moveit diff --git a/core/src/scope_guard b/core/src/scope_guard new file mode 160000 index 000000000..71a045281 --- /dev/null +++ b/core/src/scope_guard @@ -0,0 +1 @@ +Subproject commit 71a04528184db1749dd08ebbbf4daf3b5dca21fd diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index dd02dcab0..588ba837f 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -37,10 +37,16 @@ */ #include +#include #include #include #include #include +#if __has_include() +#include +#else +#include +#endif using namespace trajectory_processing; @@ -52,6 +58,7 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("CartesianPath"); CartesianPath::CartesianPath() { auto& p = properties(); + p.declare("ik_frame", "frame to move linearly (use for joint-space target)"); p.declare("step_size", 0.01, "step size between consecutive waypoints"); p.declare("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step"); p.declare("min_fraction", 1.0, "fraction of motion required for success"); @@ -63,18 +70,31 @@ CartesianPath::CartesianPath() { void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {} +void CartesianPath::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) { + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.frame_id = link; + pose_msg.pose = tf2::toMsg(pose); + setIKFrame(pose_msg); +} + PlannerInterface::Result CartesianPath::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::msg::Constraints& path_constraints) { - const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip(); - if (!link) - return { false, "no unique tip for joint model group: " + jmg->getName() }; + const auto& props = properties(); + const moveit::core::LinkModel* link; + std::string error_msg; + Eigen::Isometry3d ik_pose_world; + + if (!utils::getRobotTipForFrame(props.property("ik_frame"), *from, jmg, error_msg, link, ik_pose_world)) + return { false, error_msg }; + + Eigen::Isometry3d offset = from->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; // reach pose of forward kinematics - return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg, - std::min(timeout, properties().get("timeout")), result, path_constraints); + return plan(from, *link, offset, to->getCurrentState().getGlobalLinkTransform(link), jmg, + std::min(timeout, props.get("timeout")), result, path_constraints); } PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, @@ -110,8 +130,9 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene result->addSuffixWayPoint(waypoint, 0.0); auto timing = props.get("time_parameterization"); - timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), - props.get("max_acceleration_scaling_factor")); + if (timing) + timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), + props.get("max_acceleration_scaling_factor")); if (achieved_fraction < props.get("min_fraction")) { return { false, "min_fraction not met. Achieved: " + std::to_string(achieved_fraction) }; diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index 9fda1daf5..8c092e0e3 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -105,8 +105,9 @@ PlannerInterface::Result JointInterpolationPlanner::plan(const planning_scene::P return { false, "Goal state is out of bounds!" }; auto timing = props.get("time_parameterization"); - timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), - props.get("max_acceleration_scaling_factor")); + if (timing) + 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"); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 4e78b89d8..73536cbf0 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -43,8 +43,7 @@ #include -#include - +#include #include #include #include @@ -149,12 +148,11 @@ void StagePrivate::validateConnectivity() const { // check that the required interface is provided InterfaceFlags required = requiredInterface(); InterfaceFlags actual = interfaceFlags(); - if ((required & actual) != required) { - boost::format desc("actual interface %1% %2% does not match required interface %3% %4%"); - desc % flowSymbol(actual) % flowSymbol(actual); - desc % flowSymbol(required) % flowSymbol(required); - throw InitStageException(*me(), desc.str()); - } + if ((required & actual) != required) + throw InitStageException(*me(), + fmt::format("actual interface {} {} does not match required interface {} {}", + flowSymbol(actual), flowSymbol(actual), + flowSymbol(required), flowSymbol(required))); } bool StagePrivate::storeSolution(const SolutionBasePtr& solution, const InterfaceState* from, @@ -219,28 +217,32 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, newSolution(solution); } -void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) { +void StagePrivate::spawn(InterfaceState&& from, InterfaceState&& to, const SolutionBasePtr& solution) { assert(prevEnds() && nextStarts()); - computeCost(state, state, *solution); + computeCost(from, to, *solution); if (!storeSolution(solution, nullptr, nullptr)) return; // solution dropped - auto from = states_.insert(states_.end(), InterfaceState(state)); // copy - auto to = states_.insert(states_.end(), std::move(state)); + auto from_it = states_.insert(states_.end(), std::move(from)); + auto to_it = states_.insert(states_.end(), std::move(to)); - solution->setStartState(*from); - solution->setEndState(*to); + solution->setStartState(*from_it); + solution->setEndState(*to_it); if (!solution->isFailure()) { - prevEnds()->add(*from); - nextStarts()->add(*to); + prevEnds()->add(*from_it); + nextStarts()->add(*to_it); } newSolution(solution); } +void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) { + spawn(InterfaceState(state), std::move(state), solution); +} + void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) { computeCost(from, to, *solution); @@ -354,7 +356,7 @@ void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) { impl->properties_.reset(); if (impl->parent()) { try { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Properties"), "init '" << name() << "'"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Properties"), fmt::format("init '{}'", name())); impl->properties_.performInitFrom(PARENT, impl->parent()->properties()); } catch (const Property::error& e) { std::ostringstream oss; @@ -544,17 +546,16 @@ void PropagatingEitherWayPrivate::resolveInterface(InterfaceFlags expected) { dir = PropagatingEitherWay::FORWARD; else if ((expected & START_IF_MASK) == WRITES_PREV_END || (expected & END_IF_MASK) == READS_END) dir = PropagatingEitherWay::BACKWARD; - else { - boost::format desc("propagator cannot satisfy expected interface %1% %2%"); - desc % flowSymbol(expected) % flowSymbol(expected); - throw InitStageException(*me(), desc.str()); - } - if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_) { - boost::format desc("configured interface (%1% %2%) does not match expected one (%3% %4%)"); - desc % flowSymbol(required_interface_) % flowSymbol(required_interface_); - desc % flowSymbol(expected) % flowSymbol(expected); - throw InitStageException(*me(), desc.str()); - } + else + throw InitStageException(*me(), + fmt::format("propagator cannot satisfy expected interface {} {}", + flowSymbol(expected), flowSymbol(expected))); + if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_) + throw InitStageException(*me(), + fmt::format("configured interface ({} {}) does not match expected one ({} {})", + flowSymbol(required_interface_), + flowSymbol(required_interface_), + flowSymbol(expected), flowSymbol(expected))); initInterface(dir); } @@ -688,6 +689,10 @@ void GeneratorPrivate::compute() { Generator::Generator(GeneratorPrivate* impl) : ComputeBase(impl) {} Generator::Generator(const std::string& name) : Generator(new GeneratorPrivate(this, name)) {} +void Generator::spawn(InterfaceState&& from, InterfaceState&& to, SubTrajectory&& t) { + pimpl()->spawn(std::move(from), std::move(to), std::make_shared(std::move(t))); +} + void Generator::spawn(InterfaceState&& state, SubTrajectory&& t) { pimpl()->spawn(std::move(state), std::make_shared(std::move(t))); } @@ -804,7 +809,7 @@ void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags continue; // re-enable the opposing state oit (and its associated solution branch) if its status is ARMED - // https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202 + // https://github.com/moveit/moveit_task_constructor/pull/309#issuecomment-974636202 if (oit->priority().status() == InterfaceState::Status::ARMED) { oit_to_enable.push_back(oit); have_enabled_opposites = true; @@ -894,7 +899,6 @@ std::ostream& operator<<(std::ostream& os, const PendingPairsPrinter& p) { os << "---"; return os; } -static const rclcpp::Logger LOGGER = rclcpp::get_logger("Connecting"); Connecting::Connecting(const std::string& name) : ComputeBase(new ConnectingPrivate(this, name)) {} @@ -908,38 +912,33 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta const planning_scene::PlanningSceneConstPtr& from = from_state.scene(); const planning_scene::PlanningSceneConstPtr& to = to_state.scene(); - if (from->getWorld()->size() != to->getWorld()->size()) { - RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different number of collision objects"); + auto false_with_debug = [](auto... args) { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Connecting"), fmt::format(args...)); return false; - } + }; + + if (from->getWorld()->size() != to->getWorld()->size()) + return false_with_debug("{}: different number of collision objects", name()); // both scenes should have the same set of collision objects, at the same location for (const auto& from_object_pair : *from->getWorld()) { const std::string& from_object_name = from_object_pair.first; const collision_detection::World::ObjectPtr& from_object = from_object_pair.second; const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_name); - if (!to_object) { - RCLCPP_DEBUG_STREAM(LOGGER, name() << ": object missing: " << from_object_name); - return false; - } + if (!to_object) + return false_with_debug("{}: object missing: {}", name(), from_object_name); - if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4)) { - RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object pose: " << from_object_name); - return false; // transforms do not match - } + if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4)) + return false_with_debug("{}: different object pose: {}", name(), from_object_name); - if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) { - RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object shapes: " << from_object_name); - return false; // shapes not matching - } + if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) + return false_with_debug("{}: different object shapes: {}", name(), from_object_name); for (auto from_it = from_object->shape_poses_.cbegin(), from_end = from_object->shape_poses_.cend(), to_it = to_object->shape_poses_.cbegin(); from_it != from_end; ++from_it, ++to_it) - if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) { - RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different shape pose: " << from_object_name); - return false; // transforms do not match - } + if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) + return false_with_debug("{}: different shape pose: {}", name(), from_object_name); } // Also test for attached objects which have a different storage @@ -947,41 +946,32 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta std::vector to_attached; from->getCurrentState().getAttachedBodies(from_attached); to->getCurrentState().getAttachedBodies(to_attached); - if (from_attached.size() != to_attached.size()) { - RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different number of objects"); - return false; - } + if (from_attached.size() != to_attached.size()) + return false_with_debug("{}: different number of objects", name()); for (const moveit::core::AttachedBody* from_object : from_attached) { auto it = std::find_if(to_attached.cbegin(), to_attached.cend(), [from_object](const moveit::core::AttachedBody* object) { return object->getName() == from_object->getName(); }); - if (it == to_attached.cend()) { - RCLCPP_DEBUG_STREAM(LOGGER, name() << ": object missing: " << from_object->getName()); - return false; - } + if (it == to_attached.cend()) + return false_with_debug("{}: object missing: {}", name(), from_object->getName()); + const moveit::core::AttachedBody* to_object = *it; - if (from_object->getAttachedLink() != to_object->getAttachedLink()) { - RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different attach links: " << from_object->getName() << " attached to " - << from_object->getAttachedLinkName() << " / " - << to_object->getAttachedLinkName()); - return false; // links not matching - } - if (from_object->getShapes().size() != to_object->getShapes().size()) { - RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object shapes: " << from_object->getName()); - return false; // shapes not matching - } + if (from_object->getAttachedLink() != to_object->getAttachedLink()) + return false_with_debug("{}: different attach links: {} attached to {} vs. {}", // + name(), from_object->getName(), // + from_object->getAttachedLink()->getName(), to_object->getAttachedLink()->getName()); + + if (from_object->getShapes().size() != to_object->getShapes().size()) + return false_with_debug("{}: different object shapes: {}", name(), from_object->getName()); auto from_it = from_object->getShapePosesInLinkFrame().cbegin(); auto from_end = from_object->getShapePosesInLinkFrame().cend(); auto to_it = to_object->getShapePosesInLinkFrame().cbegin(); for (; from_it != from_end; ++from_it, ++to_it) - if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) { - RCLCPP_DEBUG_STREAM(LOGGER, name() - << ": different pose of attached object shape: " << from_object->getName()); - return false; // transforms do not match - } + if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) + return false_with_debug("{}: different pose of attached object shape: {}", name(), from_object->getName()); } return true; } diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 76109283f..0c03ff472 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -11,6 +11,7 @@ add_library(${PROJECT_NAME}_stages SHARED ${PROJECT_INCLUDE}/stages/generate_place_pose.h ${PROJECT_INCLUDE}/stages/compute_ik.h ${PROJECT_INCLUDE}/stages/passthrough.h + ${PROJECT_INCLUDE}/stages/noop.h ${PROJECT_INCLUDE}/stages/predicate_filter.h ${PROJECT_INCLUDE}/stages/connect.h diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 463603122..224044a9c 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -52,6 +52,7 @@ #include #include #include +#include namespace moveit { namespace task_constructor { @@ -68,6 +69,8 @@ ComputeIK::ComputeIK(const std::string& name, Stage::pointer&& child) : WrapperB p.declare("ignore_collisions", false); p.declare("min_solution_distance", 0.1, "minimum distance between seperate IK solutions for the same target"); + p.declare("constraints", moveit_msgs::msg::Constraints(), + "additional constraints to obey"); // ik_frame and target_pose are read from the interface p.declare("ik_frame", "frame to be moved towards goal pose"); @@ -93,8 +96,9 @@ void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string& struct IKSolution { std::vector joint_positions; - bool feasible; collision_detection::Contact contact; + bool collision_free; + bool satisfies_constraints; }; using IKSolutions = std::vector; @@ -303,7 +307,7 @@ void ComputeIK::compute() { tf2::fromMsg(ik_pose_msg.pose, ik_pose); if (!scene->getCurrentState().knowsFrameTransform(ik_pose_msg.header.frame_id)) { - RCLCPP_WARN_STREAM(LOGGER, "ik frame unknown in robot: '" << ik_pose_msg.header.frame_id << "'"); + RCLCPP_WARN_STREAM(LOGGER, fmt::format("ik frame unknown in robot: '{}'", ik_pose_msg.header.frame_id)); return; } ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose; @@ -362,8 +366,11 @@ void ComputeIK::compute() { double min_solution_distance = props.get("min_solution_distance"); + kinematic_constraints::KinematicConstraintSet constraint_set(robot_model); + constraint_set.add(props.get("constraints"), scene->getTransforms()); + IKSolutions ik_solutions; - auto is_valid = [scene, ignore_collisions, min_solution_distance, + auto is_valid = [scene, ignore_collisions, min_solution_distance, &constraint_set = std::as_const(constraint_set), &ik_solutions](moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg, const double* joint_positions) { for (const auto& sol : ik_solutions) { @@ -371,20 +378,28 @@ void ComputeIK::compute() { return false; // too close to already found solution } state->setJointGroupPositions(jmg, joint_positions); + state->update(); + ik_solutions.emplace_back(); auto& solution{ ik_solutions.back() }; state->copyJointGroupPositions(jmg, solution.joint_positions); + + // validate constraints + solution.satisfies_constraints = constraint_set.decide(*state).satisfied; + + // check for collisions collision_detection::CollisionRequest req; 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; + solution.collision_free = ignore_collisions || !res.collision; if (!res.contacts.empty()) { solution.contact = res.contacts.begin()->second.front(); } - return solution.feasible; + + return solution.satisfies_constraints && solution.collision_free; }; uint32_t max_ik_solutions = props.get("max_ik_solutions"); @@ -414,14 +429,16 @@ void ComputeIK::compute() { solution.setComment(s.comment()); std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers())); - if (ik_solutions[i].feasible) + if (ik_solutions[i].collision_free && ik_solutions[i].satisfies_constraints) // compute cost as distance to compare_pose solution.setCost(s.cost() + jmg->distance(ik_solutions[i].joint_positions.data(), compare_pose.data())); - else { // found an IK solution, but this was not valid + else if (!ik_solutions[i].collision_free) { // solution was in collision std::stringstream ss; ss << "Collision between '" << ik_solutions[i].contact.body_name_1 << "' and '" << ik_solutions[i].contact.body_name_2 << "'"; solution.markAsFailure(ss.str()); + } else if (!ik_solutions[i].satisfies_constraints) { // solution was violating constraints + solution.markAsFailure("Constraints violated"); } // set scene's robot state moveit::core::RobotState& solution_state = solution_scene->getCurrentStateNonConst(); diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 571f55deb..509b28f81 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -42,6 +42,15 @@ #include #include +#include +#include + +#if FMT_VERSION >= 90000 +template <> +struct fmt::formatter, 0, Eigen::Stride<0, 0>>>> + : fmt::ostream_formatter +{}; +#endif using namespace trajectory_processing; @@ -96,7 +105,7 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) { try { merged_jmg_.reset(task_constructor::merge(groups)); } catch (const std::runtime_error& e) { - RCLCPP_INFO_STREAM(LOGGER, this->name() << ": " << e.what() << ". Disabling merging."); + RCLCPP_INFO_STREAM(LOGGER, fmt::format("{}: {}. Disabling merging.", this->name(), e.what())); } } @@ -127,8 +136,8 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState& Eigen::Map positions_from(from.getJointPositions(jm), num); Eigen::Map positions_to(to.getJointPositions(jm), num); if (!(positions_from - positions_to).isZero(1e-4)) { - RCLCPP_INFO_STREAM(LOGGER, "Deviation in joint " << jm->getName() << ": [" << positions_from.transpose() - << "] != [" << positions_to.transpose() << "]"); + RCLCPP_INFO_STREAM(LOGGER, fmt::format("Deviation in joint {}: [{}] != [{}]", jm->getName(), + positions_from.transpose(), positions_to.transpose())); return false; } } @@ -261,7 +270,6 @@ SubTrajectoryPtr Connect::merge(const std::vector& sub_ properties().get("path_constraints"))) return SubTrajectoryPtr(); - return std::make_shared(trajectory); return std::make_shared(trajectory, 0.0, std::string(""), planner_ids); } } // namespace stages diff --git a/core/src/stages/current_state.cpp b/core/src/stages/current_state.cpp index f03b37adb..5a5d4c158 100644 --- a/core/src/stages/current_state.cpp +++ b/core/src/stages/current_state.cpp @@ -75,10 +75,7 @@ void CurrentState::compute() { scene_ = std::make_shared(robot_model_); // Add random ID to prevent warnings about multiple publishers within the same node - rclcpp::NodeOptions options; - options.arguments( - { "--ros-args", "-r", "__node:=current_state_" + std::to_string(reinterpret_cast(this)) }); - auto node = rclcpp::Node::make_shared("_", options); + auto node = rclcpp::Node::make_shared("current_state_" + std::to_string(reinterpret_cast(this))); auto client = node->create_client("get_planning_scene"); auto timeout = std::chrono::duration(this->timeout()); diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index 840b700be..f9580c567 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -50,6 +50,7 @@ #endif #include #include +#include namespace vm = visualization_msgs; namespace cd = collision_detection; @@ -84,7 +85,8 @@ bool computeCorrection(const std::vector& contacts, Eigen::Vector3d correction.setZero(); for (const cd::Contact& c : contacts) { if ((c.body_type_1 != cd::BodyTypes::WORLD_OBJECT && c.body_type_2 != cd::BodyTypes::WORLD_OBJECT)) { - RCLCPP_WARN_STREAM(LOGGER, "Cannot fix collision between " << c.body_name_1 << " and " << c.body_name_2); + RCLCPP_WARN_STREAM(LOGGER, + fmt::format("Cannot fix collision between {} and {}", c.body_name_1, c.body_name_2)); return false; } if (c.body_type_1 == cd::BodyTypes::WORLD_OBJECT) diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index ae0cc0e15..5c126f4fd 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -41,6 +41,7 @@ #include #include +#include namespace moveit { namespace task_constructor { @@ -60,8 +61,9 @@ void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& void ModifyPlanningScene::addObject(const moveit_msgs::msg::CollisionObject& collision_object) { if (collision_object.operation != moveit_msgs::msg::CollisionObject::ADD) { - RCLCPP_ERROR_STREAM(LOGGER, name() << ": addObject is called with object's operation not set " - "to ADD -- ignoring the object"); + RCLCPP_ERROR_STREAM( + LOGGER, + fmt::format("{}: addObject is called with object's operation not set to ADD -- ignoring the object", name())); return; } collision_objects_.push_back(collision_object); @@ -144,6 +146,17 @@ std::pair ModifyPlanningScene::apply(const Interf if (callback_) callback_(scene, properties()); + + // check for collisions + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + req.contacts = true; + req.max_contacts = 1; + scene->checkCollision(req, res); + if (res.collision) { + const auto contact = res.contacts.begin()->second.front(); + traj.markAsFailure(contact.body_name_1 + " colliding with " + contact.body_name_2); + } } catch (const std::exception& e) { traj.markAsFailure(e.what()); } diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index af078b4b8..46e4208bc 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -191,6 +191,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning double max_distance = props.get("max_distance"); double min_distance = props.get("min_distance"); + + // if min_distance == max_distance, resort to moving full distance (negative min_distance) + if (max_distance > 0.0 && std::fabs(max_distance - min_distance) < std::numeric_limits::epsilon()) + min_distance = -1.0; + const auto& path_constraints = props.get("path_constraints"); robot_trajectory::RobotTrajectoryPtr robot_trajectory; @@ -207,10 +212,13 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning } else { // Cartesian targets require an IK reference frame const moveit::core::LinkModel* link; + std::string error_msg; Eigen::Isometry3d ik_pose_world; - if (!utils::getRobotTipForFrame(props.property("ik_frame"), *scene, jmg, solution, link, ik_pose_world)) + if (!utils::getRobotTipForFrame(props.property("ik_frame"), *scene, jmg, error_msg, link, ik_pose_world)) { + solution.markAsFailure(error_msg); return false; + } bool use_rotation_distance = false; // measure achieved distance as rotation? Eigen::Vector3d linear; // linear translation @@ -297,7 +305,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning comment = result.message; solution.setPlannerId(planner_->getPlannerId()); - if (robot_trajectory) { // the following requires a robot_trajectory returned from planning + if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) { // the following requires a robot_trajectory + // returned from planning moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); reached_state->updateLinkTransforms(); const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset; @@ -332,7 +341,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning } // store result - if (robot_trajectory) { + if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) { scene->setCurrentState(robot_trajectory->getLastWayPoint()); if (dir == Interface::BACKWARD) robot_trajectory->reverse(); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 30651f9a5..99a6c55d7 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -222,8 +222,12 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP const moveit::core::LinkModel* link; Eigen::Isometry3d ik_pose_world; - if (!utils::getRobotTipForFrame(props.property("ik_frame"), *scene, jmg, solution, link, ik_pose_world)) + std::string error_msg; + + if (!utils::getRobotTipForFrame(props.property("ik_frame"), *scene, jmg, error_msg, link, ik_pose_world)) { + solution.markAsFailure(error_msg); return false; + } if (!getPoseGoal(goal, scene, target) && !getPointGoal(goal, ik_pose_world, scene, target)) { solution.markAsFailure(std::string("invalid goal type: ") + goal.type().name()); diff --git a/core/src/stages/passthrough.cpp b/core/src/stages/passthrough.cpp index beefb21f8..32c78a998 100644 --- a/core/src/stages/passthrough.cpp +++ b/core/src/stages/passthrough.cpp @@ -34,19 +34,6 @@ /* Author: Michael 'v4hn' Goerner */ #include -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include -#include namespace moveit { namespace task_constructor { diff --git a/core/src/task.cpp b/core/src/task.cpp index 0610992cc..4c88d261a 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -46,6 +46,8 @@ #include #include +#include + #include using namespace std::chrono_literals; @@ -95,6 +97,7 @@ const ContainerBase* TaskPrivate::stages() const { Task::Task(const std::string& ns, bool introspection, ContainerBase::pointer&& container) : WrapperBase(new TaskPrivate(this, ns), std::move(container)) { + setPruning(false); setTimeout(std::numeric_limits::max()); // monitor state on commandline @@ -236,6 +239,9 @@ void Task::compute() { } moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) { + // ensure the preempt request is resetted once this method exits + auto guard = sg::make_scope_guard([this]() noexcept { this->resetPreemptRequest(); }); + auto impl = pimpl(); init(); @@ -247,7 +253,6 @@ moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) { explainFailure(); return error_code; }; - impl->preempt_requested_ = false; const double available_time = timeout(); const auto start_time = std::chrono::steady_clock::now(); while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) { @@ -268,13 +273,14 @@ void Task::preempt() { pimpl()->preempt_requested_ = true; } +void Task::resetPreemptRequest() { + pimpl()->preempt_requested_ = false; +} + moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) { // Add random ID to prevent warnings about multiple publishers within the same node - rclcpp::NodeOptions options; - options.arguments( - { "--ros-args", "-r", - "__node:=moveit_task_constructor_executor_" + std::to_string(reinterpret_cast(this)) }); - auto node = rclcpp::Node::make_shared("_", options); + auto node = rclcpp::Node::make_shared("moveit_task_constructor_executor_" + + std::to_string(reinterpret_cast(this))); auto ac = rclcpp_action::create_client( node, "execute_task_solution"); if (!ac->wait_for_action_server(0.5s)) { diff --git a/core/src/utils.cpp b/core/src/utils.cpp index 8cfc480e7..d5ee5119a 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -52,7 +52,7 @@ namespace task_constructor { namespace utils { bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene, - const moveit::core::JointModelGroup* jmg, SolutionBase& solution, + const moveit::core::JointModelGroup* jmg, std::string& error_msg, const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame) { auto get_tip = [&jmg]() -> const moveit::core::LinkModel* { // determine IK frame from group @@ -64,10 +64,12 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin return tips[0]; }; + error_msg = ""; + if (property.value().empty()) { // property undefined robot_link = get_tip(); if (!robot_link) { - solution.markAsFailure("missing ik_frame"); + error_msg = "missing ik_frame"; return false; } tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link); @@ -81,13 +83,13 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin if (!found && !ik_pose_msg.header.frame_id.empty()) { std::stringstream ss; ss << "ik_frame specified in unknown frame '" << ik_pose_msg.header.frame_id << "'"; - solution.markAsFailure(ss.str()); + error_msg = ss.str(); return false; } if (!robot_link) robot_link = get_tip(); if (!robot_link) { - solution.markAsFailure("ik_frame doesn't specify a link frame"); + error_msg = "ik_frame doesn't specify a link frame"; return false; } else if (!found) { // use robot link's frame as reference by default ref_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link); diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index afa8e24c8..5e4ce4b89 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -54,8 +54,8 @@ if (BUILD_TESTING) mtc_add_gmock(test_cost_queue.cpp) mtc_add_gmock(test_interface_state.cpp) - 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_move_to.cpp test.launch.py) + mtc_add_gtest(test_move_relative.cpp test.launch.py) mtc_add_gtest(test_pipeline_planner.cpp) # building these integration tests works without moveit config packages diff --git a/core/test/models.cpp b/core/test/models.cpp index 3abe5ab12..8ee16f792 100644 --- a/core/test/models.cpp +++ b/core/test/models.cpp @@ -17,8 +17,3 @@ RobotModelPtr getModel() { builder.addEndEffector("eef", "link2", "group", "eef_group"); return builder.build(); } - -moveit::core::RobotModelPtr loadModel(const rclcpp::Node::SharedPtr& node) { - robot_model_loader::RobotModelLoader loader(node); - return loader.getModel(); -} diff --git a/core/test/models.h b/core/test/models.h index 760235222..43fbd850a 100644 --- a/core/test/models.h +++ b/core/test/models.h @@ -11,6 +11,3 @@ MOVEIT_CLASS_FORWARD(RobotModel); // get a hard-coded model moveit::core::RobotModelPtr getModel(); - -// load a model from robot_description -moveit::core::RobotModelPtr loadModel(const rclcpp::Node::SharedPtr& node); diff --git a/core/test/pick_pa10.test b/core/test/pick_pa10.test deleted file mode 100644 index 3d62be62d..000000000 --- a/core/test/pick_pa10.test +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/core/test/pick_pr2.test b/core/test/pick_pr2.test deleted file mode 100644 index c2f8767bb..000000000 --- a/core/test/pick_pr2.test +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - [/move_group/fake_controller_joint_states] - - - - - - diff --git a/core/test/pick_ur5.test b/core/test/pick_ur5.test deleted file mode 100644 index 8c152fe43..000000000 --- a/core/test/pick_ur5.test +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - [/move_group/fake_controller_joint_states] - - - - - - diff --git a/core/test/move_to.launch.py b/core/test/test.launch.py similarity index 89% rename from core/test/move_to.launch.py rename to core/test/test.launch.py index 023283bea..dd574a413 100644 --- a/core/test/move_to.launch.py +++ b/core/test/test.launch.py @@ -19,6 +19,10 @@ def generate_test_description(): .to_moveit_configs() ) + # increase the timeout for the kinematics solver to fix flaky IK for asan + k = moveit_config.robot_description_kinematics["robot_description_kinematics"] + k["panda_arm"]["kinematics_solver_timeout"] = 5.0 + test_exec = Node( executable=[ LaunchConfiguration("test_binary"), diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 876a1e6d3..3746c2390 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -672,3 +672,31 @@ TEST(Task, timeout) { EXPECT_TRUE(t.plan()); EXPECT_EQ(t.solutions().size(), 2u); } + +// https://github.com/moveit/moveit_task_constructor/pull/597 +// start planning in another thread, then preempt it in this thread +TEST(Task, preempt) { + moveit::core::MoveItErrorCode ec; + resetMockupIds(); + + Task t; + t.setRobotModel(getModel()); + + auto timeout = std::chrono::milliseconds(10); + t.add(std::make_unique(PredefinedCosts::constant(0.0))); + t.add(std::make_unique(timeout)); + + // preempt before preempt_request_ is reset in plan() + { + std::thread thread{ [&ec, &t, timeout] { + std::this_thread::sleep_for(timeout); + ec = t.plan(1); + } }; + t.preempt(); + thread.join(); + } + + EXPECT_EQ(ec, moveit::core::MoveItErrorCode::PREEMPTED); + EXPECT_EQ(t.solutions().size(), 0u); + EXPECT_TRUE(t.plan(1)); // make sure the preempt request has been resetted on the previous call to plan() +} diff --git a/core/test/test_cost_queue.cpp b/core/test/test_cost_queue.cpp index 1d6657048..4aba46daa 100644 --- a/core/test/test_cost_queue.cpp +++ b/core/test/test_cost_queue.cpp @@ -5,10 +5,6 @@ #include #include -#ifndef TYPED_TEST_SUITE -#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES) -#endif - namespace mtc = moveit::task_constructor; // type-trait functions for OrderedTest diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 1aa3bce80..8b49c8c41 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -157,6 +157,7 @@ TEST(CostTerm, SolutionConnected) { VerifySolutionCostTerm(Standalone& container, Stage* creator) : container_{ container }, creator_{ creator } {} + using TrajectoryCostTerm::operator(); double operator()(const SubTrajectory& s, std::string& /*comment*/) const override { EXPECT_EQ(&*container_.state_start, const_cast(container_.pimpl())->internalToExternalMap().at(s.start())) diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp index d8b07483c..49dbc69c8 100644 --- a/core/test/test_fallback.cpp +++ b/core/test/test_fallback.cpp @@ -1,7 +1,8 @@ #include #include #include -#include +#include +#include #include #include "stage_mockups.h" @@ -160,6 +161,26 @@ TEST_F(FallbacksFixturePropagate, activeChildReset) { EXPECT_COSTS(fwd2->solutions(), testing::IsEmpty()); } +// https://github.com/moveit/moveit_task_constructor/issues/581#issuecomment-2147985474 +TEST_F(FallbacksFixturePropagate, filterPropagatesFailures) { + t.add(std::make_unique(PredefinedCosts::single(0.0))); + + auto fallbacks = std::make_unique("Fallbacks"); + auto add_filtered_fwd = [&fallbacks](double cost, bool accept) { + auto fwd = std::make_unique(PredefinedCosts::constant(cost)); + auto filter = std::make_unique("filter", std::move(fwd)); + filter->setPredicate([accept](const SolutionBase& /*solution*/, std::string& /*comment*/) { return accept; }); + fallbacks->add(std::move(filter)); + }; + add_filtered_fwd(INF, false); // Propagate fails, filter rejects + add_filtered_fwd(2.0, true); // Propagate succeeds, filter accepts + fallbacks->add(std::make_unique()); + t.add(std::move(fallbacks)); + + EXPECT_TRUE(t.plan()); + EXPECT_COSTS(t.solutions(), testing::ElementsAre(2.)); +} + using FallbacksFixtureConnect = TaskTestBase; TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) { diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp index 5a36457f9..80a0ee810 100644 --- a/core/test/test_move_relative.cpp +++ b/core/test/test_move_relative.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -13,6 +14,10 @@ #include +#ifndef TYPED_TEST_SUITE // for Melodic +#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES) +#endif + using namespace moveit::task_constructor; using namespace planning_scene; using namespace moveit::core; @@ -20,40 +25,55 @@ using namespace moveit::core; constexpr double TAU{ 2 * M_PI }; constexpr double EPS{ 5e-5 }; +template +std::shared_ptr create(const rclcpp::Node::SharedPtr&); +template <> +solvers::CartesianPathPtr create(const rclcpp::Node::SharedPtr&) { + return std::make_shared(); +} +template <> +solvers::PipelinePlannerPtr create(const rclcpp::Node::SharedPtr& node) { + auto p = std::make_shared(node, "pilz_industrial_motion_planner", "LIN"); + p->setProperty("max_velocity_scaling_factor", 0.1); + p->setProperty("max_acceleration_scaling_factor", 0.1); + return p; +} + // provide a basic test fixture that prepares a Task +template struct PandaMoveRelative : public testing::Test { Task t; stages::MoveRelative* move; PlanningScenePtr scene; - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("panda_move_relative"); + std::shared_ptr planner; const JointModelGroup* group; - PandaMoveRelative() { - t.setRobotModel(loadModel(node)); + PandaMoveRelative() : planner(create(rclcpp::Node::make_shared("panda_move_relative"))) { + t.loadRobotModel(rclcpp::Node::make_shared("panda_move_relative")); group = t.getRobotModel()->getJointModelGroup("panda_arm"); scene = std::make_shared(t.getRobotModel()); scene->getCurrentStateNonConst().setToDefaultValues(); - scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready"); + scene->getCurrentStateNonConst().setToDefaultValues(group, "ready"); t.add(std::make_unique("start", scene)); - auto move_relative = std::make_unique("move", std::make_shared()); + auto move_relative = std::make_unique("move", planner); move_relative->setGroup(group->getName()); move = move_relative.get(); t.add(std::move(move_relative)); } }; - -moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string& id) { - moveit_msgs::msg::AttachedCollisionObject aco; - aco.link_name = "panda_hand"; - aco.object.header.frame_id = aco.link_name; - aco.object.operation = aco.object.ADD; - aco.object.id = id; - aco.object.primitives.resize(1, [] { +using PandaMoveRelativeCartesian = PandaMoveRelative; + +moveit_msgs::msg::CollisionObject createObject(const std::string& id, const geometry_msgs::msg::Pose& pose) { + moveit_msgs::msg::CollisionObject co; + co.header.frame_id = "panda_hand"; + co.operation = co.ADD; + co.id = id; + co.primitives.resize(1, [] { shape_msgs::msg::SolidPrimitive p; p.type = p.SPHERE; p.dimensions.resize(1); @@ -61,10 +81,23 @@ moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string return p; }()); + co.pose = pose; + return co; +} + +moveit_msgs::msg::CollisionObject createObject(const std::string& id) { geometry_msgs::msg::Pose p; p.position.x = 0.1; p.orientation.w = 1.0; - aco.object.pose = p; + + return createObject(id, p); +} + +moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string& id) { + moveit_msgs::msg::AttachedCollisionObject aco; + aco.link_name = "panda_hand"; + aco.object = createObject(id); + return aco; } @@ -80,13 +113,13 @@ void expect_const_position(const SolutionBaseConstPtr& solution, const std::stri } } -#define EXPECT_CONST_POSITION(...) \ - { \ - SCOPED_TRACE("expect_constant_position(" #__VA_ARGS__ ")"); \ - expect_const_position(__VA_ARGS__); \ +#define EXPECT_CONST_POSITION(...) \ + { \ + SCOPED_TRACE("expect_const_position(" #__VA_ARGS__ ")"); \ + expect_const_position(__VA_ARGS__); \ } -TEST_F(PandaMoveRelative, cartesianRotateEEF) { +TEST_F(PandaMoveRelativeCartesian, cartesianRotateEEF) { move->setDirection([] { geometry_msgs::msg::TwistStamped twist; twist.header.frame_id = "world"; @@ -98,7 +131,7 @@ TEST_F(PandaMoveRelative, cartesianRotateEEF) { EXPECT_CONST_POSITION(move->solutions().front(), group->getOnlyOneEndEffectorTip()->getName()); } -TEST_F(PandaMoveRelative, cartesianCircular) { +TEST_F(PandaMoveRelativeCartesian, cartesianCircular) { const std::string tip = "panda_hand"; auto offset = Eigen::Translation3d(0, 0, 0.1); move->setIKFrame(offset, tip); @@ -113,7 +146,7 @@ TEST_F(PandaMoveRelative, cartesianCircular) { EXPECT_CONST_POSITION(move->solutions().front(), tip, Eigen::Isometry3d(offset)); } -TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) { +TEST_F(PandaMoveRelativeCartesian, cartesianRotateAttachedIKFrame) { const std::string attached_object{ "attached_object" }; scene->processAttachedCollisionObjectMsg(createAttachedObject(attached_object)); move->setIKFrame(attached_object); @@ -129,6 +162,35 @@ TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) { EXPECT_CONST_POSITION(move->solutions().front(), attached_object); } +using PlannerTypes = ::testing::Types; +TYPED_TEST_SUITE(PandaMoveRelative, PlannerTypes); +TYPED_TEST(PandaMoveRelative, cartesianCollisionMinMaxDistance) { + if (std::is_same::value) + GTEST_SKIP(); // Pilz PipelinePlanner current fails this test (see #538) + + const std::string object{ "object" }; + geometry_msgs::msg::Pose object_pose; + object_pose.position.z = 0.4; + object_pose.orientation.w = 1.0; + + this->scene->processCollisionObjectMsg(createObject(object, object_pose)); + + this->move->setIKFrame("panda_hand"); + geometry_msgs::msg::Vector3Stamped v; + v.header.frame_id = "panda_hand"; + v.vector.z = 0.5; + this->move->setDirection(v); + EXPECT_FALSE(this->t.plan()) << "Plan should fail due to collision"; + + this->t.reset(); + v.vector.z = 1.0; + this->move->setDirection(v); + this->move->setMinMaxDistance(0.1, 0.5); + EXPECT_TRUE(this->t.plan()) << "Plan should succeed due to min distance reached"; + auto trajectory = std::dynamic_pointer_cast(this->move->solutions().front()); + EXPECT_TRUE(this->scene->isPathValid(*trajectory->trajectory(), "panda_arm", false)); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp index 7bfc756bf..994b23602 100644 --- a/core/test/test_move_to.cpp +++ b/core/test/test_move_to.cpp @@ -34,20 +34,22 @@ struct PandaMoveTo : public testing::Test Task t; stages::MoveTo* move_to; PlanningScenePtr scene; - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("panda_move_to"); + rclcpp::Node::SharedPtr node; PandaMoveTo() { + node = rclcpp::Node::make_shared("panda_move_to"); t.loadRobotModel(node); + auto group = t.getRobotModel()->getJointModelGroup("panda_arm"); + scene = std::make_shared(t.getRobotModel()); scene->getCurrentStateNonConst().setToDefaultValues(); - scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), - "extended"); + scene->getCurrentStateNonConst().setToDefaultValues(group, "extended"); t.add(std::make_unique("start", scene)); auto move = std::make_unique("move", std::make_shared()); move_to = move.get(); - move_to->setGroup("panda_arm"); + move_to->setGroup(group->getName()); t.add(std::move(move)); } }; @@ -162,7 +164,7 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) { // will strongly deviate from the joint-space goal. TEST(Panda, connectCartesianBranchesFails) { Task t; - t.setRobotModel(loadModel(rclcpp::Node::make_shared("panda_move_to"))); + t.loadRobotModel(rclcpp::Node::make_shared("panda_move_to")); auto scene = std::make_shared(t.getRobotModel()); scene->getCurrentStateNonConst().setToDefaultValues(); scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready"); diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index 60e5d54aa..a9a5fdd56 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -6,13 +6,12 @@ #include #include -#ifndef TYPED_TEST_SUITE -#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES) -#endif - using namespace moveit::task_constructor; -using Pruning = TaskTestBase; +struct Pruning : TaskTestBase +{ + Pruning() : TaskTestBase() { t.setPruning(true); } +}; TEST_F(Pruning, PropagatorFailure) { auto back = add(t, new BackwardMockup()); @@ -25,6 +24,18 @@ TEST_F(Pruning, PropagatorFailure) { EXPECT_EQ(back->runs_, 0u); } +// Same as the previous test, except pruning is disabled for the whole task +TEST_F(Pruning, DisabledPruningPropagatorFailure) { + t.setPruning(false); + auto back = add(t, new BackwardMockup()); + add(t, new GeneratorMockup({ 0 })); + add(t, new ForwardMockup({ INF })); + EXPECT_FALSE(t.plan()); + ASSERT_EQ(t.solutions().size(), 0u); + // ForwardMockup fails, since we have pruning disabled backward should run + EXPECT_EQ(back->runs_, 1u); +} + TEST_F(Pruning, PruningMultiForward) { add(t, new BackwardMockup()); add(t, new BackwardMockup()); @@ -69,23 +80,20 @@ TEST_F(Pruning, ConnectReactivatesPrunedPaths) { // same as before, but wrapping Connect into a container template struct PruningContainerTests : public Pruning -{ - void test() { - add(t, new BackwardMockup); - add(t, new GeneratorMockup({ 0 })); - auto c = new T(); - add(*c, new ConnectMockup()); - add(t, c); - add(t, new GeneratorMockup({ 0 })); - - EXPECT_TRUE(t.plan()); - EXPECT_EQ(t.solutions().size(), 1u); - } -}; -using ContainerTypes = ::testing::Types; // TODO: fails for Fallbacks! +{}; + +using ContainerTypes = ::testing::Types; TYPED_TEST_SUITE(PruningContainerTests, ContainerTypes); TYPED_TEST(PruningContainerTests, ConnectReactivatesPrunedPaths) { - this->test(); + this->add(this->t, new BackwardMockup); + this->add(this->t, new GeneratorMockup({ 0 })); + auto c = new TypeParam(); + this->add(*c, new ConnectMockup()); + this->add(this->t, c); + this->add(this->t, new GeneratorMockup({ 0 })); + + EXPECT_TRUE(this->t.plan()); + EXPECT_EQ(this->t.solutions().size(), 1u); } TEST_F(Pruning, ConnectConnectForward) { @@ -134,6 +142,19 @@ TEST_F(Pruning, PropagateIntoContainer) { EXPECT_EQ(con->runs_, 0u); } +TEST_F(Pruning, DISABLED_PropagateIntoContainerAndReactivate) { + add(t, new GeneratorMockup({ 0 })); + + auto serial = add(t, new SerialContainer()); + auto con = add(*serial, new ConnectMockup({ 10, 20 })); + add(*serial, new GeneratorMockup({ 0, 1 })); + + add(t, new ForwardMockup({ INF, 0 })); + + EXPECT_TRUE(t.plan()); + EXPECT_EQ(con->runs_, 1u); +} + TEST_F(Pruning, PropagateFromContainerPull) { auto back = add(t, new BackwardMockup()); add(t, new BackwardMockup()); diff --git a/core/test/test_serial.cpp b/core/test/test_serial.cpp index aefcba94a..ded84eb95 100644 --- a/core/test/test_serial.cpp +++ b/core/test/test_serial.cpp @@ -46,7 +46,7 @@ struct TestBase : public TaskTestBase }; using ConnectConnect = TestBase; -// https://github.com/ros-planning/moveit_task_constructor/issues/182 +// https://github.com/moveit/moveit_task_constructor/issues/182 TEST_F(ConnectConnect, SuccSucc) { add(t, new GeneratorMockup({ 1.0, 2.0, 3.0 })); add(t, new Connect()); @@ -58,7 +58,7 @@ TEST_F(ConnectConnect, SuccSucc) { EXPECT_COSTS(t.solutions(), ::testing::ElementsAre(11, 12, 13, 21, 22, 23)); } -// https://github.com/ros-planning/moveit_task_constructor/issues/218 +// https://github.com/moveit/moveit_task_constructor/issues/218 TEST_F(ConnectConnect, FailSucc) { add(t, new GeneratorMockup()); add(t, new Connect({ INF }, true)); @@ -70,7 +70,7 @@ TEST_F(ConnectConnect, FailSucc) { EXPECT_FALSE(t.plan()); } -// https://github.com/ros-planning/moveit_task_constructor/issues/485#issuecomment-1760606116 +// https://github.com/moveit/moveit_task_constructor/issues/485#issuecomment-1760606116 TEST_F(ConnectConnect, UniqueEnumeration) { add(t, new GeneratorMockup({ 1.0, 2.0, 3.0 })); auto con1 = add(t, new ConnectMockup()); diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index ea171691e..3125f740d 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -64,6 +64,23 @@ demo(pick_place_demo) target_link_libraries(${PROJECT_NAME}_pick_place_task pick_place_demo_parameters) target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task) +install(PROGRAMS + scripts/alternatives.py + scripts/cartesian.py + scripts/compute_ik.py + scripts/constrained.py + scripts/current_state.py + scripts/fallbacks.py + scripts/fix_collision_objects.py + scripts/fixed_state.py + scripts/generate_pose.py + scripts/merger.py + scripts/modify_planning_scene.py + scripts/multi_planner.py + scripts/pickplace.py + scripts/properties.py + DESTINATION lib/${PROJECT_NAME}) + install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME} ) diff --git a/demo/launch/cartesian.launch.py b/demo/launch/cartesian.launch.py deleted file mode 100644 index e99c283af..000000000 --- a/demo/launch/cartesian.launch.py +++ /dev/null @@ -1,25 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -from moveit_configs_utils import MoveItConfigsBuilder - - -def generate_launch_description(): - moveit_config = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description(file_path="config/panda.urdf.xacro") - .to_moveit_configs() - ) - - cartesian_task = Node( - package="moveit_task_constructor_demo", - executable="cartesian", - output="screen", - parameters=[ - moveit_config.joint_limits, - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - ], - ) - - return LaunchDescription([cartesian_task]) diff --git a/demo/launch/fallbacks_move_to.launch.py b/demo/launch/fallbacks_move_to.launch.py deleted file mode 100644 index f52c9f2fe..000000000 --- a/demo/launch/fallbacks_move_to.launch.py +++ /dev/null @@ -1,28 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -from moveit_configs_utils import MoveItConfigsBuilder - - -def generate_launch_description(): - moveit_config = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description(file_path="config/panda.urdf.xacro") - .planning_pipelines(pipelines=["ompl", "pilz_industrial_motion_planner"]) - .to_moveit_configs() - ) - - fallbacks_move_to_task = Node( - package="moveit_task_constructor_demo", - executable="fallbacks_move_to", - output="screen", - parameters=[ - moveit_config.pilz_cartesian_limits, - moveit_config.joint_limits, - moveit_config.planning_pipelines, - moveit_config.robot_description, - moveit_config.robot_description_kinematics, - moveit_config.robot_description_semantic, - ], - ) - - return LaunchDescription([fallbacks_move_to_task]) diff --git a/demo/launch/ik_clearance_cost.launch.py b/demo/launch/ik_clearance_cost.launch.py deleted file mode 100644 index 348624a51..000000000 --- a/demo/launch/ik_clearance_cost.launch.py +++ /dev/null @@ -1,25 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -from moveit_configs_utils import MoveItConfigsBuilder - - -def generate_launch_description(): - moveit_config = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description(file_path="config/panda.urdf.xacro") - .to_moveit_configs() - ) - - cartesian_task = Node( - package="moveit_task_constructor_demo", - executable="ik_clearance_cost", - output="screen", - parameters=[ - moveit_config.joint_limits, - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - ], - ) - - return LaunchDescription([cartesian_task]) diff --git a/demo/launch/modular.launch.py b/demo/launch/modular.launch.py deleted file mode 100644 index 2b9cda5ee..000000000 --- a/demo/launch/modular.launch.py +++ /dev/null @@ -1,25 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -from moveit_configs_utils import MoveItConfigsBuilder - - -def generate_launch_description(): - moveit_config = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description(file_path="config/panda.urdf.xacro") - .to_moveit_configs() - ) - - modular_task = Node( - package="moveit_task_constructor_demo", - executable="modular", - output="screen", - parameters=[ - moveit_config.joint_limits, - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - ], - ) - - return LaunchDescription([modular_task]) diff --git a/demo/launch/pickplace.launch.py b/demo/launch/pickplace.launch.py deleted file mode 100644 index d332dbdb4..000000000 --- a/demo/launch/pickplace.launch.py +++ /dev/null @@ -1,36 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node -from moveit_configs_utils import MoveItConfigsBuilder - - -def generate_launch_description(): - moveit_config = ( - MoveItConfigsBuilder("moveit_resources_panda") - .planning_pipelines(pipelines=["ompl"]) - .robot_description(file_path="config/panda.urdf.xacro") - .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") - .to_moveit_configs() - ) - - pick_place_demo = Node( - package="moveit_task_constructor_demo", - executable="pick_place_demo", - output="screen", - parameters=[ - os.path.join( - get_package_share_directory("moveit_task_constructor_demo"), - "config", - "panda_config.yaml", - ), - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.planning_pipelines, - moveit_config.joint_limits, - ], - ) - - return LaunchDescription([pick_place_demo]) diff --git a/demo/launch/alternative_path_costs.launch.py b/demo/launch/run.launch.py similarity index 73% rename from demo/launch/alternative_path_costs.launch.py rename to demo/launch/run.launch.py index 33aa1179e..a4cd1b1c3 100644 --- a/demo/launch/alternative_path_costs.launch.py +++ b/demo/launch/run.launch.py @@ -1,4 +1,6 @@ from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from moveit_configs_utils import MoveItConfigsBuilder @@ -7,21 +9,21 @@ def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") - .planning_pipelines(pipelines=["ompl"]) .to_moveit_configs() ) - cartesian_task = Node( + node = Node( package="moveit_task_constructor_demo", - executable="alternative_path_costs", + executable=LaunchConfiguration("exe"), output="screen", parameters=[ - moveit_config.joint_limits, moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, + moveit_config.joint_limits, moveit_config.planning_pipelines, ], ) - return LaunchDescription([cartesian_task]) + arg = DeclareLaunchArgument(name="exe") + return LaunchDescription([arg, node]) diff --git a/demo/package.xml b/demo/package.xml index d872cf1c9..eb34e5003 100644 --- a/demo/package.xml +++ b/demo/package.xml @@ -19,6 +19,7 @@ moveit_configs_utils moveit_resources_panda_moveit_config moveit_task_constructor_capabilities + py_binding_tools ament_cmake diff --git a/demo/scripts/alternatives.py b/demo/scripts/alternatives.py index 7bdfa20e4..3e2f81cda 100755 --- a/demo/scripts/alternatives.py +++ b/demo/scripts/alternatives.py @@ -1,17 +1,20 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit_commander.roscpp_initializer import roscpp_initialize +import rclcpp import time -roscpp_initialize("mtc_tutorial_alternatives") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # Use the joint interpolation planner jointPlanner = core.JointInterpolationPlanner() # Create a task task = core.Task() +task.name = "alternatives" +task.loadRobotModel(node) # Start from current robot state currentState = stages.CurrentState("current state") diff --git a/demo/scripts/cartesian.py b/demo/scripts/cartesian.py index bd304de59..03821fa5b 100755 --- a/demo/scripts/cartesian.py +++ b/demo/scripts/cartesian.py @@ -1,15 +1,17 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- +from math import pi +import time + from std_msgs.msg import Header from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3 from moveit.task_constructor import core, stages -from math import pi -import time -from moveit_commander.roscpp_initializer import roscpp_initialize +import rclcpp -roscpp_initialize("mtc_tutorial") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # [cartesianTut1] group = "panda_arm" @@ -23,6 +25,8 @@ # [cartesianTut3] task = core.Task() +task.name = "cartesian" +task.loadRobotModel(node) # start from current robot state task.add(stages.CurrentState("current state")) @@ -33,7 +37,7 @@ move = stages.MoveRelative("x +0.2", cartesian) move.group = group header = Header(frame_id="world") -move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2, 0, 0))) +move.setDirection(Vector3Stamped(header=header, vector=Vector3(x=0.2, y=0.0, z=0.0))) task.add(move) # [initAndConfigMoveRelative] @@ -41,7 +45,7 @@ # move along y move = stages.MoveRelative("y -0.3", cartesian) move.group = group -move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, -0.3, 0))) +move.setDirection(Vector3Stamped(header=header, vector=Vector3(x=0.0, y=-0.3, z=0.0))) task.add(move) # [cartesianTut4] @@ -49,7 +53,9 @@ # rotate about z move = stages.MoveRelative("rz +45°", cartesian) move.group = group -move.setDirection(TwistStamped(header=header, twist=Twist(angular=Vector3(0, 0, pi / 4.0)))) +move.setDirection( + TwistStamped(header=header, twist=Twist(angular=Vector3(x=0.0, y=0.0, z=pi / 4.0))) +) task.add(move) # [cartesianTut5] diff --git a/demo/scripts/compute_ik.py b/demo/scripts/compute_ik.py index e910991e5..db8838e6f 100755 --- a/demo/scripts/compute_ik.py +++ b/demo/scripts/compute_ik.py @@ -1,26 +1,29 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from geometry_msgs.msg import PoseStamped, Pose, Vector3 +from geometry_msgs.msg import PoseStamped, Pose, Point from std_msgs.msg import Header import time -from moveit_commander.roscpp_initializer import roscpp_initialize +import rclcpp -roscpp_initialize("mtc_tutorial_compute_ik") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # Specify the planning group group = "panda_arm" # Create a task task = core.Task() +task.name = "compute IK" +task.loadRobotModel(node) # Add a stage to retrieve the current state task.add(stages.CurrentState("current state")) # Add a planning stage connecting the generator stages -planner = core.PipelinePlanner() # create default planning pipeline +planner = core.PipelinePlanner(node) # create default planning pipeline task.add(stages.Connect("connect", [(group, planner)])) # operate on group del planner # Delete PipelinePlanner when not explicitly needed anymore @@ -32,7 +35,7 @@ generator.setMonitoredStage(task["current state"]) # Configure target pose # [propertyTut13] -pose = Pose(position=Vector3(z=0.2)) +pose = Pose(position=Point(z=0.2)) generator.pose = PoseStamped(header=Header(frame_id="panda_link8"), pose=pose) # [propertyTut13] diff --git a/demo/scripts/constrained.py b/demo/scripts/constrained.py new file mode 100755 index 000000000..8f0abd3f9 --- /dev/null +++ b/demo/scripts/constrained.py @@ -0,0 +1,67 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +from std_msgs.msg import Header +from geometry_msgs.msg import Vector3Stamped, Vector3, Pose +from moveit_msgs.msg import CollisionObject, Constraints, OrientationConstraint +from shape_msgs.msg import SolidPrimitive +from moveit.task_constructor import core, stages +import time + +import rclcpp + +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") + +group = "panda_arm" +planner = core.PipelinePlanner(node) + +task = core.Task() +task.name = "constrained" +task.loadRobotModel(node) + +task.add(stages.CurrentState("current state")) + +co = CollisionObject() +co.header.frame_id = "world" +co.id = "obstacle" +sphere = SolidPrimitive() +sphere.type = sphere.SPHERE +sphere.dimensions.insert(sphere.SPHERE_RADIUS, 0.1) + +pose = Pose() +pose.position.x = 0.3 +pose.position.y = 0.2 +pose.position.z = 0.5 +pose.orientation.w = 1.0 +co.primitives.append(sphere) +co.primitive_poses.append(pose) +co.operation = co.ADD + +mps = stages.ModifyPlanningScene("modify planning scene") +mps.addObject(co) +task.add(mps) + +move = stages.MoveRelative("y +0.4", planner) +move.group = group +header = Header(frame_id="world") +move.setDirection(Vector3Stamped(header=header, vector=Vector3(x=0.0, y=0.4, z=0.0))) + +constraints = Constraints() +oc = OrientationConstraint() +oc.header.frame_id = "world" +oc.link_name = "panda_hand" +oc.orientation.x = 1.0 +oc.orientation.w = 0.0 +oc.absolute_x_axis_tolerance = 0.01 +oc.absolute_y_axis_tolerance = 0.01 +oc.absolute_z_axis_tolerance = 0.01 +oc.weight = 1.0 +constraints.orientation_constraints.append(oc) +move.path_constraints = constraints + +task.add(move) + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(100) diff --git a/demo/scripts/current_state.py b/demo/scripts/current_state.py index e8011dbe4..82c73a5ac 100755 --- a/demo/scripts/current_state.py +++ b/demo/scripts/current_state.py @@ -1,14 +1,17 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit_commander.roscpp_initializer import roscpp_initialize +import rclcpp import time -roscpp_initialize("mtc_tutorial_current_state") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # Create a task task = core.Task() +task.name = "current state" +task.loadRobotModel(node) # Get the current robot state currentState = stages.CurrentState("current state") diff --git a/demo/scripts/fallbacks.py b/demo/scripts/fallbacks.py index f99016c2c..32c509665 100755 --- a/demo/scripts/fallbacks.py +++ b/demo/scripts/fallbacks.py @@ -1,11 +1,12 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit_commander.roscpp_initializer import roscpp_initialize +import rclcpp import time -roscpp_initialize("mtc_tutorial_fallbacks") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # use cartesian and joint interpolation planners cartesianPlanner = core.CartesianPath() @@ -13,6 +14,8 @@ # initialize the mtc task task = core.Task() +task.name = "fallbacks" +task.loadRobotModel(node) # add the current planning scene state to the task hierarchy currentState = stages.CurrentState("Current State") diff --git a/demo/scripts/fix_collision_objects.py b/demo/scripts/fix_collision_objects.py index 3fdd14e11..ba4cd8587 100755 --- a/demo/scripts/fix_collision_objects.py +++ b/demo/scripts/fix_collision_objects.py @@ -1,14 +1,17 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit_commander.roscpp_initializer import roscpp_initialize +import rclcpp import time -roscpp_initialize("mtc_tutorial_current_state") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # Create a task task = core.Task() +task.name = "fix collision objects" +task.loadRobotModel(node) # Add the current state to the task hierarchy task.add(stages.CurrentState("current state")) diff --git a/demo/scripts/fixed_state.py b/demo/scripts/fixed_state.py index 7ada93eb8..f56a0ec95 100755 --- a/demo/scripts/fixed_state.py +++ b/demo/scripts/fixed_state.py @@ -1,21 +1,22 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.core import planning_scene from moveit.task_constructor import core, stages -from moveit_commander.roscpp_initializer import roscpp_initialize +import rclcpp from moveit.core.planning_scene import PlanningScene import time -roscpp_initialize("mtc_tutorial_current_state") - +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # Create a task task = core.Task() +task.name = "fixed state" # [initAndConfigFixedState] # Initialize a PlanningScene for use in a FixedState stage -task.loadRobotModel() # load the robot model (usually done in init()) +task.loadRobotModel(node) # load the robot model (usually done in init()) planningScene = PlanningScene(task.getRobotModel()) # Create a FixedState stage and pass the created PlanningScene as its state diff --git a/demo/scripts/generate_pose.py b/demo/scripts/generate_pose.py index ce8aa705c..2a20d84cc 100755 --- a/demo/scripts/generate_pose.py +++ b/demo/scripts/generate_pose.py @@ -1,18 +1,21 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages from geometry_msgs.msg import PoseStamped -from moveit_commander.roscpp_initializer import roscpp_initialize +import rclcpp import time -roscpp_initialize("mtc_tutorial_compute_ik") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # Specify the planning group group = "panda_arm" # Create a task task = core.Task() +task.name = "generate pose" +task.loadRobotModel(node) # Get the current robot state currentState = stages.CurrentState("current state") @@ -20,8 +23,7 @@ # Create a planner instance that is used to connect # the current state to the grasp approach pose -pipelinePlanner = core.PipelinePlanner() -pipelinePlanner.planner = "RRTConnectkConfigDefault" +pipelinePlanner = core.PipelinePlanner(node, "ompl", "RRTConnectkConfigDefault") planners = [(group, pipelinePlanner)] # Connect the two stages diff --git a/demo/scripts/merger.py b/demo/scripts/merger.py index f2d261391..279d81cf9 100755 --- a/demo/scripts/merger.py +++ b/demo/scripts/merger.py @@ -1,17 +1,20 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit_commander.roscpp_initializer import roscpp_initialize +import rclcpp import time -roscpp_initialize("mtc_tutorial_merger") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # use the joint interpolation planner planner = core.JointInterpolationPlanner() # the task will contain our stages task = core.Task() +task.name = "merger" +task.loadRobotModel(node) # start from current robot state currentState = stages.CurrentState("current state") diff --git a/demo/scripts/modify_planning_scene.py b/demo/scripts/modify_planning_scene.py index ef5385da3..d64ba7a46 100755 --- a/demo/scripts/modify_planning_scene.py +++ b/demo/scripts/modify_planning_scene.py @@ -1,17 +1,20 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages from moveit_msgs.msg import CollisionObject from shape_msgs.msg import SolidPrimitive from geometry_msgs.msg import PoseStamped -from moveit_commander.roscpp_initializer import roscpp_initialize +import rclcpp import time -roscpp_initialize("mtc_tutorial_modify_planning_scene") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # Create a task task = core.Task() +task.name = "modify planning scene" +task.loadRobotModel(node) # Add the current state to the task hierarchy task.add(stages.CurrentState("current state")) diff --git a/demo/scripts/multi_planner.py b/demo/scripts/multi_planner.py new file mode 100755 index 000000000..948dd96a3 --- /dev/null +++ b/demo/scripts/multi_planner.py @@ -0,0 +1,69 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +import rclcpp +import time + +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") + +ompl_planner = core.PipelinePlanner(node, "ompl", "RRTConnectkConfigDefault") +pilz_planner = core.PipelinePlanner(node, "pilz_industrial_motion_planner", "PTP") +multiPlanner = core.MultiPlanner() +multiPlanner.add(pilz_planner, ompl_planner) + +# Create a task +task = core.Task() +task.name = "multi planner" +task.loadRobotModel(node) + +# Start from current robot state +currentState = stages.CurrentState("current state") + +# Add the current state to the task hierarchy +task.add(currentState) + +# The alternatives stage supports multiple execution paths +alternatives = core.Alternatives("Alternatives") + +# goal 1 +goalConfig1 = { + "panda_joint1": 1.0, + "panda_joint2": -1.0, + "panda_joint3": 0.0, + "panda_joint4": -2.5, + "panda_joint5": 1.0, + "panda_joint6": 1.0, + "panda_joint7": 1.0, +} + +# goal 2 +goalConfig2 = { + "panda_joint1": -3.0, + "panda_joint2": -1.0, + "panda_joint3": 0.0, + "panda_joint4": -2.0, + "panda_joint5": 1.0, + "panda_joint6": 2.0, + "panda_joint7": 0.5, +} + +# First motion plan to compare +moveTo1 = stages.MoveTo("Move To Goal Configuration 1", multiPlanner) +moveTo1.group = "panda_arm" +moveTo1.setGoal(goalConfig1) +alternatives.insert(moveTo1) + +# Second motion plan to compare +moveTo2 = stages.MoveTo("Move To Goal Configuration 2", multiPlanner) +moveTo2.group = "panda_arm" +moveTo2.setGoal(goalConfig2) +alternatives.insert(moveTo2) + +# Add the alternatives stage to the task hierarchy +task.add(alternatives) + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/pickplace.py b/demo/scripts/pickplace.py index e71af604e..e33503495 100755 --- a/demo/scripts/pickplace.py +++ b/demo/scripts/pickplace.py @@ -1,13 +1,14 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- -from moveit_commander.roscpp_initializer import roscpp_initialize +import rclcpp from moveit.task_constructor import core, stages from moveit_commander import PlanningSceneInterface from geometry_msgs.msg import PoseStamped, TwistStamped import time -roscpp_initialize("pickplace") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # [pickAndPlaceTut1] # Specify robot parameters @@ -40,8 +41,8 @@ # [pickAndPlaceTut3] # Create a task -task = core.Task("PandaPickPipelineExample") -task.enableIntrospection() +task = core.Task() +task.name = "pick + place" # [pickAndPlaceTut3] # [pickAndPlaceTut4] @@ -51,8 +52,7 @@ # [initAndConfigConnect] # Create a planner instance that is used to connect # the current state to the grasp approach pose -pipeline = core.PipelinePlanner() -pipeline.planner = "RRTConnectkConfigDefault" +pipeline = core.PipelinePlanner(node, "ompl", "RRTConnectkConfigDefault") planners = [(arm, pipeline)] # Connect the two stages diff --git a/demo/scripts/properties.py b/demo/scripts/properties.py old mode 100644 new mode 100755 index cffc0e0fc..75d32a935 --- a/demo/scripts/properties.py +++ b/demo/scripts/properties.py @@ -1,21 +1,14 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages from geometry_msgs.msg import PoseStamped import time -from moveit_commander.roscpp_initializer import roscpp_initialize +import rclcpp -roscpp_initialize("mtc_tutorial") - -# Create a task container -task = core.Task() - -# [propertyTut10] -# Create a current state to capture the current planning scene state -currentState = stages.CurrentState("Current State") -# [propertyTut10] +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # [propertyTut1] # Create a property @@ -86,15 +79,9 @@ print(i, "\t\t", pm2[i]) print("\n") -# [propertyTut11] +# [propertyTut10] +# Create a stage +stage = stages.CurrentState("Current State") # Access the property map of the stage -props = currentState.properties -# [propertyTut11] - -# Add the stage to the task hierarchy -task.add(currentState) - -if task.plan(): - task.publish(task.solutions[0]) - -time.sleep(100) +props = stage.properties +# [propertyTut10] diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 71c340f76..692961955 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -108,10 +108,13 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t task_.reset(); task_.reset(new moveit::task_constructor::Task()); + // Individual movement stages are collected within the Task object Task& t = *task_; t.stages()->setName(task_name_); t.loadRobotModel(node); + /* Create planners used in various stages. Various options are available, + namely Cartesian, MoveIt pipeline, and joint interpolation. */ // Sampling planner auto sampling_planner = std::make_shared(node); sampling_planner->setProperty("goal_joint_tolerance", 1e-5); @@ -156,7 +159,7 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t * * ***************************************************/ Stage* initial_state_ptr = nullptr; - { // Open Hand + { auto stage = std::make_unique("open hand", sampling_planner); stage->setGroup(params.hand_group_name); stage->setGoal(params.hand_open_pose); @@ -169,7 +172,8 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t * Move to Pick * * * ***************************************************/ - { // Move-to pre-grasp + // Connect initial open-hand state with pre-grasp pose defined in the following + { auto stage = std::make_unique( "move to pick", stages::Connect::GroupPlannerVector{ { params.arm_group_name, sampling_planner } }); stage->setTimeout(5.0); @@ -184,6 +188,7 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t ***************************************************/ Stage* pick_stage_ptr = nullptr; { + // A SerialContainer combines several sub-stages, here for picking the object auto grasp = std::make_unique("pick object"); t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); @@ -192,10 +197,11 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t ---- * Approach Object * ***************************************************/ { + // Move the eef link forward along its z-axis by an amount within the given min-max range auto stage = std::make_unique("approach object", cartesian_planner); stage->properties().set("marker_ns", "approach_object"); - stage->properties().set("link", params.hand_frame); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->properties().set("link", params.hand_frame); // link to perform IK for + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); // inherit group from parent stage stage->setMinMaxDistance(params.approach_object_min_dist, params.approach_object_max_dist); // Set hand forward direction @@ -210,22 +216,24 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t ---- * Generate Grasp Pose * ***************************************************/ { - // Sample grasp pose + // Sample grasp pose candidates in angle increments around the z-axis of the object auto stage = std::make_unique("generate grasp pose"); stage->properties().configureInitFrom(Stage::PARENT); stage->properties().set("marker_ns", "grasp_pose"); stage->setPreGraspPose(params.hand_open_pose); - stage->setObject(params.object_name); + stage->setObject(params.object_name); // object to sample grasps for stage->setAngleDelta(M_PI / 12); stage->setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions - // Compute IK + // Compute IK for sampled grasp poses auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); - wrapper->setMaxIKSolutions(8); + wrapper->setMaxIKSolutions(8); // limit number of solutions wrapper->setMinSolutionDistance(1.0); + // define virtual frame to reach the target_pose wrapper->setIKFrame(vectorToEigen(params.grasp_frame_transform), params.hand_frame); - wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); - wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); // inherit properties from parent + wrapper->properties().configureInitFrom(Stage::INTERFACE, + { "target_pose" }); // inherit property from child solution grasp->insert(std::move(wrapper)); } @@ -233,6 +241,7 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t ---- * Allow Collision (hand object) * ***************************************************/ { + // Modify planning scene (w/o altering the robot's pose) to allow touching the object for picking auto stage = std::make_unique("allow collision (hand,object)"); stage->allowCollisions( params.object_name, @@ -256,7 +265,7 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t ***************************************************/ { auto stage = std::make_unique("attach object"); - stage->attachObject(params.object_name, params.hand_frame); + stage->attachObject(params.object_name, params.hand_frame); // attach object to hand_frame_ grasp->insert(std::move(stage)); } @@ -308,6 +317,7 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t * * *****************************************************/ { + // Connect the grasped state to the pre-place state, i.e. realize the object transport auto stage = std::make_unique( "move to place", stages::Connect::GroupPlannerVector{ { params.arm_group_name, sampling_planner } }); stage->setTimeout(5.0); @@ -320,6 +330,7 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t * Place Object * * * *****************************************************/ + // All placing sub-stages are collected within a serial container again { auto place = std::make_unique("place object"); t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); @@ -454,13 +465,6 @@ bool PickPlaceTask::execute() { moveit_msgs::msg::MoveItErrorCodes execute_result; execute_result = task_->execute(*task_->solutions().front()); - // // If you want to inspect the goal message, use this instead: - // actionlib::SimpleActionClient - // execute("execute_task_solution", true); execute.waitForServer(); - // moveit_task_constructor_msgs::msg::ExecuteTaskSolution::Goal execute_goal; - // task_->solutions().front()->toMsg(execute_goal.solution); - // execute.sendGoalAndWait(execute_goal); - // execute_result = execute.getResult()->error_code; if (execute_result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed and returned: " << execute_result.val); diff --git a/demo/test/pick_place.test b/demo/test/pick_place.test deleted file mode 100644 index 116bafb18..000000000 --- a/demo/test/pick_place.test +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/rviz_marker_tools/CMakeLists.txt b/rviz_marker_tools/CMakeLists.txt index e34709f3d..8a8dc5729 100644 --- a/rviz_marker_tools/CMakeLists.txt +++ b/rviz_marker_tools/CMakeLists.txt @@ -11,7 +11,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS geometry_msgs rclcpp tf2_eigen - urdfdom_headers + urdfdom visualization_msgs ) diff --git a/rviz_marker_tools/package.xml b/rviz_marker_tools/package.xml index c00d024fe..0c7981801 100644 --- a/rviz_marker_tools/package.xml +++ b/rviz_marker_tools/package.xml @@ -16,7 +16,7 @@ moveit_common rclcpp tf2_eigen - urdfdom_headers + urdfdom visualization_msgs diff --git a/visualization/CMakeLists.txt b/visualization/CMakeLists.txt index 50e849561..82de00f24 100644 --- a/visualization/CMakeLists.txt +++ b/visualization/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(moveit_task_constructor_visualization) find_package(ament_cmake REQUIRED) +find_package(fmt REQUIRED) find_package(Boost REQUIRED) find_package(moveit_common REQUIRED) moveit_package() diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 6e7faf184..df63a2164 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -189,11 +189,8 @@ RemoteTaskModel::RemoteTaskModel(const std::string& service_name, const planning : BaseTaskModel(scene, display_context, parent), root_(new Node(nullptr)) { id_to_stage_[0] = root_; // root node has ID 0 // Add random ID to prevent warnings about multiple publishers within the same node - rclcpp::NodeOptions options; - options.arguments({ "--ros-args", "-r", - "__node:=get_solution_node_" + std::to_string(reinterpret_cast(this)), "-r", - "__ns:=/moveit_task_constructor/remote_task_model" }); - node_ = rclcpp::Node::make_shared("_", options); + node_ = rclcpp::Node::make_shared("get_solution_node_" + std::to_string(reinterpret_cast(this)), + "/moveit_task_constructor/remote_task_model"); // service to request solutions get_solution_client_ = node_->create_client(service_name); } diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 605321f9c..9a5453d5f 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -228,9 +228,7 @@ void setExpanded(QTreeView* view, const QModelIndex& index, bool expand, int dep TaskViewPrivate::TaskViewPrivate(TaskView* view) : q_ptr(view) { setupUi(view); - rclcpp::NodeOptions options; - options.arguments({ "--ros-args", "-r", "__node:=task_view_private" }); - node_ = rclcpp::Node::make_shared("_", "", options); + node_ = rclcpp::Node::make_shared("task_view_private", ""); exec_action_client_ = rclcpp_action::create_client( node_, "execute_task_solution"); diff --git a/visualization/motion_planning_tasks/test/CMakeLists.txt b/visualization/motion_planning_tasks/test/CMakeLists.txt index 97667690e..806a01ce0 100644 --- a/visualization/motion_planning_tasks/test/CMakeLists.txt +++ b/visualization/motion_planning_tasks/test/CMakeLists.txt @@ -19,6 +19,6 @@ if (BUILD_TESTING) ament_add_gtest_executable(${PROJECT_NAME}-test-task_model test_task_model.cpp) target_link_libraries(${PROJECT_NAME}-test-task_model motion_planning_tasks_rviz_plugin) - add_launch_test(test_task_model.launch.py - ARGS "test_binary_dir:=$") + add_launch_test(test.launch.py + ARGS "test_binary:=$") endif() diff --git a/visualization/motion_planning_tasks/test/test_task_model.launch.py b/visualization/motion_planning_tasks/test/test.launch.py similarity index 60% rename from visualization/motion_planning_tasks/test/test_task_model.launch.py rename to visualization/motion_planning_tasks/test/test.launch.py index 40cf5cbca..3afb47b02 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.launch.py +++ b/visualization/motion_planning_tasks/test/test.launch.py @@ -13,36 +13,29 @@ @pytest.mark.launch_test def generate_test_description(): - test_task_model = GTest( - path=[ - PathJoinSubstitution( - [ - LaunchConfiguration("test_binary_dir"), - "moveit_task_constructor_visualization-test-task_model", - ] - ) - ], + test_exec = GTest( + path=[LaunchConfiguration("test_binary")], output="screen", ) return ( LaunchDescription( [ DeclareLaunchArgument( - name="test_binary_dir", - description="Binary directory of package containing test executables", + name="test_binary", + description="Test executable", ), - test_task_model, + test_exec, KeepAliveProc(), ReadyToTest(), ] ), - {"test_task_model": test_task_model}, + {"test_exec": test_exec}, ) class TestTerminatingProcessStops(unittest.TestCase): - def test_gtest_run_complete(self, proc_info, test_task_model): - proc_info.assertWaitForShutdown(process=test_task_model, timeout=4000.0) + def test_gtest_run_complete(self, proc_info, test_exec): + proc_info.assertWaitForShutdown(process=test_exec, timeout=4000.0) @launch_testing.post_shutdown_test() diff --git a/visualization/motion_planning_tasks/test/test_merge_models.cpp b/visualization/motion_planning_tasks/test/test_merge_models.cpp index e31a0641d..7a060ab0c 100644 --- a/visualization/motion_planning_tasks/test/test_merge_models.cpp +++ b/visualization/motion_planning_tasks/test/test_merge_models.cpp @@ -60,7 +60,6 @@ void modifySourceModel(QAbstractItemModel* src, T* proxy, const QModelIndex& src bool called = false; const char* new_value = "foo"; -#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) auto con = proxy->connect(proxy, &T::dataChanged, [&called, &src_index, new_value](const QModelIndex& topLeft, const QModelIndex& bottomRight) { @@ -73,14 +72,11 @@ void modifySourceModel(QAbstractItemModel* src, T* proxy, const QModelIndex& src EXPECT_STREQ(topLeft.data().toString().toLatin1().data(), new_value); called = true; }); -#endif EXPECT_TRUE(src->setData(src_index, new_value)); -#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) proxy->disconnect(con); EXPECT_TRUE(called); -#endif } TEST(TreeMergeModel, basics) { diff --git a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp index a8c63f642..fd0720ed4 100644 --- a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp +++ b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp @@ -226,13 +226,8 @@ class FlatMergeProxyModelPrivate void _q_sourceRowsMoved(const QModelIndex& sourceParent, int sourceStart, int sourceEnd, const QModelIndex& destParent, int dest); -#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) // NOLINTNEXTLINE(readability-identifier-naming) void _q_sourceDataChanged(const QModelIndex& topLeft, const QModelIndex& bottomRight, const QVector& roles); -#else - // NOLINTNEXTLINE(readability-identifier-naming) - void _q_sourceDataChanged(const QModelIndex& topLeft, const QModelIndex& bottomRight); // NOLINT -#endif }; FlatMergeProxyModel::FlatMergeProxyModel(QObject* parent) : QAbstractItemModel(parent) { @@ -448,13 +443,8 @@ bool FlatMergeProxyModel::insertModel(QAbstractItemModel* model, int pos) { SLOT(_q_sourceRowsAboutToBeMoved(QModelIndex, int, int, QModelIndex, int))); connect(model, SIGNAL(rowsMoved(QModelIndex, int, int, QModelIndex, int)), this, SLOT(_q_sourceRowsMoved(QModelIndex, int, int, QModelIndex, int))); -#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) connect(model, SIGNAL(dataChanged(QModelIndex, QModelIndex, QVector)), this, SLOT(_q_sourceDataChanged(QModelIndex, QModelIndex, QVector))); -#else - connect(model, SIGNAL(dataChanged(QModelIndex, QModelIndex)), this, - SLOT(_q_sourceDataChanged(QModelIndex, QModelIndex))); -#endif return true; } @@ -504,13 +494,8 @@ void FlatMergeProxyModel::onRemoveModel(QAbstractItemModel* model) { SLOT(_q_sourceRowsAboutToBeMoved(QModelIndex, int, int, QModelIndex, int))); disconnect(model, SIGNAL(rowsMoved(QModelIndex, int, int, QModelIndex, int)), this, SLOT(_q_sourceRowsMoved(QModelIndex, int, int, QModelIndex, int))); -#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) disconnect(model, SIGNAL(dataChanged(QModelIndex, QModelIndex, QVector)), this, SLOT(_q_sourceDataChanged(QModelIndex, QModelIndex, QVector))); -#else - disconnect(model, SIGNAL(dataChanged(QModelIndex, QModelIndex)), this, - SLOT(_q_sourceDataChanged(QModelIndex, QModelIndex))); -#endif } bool FlatMergeProxyModelPrivate::removeModel(std::vector::iterator it, bool call) { @@ -587,18 +572,11 @@ void FlatMergeProxyModelPrivate::_q_sourceRowsRemoved(const QModelIndex& parent, q_ptr->endRemoveRows(); } -#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) // NOLINTNEXTLINE(readability-identifier-naming) void FlatMergeProxyModelPrivate::_q_sourceDataChanged(const QModelIndex& topLeft, const QModelIndex& bottomRight, const QVector& roles) { q_ptr->dataChanged(mapFromSource(topLeft), mapFromSource(bottomRight), roles); } -#else -// NOLINTNEXTLINE(readability-identifier-naming) -void FlatMergeProxyModelPrivate::_q_sourceDataChanged(const QModelIndex& topLeft, const QModelIndex& bottomRight) { - q_ptr->dataChanged(mapFromSource(topLeft), mapFromSource(bottomRight)); -} -#endif } // namespace utils } // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.h b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.h index 653834d3e..feced7e4f 100644 --- a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.h +++ b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.h @@ -110,12 +110,7 @@ public Q_SLOTS: Q_PRIVATE_SLOT(d_func(), void _q_sourceRowsRemoved(QModelIndex, int, int)) Q_PRIVATE_SLOT(d_func(), void _q_sourceRowsAboutToBeMoved(QModelIndex, int, int, QModelIndex, int)) Q_PRIVATE_SLOT(d_func(), void _q_sourceRowsMoved(QModelIndex, int, int, QModelIndex, int)) - -#if QT_VERSION >= 0x050000 // due to moc issue we need to use the hex number Q_PRIVATE_SLOT(d_func(), void _q_sourceDataChanged(QModelIndex, QModelIndex, QVector)) -#else - Q_PRIVATE_SLOT(d_func(), void _q_sourceDataChanged(QModelIndex, QModelIndex)) -#endif }; } // namespace utils } // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp b/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp index 2f54f9c0b..849442393 100644 --- a/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp +++ b/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp @@ -211,13 +211,8 @@ class TreeMergeProxyModelPrivate void _q_sourceRowsMoved(const QModelIndex& sourceParent, int sourceStart, int sourceEnd, const QModelIndex& destParent, int dest); -#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) // NOLINTNEXTLINE(readability-identifier-naming) void _q_sourceDataChanged(const QModelIndex& topLeft, const QModelIndex& bottomRight, const QVector& roles); -#else - // NOLINTNEXTLINE(readability-identifier-naming) - void _q_sourceDataChanged(const QModelIndex& topLeft, const QModelIndex& bottomRight); -#endif }; TreeMergeProxyModel::TreeMergeProxyModel(QObject* parent) : QAbstractItemModel(parent) { @@ -421,13 +416,8 @@ bool TreeMergeProxyModel::insertModel(const QString& name, QAbstractItemModel* m SLOT(_q_sourceRowsAboutToBeMoved(QModelIndex, int, int, QModelIndex, int))); connect(model, SIGNAL(rowsMoved(QModelIndex, int, int, QModelIndex, int)), this, SLOT(_q_sourceRowsMoved(QModelIndex, int, int, QModelIndex, int))); -#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) connect(model, SIGNAL(dataChanged(QModelIndex, QModelIndex, QVector)), this, SLOT(_q_sourceDataChanged(QModelIndex, QModelIndex, QVector))); -#else - connect(model, SIGNAL(dataChanged(QModelIndex, QModelIndex)), this, - SLOT(_q_sourceDataChanged(QModelIndex, QModelIndex))); -#endif return true; } @@ -482,13 +472,8 @@ void TreeMergeProxyModel::onRemoveModel(QAbstractItemModel* model) { SLOT(_q_sourceRowsAboutToBeMoved(QModelIndex, int, int, QModelIndex, int))); QObject::disconnect(model, SIGNAL(rowsMoved(QModelIndex, int, int, QModelIndex, int)), this, SLOT(_q_sourceRowsMoved(QModelIndex, int, int, QModelIndex, int))); -#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) QObject::disconnect(model, SIGNAL(dataChanged(QModelIndex, QModelIndex, QVector)), this, SLOT(_q_sourceDataChanged(QModelIndex, QModelIndex, QVector))); -#else - QObject::disconnect(model, SIGNAL(dataChanged(QModelIndex, QModelIndex)), this, - SLOT(_q_sourceDataChanged(QModelIndex, QModelIndex))); -#endif } bool TreeMergeProxyModelPrivate::removeModel(std::vector::iterator it, bool call) { @@ -559,18 +544,11 @@ void TreeMergeProxyModelPrivate::_q_sourceRowsRemoved(const QModelIndex& parent, q_ptr->endRemoveRows(); } -#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) // NOLINTNEXTLINE(readability-identifier-naming) void TreeMergeProxyModelPrivate::_q_sourceDataChanged(const QModelIndex& topLeft, const QModelIndex& bottomRight, const QVector& roles) { q_ptr->dataChanged(mapFromSource(topLeft), mapFromSource(bottomRight), roles); } -#else -// NOLINTNEXTLINE(readability-identifier-naming) -void TreeMergeProxyModelPrivate::_q_sourceDataChanged(const QModelIndex& topLeft, const QModelIndex& bottomRight) { - q_ptr->dataChanged(mapFromSource(topLeft), mapFromSource(bottomRight)); -} -#endif } // namespace utils } // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.h b/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.h index 0c9580af6..df43128bf 100644 --- a/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.h +++ b/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.h @@ -110,12 +110,7 @@ public Q_SLOTS: Q_PRIVATE_SLOT(d_func(), void _q_sourceRowsRemoved(QModelIndex, int, int)) Q_PRIVATE_SLOT(d_func(), void _q_sourceRowsAboutToBeMoved(QModelIndex, int, int, QModelIndex, int)) Q_PRIVATE_SLOT(d_func(), void _q_sourceRowsMoved(QModelIndex, int, int, QModelIndex, int)) - -#if QT_VERSION >= 0x050000 // due to moc issue we need to use the hex number Q_PRIVATE_SLOT(d_func(), void _q_sourceDataChanged(QModelIndex, QModelIndex, QVector)) -#else - Q_PRIVATE_SLOT(d_func(), void _q_sourceDataChanged(QModelIndex, QModelIndex)) -#endif }; } // namespace utils } // namespace moveit_rviz_plugin diff --git a/visualization/package.xml b/visualization/package.xml index 8a6467fdd..9fca78b1c 100644 --- a/visualization/package.xml +++ b/visualization/package.xml @@ -10,6 +10,7 @@ ament_cmake + fmt qtbase5-dev moveit_core moveit_task_constructor_msgs diff --git a/visualization/visualization_tools/CMakeLists.txt b/visualization/visualization_tools/CMakeLists.txt index 0ae4deb76..3d929e7f2 100644 --- a/visualization/visualization_tools/CMakeLists.txt +++ b/visualization/visualization_tools/CMakeLists.txt @@ -21,6 +21,7 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V target_link_libraries(${MOVEIT_LIB_NAME} ${QT_LIBRARIES} rviz_ogre_vendor::OgreMain + fmt::fmt ) target_include_directories(${MOVEIT_LIB_NAME} PUBLIC include diff --git a/visualization/visualization_tools/src/display_solution.cpp b/visualization/visualization_tools/src/display_solution.cpp index ce3ac3bfa..305cb32b1 100644 --- a/visualization/visualization_tools/src/display_solution.cpp +++ b/visualization/visualization_tools/src/display_solution.cpp @@ -38,8 +38,8 @@ #include #include #include -#include #include +#include static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.display_solution"); @@ -90,11 +90,10 @@ const MarkerVisualizationPtr DisplaySolution::markers(const DisplaySolution::Ind void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& start_scene, const moveit_task_constructor_msgs::msg::Solution& msg) { - if (msg.start_scene.robot_model_name != start_scene->getRobotModel()->getName()) { - static boost::format fmt("Solution for model '%s' but model '%s' was expected"); - fmt % msg.start_scene.robot_model_name.c_str() % start_scene->getRobotModel()->getName().c_str(); - throw std::invalid_argument(fmt.str()); - } + if (msg.start_scene.robot_model_name != start_scene->getRobotModel()->getName()) + throw std::invalid_argument(fmt::format("Solution for model '{}' but model '{}' was expected", + msg.start_scene.robot_model_name, + start_scene->getRobotModel()->getName())); // initialize parent scene from solution's start scene start_scene->setPlanningSceneMsg(msg.start_scene);