From 5ae22da8db152fb449b806211f9fc67f8e4f0757 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 19 Mar 2024 14:47:16 +0100 Subject: [PATCH 01/55] fixup! Add planner_id to SubTrajectory info (#490) --- core/src/stages/connect.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index be88a7538..823750dd1 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -261,7 +261,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 From 560aa0a476c746966fdcdf494d0316f5212a089a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20Garc=C3=ADa=20L=C3=B3pez?= Date: Wed, 20 Mar 2024 15:32:15 +0100 Subject: [PATCH 02/55] Add Generator::spawn(from, to, trajectory) variant (#546) --- core/include/moveit/task_constructor/stage.h | 1 + .../include/moveit/task_constructor/stage_p.h | 1 + core/src/stage.cpp | 24 ++++++++++++------- 3 files changed, 18 insertions(+), 8 deletions(-) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index a4b0eb794..74fb3941e 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -365,6 +365,7 @@ class Generator : public ComputeBase virtual bool canCompute() const = 0; virtual void compute() = 0; + void spawn(InterfaceState&& from, InterfaceState&& to, SubTrajectory&& trajectory); void spawn(InterfaceState&& state, SubTrajectory&& trajectory); void spawn(InterfaceState&& state, double cost) { SubTrajectory trajectory; diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index afbddfac9..858d51ea8 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -136,6 +136,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); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index a88e7016c..171497ebb 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -219,28 +219,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); @@ -688,6 +692,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))); } From 6b0f2c872f68595637e54935e083f15d32924e16 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 19 Mar 2024 15:26:05 +0100 Subject: [PATCH 03/55] rviz_marker_tools: add missing dependency on urdfdom --- rviz_marker_tools/CMakeLists.txt | 3 +++ rviz_marker_tools/package.xml | 1 + 2 files changed, 4 insertions(+) diff --git a/rviz_marker_tools/CMakeLists.txt b/rviz_marker_tools/CMakeLists.txt index eb2c57032..08f6be613 100644 --- a/rviz_marker_tools/CMakeLists.txt +++ b/rviz_marker_tools/CMakeLists.txt @@ -7,6 +7,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp tf2_eigen ) +find_package(urdfdom REQUIRED) catkin_package( LIBRARIES @@ -17,6 +18,8 @@ catkin_package( geometry_msgs visualization_msgs roscpp + DEPENDS + urdfdom ) set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}) diff --git a/rviz_marker_tools/package.xml b/rviz_marker_tools/package.xml index ce77d9aa3..652563746 100644 --- a/rviz_marker_tools/package.xml +++ b/rviz_marker_tools/package.xml @@ -13,4 +13,5 @@ geometry_msgs roscpp tf2_eigen + liburdfdom-dev From 819e560ed4629b121cd5573c35f9bab01eceafe0 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Sun, 5 May 2024 13:30:36 -0400 Subject: [PATCH 04/55] Improve description of max_distance property of Connect stage (#564) --- core/src/stages/connect.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 86dd119e7..11053eb3b 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -55,7 +55,8 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); - p.declare("max_distance", 1e-4, "maximally accepted distance between end and goal sate"); + p.declare("max_distance", 1e-4, + "maximally accepted joint configuration distance between trajectory endpoint and goal state"); p.declare("path_constraints", moveit_msgs::Constraints(), "constraints to maintain during trajectory"); properties().declare("merge_time_parameterization", From 046309d60a35f95f23b6f9f28027a204292718a7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 8 May 2024 10:04:15 +0200 Subject: [PATCH 05/55] Simplify rclcpp::Node creation Directly pass node name and namespace instead of using options. --- core/src/introspection.cpp | 4 +--- core/src/stages/current_state.cpp | 5 +---- core/src/task.cpp | 7 ++----- .../motion_planning_tasks/src/remote_task_model.cpp | 7 ++----- visualization/motion_planning_tasks/src/task_panel.cpp | 4 +--- 5 files changed, 7 insertions(+), 20 deletions(-) diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index d8e938483..2eb1d2a8e 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -108,9 +108,7 @@ 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()); 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/task.cpp b/core/src/task.cpp index 0610992cc..845864b07 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -270,11 +270,8 @@ void Task::preempt() { 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/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"); From ad19ea5479b00523995ac3e86b5b89295787806b Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Sun, 12 May 2024 05:03:47 -0400 Subject: [PATCH 06/55] Add ability to move CollisionObjects (#567) --- .../task_constructor/stages/modify_planning_scene.h | 2 ++ core/src/stages/modify_planning_scene.cpp | 13 ++++++++++++- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h index 802daed33..51972be16 100644 --- a/core/include/moveit/task_constructor/stages/modify_planning_scene.h +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -81,6 +81,8 @@ class ModifyPlanningScene : public PropagatingEitherWay void addObject(const moveit_msgs::CollisionObject& collision_object); /// Remove an object from the planning scene void removeObject(const std::string& object_name); + /// Move an object from the planning scene + void moveObject(const moveit_msgs::CollisionObject& collision_object); /// conviency methods accepting a single object name inline void attachObject(const std::string& object, const std::string& link); diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index 6b1e7fc84..c321e9d1e 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -72,6 +72,15 @@ void ModifyPlanningScene::removeObject(const std::string& object_name) { collision_objects_.push_back(obj); } +void ModifyPlanningScene::moveObject(const moveit_msgs::CollisionObject& collision_object) { + if (collision_object.operation != moveit_msgs::CollisionObject::MOVE) { + ROS_ERROR_STREAM_NAMED("ModifyPlanningScene", name() << ": moveObject is called with object's operation not set " + "to MOVE -- ignoring the object"); + return; + } + collision_objects_.push_back(collision_object); +} + void ModifyPlanningScene::allowCollisions(const Names& first, const Names& second, bool allow) { collision_matrix_edits_.push_back(CollisionMatrixPairs({ first, second, allow })); } @@ -128,7 +137,7 @@ std::pair ModifyPlanningScene::apply(const Interf InterfaceState state(scene); SubTrajectory traj; try { - // add/remove objects + // add/remove/move objects for (auto& collision_object : collision_objects_) processCollisionObject(*scene, collision_object, invert); @@ -157,6 +166,8 @@ void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene& const_cast(object).operation = moveit_msgs::CollisionObject::REMOVE; else if (op == moveit_msgs::CollisionObject::REMOVE) throw std::runtime_error("cannot apply removeObject() backwards"); + else if (op == moveit_msgs::CollisionObject::MOVE) + throw std::runtime_error("cannot apply moveObject() backwards"); } scene.processCollisionObjectMsg(object); From 405755595a82c0434610a0978faedbe7e188579d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 16 Apr 2024 12:51:32 +0200 Subject: [PATCH 07/55] Switch to package py_binding_tools --- .github/workflows/ci.yaml | 2 +- core/CMakeLists.txt | 1 + core/include/moveit/python/task_constructor/properties.h | 2 +- core/package.xml | 2 +- core/python/bindings/src/stages.cpp | 2 +- core/python/test/rostest_mps.py | 4 ++-- core/python/test/rostest_mtc.py | 4 ++-- core/python/test/rostest_trampoline.py | 4 ++-- demo/package.xml | 1 + demo/scripts/alternatives.py | 4 ++-- demo/scripts/cartesian.py | 4 ++-- demo/scripts/compute_ik.py | 4 ++-- demo/scripts/current_state.py | 4 ++-- demo/scripts/fallbacks.py | 4 ++-- demo/scripts/fix_collision_objects.py | 4 ++-- demo/scripts/fixed_state.py | 4 ++-- demo/scripts/generate_pose.py | 4 ++-- demo/scripts/merger.py | 4 ++-- demo/scripts/modify_planning_scene.py | 4 ++-- demo/scripts/pickplace.py | 4 ++-- demo/scripts/properties.py | 4 ++-- 21 files changed, 36 insertions(+), 34 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 45303210e..6b61b816c 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -130,7 +130,7 @@ jobs: - name: Install dependencies run: | apt-get update -q - apt-get install -y doxygen graphviz python3-pip + apt-get install -y doxygen graphviz python3-pip ros-noetic-py-binding-tools pip install -r core/doc/requirements.txt - name: Download ICI workspace diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 8568162a3..1dd5f0f6a 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp visualization_msgs rviz_marker_tools + py_binding_tools ) option(MOVEIT_CI_WARNINGS "Enable all warnings used by CI" OFF) # We use our own set of warnings diff --git a/core/include/moveit/python/task_constructor/properties.h b/core/include/moveit/python/task_constructor/properties.h index a164f5530..9e6a884cc 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 diff --git a/core/package.xml b/core/package.xml index 80d3a7062..1078fb066 100644 --- a/core/package.xml +++ b/core/package.xml @@ -25,6 +25,7 @@ moveit_ros_planning moveit_ros_planning_interface moveit_task_constructor_msgs + py_binding_tools visualization_msgs rviz_marker_tools @@ -32,7 +33,6 @@ rostest moveit_resources_fanuc_moveit_config moveit_planners - moveit_commander diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index 191e13d96..e1c5cc7a5 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include namespace py = pybind11; using namespace py::literals; diff --git a/core/python/test/rostest_mps.py b/core/python/test/rostest_mps.py index 4f49b797a..f9d6164d0 100755 --- a/core/python/test/rostest_mps.py +++ b/core/python/test/rostest_mps.py @@ -3,7 +3,7 @@ from __future__ import print_function import unittest import rostest -from moveit_commander.roscpp_initializer import roscpp_initialize +from py_binding_tools import roscpp_init from moveit_commander import PlanningSceneInterface from moveit.task_constructor import core, stages from geometry_msgs.msg import PoseStamped @@ -113,5 +113,5 @@ def test_bw_remove_object(self): if __name__ == "__main__": - roscpp_initialize("test_mtc") + roscpp_init("test_mtc") rostest.rosrun("mtc", "mps", TestModifyPlanningScene) diff --git a/core/python/test/rostest_mtc.py b/core/python/test/rostest_mtc.py index 958066ad6..214d7449b 100755 --- a/core/python/test/rostest_mtc.py +++ b/core/python/test/rostest_mtc.py @@ -3,7 +3,7 @@ from __future__ import print_function import unittest import rostest -from moveit_commander.roscpp_initializer import roscpp_initialize +from py_binding_tools import roscpp_init from moveit.task_constructor import core, stages from geometry_msgs.msg import PoseStamped, Pose from geometry_msgs.msg import Vector3Stamped, Vector3 @@ -55,5 +55,5 @@ def createDisplacement(group, displacement): if __name__ == "__main__": - roscpp_initialize("test_mtc") + roscpp_init("test_mtc") rostest.rosrun("mtc", "base", Test) diff --git a/core/python/test/rostest_trampoline.py b/core/python/test/rostest_trampoline.py index 562adf7b7..42363d4e8 100755 --- a/core/python/test/rostest_trampoline.py +++ b/core/python/test/rostest_trampoline.py @@ -4,7 +4,7 @@ from __future__ import print_function import unittest import rostest -from moveit_commander.roscpp_initializer import roscpp_initialize +from py_binding_tools import roscpp_init from moveit.task_constructor import core, stages from moveit.core.planning_scene import PlanningScene from geometry_msgs.msg import Vector3Stamped, Vector3, PoseStamped @@ -131,5 +131,5 @@ def test_propagator(self): if __name__ == "__main__": - roscpp_initialize("test_mtc") + roscpp_init("test_mtc") rostest.rosrun("mtc", "trampoline", TestTrampolines) diff --git a/demo/package.xml b/demo/package.xml index 14dc16db5..6b53d334c 100644 --- a/demo/package.xml +++ b/demo/package.xml @@ -23,6 +23,7 @@ moveit_task_constructor_capabilities moveit_resources_panda_moveit_config + py_binding_tools rostest moveit_fake_controller_manager diff --git a/demo/scripts/alternatives.py b/demo/scripts/alternatives.py index 7bdfa20e4..1240af376 100755 --- a/demo/scripts/alternatives.py +++ b/demo/scripts/alternatives.py @@ -2,10 +2,10 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit_commander.roscpp_initializer import roscpp_initialize +from py_binding_tools import roscpp_init import time -roscpp_initialize("mtc_tutorial_alternatives") +roscpp_init("mtc_tutorial_alternatives") # Use the joint interpolation planner jointPlanner = core.JointInterpolationPlanner() diff --git a/demo/scripts/cartesian.py b/demo/scripts/cartesian.py index bd304de59..627fa06e0 100755 --- a/demo/scripts/cartesian.py +++ b/demo/scripts/cartesian.py @@ -7,9 +7,9 @@ from math import pi import time -from moveit_commander.roscpp_initializer import roscpp_initialize +from py_binding_tools import roscpp_init -roscpp_initialize("mtc_tutorial") +roscpp_init("mtc_tutorial") # [cartesianTut1] group = "panda_arm" diff --git a/demo/scripts/compute_ik.py b/demo/scripts/compute_ik.py index e910991e5..1a85f89b8 100755 --- a/demo/scripts/compute_ik.py +++ b/demo/scripts/compute_ik.py @@ -6,9 +6,9 @@ from std_msgs.msg import Header import time -from moveit_commander.roscpp_initializer import roscpp_initialize +from py_binding_tools import roscpp_init -roscpp_initialize("mtc_tutorial_compute_ik") +roscpp_init("mtc_tutorial_compute_ik") # Specify the planning group group = "panda_arm" diff --git a/demo/scripts/current_state.py b/demo/scripts/current_state.py index e8011dbe4..f4ef0ab3a 100755 --- a/demo/scripts/current_state.py +++ b/demo/scripts/current_state.py @@ -2,10 +2,10 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit_commander.roscpp_initializer import roscpp_initialize +from py_binding_tools import roscpp_init import time -roscpp_initialize("mtc_tutorial_current_state") +roscpp_init("mtc_tutorial_current_state") # Create a task task = core.Task() diff --git a/demo/scripts/fallbacks.py b/demo/scripts/fallbacks.py index f99016c2c..0c1ae6c12 100755 --- a/demo/scripts/fallbacks.py +++ b/demo/scripts/fallbacks.py @@ -2,10 +2,10 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit_commander.roscpp_initializer import roscpp_initialize +from py_binding_tools import roscpp_init import time -roscpp_initialize("mtc_tutorial_fallbacks") +roscpp_init("mtc_tutorial_fallbacks") # use cartesian and joint interpolation planners cartesianPlanner = core.CartesianPath() diff --git a/demo/scripts/fix_collision_objects.py b/demo/scripts/fix_collision_objects.py index 3fdd14e11..3104b4c03 100755 --- a/demo/scripts/fix_collision_objects.py +++ b/demo/scripts/fix_collision_objects.py @@ -2,10 +2,10 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit_commander.roscpp_initializer import roscpp_initialize +from py_binding_tools import roscpp_init import time -roscpp_initialize("mtc_tutorial_current_state") +roscpp_init("mtc_tutorial_current_state") # Create a task task = core.Task() diff --git a/demo/scripts/fixed_state.py b/demo/scripts/fixed_state.py index 7ada93eb8..99b5df709 100755 --- a/demo/scripts/fixed_state.py +++ b/demo/scripts/fixed_state.py @@ -3,11 +3,11 @@ from moveit.core import planning_scene from moveit.task_constructor import core, stages -from moveit_commander.roscpp_initializer import roscpp_initialize +from py_binding_tools import roscpp_init from moveit.core.planning_scene import PlanningScene import time -roscpp_initialize("mtc_tutorial_current_state") +roscpp_init("mtc_tutorial_current_state") # Create a task diff --git a/demo/scripts/generate_pose.py b/demo/scripts/generate_pose.py index ce8aa705c..c9503d42c 100755 --- a/demo/scripts/generate_pose.py +++ b/demo/scripts/generate_pose.py @@ -3,10 +3,10 @@ from moveit.task_constructor import core, stages from geometry_msgs.msg import PoseStamped -from moveit_commander.roscpp_initializer import roscpp_initialize +from py_binding_tools import roscpp_init import time -roscpp_initialize("mtc_tutorial_compute_ik") +roscpp_init("mtc_tutorial_compute_ik") # Specify the planning group group = "panda_arm" diff --git a/demo/scripts/merger.py b/demo/scripts/merger.py index f2d261391..9c0c28d46 100755 --- a/demo/scripts/merger.py +++ b/demo/scripts/merger.py @@ -2,10 +2,10 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit_commander.roscpp_initializer import roscpp_initialize +from py_binding_tools import roscpp_init import time -roscpp_initialize("mtc_tutorial_merger") +roscpp_init("mtc_tutorial_merger") # use the joint interpolation planner planner = core.JointInterpolationPlanner() diff --git a/demo/scripts/modify_planning_scene.py b/demo/scripts/modify_planning_scene.py index ef5385da3..d68a721d3 100755 --- a/demo/scripts/modify_planning_scene.py +++ b/demo/scripts/modify_planning_scene.py @@ -5,10 +5,10 @@ from moveit_msgs.msg import CollisionObject from shape_msgs.msg import SolidPrimitive from geometry_msgs.msg import PoseStamped -from moveit_commander.roscpp_initializer import roscpp_initialize +from py_binding_tools import roscpp_init import time -roscpp_initialize("mtc_tutorial_modify_planning_scene") +roscpp_init("mtc_tutorial_modify_planning_scene") # Create a task task = core.Task() diff --git a/demo/scripts/pickplace.py b/demo/scripts/pickplace.py index e71af604e..4723450a6 100755 --- a/demo/scripts/pickplace.py +++ b/demo/scripts/pickplace.py @@ -1,13 +1,13 @@ #! /usr/bin/env python # -*- coding: utf-8 -*- -from moveit_commander.roscpp_initializer import roscpp_initialize +from py_binding_tools import roscpp_init from moveit.task_constructor import core, stages from moveit_commander import PlanningSceneInterface from geometry_msgs.msg import PoseStamped, TwistStamped import time -roscpp_initialize("pickplace") +roscpp_init("pickplace") # [pickAndPlaceTut1] # Specify robot parameters diff --git a/demo/scripts/properties.py b/demo/scripts/properties.py index cffc0e0fc..d7198e1e0 100644 --- a/demo/scripts/properties.py +++ b/demo/scripts/properties.py @@ -5,9 +5,9 @@ from geometry_msgs.msg import PoseStamped import time -from moveit_commander.roscpp_initializer import roscpp_initialize +from py_binding_tools import roscpp_init -roscpp_initialize("mtc_tutorial") +roscpp_init("mtc_tutorial") # Create a task container task = core.Task() From 9fd2f1f858fef7ef6b7423f2033653b0cd8eb661 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 16 Apr 2024 19:57:00 +0200 Subject: [PATCH 08/55] Drop Melodic support - py_binding_tools is released into Noetic only - drop Python2 support - drop TYPED_TEST_CASE --- .github/workflows/ci.yaml | 4 ---- capabilities/CMakeLists.txt | 5 ----- capabilities/src/execute_task_solution_capability.cpp | 5 ----- core/python/bindings/src/properties.cpp | 9 --------- core/python/bindings/src/stages.cpp | 2 +- core/test/test_cost_queue.cpp | 4 ---- core/test/test_pruning.cpp | 4 ---- 7 files changed, 1 insertion(+), 32 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 6b61b816c..3649aeafc 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -19,10 +19,6 @@ jobs: fail-fast: false matrix: env: - - IMAGE: master-source - TARGET_CMAKE_ARGS: >- - -DCMAKE_BUILD_TYPE=Release - -DCMAKE_CXX_FLAGS="-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls" - IMAGE: noetic-source NAME: ccov TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage" diff --git a/capabilities/CMakeLists.txt b/capabilities/CMakeLists.txt index 5a9fa0a55..d0503a91f 100644 --- a/capabilities/CMakeLists.txt +++ b/capabilities/CMakeLists.txt @@ -26,16 +26,11 @@ set(CMAKE_REQUIRED_INCLUDES ${catkin_INCLUDE_DIRS}) set(CMAKE_REQUIRED_LIBRARIES ${catkin_LIBRARIES}) set(CMAKE_REQUIRED_FLAGS -Wno-error) include(CheckCXXSymbolExists) -check_cxx_symbol_exists(moveit::core::MoveItErrorCode::toString "moveit/utils/moveit_error_code.h" HAVE_MOVEIT_ERROR_CODE_TO_STRING) -if(NOT HAVE_MOVEIT_ERROR_CODE_TO_STRING) - set(HAVE_MOVEIT_ERROR_CODE_TO_STRING 0) -endif() add_library(${PROJECT_NAME} src/execute_task_solution_capability.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) -target_compile_definitions(${PROJECT_NAME} PRIVATE HAVE_MOVEIT_ERROR_CODE_TO_STRING=${HAVE_MOVEIT_ERROR_CODE_TO_STRING}) target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 423129edf..3c127d5dd 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -109,12 +109,7 @@ void ExecuteTaskSolutionCapability::execCallback( result.error_code = context_->plan_execution_->executeAndMonitor(plan); } -#if HAVE_MOVEIT_ERROR_CODE_TO_STRING const std::string response = moveit::core::MoveItErrorCode::toString(result.error_code); -#else - const std::string response = context_->plan_execution_->getErrorCodeString(result.error_code); -#endif - if (result.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) as_->setSucceeded(result, response); else if (result.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED) diff --git a/core/python/bindings/src/properties.cpp b/core/python/bindings/src/properties.cpp index e53d7a293..e6e147aa3 100644 --- a/core/python/bindings/src/properties.cpp +++ b/core/python/bindings/src/properties.cpp @@ -129,20 +129,11 @@ 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); diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index e1c5cc7a5..4c79080bf 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -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, 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_pruning.cpp b/core/test/test_pruning.cpp index 60e5d54aa..3be841178 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -6,10 +6,6 @@ #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; From e2812d933357e42066bc9c5ba7d0d7d2a5a03990 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 16 Apr 2024 19:57:20 +0200 Subject: [PATCH 09/55] Drop Kinetic support --- .../test/test_merge_models.cpp | 4 ---- .../utils/flat_merge_proxy_model.cpp | 22 ------------------- .../utils/flat_merge_proxy_model.h | 5 ----- .../utils/tree_merge_proxy_model.cpp | 22 ------------------- .../utils/tree_merge_proxy_model.h | 5 ----- .../visualization_tools/CMakeLists.txt | 5 ----- .../src/marker_visualization.cpp | 13 ----------- 7 files changed, 76 deletions(-) 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/visualization_tools/CMakeLists.txt b/visualization/visualization_tools/CMakeLists.txt index de0eb39d9..58cb29f1a 100644 --- a/visualization/visualization_tools/CMakeLists.txt +++ b/visualization/visualization_tools/CMakeLists.txt @@ -2,11 +2,6 @@ set(MOVEIT_LIB_NAME moveit_task_visualization_tools) set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/visualization_tools) -# TODO: Remove when Kinetic support is dropped -if(rviz_VERSION VERSION_LESS 1.13.1) # Does rviz supports TF2? - add_definitions(-DRVIZ_TF1) -endif() - set(HEADERS ${PROJECT_INCLUDE}/display_solution.h ${PROJECT_INCLUDE}/marker_visualization.h diff --git a/visualization/visualization_tools/src/marker_visualization.cpp b/visualization/visualization_tools/src/marker_visualization.cpp index 9e3ceaed4..94714990d 100644 --- a/visualization/visualization_tools/src/marker_visualization.cpp +++ b/visualization/visualization_tools/src/marker_visualization.cpp @@ -7,9 +7,6 @@ #include #include #include -#ifndef RVIZ_TF1 -#include -#endif #include #include @@ -71,15 +68,6 @@ bool MarkerVisualization::createMarkers(rviz::DisplayContext* context, Ogre::Sce Ogre::Vector3 pos = Ogre::Vector3::ZERO; try { -#ifdef RVIZ_TF1 - tf::TransformListener* tf = context->getFrameManager()->getTFClient(); - tf::StampedTransform tm; - tf->lookupTransform(planning_frame_, fixed_frame, ros::Time(), tm); - auto q = tm.getRotation(); - auto p = tm.getOrigin(); - quat = Ogre::Quaternion(q.w(), -q.x(), -q.y(), -q.z()); - pos = Ogre::Vector3(p.x(), p.y(), p.z()); -#else std::shared_ptr tf = context->getFrameManager()->getTF2BufferPtr(); geometry_msgs::TransformStamped tm; tm = tf->lookupTransform(planning_frame_, fixed_frame, ros::Time()); @@ -87,7 +75,6 @@ bool MarkerVisualization::createMarkers(rviz::DisplayContext* context, Ogre::Sce auto p = tm.transform.translation; quat = Ogre::Quaternion(q.w, -q.x, -q.y, -q.z); pos = Ogre::Vector3(p.x, p.y, p.z); -#endif } catch (const tf2::TransformException& e) { ROS_WARN_STREAM_NAMED("MarkerVisualization", e.what()); return false; From 92b917d2640a0ac13a4534cf982e9598be05c5f9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 16 Apr 2024 21:35:38 +0200 Subject: [PATCH 10/55] CI: update actions --- .github/workflows/ci.yaml | 8 ++++---- .pre-commit-config.yaml | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 3649aeafc..83a0d3e9c 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -105,7 +105,7 @@ jobs: workdir: ${{ env.BASEDIR }}/target_ws ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"' - name: Upload codecov report - uses: codecov/codecov-action@v3 + uses: codecov/codecov-action@v4 if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0' with: files: ${{ env.BASEDIR }}/target_ws/coverage.info @@ -121,7 +121,7 @@ jobs: - name: Checkout uses: actions/checkout@v4 - name: Setup Pages - uses: actions/configure-pages@v2 + uses: actions/configure-pages@v5 - name: Install dependencies run: | @@ -146,7 +146,7 @@ jobs: sphinx-build -W -b linkcheck core/doc _site - name: Upload pages artifact - uses: actions/upload-pages-artifact@v1 + uses: actions/upload-pages-artifact@v3 # Deployment job deploy: @@ -159,4 +159,4 @@ jobs: steps: - name: Deploy to GitHub Pages id: deployment - uses: actions/deploy-pages@v1 + uses: actions/deploy-pages@v4 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 961699b0e..1c9190e56 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.0.1 + rev: v4.6.0 hooks: - id: check-added-large-files - id: check-case-conflict @@ -29,7 +29,7 @@ repos: - id: trailing-whitespace - repo: https://github.com/psf/black - rev: 22.3.0 + rev: 24.4.2 hooks: - id: black args: ["--line-length", "100"] From 8ede80d26d98a8dff8846a480107fa2b59640eb5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 2 May 2024 16:34:47 +0200 Subject: [PATCH 11/55] Example of constrained orientation planning --- demo/scripts/constrained.py | 62 +++++++++++++++++++++++++++++++++++++ 1 file changed, 62 insertions(+) create mode 100644 demo/scripts/constrained.py diff --git a/demo/scripts/constrained.py b/demo/scripts/constrained.py new file mode 100644 index 000000000..c211efdfa --- /dev/null +++ b/demo/scripts/constrained.py @@ -0,0 +1,62 @@ +#! /usr/bin/env python +# -*- 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 + +from py_binding_tools import roscpp_init + +roscpp_init("mtc_tutorial") + +group = "panda_arm" +planner = core.PipelinePlanner() + +task = core.Task() +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(0, 0.4, 0))) + +constraints = Constraints() +oc = OrientationConstraint() +oc.header.frame_id = "world" +oc.link_name = "panda_hand" +oc.orientation.x = 1.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) From 74b1e5e5d3a3a9bc6f358139f5c7c1aac130c970 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 15 May 2024 12:55:55 +0200 Subject: [PATCH 12/55] CI: prerelease.yaml: make ROS_DISTRO an interactive input --- .github/workflows/prerelease.yaml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) 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" From a8896e4e5d76d37726278eeb3328daa2c71156e5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 23 May 2024 13:53:12 +0200 Subject: [PATCH 13/55] Improve TypeError exceptions - use pybind11 API instead of Python C API - provide more informative error messages --- core/python/bindings/src/core.cpp | 5 ++-- core/python/bindings/src/properties.cpp | 31 ++++++++++++------------- core/python/bindings/src/stages.cpp | 2 ++ 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp index d3f953aeb..a9f017ca1 100644 --- a/core/python/bindings/src/core.cpp +++ b/core/python/bindings/src/core.cpp @@ -86,9 +86,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); } diff --git a/core/python/bindings/src/properties.cpp b/core/python/bindings/src/properties.cpp index e6e147aa3..142ec3e74 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); @@ -115,12 +121,9 @@ 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(); + } catch (const py::error_already_set& e) { + // object is not a ROS message type, return it's class name instead + return o.attr("__class__").attr("__name__").cast(); } } @@ -138,12 +141,8 @@ boost::any PropertyConverterRegistry::fromPython(const py::object& po) { 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/stages.cpp b/core/python/bindings/src/stages.cpp index 4c79080bf..db263ffa5 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -323,6 +323,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); From 9c05305effa7173a94f3befdae09386763e04f0a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 24 May 2024 10:49:14 +0200 Subject: [PATCH 14/55] ModifyPlanningScene: check state for collisions --- core/src/stages/modify_planning_scene.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index c321e9d1e..c24a18a1d 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -151,6 +151,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()); } From 54e653ebdbdee4085c2e0dba040fd4f037f5df9e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 24 May 2024 12:02:20 +0200 Subject: [PATCH 15/55] Fix failing assertion The cost of a newly created Priority does not need to be finite. An example occurs in the Pick+Place demo: the CurrentState solution is filtered by an applicibility test, which may set the cost to infinity while lifting the solution to the wrapper stage. In this case, new InterfaceStates are created from the infinite cost solution. Consequently, the state should be marked as PRUNED. --- core/include/moveit/task_constructor/storage.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 0b7dd54cb..e0796980d 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -94,10 +94,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) {} From a0c00646c2c93da9eea3d625f0865e8d22b248e4 Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Fri, 24 May 2024 08:34:27 -0400 Subject: [PATCH 16/55] Add NoOp stage (#534) This stage can be used to add arbitrary user-defined properties w/o modifying the PlanningScene state or adding a trajectory. Co-authored-by: Robert Haschke --- .../moveit/task_constructor/stages/noop.h | 64 +++++++++++++++++++ core/src/stages/CMakeLists.txt | 1 + 2 files changed, 65 insertions(+) create mode 100644 core/include/moveit/task_constructor/stages/noop.h 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/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index cc0fa03b2..25b1e5d64 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -11,6 +11,7 @@ add_library(${PROJECT_NAME}_stages ${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 From 7666f73e046afa5b5f43b555032cb77a82cdaf64 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 25 May 2024 19:12:42 +0200 Subject: [PATCH 17/55] Simplify formatting code with https://github.com/fmtlib (#499) * Simplify formatting code with https://github.com/fmtlib * Update to clang-format-12 --- .github/workflows/format.yaml | 4 +- .pre-commit-config.yaml | 2 +- capabilities/CMakeLists.txt | 3 +- capabilities/package.xml | 1 + .../src/execute_task_solution_capability.cpp | 6 +- core/CMakeLists.txt | 1 + .../include/moveit/task_constructor/stage_p.h | 3 +- core/package.xml | 1 + core/src/CMakeLists.txt | 2 +- core/src/container.cpp | 135 +++++++++--------- core/src/cost_terms.cpp | 36 ++--- core/src/properties.cpp | 10 +- core/src/stage.cpp | 107 ++++++-------- core/src/stages/compute_ik.cpp | 6 +- core/src/stages/connect.cpp | 8 +- core/src/stages/fix_collision_objects.cpp | 3 +- core/src/stages/modify_planning_scene.cpp | 8 +- core/src/stages/simple_grasp.cpp | 2 +- core/test/test_stage.cpp | 2 +- visualization/CMakeLists.txt | 1 + visualization/package.xml | 1 + .../visualization_tools/CMakeLists.txt | 1 + .../src/display_solution.cpp | 11 +- 23 files changed, 170 insertions(+), 184 deletions(-) diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index 3058626b9..a40899e8d 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -16,8 +16,8 @@ jobs: - uses: actions/checkout@v4 with: submodules: recursive - - name: Install clang-format-10 - run: sudo apt-get install clang-format-10 + - name: Install clang-format-12 + run: sudo apt-get install clang-format-12 - uses: rhaschke/install-catkin_lint-action@v1.0 with: distro: noetic diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 1c9190e56..aa133c806 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -39,7 +39,7 @@ repos: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format-10 + entry: clang-format-12 language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ["-fallback-style=none", "-i"] diff --git a/capabilities/CMakeLists.txt b/capabilities/CMakeLists.txt index d0503a91f..a5cf37ce0 100644 --- a/capabilities/CMakeLists.txt +++ b/capabilities/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_task_constructor_capabilities) +find_package(fmt REQUIRED) find_package(catkin REQUIRED COMPONENTS actionlib moveit_core @@ -32,7 +33,7 @@ add_library(${PROJECT_NAME} ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} fmt::fmt) install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/capabilities/package.xml b/capabilities/package.xml index 98487b18b..28390cb00 100644 --- a/capabilities/package.xml +++ b/capabilities/package.xml @@ -12,6 +12,7 @@ catkin + fmt moveit_core moveit_ros_move_group actionlib diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 3c127d5dd..2a3afa9ed 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -43,6 +43,7 @@ #include #include #include +#include namespace { @@ -152,8 +153,9 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr if (!joint_names.empty()) { group = findJointModelGroup(*model, joint_names); if (!group) { - ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "Could not find JointModelGroup that actuates {" - << boost::algorithm::join(joint_names, ", ") << "}"); + ROS_ERROR_STREAM_NAMED( + "ExecuteTaskSolution", + fmt::format("Could not find JointModelGroup that actuates {{{}}}", fmt::join(joint_names, ", "))); return false; } ROS_DEBUG_NAMED("ExecuteTaskSolution", "Using JointModelGroup '%s' for execution", diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 1dd5f0f6a..12af7292b 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_task_constructor_core) find_package(Boost REQUIRED) +find_package(fmt REQUIRED) find_package(catkin REQUIRED COMPONENTS roslint tf2_eigen diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 858d51ea8..5d359b5c6 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -44,6 +44,7 @@ #include #include +#include #include #include @@ -144,7 +145,7 @@ class StagePrivate void newSolution(const SolutionBasePtr& solution); bool storeFailures() const { return introspection_ != nullptr; } void runCompute() { - ROS_DEBUG_STREAM_NAMED("Stage", "Computing stage '" << name() << "'"); + ROS_DEBUG_STREAM_NAMED("Stage", fmt::format("Computing stage '{}'", name())); auto compute_start_time = std::chrono::steady_clock::now(); try { compute(); diff --git a/core/package.xml b/core/package.xml index 1078fb066..dae50cb97 100644 --- a/core/package.xml +++ b/core/package.xml @@ -19,6 +19,7 @@ roslint roscpp + fmt tf2_eigen geometry_msgs moveit_core diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index 45cefe57c..135b0c986 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -37,7 +37,7 @@ add_library(${PROJECT_NAME} solvers/pipeline_planner.cpp solvers/multi_planner.cpp ) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} fmt::fmt) target_include_directories(${PROJECT_NAME} PUBLIC $ PUBLIC $ diff --git a/core/src/container.cpp b/core/src/container.cpp index 0fe38c795..f0feeaf91 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include using namespace std::placeholders; @@ -232,7 +232,7 @@ inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Prio } void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { - ROS_DEBUG_STREAM_NAMED("Pruning", "'" << child.name() << "' generated a failure"); + ROS_DEBUG_STREAM_NAMED("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 @@ -503,8 +503,8 @@ struct SolutionCollector }; void SerialContainer::onNewSolution(const SolutionBase& current) { - ROS_DEBUG_STREAM_NAMED("SerialContainer", "'" << this->name() << "' received solution of child stage '" - << current.creator()->name() << "'"); + ROS_DEBUG_STREAM_NAMED("SerialContainer", fmt::format("'{}' received solution of child stage '{}'", this->name(), + current.creator()->name())); // failures should never trigger this callback assert(!current.isFailure()); @@ -573,10 +573,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))); } } @@ -586,12 +586,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 @@ -649,8 +647,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; @@ -705,7 +703,6 @@ void SerialContainer::compute() { } } - ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name) : ContainerBasePrivate(me, name) {} @@ -744,11 +741,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); }); } @@ -763,14 +760,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 { @@ -784,7 +779,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); } @@ -794,8 +790,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) { @@ -876,7 +872,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()) { @@ -899,11 +895,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)); } @@ -933,8 +928,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? @@ -947,12 +942,13 @@ void FallbacksPrivateCommon::compute() { inline void FallbacksPrivateCommon::nextChild() { if (std::next(current_) != children().end()) - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*current_)->name() << "' failed, trying next one."); + ROS_DEBUG_STREAM_NAMED("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()); @@ -963,27 +959,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(); } @@ -1003,8 +999,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 @@ -1022,9 +1018,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. @@ -1032,13 +1028,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(); } @@ -1055,7 +1049,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; @@ -1072,7 +1066,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_); @@ -1093,7 +1088,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) { @@ -1115,7 +1109,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() { @@ -1292,8 +1286,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 fd467ff9b..8ba3aac91 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -44,7 +44,7 @@ #include -#include +#include #include namespace moveit { @@ -206,9 +206,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(); } @@ -243,8 +241,8 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const auto& state_properties{ state->properties() }; auto& stage_properties{ s.creator()->properties() }; request.group_name = state_properties.hasProperty(group_property) ? - state_properties.get(group_property) : - stage_properties.get(group_property); + state_properties.get(group_property) : + stage_properties.get(group_property); // look at all forbidden collisions involving group_name request.enableGroup(state->scene()->getRobotModel()); @@ -274,11 +272,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 }; @@ -290,13 +287,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)); @@ -307,10 +302,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/properties.cpp b/core/src/properties.cpp index 95858704b..95bd2570c 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -37,7 +37,8 @@ */ #include -#include +#include +#include #include #include @@ -290,8 +291,8 @@ void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMa } catch (const Property::undefined&) { } - ROS_DEBUG_STREAM_NAMED(LOGNAME, pair.first << ": " << p.initialized_from_ << " -> " << source << ": " - << Property::serialize(value)); + ROS_DEBUG_STREAM_NAMED(LOGNAME, fmt::format("{}: {} -> {}: {}", pair.first, p.initialized_from_, source, + Property::serialize(value))); p.setCurrentValue(value); p.initialized_from_ = source; } @@ -317,9 +318,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/stage.cpp b/core/src/stage.cpp index 171497ebb..5eec3e7fc 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -42,8 +42,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, @@ -358,7 +356,7 @@ void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) { impl->properties_.reset(); if (impl->parent()) { try { - ROS_DEBUG_STREAM_NAMED("Properties", "init '" << name() << "'"); + ROS_DEBUG_STREAM_NAMED("Properties", fmt::format("init '{}'", name())); impl->properties_.performInitFrom(PARENT, impl->parent()->properties()); } catch (const Property::error& e) { std::ostringstream oss; @@ -548,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); } @@ -915,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()) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different number of collision objects"); + auto false_with_debug = [](auto... args) { + ROS_DEBUG_STREAM_NAMED("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) { - ROS_DEBUG_STREAM_NAMED("Connecting", 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)) { - ROS_DEBUG_STREAM_NAMED("Connecting", 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()) { - ROS_DEBUG_STREAM_NAMED("Connecting", 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)) { - ROS_DEBUG_STREAM_NAMED("Connecting", 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 @@ -954,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()) { - ROS_DEBUG_STREAM_NAMED("Connecting", 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()) { - ROS_DEBUG_STREAM_NAMED("Connecting", 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()) { - ROS_DEBUG_STREAM_NAMED("Connecting", 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()) { - ROS_DEBUG_STREAM_NAMED("Connecting", 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)) { - ROS_DEBUG_STREAM_NAMED("Connecting", - 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/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 738a66178..4822998ad 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -48,6 +48,7 @@ #include #include #include +#include namespace moveit { namespace task_constructor { @@ -286,7 +287,7 @@ void ComputeIK::compute() { if (value.empty()) { // property undefined // determine IK link from eef/group if (!(link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second) : - jmg->getOnlyOneEndEffectorTip())) { + jmg->getOnlyOneEndEffectorTip())) { ROS_WARN_STREAM_NAMED("ComputeIK", "Failed to derive IK target link"); return; } @@ -298,7 +299,8 @@ void ComputeIK::compute() { tf2::fromMsg(ik_pose_msg.pose, ik_pose); if (!scene->getCurrentState().knowsFrameTransform(ik_pose_msg.header.frame_id)) { - ROS_WARN_STREAM_NAMED("ComputeIK", "ik frame unknown in robot: '" << ik_pose_msg.header.frame_id << "'"); + ROS_WARN_STREAM_NAMED("ComputeIK", + 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; diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 11053eb3b..c235463b3 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -42,6 +42,8 @@ #include #include +#include +#include using namespace trajectory_processing; @@ -95,7 +97,7 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) { try { merged_jmg_.reset(task_constructor::merge(groups)); } catch (const std::runtime_error& e) { - ROS_INFO_STREAM_NAMED("Connect", this->name() << ": " << e.what() << ". Disabling merging."); + ROS_INFO_STREAM_NAMED("Connect", fmt::format("{}: {}. Disabling merging.", this->name(), e.what())); } } @@ -126,8 +128,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)) { - ROS_INFO_STREAM_NAMED("Connect", "Deviation in joint " << jm->getName() << ": [" << positions_from.transpose() - << "] != [" << positions_to.transpose() << "]"); + ROS_INFO_STREAM_NAMED("Connect", fmt::format("Deviation in joint {}: [{}] != [{}]", jm->getName(), + positions_from.transpose(), positions_to.transpose())); return false; } } diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index cea34e0a1..f371191f0 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -46,6 +46,7 @@ #include #include #include +#include namespace vm = visualization_msgs; namespace cd = collision_detection; @@ -79,7 +80,7 @@ bool computeCorrection(const std::vector& contacts, Eigen::Vector3d for (const cd::Contact& c : contacts) { if ((c.body_type_1 != cd::BodyTypes::WORLD_OBJECT && c.body_type_2 != cd::BodyTypes::WORLD_OBJECT)) { ROS_WARN_STREAM_NAMED("FixCollisionObjects", - "Cannot fix collision between " << c.body_name_1 << " and " << c.body_name_2); + 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 c24a18a1d..3a5389b35 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 { @@ -58,8 +59,9 @@ void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& void ModifyPlanningScene::addObject(const moveit_msgs::CollisionObject& collision_object) { if (collision_object.operation != moveit_msgs::CollisionObject::ADD) { - ROS_ERROR_STREAM_NAMED("ModifyPlanningScene", name() << ": addObject is called with object's operation not set " - "to ADD -- ignoring the object"); + ROS_ERROR_STREAM_NAMED( + "ModifyPlanningScene", + fmt::format("{}: addObject is called with object's operation not set to ADD -- ignoring the object", name())); return; } collision_objects_.push_back(collision_object); @@ -110,7 +112,7 @@ void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene& scene, if (invert) attach = !attach; obj.object.operation = attach ? static_cast(moveit_msgs::CollisionObject::ADD) : - static_cast(moveit_msgs::CollisionObject::REMOVE); + static_cast(moveit_msgs::CollisionObject::REMOVE); for (const std::string& name : pair.second.first) { obj.object.id = name; scene.processAttachedCollisionObjectMsg(obj); diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index 85ed2fdf8..8268eb855 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -129,7 +129,7 @@ void SimpleGraspBase::setup(std::unique_ptr&& generator, bool forward) { const std::string& eef = p.get("eef"); moveit_msgs::AttachedCollisionObject obj; obj.object.operation = forward ? static_cast(moveit_msgs::CollisionObject::ADD) : - static_cast(moveit_msgs::CollisionObject::REMOVE); + static_cast(moveit_msgs::CollisionObject::REMOVE); obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second; obj.object.id = p.get("object"); scene->processAttachedCollisionObjectMsg(obj); diff --git a/core/test/test_stage.cpp b/core/test/test_stage.cpp index 1a81b4387..298d8c3fb 100644 --- a/core/test/test_stage.cpp +++ b/core/test/test_stage.cpp @@ -119,7 +119,7 @@ void attachObject(PlanningScene& scene, const std::string& object, const std::st moveit_msgs::AttachedCollisionObject obj; obj.link_name = link; obj.object.operation = attach ? static_cast(moveit_msgs::CollisionObject::ADD) : - static_cast(moveit_msgs::CollisionObject::REMOVE); + static_cast(moveit_msgs::CollisionObject::REMOVE); obj.object.id = object; scene.processAttachedCollisionObjectMsg(obj); } diff --git a/visualization/CMakeLists.txt b/visualization/CMakeLists.txt index 9185ab3b7..d72aaebf7 100644 --- a/visualization/CMakeLists.txt +++ b/visualization/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_task_constructor_visualization) +find_package(fmt REQUIRED) find_package(catkin REQUIRED COMPONENTS moveit_core moveit_ros_visualization diff --git a/visualization/package.xml b/visualization/package.xml index 12c1304a6..3dfb14689 100644 --- a/visualization/package.xml +++ b/visualization/package.xml @@ -10,6 +10,7 @@ catkin + fmt qtbase5-dev moveit_core moveit_task_constructor_msgs diff --git a/visualization/visualization_tools/CMakeLists.txt b/visualization/visualization_tools/CMakeLists.txt index 58cb29f1a..491cfb776 100644 --- a/visualization/visualization_tools/CMakeLists.txt +++ b/visualization/visualization_tools/CMakeLists.txt @@ -25,6 +25,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} + fmt::fmt ) target_include_directories(${MOVEIT_LIB_NAME} PUBLIC include) target_include_directories(${MOVEIT_LIB_NAME} SYSTEM diff --git a/visualization/visualization_tools/src/display_solution.cpp b/visualization/visualization_tools/src/display_solution.cpp index bfb704c3b..498875d42 100644 --- a/visualization/visualization_tools/src/display_solution.cpp +++ b/visualization/visualization_tools/src/display_solution.cpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include namespace moveit_rviz_plugin { @@ -88,11 +88,10 @@ const MarkerVisualizationPtr DisplaySolution::markers(const DisplaySolution::Ind void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& start_scene, const moveit_task_constructor_msgs::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); From 076903a738229833e55da8b350ede685f91d0cea Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 25 May 2024 20:01:51 +0200 Subject: [PATCH 18/55] Drop requirement on py_binding_tools python bindings are not yet supported --- core/CMakeLists.txt | 1 - core/package.xml | 1 - demo/package.xml | 1 - 3 files changed, 3 deletions(-) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index df66e19cc..d10d2f5ba 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -11,7 +11,6 @@ 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) diff --git a/core/package.xml b/core/package.xml index b6b90f963..3cb44239b 100644 --- a/core/package.xml +++ b/core/package.xml @@ -19,7 +19,6 @@ moveit_ros_planning moveit_ros_planning_interface moveit_task_constructor_msgs - py_binding_tools rclcpp rviz_marker_tools tf2_eigen diff --git a/demo/package.xml b/demo/package.xml index eb34e5003..d872cf1c9 100644 --- a/demo/package.xml +++ b/demo/package.xml @@ -19,7 +19,6 @@ moveit_configs_utils moveit_resources_panda_moveit_config moveit_task_constructor_capabilities - py_binding_tools ament_cmake From 92efc140434c12ca6c2b9836b5623e7f4dcec3ef Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 25 May 2024 21:30:30 +0200 Subject: [PATCH 19/55] Rename ros-planning org --- .github/workflows/ci.yaml | 4 ++-- README.md | 2 +- core/package.xml | 6 +++--- core/src/container.cpp | 2 +- core/src/stage.cpp | 2 +- core/test/test_serial.cpp | 6 +++--- 6 files changed, 11 insertions(+), 11 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 83a0d3e9c..1ee865b53 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -112,7 +112,7 @@ jobs: doc: # https://devblogs.microsoft.com/cppblog/clear-functional-c-documentation-with-sphinx-breathe-doxygen-cmake - if: github.repository_owner == 'ros-planning' + if: github.repository_owner == 'moveit' runs-on: ubuntu-latest needs: ici container: @@ -150,7 +150,7 @@ jobs: # Deployment job deploy: - if: github.repository_owner == 'ros-planning' && github.ref == 'refs/heads/master' + if: github.repository_owner == 'moveit' && github.ref == 'refs/heads/master' runs-on: ubuntu-latest needs: doc environment: 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/core/package.xml b/core/package.xml index dae50cb97..20c9fd8ae 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 diff --git a/core/src/container.cpp b/core/src/container.cpp index f0feeaf91..b4840d491 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -211,7 +211,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 diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 5eec3e7fc..97c99826b 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -809,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; 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()); From 747bb29c7a3868b109ec95c0d337cfdf9df8d101 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 26 May 2024 07:42:01 +0200 Subject: [PATCH 20/55] CI: add jazzy build --- .github/workflows/ci.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 6134243c5..474751cca 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -24,13 +24,13 @@ jobs: 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 +38,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 From f79c6c537ef50c4fda8e75a2904583477164e650 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 26 May 2024 20:45:23 +0200 Subject: [PATCH 21/55] Connect: Relax validity check of reached end state (#542) Looks like Rolling's MoveIt uses a more relaxed goal constraint threshold than Humble. For this reason, all end states reached by Connect solutions of pick+place demo are rejected. This commit relaxes the max_distance threshold of Connect accordingly. --- core/src/stages/connect.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 823750dd1..8e17a58c9 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -57,7 +57,7 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); - p.declare("max_distance", 1e-4, "maximally accepted distance between end and goal sate"); + p.declare("max_distance", 1e-2, "maximally accepted distance between end and goal sate"); p.declare("path_constraints", moveit_msgs::msg::Constraints(), "constraints to maintain during trajectory"); properties().declare("merge_time_parameterization", From 3b4ea48c181eb672ac624aa8bb228dde23084c9e Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Mon, 27 May 2024 10:16:57 -0400 Subject: [PATCH 22/55] Add unittest cartesianCollisionMinMaxDistance (#538) ... to illustrate that MoveRelative's min-max constraint fails with PipelinePlanners (e.g. Pilz) returning a partially invalid trajectory: MTC does not truncate the trajectory to its valid part and thus fails, even if the valid part fits the given min-max range. This logic is only supported for the CartesianPath planner for now. Co-authored-by: Robert Haschke --- core/test/move_relative.test | 2 +- core/test/test_move_relative.cpp | 100 +++++++++++++++++++++++++------ 2 files changed, 83 insertions(+), 19 deletions(-) diff --git a/core/test/move_relative.test b/core/test/move_relative.test index 6dbb04465..6530b5a00 100644 --- a/core/test/move_relative.test +++ b/core/test/move_relative.test @@ -1,5 +1,5 @@ - + diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp index 43caa12fa..9c91ae93e 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,16 +25,33 @@ using namespace moveit::core; constexpr double TAU{ 2 * M_PI }; constexpr double EPS{ 5e-5 }; +template +std::shared_ptr create(); +template <> +solvers::CartesianPathPtr create() { + return std::make_shared(); +} +template <> +solvers::PipelinePlannerPtr create() { + auto p = std::make_shared("pilz_industrial_motion_planner"); + p->setPlannerId("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; + std::shared_ptr planner; const JointModelGroup* group; - PandaMoveRelative() { + PandaMoveRelative() : planner(create()) { t.setRobotModel(loadModel()); group = t.getRobotModel()->getJointModelGroup("panda_arm"); @@ -39,20 +61,20 @@ struct PandaMoveRelative : public testing::Test scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "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::AttachedCollisionObject createAttachedObject(const std::string& id) { - moveit_msgs::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::CollisionObject createObject(const std::string& id, const geometry_msgs::Pose& pose) { + moveit_msgs::CollisionObject co; + co.header.frame_id = "panda_hand"; + co.operation = co.ADD; + co.id = id; + co.primitives.resize(1, [] { shape_msgs::SolidPrimitive p; p.type = p.SPHERE; p.dimensions.resize(1); @@ -60,10 +82,23 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) return p; }()); + co.pose = pose; + return co; +} + +moveit_msgs::CollisionObject createObject(const std::string& id) { geometry_msgs::Pose p; p.position.x = 0.1; p.orientation.w = 1.0; - aco.object.pose = p; + + return createObject(id, p); +} + +moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) { + moveit_msgs::AttachedCollisionObject aco; + aco.link_name = "panda_hand"; + aco.object = createObject(id); + return aco; } @@ -79,13 +114,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::TwistStamped twist; twist.header.frame_id = "world"; @@ -97,7 +132,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); @@ -112,7 +147,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); @@ -128,6 +163,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::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::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); ros::init(argc, argv, "move_relative_test"); From 227d4752825a994956595f370500e7e8b6928982 Mon Sep 17 00:00:00 2001 From: VideoSystemsTech <61198558+VideoSystemsTech@users.noreply.github.com> Date: Mon, 27 May 2024 16:19:09 +0200 Subject: [PATCH 23/55] Expose MultiPlanner to Python (#474) Co-authored-by: Robert Haschke --- core/python/bindings/src/core.cpp | 18 +------- core/python/bindings/src/solvers.cpp | 24 ++++++++++ core/python/bindings/src/utils.h | 26 +++++++++++ demo/scripts/multi_planner.py | 68 ++++++++++++++++++++++++++++ 4 files changed, 119 insertions(+), 17 deletions(-) create mode 100644 core/python/bindings/src/utils.h create mode 100755 demo/scripts/multi_planner.py diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp index a9f017ca1..c1b488482 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()) diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index a1d3b405f..ef60fb822 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 "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 { @@ -116,6 +119,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/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/demo/scripts/multi_planner.py b/demo/scripts/multi_planner.py new file mode 100755 index 000000000..4b26cef84 --- /dev/null +++ b/demo/scripts/multi_planner.py @@ -0,0 +1,68 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from py_binding_tools import roscpp_init +import time + +roscpp_init("mtc_tutorial") + +ompl_pipelinePlanner = core.PipelinePlanner("ompl") +ompl_pipelinePlanner.planner = "RRTConnectkConfigDefault" +pilz_pipelinePlanner = core.PipelinePlanner("pilz_industrial_motion_planner") +pilz_pipelinePlanner.planner = "PTP" +multiPlanner = core.MultiPlanner() +multiPlanner.add(pilz_pipelinePlanner, ompl_pipelinePlanner) + +# Create a task +task = core.Task() + +# 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) From a9ddbe19986ea2a5c8ad1459e424e66f67f4dfd4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jafar=20Uru=C3=A7?= Date: Tue, 28 May 2024 06:26:10 +0100 Subject: [PATCH 24/55] Update authors and maintainers (#425) --- core/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/package.xml b/core/package.xml index 3cb44239b..b10c3f67f 100644 --- a/core/package.xml +++ b/core/package.xml @@ -33,7 +33,7 @@ launch_testing_ament_cmake - + moveit_resources_fanuc_moveit_config moveit_planners From ad5c878f1914965d1b70e8c6dd93f8fe97a74ad9 Mon Sep 17 00:00:00 2001 From: VideoSystemsTech <61198558+VideoSystemsTech@users.noreply.github.com> Date: Tue, 28 May 2024 16:54:10 +0200 Subject: [PATCH 25/55] ComputeIK: Allow additional constraints for filtering solutions (#464) Add "constraint" property. Co-authored-by: Robert Haschke --- core/python/bindings/src/stages.cpp | 3 +++ core/src/stages/compute_ik.cpp | 27 +++++++++++++++++++++------ 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index db263ffa5..fc8c5949e 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -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 diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 4822998ad..29cf79afd 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -63,6 +63,7 @@ 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::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"); @@ -88,8 +89,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; @@ -359,8 +361,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) { @@ -368,20 +373,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"); @@ -411,14 +424,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(); From bb047c894cd2a395a672abdd47ec76d709d1cb1e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 29 May 2024 09:04:42 +0200 Subject: [PATCH 26/55] Cleanup unit tests - Unify move_to.launch.py and test_task_model.launch.py - Rename them to test.launch.py as they are independent of the executable - Move Node creation to the fixture constructor - Replace Task::setRobotModel(loadModel()) with Task::loadRobotModel() --- core/test/CMakeLists.txt | 4 ++-- core/test/models.cpp | 5 ---- core/test/models.h | 3 --- .../{move_to.launch.py => test.launch.py} | 0 core/test/test_move_relative.cpp | 6 ++--- core/test/test_move_to.cpp | 12 ++++++---- .../motion_planning_tasks/test/CMakeLists.txt | 4 ++-- ...st_task_model.launch.py => test.launch.py} | 23 +++++++------------ 8 files changed, 22 insertions(+), 35 deletions(-) rename core/test/{move_to.launch.py => test.launch.py} (100%) rename visualization/motion_planning_tasks/test/{test_task_model.launch.py => test.launch.py} (60%) 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/move_to.launch.py b/core/test/test.launch.py similarity index 100% rename from core/test/move_to.launch.py rename to core/test/test.launch.py diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp index 5a36457f9..482ee4e24 100644 --- a/core/test/test_move_relative.cpp +++ b/core/test/test_move_relative.cpp @@ -26,18 +26,18 @@ struct PandaMoveRelative : public testing::Test Task t; stages::MoveRelative* move; PlanningScenePtr scene; - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("panda_move_relative"); + rclcpp::Node::SharedPtr node; const JointModelGroup* group; PandaMoveRelative() { - t.setRobotModel(loadModel(node)); + 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()); 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/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() From edf2605a1d165f0e8965cd1620ca32dabbe081a5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 29 May 2024 11:33:40 +0200 Subject: [PATCH 27/55] TEMP: Workaround for unittests failing during shutdown Using quick_exit instead of return/exit avoids calling all cleanup functions, which fail due to a bug in rmw_fastrtps_cpp. --- core/test/test_move_relative.cpp | 3 ++- visualization/motion_planning_tasks/test/test_task_model.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp index 482ee4e24..04133fb25 100644 --- a/core/test/test_move_relative.cpp +++ b/core/test/test_move_relative.cpp @@ -133,5 +133,6 @@ int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); - return RUN_ALL_TESTS(); + // Using quick_exit to avoid calling cleanup functions, which cause a segfault in rmw + quick_exit(RUN_ALL_TESTS()); } diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 2e0721609..9bd9d3305 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -239,5 +239,6 @@ int main(int argc, char** argv) { auto test_result = RUN_ALL_TESTS(); app.exit(test_result); }); - return app.exec(); + // Using quick_exit to avoid calling cleanup functions, which cause a segfault in rmw + quick_exit(app.exec()); } From 8fa66a6d2c268a2a41e938a435d7701969b762dc Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 29 May 2024 15:19:09 +0200 Subject: [PATCH 28/55] Update pybind11 to version 2.12.0 Currently used branch was not interoperable with a standard version of pybind11. --- .pre-commit-config.yaml | 2 +- core/python/bindings/src/core.cpp | 6 ++++-- core/python/pybind11 | 2 +- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index aa133c806..cff5e8c05 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -46,7 +46,7 @@ repos: - id: catkin_lint name: catkin_lint description: Check package.xml and cmake files - entry: catkin_lint . + entry: catkin_lint . --ignore duplicate_cmd language: system always_run: true pass_filenames: false diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp index c1b488482..2d2dfa3f6 100644 --- a/core/python/bindings/src/core.cpp +++ b/core/python/bindings/src/core.cpp @@ -80,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 { @@ -89,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()); } }); 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 From 177e19de1fe52a2f1c6668478861c2013e37922f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 29 Jun 2024 17:55:45 +0200 Subject: [PATCH 29/55] Generalize utils::getRobotTipForFrame() ... to return error_msg instead of calling markAsFailure() on a solution --- core/include/moveit/task_constructor/utils.h | 3 +-- core/src/stages/move_relative.cpp | 5 ++++- core/src/stages/move_to.cpp | 6 +++++- core/src/utils.cpp | 10 ++++++---- 4 files changed, 16 insertions(+), 8 deletions(-) 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/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 6833ac264..3f1b915ed 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -200,10 +200,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 diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index ccc0fee8f..f35e2ea21 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -213,8 +213,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/utils.cpp b/core/src/utils.cpp index 376692771..122471e04 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -48,7 +48,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 @@ -60,10 +60,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); @@ -77,13 +79,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); From b1336dc210998b551cce7f1f0ded2022c6c784d7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 29 Jun 2024 17:58:29 +0200 Subject: [PATCH 30/55] CartesianPath: allow ik_frame definition ... if start and end are given as joint-space poses --- .../task_constructor/solvers/cartesian_path.h | 4 +++ core/src/solvers/cartesian_path.cpp | 26 +++++++++++++++---- 2 files changed, 25 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 1a4552d7c..3595bc651 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::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/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index f15074908..3a0aa4089 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -37,10 +37,12 @@ */ #include +#include #include #include #include #include +#include using namespace trajectory_processing; @@ -50,6 +52,7 @@ namespace solvers { 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"); @@ -59,18 +62,31 @@ CartesianPath::CartesianPath() { void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {} +void CartesianPath::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) { + geometry_msgs::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::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, From a84479cc58191b32a41928b435e81b0f9d64389b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 1 Jul 2024 13:11:14 +0200 Subject: [PATCH 31/55] Avoid segfault if TimeParameterization is not set --- core/src/solvers/cartesian_path.cpp | 5 +++-- core/src/solvers/joint_interpolation.cpp | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 3a0aa4089..e2c0c80c8 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -121,8 +121,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 ee0a76056..75f0fb3ca 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -103,8 +103,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"); From 42a08d64447fb8b0a7ef17302bf83255dc640295 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 3 Jul 2024 13:39:45 +0200 Subject: [PATCH 32/55] PassThrough: cleanup unused headers --- .../moveit/task_constructor/stages/passthrough.h | 3 --- core/src/stages/passthrough.cpp | 14 -------------- 2 files changed, 17 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/passthrough.h b/core/include/moveit/task_constructor/stages/passthrough.h index 61f1c40dc..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/src/stages/passthrough.cpp b/core/src/stages/passthrough.cpp index 4ca934827..32c78a998 100644 --- a/core/src/stages/passthrough.cpp +++ b/core/src/stages/passthrough.cpp @@ -34,20 +34,6 @@ /* Author: Michael 'v4hn' Goerner */ #include -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include namespace moveit { namespace task_constructor { From 48bde8daaba9d9cfbd6a79c7fdd2a681777a71dc Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 3 Jul 2024 13:09:24 +0200 Subject: [PATCH 33/55] test_pruning.cpp: Extend test to ParallelContainer --- core/test/test_pruning.cpp | 27 ++++++++++++--------------- 1 file changed, 12 insertions(+), 15 deletions(-) diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index 3be841178..dc1b4ddf2 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -65,23 +65,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) { From 93a39641389b6885716134020b848ea809f18fdd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 3 Jul 2024 13:31:49 +0200 Subject: [PATCH 34/55] test_pruning.cpp: Add new test --- core/test/test_pruning.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index dc1b4ddf2..5c92c6c38 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -127,6 +127,19 @@ TEST_F(Pruning, PropagateIntoContainer) { EXPECT_EQ(con->runs_, 0u); } +TEST_F(Pruning, 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()); From 907014cd571ed9ac974c88bf1c481280a1126300 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 3 Jul 2024 21:32:40 +0200 Subject: [PATCH 35/55] Disable pruning by default --- core/CMakeLists.txt | 4 ++++ core/src/container.cpp | 6 ++++++ core/test/CMakeLists.txt | 2 ++ 3 files changed, 12 insertions(+) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 12af7292b..9d3b4d991 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(catkin REQUIRED COMPONENTS ) option(MOVEIT_CI_WARNINGS "Enable all warnings used by CI" OFF) # We use our own set of warnings +option(ENABLE_PRUNING "Enable pruning on failed solutions" OFF) moveit_build_options() catkin_python_setup() @@ -39,6 +40,9 @@ catkin_package( ) add_compile_options(-fvisibility-inlines-hidden) +if(ENABLE_PRUNING) + add_compile_definitions(ENABLE_PRUNING) +endif() set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor) diff --git a/core/src/container.cpp b/core/src/container.cpp index b4840d491..82cf907bf 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -232,6 +232,7 @@ inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Prio } void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { +#ifdef ENABLE_PRUNING ROS_DEBUG_STREAM_NAMED("Pruning", fmt::format("'{}' generated a failure", child.name())); switch (child.pimpl()->interfaceFlags()) { case GENERATE: @@ -251,6 +252,11 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState setStatus(&child, from, to, InterfaceState::Status::ARMED); break; } +#else + (void)child; + (void)from; + (void)to; +#endif // printChildrenInterfaces(*this, false, child); } diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 09e6d477b..492d84a58 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -38,7 +38,9 @@ if (CATKIN_ENABLE_TESTING) mtc_add_gtest(test_stage.cpp) mtc_add_gtest(test_container.cpp) mtc_add_gmock(test_serial.cpp) +if(ENABLE_PRUNING) mtc_add_gmock(test_pruning.cpp) +endif() mtc_add_gtest(test_properties.cpp) mtc_add_gtest(test_cost_terms.cpp) From fbc05e44966dd4a2cd92df558a2d151ac56a08f3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 6 Jul 2024 16:51:14 +0200 Subject: [PATCH 36/55] Add property to enable/disable pruning at runtime (#590) --- core/CMakeLists.txt | 4 ---- .../moveit/task_constructor/container.h | 4 ++++ core/include/moveit/task_constructor/task.h | 3 +++ core/src/container.cpp | 14 +++++++------- core/src/task.cpp | 1 + core/test/CMakeLists.txt | 2 -- core/test/test_pruning.cpp | 19 +++++++++++++++++-- 7 files changed, 32 insertions(+), 15 deletions(-) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 9d3b4d991..12af7292b 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -18,7 +18,6 @@ find_package(catkin REQUIRED COMPONENTS ) option(MOVEIT_CI_WARNINGS "Enable all warnings used by CI" OFF) # We use our own set of warnings -option(ENABLE_PRUNING "Enable pruning on failed solutions" OFF) moveit_build_options() catkin_python_setup() @@ -40,9 +39,6 @@ catkin_package( ) add_compile_options(-fvisibility-inlines-hidden) -if(ENABLE_PRUNING) - add_compile_definitions(ENABLE_PRUNING) -endif() set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 2dcd325f4..24e3b1ec4 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/task.h b/core/include/moveit/task_constructor/task.h index 91d49e733..00ea354c2 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -117,6 +117,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 diff --git a/core/src/container.cpp b/core/src/container.cpp index 82cf907bf..265d8d00f 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -232,7 +232,9 @@ inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Prio } void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { -#ifdef ENABLE_PRUNING + if (!static_cast(me_)->pruning()) + return; + ROS_DEBUG_STREAM_NAMED("Pruning", fmt::format("'{}' generated a failure", child.name())); switch (child.pimpl()->interfaceFlags()) { case GENERATE: @@ -252,11 +254,6 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState setStatus(&child, from, to, InterfaceState::Status::ARMED); break; } -#else - (void)child; - (void)from; - (void)to; -#endif // printChildrenInterfaces(*this, false, child); } @@ -328,7 +325,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(); diff --git a/core/src/task.cpp b/core/src/task.cpp index f6c3b864d..f969cf9e8 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -92,6 +92,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 diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 492d84a58..09e6d477b 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -38,9 +38,7 @@ if (CATKIN_ENABLE_TESTING) mtc_add_gtest(test_stage.cpp) mtc_add_gtest(test_container.cpp) mtc_add_gmock(test_serial.cpp) -if(ENABLE_PRUNING) mtc_add_gmock(test_pruning.cpp) -endif() mtc_add_gtest(test_properties.cpp) mtc_add_gtest(test_cost_terms.cpp) diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index 5c92c6c38..a9a5fdd56 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -8,7 +8,10 @@ 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()); @@ -21,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()); @@ -127,7 +142,7 @@ TEST_F(Pruning, PropagateIntoContainer) { EXPECT_EQ(con->runs_, 0u); } -TEST_F(Pruning, PropagateIntoContainerAndReactivate) { +TEST_F(Pruning, DISABLED_PropagateIntoContainerAndReactivate) { add(t, new GeneratorMockup({ 0 })); auto serial = add(t, new SerialContainer()); From 702710dec5f940d7e73f3b9bec3c5f719d3225ba Mon Sep 17 00:00:00 2001 From: Fabian Schuetze Date: Sat, 6 Jul 2024 19:59:05 +0200 Subject: [PATCH 37/55] Improve comments for pick-and-place task (#238) --- demo/src/pick_place_task.cpp | 42 ++++++++++++++++++++---------------- 1 file changed, 23 insertions(+), 19 deletions(-) diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 1428eacc4..92c4f72e0 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -144,6 +144,7 @@ void PickPlaceTask::loadParameters() { rosparam_shortcuts::shutdownIfError(LOGNAME, errors); } +// Initialize the task pipeline, defining individual movement stages bool PickPlaceTask::init() { ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); const std::string object = object_name_; @@ -153,10 +154,13 @@ bool PickPlaceTask::init() { 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(); + /* 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(); sampling_planner->setProperty("goal_joint_tolerance", 1e-5); @@ -201,7 +205,7 @@ bool PickPlaceTask::init() { * * ***************************************************/ Stage* initial_state_ptr = nullptr; - { // Open Hand + { auto stage = std::make_unique("open hand", sampling_planner); stage->setGroup(hand_group_name_); stage->setGoal(hand_open_pose_); @@ -214,7 +218,8 @@ bool PickPlaceTask::init() { * 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{ { arm_group_name_, sampling_planner } }); stage->setTimeout(5.0); @@ -229,6 +234,7 @@ bool PickPlaceTask::init() { ***************************************************/ 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" }); @@ -237,10 +243,11 @@ bool PickPlaceTask::init() { ---- * 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", hand_frame_); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->properties().set("link", hand_frame_); // link to perform IK for + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); // inherit group from parent stage stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_); // Set hand forward direction @@ -255,22 +262,23 @@ bool PickPlaceTask::init() { ---- * 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(hand_open_pose_); - stage->setObject(object); + stage->setObject(object); // 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); - wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); - wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); - wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); + wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); // define virtual frame to reach the 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)); } @@ -278,6 +286,7 @@ bool PickPlaceTask::init() { ---- * 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( object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), @@ -300,7 +309,7 @@ bool PickPlaceTask::init() { ***************************************************/ { auto stage = std::make_unique("attach object"); - stage->attachObject(object, hand_frame_); + stage->attachObject(object, hand_frame_); // attach object to hand_frame_ grasp->insert(std::move(stage)); } @@ -352,6 +361,7 @@ bool PickPlaceTask::init() { * * *****************************************************/ { + // 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{ { arm_group_name_, sampling_planner } }); stage->setTimeout(5.0); @@ -364,6 +374,7 @@ bool PickPlaceTask::init() { * 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" }); @@ -498,13 +509,6 @@ bool PickPlaceTask::execute() { moveit_msgs::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::ExecuteTaskSolutionGoal 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::MoveItErrorCodes::SUCCESS) { ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_result.val); From 26d146874aabb05aa6fc504d165292047507b39d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 9 Jul 2024 13:25:57 +0200 Subject: [PATCH 38/55] Fix failing unittests: remove static executor --- core/src/introspection.cpp | 7 ++++--- core/test/test_move_relative.cpp | 3 +-- .../motion_planning_tasks/test/test_task_model.cpp | 3 +-- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 2eb1d2a8e..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,7 +103,6 @@ class IntrospectionExecutor std::thread executor_thread_; size_t nodes_count_ = 0; std::mutex mutex_; - std::once_flag once_flag_; }; class IntrospectionPrivate @@ -157,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/test/test_move_relative.cpp b/core/test/test_move_relative.cpp index 04133fb25..482ee4e24 100644 --- a/core/test/test_move_relative.cpp +++ b/core/test/test_move_relative.cpp @@ -133,6 +133,5 @@ int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); - // Using quick_exit to avoid calling cleanup functions, which cause a segfault in rmw - quick_exit(RUN_ALL_TESTS()); + return RUN_ALL_TESTS(); } diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 9bd9d3305..2e0721609 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -239,6 +239,5 @@ int main(int argc, char** argv) { auto test_result = RUN_ALL_TESTS(); app.exit(test_result); }); - // Using quick_exit to avoid calling cleanup functions, which cause a segfault in rmw - quick_exit(app.exec()); + return app.exec(); } From d4db4cb707d27cb04785b999de8a997d955a2dd1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 9 Jul 2024 15:16:29 +0200 Subject: [PATCH 39/55] Fix flaky IK in asan unit test: increase timeout --- core/test/test.launch.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/core/test/test.launch.py b/core/test/test.launch.py index 023283bea..dd574a413 100644 --- a/core/test/test.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"), From 646a49f1fb3939d2202a0cdb644123f456f4020a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 9 Jul 2024 20:39:25 +0200 Subject: [PATCH 40/55] Disable ccov: lcov is broken --- .github/workflows/ci.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 474751cca..46c855907 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -19,9 +19,9 @@ 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: jazzy-source From 4debc3790400632bc7eaebabc1b684b322690e03 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 11 Jul 2024 09:49:39 +0200 Subject: [PATCH 41/55] Silence gcc's overloaded-virtual warnings --- core/include/moveit/task_constructor/cost_terms.h | 8 ++++++++ core/test/test_cost_terms.cpp | 1 + 2 files changed, 9 insertions(+) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 9728ebaa9..353464081 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::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/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index a610ca043..4494320d1 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())) From bbc34d2b97ca2e778d6aa7a43f80a2fefc66e497 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 11 Jul 2024 14:01:42 +0200 Subject: [PATCH 42/55] demo: replace individual launch files with common run.launch.py Launch individual binaries with: ros2 launch moveit_task_constructor_demo run.launch.py exe:= --- demo/launch/cartesian.launch.py | 25 ------------- demo/launch/fallbacks_move_to.launch.py | 28 --------------- demo/launch/ik_clearance_cost.launch.py | 25 ------------- demo/launch/modular.launch.py | 25 ------------- demo/launch/pickplace.launch.py | 36 ------------------- ...ive_path_costs.launch.py => run.launch.py} | 12 ++++--- 6 files changed, 7 insertions(+), 144 deletions(-) delete mode 100644 demo/launch/cartesian.launch.py delete mode 100644 demo/launch/fallbacks_move_to.launch.py delete mode 100644 demo/launch/ik_clearance_cost.launch.py delete mode 100644 demo/launch/modular.launch.py delete mode 100644 demo/launch/pickplace.launch.py rename demo/launch/{alternative_path_costs.launch.py => run.launch.py} (73%) 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]) From 93ef98ad67611a6e6a82f9a753a13498ecbe6ae9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 11 Jul 2024 15:08:30 +0200 Subject: [PATCH 43/55] Switch shebang to python3 --- core/python/test/rostest_mps.py | 2 +- core/python/test/rostest_mtc.py | 2 +- core/python/test/rostest_trampoline.py | 2 +- core/python/test/test_mtc.py | 2 +- demo/scripts/alternatives.py | 2 +- demo/scripts/cartesian.py | 2 +- demo/scripts/compute_ik.py | 2 +- demo/scripts/constrained.py | 2 +- demo/scripts/current_state.py | 2 +- demo/scripts/fallbacks.py | 2 +- demo/scripts/fix_collision_objects.py | 2 +- demo/scripts/fixed_state.py | 2 +- demo/scripts/generate_pose.py | 2 +- demo/scripts/merger.py | 2 +- demo/scripts/modify_planning_scene.py | 2 +- demo/scripts/multi_planner.py | 2 +- demo/scripts/pickplace.py | 2 +- demo/scripts/properties.py | 2 +- 18 files changed, 18 insertions(+), 18 deletions(-) mode change 100644 => 100755 demo/scripts/constrained.py mode change 100644 => 100755 demo/scripts/properties.py diff --git a/core/python/test/rostest_mps.py b/core/python/test/rostest_mps.py index f9d6164d0..56ac151b9 100755 --- a/core/python/test/rostest_mps.py +++ b/core/python/test/rostest_mps.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 from __future__ import print_function import unittest diff --git a/core/python/test/rostest_mtc.py b/core/python/test/rostest_mtc.py index 214d7449b..5db8487d4 100755 --- a/core/python/test/rostest_mtc.py +++ b/core/python/test/rostest_mtc.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 from __future__ import print_function import unittest diff --git a/core/python/test/rostest_trampoline.py b/core/python/test/rostest_trampoline.py index 42363d4e8..46641af2e 100755 --- a/core/python/test/rostest_trampoline.py +++ b/core/python/test/rostest_trampoline.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from __future__ import print_function diff --git a/core/python/test/test_mtc.py b/core/python/test/test_mtc.py index 19cd843b0..8cc73ba70 100644 --- a/core/python/test/test_mtc.py +++ b/core/python/test/test_mtc.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from __future__ import print_function diff --git a/demo/scripts/alternatives.py b/demo/scripts/alternatives.py index 1240af376..050b0ac36 100755 --- a/demo/scripts/alternatives.py +++ b/demo/scripts/alternatives.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages diff --git a/demo/scripts/cartesian.py b/demo/scripts/cartesian.py index 627fa06e0..7ab598d66 100755 --- a/demo/scripts/cartesian.py +++ b/demo/scripts/cartesian.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from std_msgs.msg import Header diff --git a/demo/scripts/compute_ik.py b/demo/scripts/compute_ik.py index 1a85f89b8..7e0e04a12 100755 --- a/demo/scripts/compute_ik.py +++ b/demo/scripts/compute_ik.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages diff --git a/demo/scripts/constrained.py b/demo/scripts/constrained.py old mode 100644 new mode 100755 index c211efdfa..5c4af3014 --- a/demo/scripts/constrained.py +++ b/demo/scripts/constrained.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from std_msgs.msg import Header diff --git a/demo/scripts/current_state.py b/demo/scripts/current_state.py index f4ef0ab3a..3a5d71106 100755 --- a/demo/scripts/current_state.py +++ b/demo/scripts/current_state.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages diff --git a/demo/scripts/fallbacks.py b/demo/scripts/fallbacks.py index 0c1ae6c12..b6d89f3c1 100755 --- a/demo/scripts/fallbacks.py +++ b/demo/scripts/fallbacks.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages diff --git a/demo/scripts/fix_collision_objects.py b/demo/scripts/fix_collision_objects.py index 3104b4c03..531e6d86a 100755 --- a/demo/scripts/fix_collision_objects.py +++ b/demo/scripts/fix_collision_objects.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages diff --git a/demo/scripts/fixed_state.py b/demo/scripts/fixed_state.py index 99b5df709..56c735aa8 100755 --- a/demo/scripts/fixed_state.py +++ b/demo/scripts/fixed_state.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.core import planning_scene diff --git a/demo/scripts/generate_pose.py b/demo/scripts/generate_pose.py index c9503d42c..39498c415 100755 --- a/demo/scripts/generate_pose.py +++ b/demo/scripts/generate_pose.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages diff --git a/demo/scripts/merger.py b/demo/scripts/merger.py index 9c0c28d46..224fcd782 100755 --- a/demo/scripts/merger.py +++ b/demo/scripts/merger.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages diff --git a/demo/scripts/modify_planning_scene.py b/demo/scripts/modify_planning_scene.py index d68a721d3..d75ff88e4 100755 --- a/demo/scripts/modify_planning_scene.py +++ b/demo/scripts/modify_planning_scene.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages diff --git a/demo/scripts/multi_planner.py b/demo/scripts/multi_planner.py index 4b26cef84..6061a057c 100755 --- a/demo/scripts/multi_planner.py +++ b/demo/scripts/multi_planner.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages diff --git a/demo/scripts/pickplace.py b/demo/scripts/pickplace.py index 4723450a6..e43a2e291 100755 --- a/demo/scripts/pickplace.py +++ b/demo/scripts/pickplace.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from py_binding_tools import roscpp_init diff --git a/demo/scripts/properties.py b/demo/scripts/properties.py old mode 100644 new mode 100755 index d7198e1e0..ee3992bae --- a/demo/scripts/properties.py +++ b/demo/scripts/properties.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages From 0fed09d431a4b9f007965cb2020eec3af68d2c0e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 12 Jul 2024 05:42:07 +0200 Subject: [PATCH 44/55] Unify Python demo scripts --- core/doc/tutorials/properties.rst | 5 ----- demo/config/mtc.rviz | 22 +++++++++++++--------- demo/scripts/alternatives.py | 3 ++- demo/scripts/cartesian.py | 1 + demo/scripts/compute_ik.py | 7 ++++--- demo/scripts/constrained.py | 2 ++ demo/scripts/current_state.py | 3 ++- demo/scripts/fallbacks.py | 3 ++- demo/scripts/fix_collision_objects.py | 3 ++- demo/scripts/fixed_state.py | 3 ++- demo/scripts/generate_pose.py | 3 ++- demo/scripts/merger.py | 3 ++- demo/scripts/modify_planning_scene.py | 3 ++- demo/scripts/multi_planner.py | 11 ++++++----- demo/scripts/pickplace.py | 6 +++--- demo/scripts/properties.py | 24 +++++------------------- 16 files changed, 50 insertions(+), 52 deletions(-) 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/demo/config/mtc.rviz b/demo/config/mtc.rviz index 771cd726a..bf6474bc0 100644 --- a/demo/config/mtc.rviz +++ b/demo/config/mtc.rviz @@ -23,20 +23,20 @@ Panels: Name: Motion Planning Tasks Tasks View: property_splitter: - - 600 + - 643 - 0 solution_sorting: column: 1 order: 0 solutions_splitter: - - 223 - - 72 + - 316 + - 102 solutions_view_columns: ~ tasks_view_columns: - - 107 - - 38 + - 194 - 38 - 38 + - 44 Preferences: PromptSaveOnExit: true Toolbars: @@ -153,6 +153,10 @@ Visualization Manager: Markers: All at once?: false Value: true + approach: true + lift: true + place: true + retract: true Name: Motion Planning Tasks Robot: Fixed Robot Color: 150; 50; 150 @@ -238,7 +242,7 @@ Visualization Manager: State Display Time: REALTIME Task Solution Topic: /mtc_tutorial/solution Tasks: - {} + pick + place: 24 Trail Step Size: 1 Value: true Enabled: true @@ -289,9 +293,9 @@ Window Geometry: collapsed: false Motion Planning Tasks - Slider: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Views: collapsed: false Width: 1450 - X: 1920 - Y: 28 + X: 2560 + Y: 9 diff --git a/demo/scripts/alternatives.py b/demo/scripts/alternatives.py index 050b0ac36..5a221a02b 100755 --- a/demo/scripts/alternatives.py +++ b/demo/scripts/alternatives.py @@ -5,13 +5,14 @@ from py_binding_tools import roscpp_init import time -roscpp_init("mtc_tutorial_alternatives") +roscpp_init("mtc_tutorial") # Use the joint interpolation planner jointPlanner = core.JointInterpolationPlanner() # Create a task task = core.Task() +task.name = "alternatives" # Start from current robot state currentState = stages.CurrentState("current state") diff --git a/demo/scripts/cartesian.py b/demo/scripts/cartesian.py index 7ab598d66..a79360124 100755 --- a/demo/scripts/cartesian.py +++ b/demo/scripts/cartesian.py @@ -23,6 +23,7 @@ # [cartesianTut3] task = core.Task() +task.name = "cartesian" # start from current robot state task.add(stages.CurrentState("current state")) diff --git a/demo/scripts/compute_ik.py b/demo/scripts/compute_ik.py index 7e0e04a12..b575c79d3 100755 --- a/demo/scripts/compute_ik.py +++ b/demo/scripts/compute_ik.py @@ -2,19 +2,20 @@ # -*- 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 py_binding_tools import roscpp_init -roscpp_init("mtc_tutorial_compute_ik") +roscpp_init("mtc_tutorial") # Specify the planning group group = "panda_arm" # Create a task task = core.Task() +task.name = "compute IK" # Add a stage to retrieve the current state task.add(stages.CurrentState("current state")) @@ -32,7 +33,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 index 5c4af3014..4efc53c2a 100755 --- a/demo/scripts/constrained.py +++ b/demo/scripts/constrained.py @@ -16,6 +16,8 @@ planner = core.PipelinePlanner() task = core.Task() +task.name = "constrained" + task.add(stages.CurrentState("current state")) co = CollisionObject() diff --git a/demo/scripts/current_state.py b/demo/scripts/current_state.py index 3a5d71106..1c23cb0c4 100755 --- a/demo/scripts/current_state.py +++ b/demo/scripts/current_state.py @@ -5,10 +5,11 @@ from py_binding_tools import roscpp_init import time -roscpp_init("mtc_tutorial_current_state") +roscpp_init("mtc_tutorial") # Create a task task = core.Task() +task.name = "current state" # Get the current robot state currentState = stages.CurrentState("current state") diff --git a/demo/scripts/fallbacks.py b/demo/scripts/fallbacks.py index b6d89f3c1..e7e3531b7 100755 --- a/demo/scripts/fallbacks.py +++ b/demo/scripts/fallbacks.py @@ -5,7 +5,7 @@ from py_binding_tools import roscpp_init import time -roscpp_init("mtc_tutorial_fallbacks") +roscpp_init("mtc_tutorial") # use cartesian and joint interpolation planners cartesianPlanner = core.CartesianPath() @@ -13,6 +13,7 @@ # initialize the mtc task task = core.Task() +task.name = "fallbacks" # 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 531e6d86a..ce671828d 100755 --- a/demo/scripts/fix_collision_objects.py +++ b/demo/scripts/fix_collision_objects.py @@ -5,10 +5,11 @@ from py_binding_tools import roscpp_init import time -roscpp_init("mtc_tutorial_current_state") +roscpp_init("mtc_tutorial") # Create a task task = core.Task() +task.name = "fix collision objects" # 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 56c735aa8..1b2e7962b 100755 --- a/demo/scripts/fixed_state.py +++ b/demo/scripts/fixed_state.py @@ -7,11 +7,12 @@ from moveit.core.planning_scene import PlanningScene import time -roscpp_init("mtc_tutorial_current_state") +roscpp_init("mtc_tutorial") # Create a task task = core.Task() +task.name = "fixed state" # [initAndConfigFixedState] # Initialize a PlanningScene for use in a FixedState stage diff --git a/demo/scripts/generate_pose.py b/demo/scripts/generate_pose.py index 39498c415..ebaf967f0 100755 --- a/demo/scripts/generate_pose.py +++ b/demo/scripts/generate_pose.py @@ -6,13 +6,14 @@ from py_binding_tools import roscpp_init import time -roscpp_init("mtc_tutorial_compute_ik") +roscpp_init("mtc_tutorial") # Specify the planning group group = "panda_arm" # Create a task task = core.Task() +task.name = "generate pose" # Get the current robot state currentState = stages.CurrentState("current state") diff --git a/demo/scripts/merger.py b/demo/scripts/merger.py index 224fcd782..30b901a69 100755 --- a/demo/scripts/merger.py +++ b/demo/scripts/merger.py @@ -5,13 +5,14 @@ from py_binding_tools import roscpp_init import time -roscpp_init("mtc_tutorial_merger") +roscpp_init("mtc_tutorial") # use the joint interpolation planner planner = core.JointInterpolationPlanner() # the task will contain our stages task = core.Task() +task.name = "merger" # 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 d75ff88e4..39da7837a 100755 --- a/demo/scripts/modify_planning_scene.py +++ b/demo/scripts/modify_planning_scene.py @@ -8,10 +8,11 @@ from py_binding_tools import roscpp_init import time -roscpp_init("mtc_tutorial_modify_planning_scene") +roscpp_init("mtc_tutorial") # Create a task task = core.Task() +task.name = "modify planning scene" # 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 index 6061a057c..3ee64dfe2 100755 --- a/demo/scripts/multi_planner.py +++ b/demo/scripts/multi_planner.py @@ -7,15 +7,16 @@ roscpp_init("mtc_tutorial") -ompl_pipelinePlanner = core.PipelinePlanner("ompl") -ompl_pipelinePlanner.planner = "RRTConnectkConfigDefault" -pilz_pipelinePlanner = core.PipelinePlanner("pilz_industrial_motion_planner") -pilz_pipelinePlanner.planner = "PTP" +ompl_planner = core.PipelinePlanner("ompl") +ompl_planner.planner = "RRTConnectkConfigDefault" +pilz_planner = core.PipelinePlanner("pilz_industrial_motion_planner") +pilz_planner.planner = "PTP" multiPlanner = core.MultiPlanner() -multiPlanner.add(pilz_pipelinePlanner, ompl_pipelinePlanner) +multiPlanner.add(pilz_planner, ompl_planner) # Create a task task = core.Task() +task.name = "multi planner" # Start from current robot state currentState = stages.CurrentState("current state") diff --git a/demo/scripts/pickplace.py b/demo/scripts/pickplace.py index e43a2e291..5105c6d12 100755 --- a/demo/scripts/pickplace.py +++ b/demo/scripts/pickplace.py @@ -7,7 +7,7 @@ from geometry_msgs.msg import PoseStamped, TwistStamped import time -roscpp_init("pickplace") +roscpp_init("mtc_tutorial") # [pickAndPlaceTut1] # Specify robot parameters @@ -40,8 +40,8 @@ # [pickAndPlaceTut3] # Create a task -task = core.Task("PandaPickPipelineExample") -task.enableIntrospection() +task = core.Task() +task.name = "pick + place" # [pickAndPlaceTut3] # [pickAndPlaceTut4] diff --git a/demo/scripts/properties.py b/demo/scripts/properties.py index ee3992bae..0a6d8c94a 100755 --- a/demo/scripts/properties.py +++ b/demo/scripts/properties.py @@ -9,14 +9,6 @@ roscpp_init("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] - # [propertyTut1] # Create a property p = core.Property() @@ -86,15 +78,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] From 6d376fb8b938376c9ec9d0a32ab8815077cc870f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 12 Jul 2024 06:23:41 +0200 Subject: [PATCH 45/55] Connect: Relax validity check of reached end state --- core/src/stages/connect.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index c235463b3..da6e3406b 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -57,7 +57,7 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); - p.declare("max_distance", 1e-4, + p.declare("max_distance", 1e-2, "maximally accepted joint configuration distance between trajectory endpoint and goal state"); p.declare("path_constraints", moveit_msgs::Constraints(), "constraints to maintain during trajectory"); From 8d2baf27398874b4cfae00672f19df1d54c25b2f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 12 Jul 2024 19:24:00 +0200 Subject: [PATCH 46/55] Cleanup unit tests ... and allow them to run via both, cmdline and pytest --- core/python/test/rostest_mps.py | 6 ++++-- core/python/test/rostest_mtc.py | 9 +++++---- core/python/test/rostest_trampoline.py | 24 ++++++++++++++++-------- core/python/test/test_mtc.py | 13 +++++-------- 4 files changed, 30 insertions(+), 22 deletions(-) diff --git a/core/python/test/rostest_mps.py b/core/python/test/rostest_mps.py index 56ac151b9..c89a1fc5b 100755 --- a/core/python/test/rostest_mps.py +++ b/core/python/test/rostest_mps.py @@ -1,6 +1,5 @@ #! /usr/bin/env python3 -from __future__ import print_function import unittest import rostest from py_binding_tools import roscpp_init @@ -9,6 +8,10 @@ from geometry_msgs.msg import PoseStamped +def setUpModule(): + roscpp_init("test_mtc") + + def make_pose(x, y, z): pose = PoseStamped() pose.header.frame_id = "base_link" @@ -113,5 +116,4 @@ def test_bw_remove_object(self): if __name__ == "__main__": - roscpp_init("test_mtc") rostest.rosrun("mtc", "mps", TestModifyPlanningScene) diff --git a/core/python/test/rostest_mtc.py b/core/python/test/rostest_mtc.py index 5db8487d4..6b6a9de60 100755 --- a/core/python/test/rostest_mtc.py +++ b/core/python/test/rostest_mtc.py @@ -1,14 +1,16 @@ #! /usr/bin/env python3 -from __future__ import print_function import unittest import rostest from py_binding_tools import roscpp_init 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(): + roscpp_init("test_mtc") class Test(unittest.TestCase): @@ -55,5 +57,4 @@ def createDisplacement(group, displacement): if __name__ == "__main__": - roscpp_init("test_mtc") rostest.rosrun("mtc", "base", Test) diff --git a/core/python/test/rostest_trampoline.py b/core/python/test/rostest_trampoline.py index 46641af2e..8d33a31d3 100755 --- a/core/python/test/rostest_trampoline.py +++ b/core/python/test/rostest_trampoline.py @@ -1,7 +1,6 @@ #! /usr/bin/env python3 # -*- coding: utf-8 -*- -from __future__ import print_function import unittest import rostest from py_binding_tools import roscpp_init @@ -12,11 +11,21 @@ PLANNING_GROUP = "manipulator" -pybind11_versions = [ - k for k in __builtins__.__dict__.keys() if k.startswith("__pybind11_internals_v") -] + +def setUpModule(): + roscpp_init("test_mtc") + + +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() ) @@ -106,12 +115,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 +140,4 @@ def test_propagator(self): if __name__ == "__main__": - roscpp_init("test_mtc") rostest.rosrun("mtc", "trampoline", TestTrampolines) diff --git a/core/python/test/test_mtc.py b/core/python/test/test_mtc.py index 8cc73ba70..ff1d7076c 100644 --- a/core/python/test/test_mtc.py +++ b/core/python/test/test_mtc.py @@ -1,16 +1,15 @@ #! /usr/bin/env python3 # -*- coding: utf-8 -*- -from __future__ import print_function -import unittest, sys +import sys +import unittest 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 class TestPropertyMap(unittest.TestCase): - def __init__(self, *args, **kwargs): - super(TestPropertyMap, self).__init__(*args, **kwargs) + def setUp(self): self.props = core.PropertyMap() def _check(self, name, value): @@ -85,8 +84,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,8 +115,7 @@ def test_allow_collisions(self): class TestStages(unittest.TestCase): - def __init__(self, *args, **kwargs): - super(TestStages, self).__init__(*args, **kwargs) + def setUp(self): self.planner = core.PipelinePlanner() def _check(self, stage, name, value): From 29d12196dad3df012dd772b33d1ca94f0cfd7cff Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 26 May 2023 16:52:46 +0200 Subject: [PATCH 47/55] Reenable python bindings --- core/CMakeLists.txt | 6 +- .../moveit/python/python_tools/ros_types.h | 90 ------------------- .../python/task_constructor/properties.h | 6 +- core/package.xml | 1 + core/python/CMakeLists.txt | 1 - core/python/bindings/CMakeLists.txt | 30 +------ core/python/bindings/src/core.cpp | 27 +----- core/python/bindings/src/properties.cpp | 13 +-- core/python/bindings/src/solvers.cpp | 9 +- core/python/bindings/src/stages.cpp | 6 +- core/python/{src => moveit}/__init__.py | 0 .../task_constructor/__init__.py | 0 .../{src => moveit}/task_constructor/core.py | 0 .../task_constructor/stages.py | 0 core/python/src/python_tools/__init__.py | 1 - demo/package.xml | 1 + 16 files changed, 29 insertions(+), 162 deletions(-) delete mode 100644 core/include/moveit/python/python_tools/ros_types.h rename core/python/{src => moveit}/__init__.py (100%) rename core/python/{src => moveit}/task_constructor/__init__.py (100%) rename core/python/{src => moveit}/task_constructor/core.py (100%) rename core/python/{src => moveit}/task_constructor/stages.py (100%) delete mode 100644 core/python/src/python_tools/__init__.py diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 1dbb4ca19..48cfd1b9d 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -12,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) @@ -26,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 @@ -42,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/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 9e6a884cc..e661fa354 100644 --- a/core/include/moveit/python/task_constructor/properties.h +++ b/core/include/moveit/python/task_constructor/properties.h @@ -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/package.xml b/core/package.xml index 72128a15e..6720f64cb 100644 --- a/core/package.xml +++ b/core/package.xml @@ -20,6 +20,7 @@ moveit_ros_planning moveit_ros_planning_interface moveit_task_constructor_msgs + py_binding_tools rclcpp rviz_marker_tools tf2_eigen diff --git a/core/python/CMakeLists.txt b/core/python/CMakeLists.txt index dcb988585..82264c28e 100644 --- a/core/python/CMakeLists.txt +++ b/core/python/CMakeLists.txt @@ -24,7 +24,6 @@ add_subdirectory(pybind11) # C++ wrapper code add_subdirectory(bindings) -ament_python_install_package("src") if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) 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 c49923f31..090e0b628 100644 --- a/core/python/bindings/src/core.cpp +++ b/core/python/bindings/src/core.cpp @@ -412,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, @@ -471,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 142ec3e74..4330d0f80 100644 --- a/core/python/bindings/src/properties.cpp +++ b/core/python/bindings/src/properties.cpp @@ -119,12 +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& e) { + 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 o.attr("__class__").attr("__name__").cast(); - } + return module + "." + name; + else + return module.substr(0, pos) + "/msg/" + name; } boost::any PropertyConverterRegistry::fromPython(const py::object& po) { diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index 04b06c2b8..b9635a808 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include "utils.h" namespace py = pybind11; @@ -69,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", @@ -81,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", diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index e366e50c8..e86ef4de6 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include #include @@ -357,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 @@ -393,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/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/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/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 From 430a69f4b7231073123cf35c2668054d78aeb77f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 13 Jul 2024 01:17:20 +0200 Subject: [PATCH 48/55] Adapt python scripts to ROS2 API changes --- core/python/CMakeLists.txt | 10 +++-- core/python/test/rostest_mps.py | 10 ++--- core/python/test/rostest_mtc.py | 14 +++++-- core/python/test/rostest_trampoline.py | 9 +++-- core/python/test/test.launch.py | 51 ++++++++++++++++++++++++++ core/python/test/test_mtc.py | 20 +++++----- demo/scripts/alternatives.py | 6 ++- demo/scripts/cartesian.py | 19 ++++++---- demo/scripts/compute_ik.py | 8 ++-- demo/scripts/constrained.py | 11 ++++-- demo/scripts/current_state.py | 6 ++- demo/scripts/fallbacks.py | 6 ++- demo/scripts/fix_collision_objects.py | 6 ++- demo/scripts/fixed_state.py | 8 ++-- demo/scripts/generate_pose.py | 9 +++-- demo/scripts/merger.py | 6 ++- demo/scripts/modify_planning_scene.py | 6 ++- demo/scripts/multi_planner.py | 12 +++--- demo/scripts/pickplace.py | 8 ++-- demo/scripts/properties.py | 5 ++- 20 files changed, 157 insertions(+), 73 deletions(-) create mode 100644 core/python/test/test.launch.py diff --git a/core/python/CMakeLists.txt b/core/python/CMakeLists.txt index 82264c28e..15e0be08c 100644 --- a/core/python/CMakeLists.txt +++ b/core/python/CMakeLists.txt @@ -27,10 +27,8 @@ add_subdirectory(bindings) if(BUILD_TESTING) 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} @@ -39,4 +37,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/test/rostest_mps.py b/core/python/test/rostest_mps.py index 1424dcac5..94e735e86 100755 --- a/core/python/test/rostest_mps.py +++ b/core/python/test/rostest_mps.py @@ -1,15 +1,13 @@ #! /usr/bin/env python3 import unittest -import rostest -from py_binding_tools import roscpp_init -from moveit_commander import PlanningSceneInterface +import rclcpp from moveit.task_constructor import core, stages from geometry_msgs.msg import PoseStamped def setUpModule(): - roscpp_init("test_mtc") + rclcpp.init() def make_pose(x, y, z): @@ -31,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): @@ -116,4 +116,4 @@ def test_bw_remove_object(self): if __name__ == "__main__": - rostest.rosrun("mtc", "mps", TestModifyPlanningScene) + unittest.main() diff --git a/core/python/test/rostest_mtc.py b/core/python/test/rostest_mtc.py index 6b6a9de60..fe0432429 100755 --- a/core/python/test/rostest_mtc.py +++ b/core/python/test/rostest_mtc.py @@ -1,8 +1,7 @@ #! /usr/bin/env python3 import unittest -import rostest -from py_binding_tools import roscpp_init +import rclcpp from moveit.task_constructor import core, stages from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import Vector3Stamped, Vector3 @@ -10,12 +9,15 @@ def setUpModule(): - roscpp_init("test_mtc") + 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 @@ -26,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()) @@ -45,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])) @@ -57,4 +63,4 @@ def createDisplacement(group, displacement): if __name__ == "__main__": - rostest.rosrun("mtc", "base", Test) + unittest.main() diff --git a/core/python/test/rostest_trampoline.py b/core/python/test/rostest_trampoline.py index 8d33a31d3..af5f2ebd2 100755 --- a/core/python/test/rostest_trampoline.py +++ b/core/python/test/rostest_trampoline.py @@ -2,8 +2,7 @@ # -*- coding: utf-8 -*- import unittest -import rostest -from py_binding_tools import roscpp_init +import rclcpp from moveit.task_constructor import core, stages from moveit.core.planning_scene import PlanningScene from geometry_msgs.msg import Vector3Stamped, Vector3, PoseStamped @@ -13,7 +12,7 @@ def setUpModule(): - roscpp_init("test_mtc") + rclcpp.init() def pybind11_versions(): @@ -100,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) @@ -140,4 +141,4 @@ def test_propagator(self): if __name__ == "__main__": - 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 ff1d7076c..bdda707ee 100644 --- a/core/python/test/test_mtc.py +++ b/core/python/test/test_mtc.py @@ -3,13 +3,19 @@ import sys 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() + + class TestPropertyMap(unittest.TestCase): def setUp(self): + self.node = rclcpp.Node("test_mtc_props") self.props = core.PropertyMap() def _check(self, name, value): @@ -28,7 +34,7 @@ def test_assign(self): self.assertRaises(TypeError, self._check, "request", MotionPlanRequest()) def test_assign_in_reference(self): - planner = core.PipelinePlanner() + planner = core.PipelinePlanner(self.node) props = planner.properties props["goal_joint_tolerance"] = 3.14 @@ -40,21 +46,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 @@ -116,7 +116,8 @@ def test_allow_collisions(self): class TestStages(unittest.TestCase): def setUp(self): - self.planner = core.PipelinePlanner() + 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) @@ -198,8 +199,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/demo/scripts/alternatives.py b/demo/scripts/alternatives.py index 5a221a02b..3e2f81cda 100755 --- a/demo/scripts/alternatives.py +++ b/demo/scripts/alternatives.py @@ -2,10 +2,11 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from py_binding_tools import roscpp_init +import rclcpp import time -roscpp_init("mtc_tutorial") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # Use the joint interpolation planner jointPlanner = core.JointInterpolationPlanner() @@ -13,6 +14,7 @@ # 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 a79360124..03821fa5b 100755 --- a/demo/scripts/cartesian.py +++ b/demo/scripts/cartesian.py @@ -1,15 +1,17 @@ #! /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 py_binding_tools import roscpp_init +import rclcpp -roscpp_init("mtc_tutorial") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # [cartesianTut1] group = "panda_arm" @@ -24,6 +26,7 @@ # [cartesianTut3] task = core.Task() task.name = "cartesian" +task.loadRobotModel(node) # start from current robot state task.add(stages.CurrentState("current state")) @@ -34,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] @@ -42,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] @@ -50,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 b575c79d3..db8838e6f 100755 --- a/demo/scripts/compute_ik.py +++ b/demo/scripts/compute_ik.py @@ -6,9 +6,10 @@ from std_msgs.msg import Header import time -from py_binding_tools import roscpp_init +import rclcpp -roscpp_init("mtc_tutorial") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # Specify the planning group group = "panda_arm" @@ -16,12 +17,13 @@ # 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 diff --git a/demo/scripts/constrained.py b/demo/scripts/constrained.py index 4efc53c2a..8f0abd3f9 100755 --- a/demo/scripts/constrained.py +++ b/demo/scripts/constrained.py @@ -8,15 +8,17 @@ from moveit.task_constructor import core, stages import time -from py_binding_tools import roscpp_init +import rclcpp -roscpp_init("mtc_tutorial") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") group = "panda_arm" -planner = core.PipelinePlanner() +planner = core.PipelinePlanner(node) task = core.Task() task.name = "constrained" +task.loadRobotModel(node) task.add(stages.CurrentState("current state")) @@ -43,13 +45,14 @@ move = stages.MoveRelative("y +0.4", planner) move.group = group header = Header(frame_id="world") -move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, 0.4, 0))) +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 diff --git a/demo/scripts/current_state.py b/demo/scripts/current_state.py index 1c23cb0c4..82c73a5ac 100755 --- a/demo/scripts/current_state.py +++ b/demo/scripts/current_state.py @@ -2,14 +2,16 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from py_binding_tools import roscpp_init +import rclcpp import time -roscpp_init("mtc_tutorial") +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 e7e3531b7..32c509665 100755 --- a/demo/scripts/fallbacks.py +++ b/demo/scripts/fallbacks.py @@ -2,10 +2,11 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from py_binding_tools import roscpp_init +import rclcpp import time -roscpp_init("mtc_tutorial") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # use cartesian and joint interpolation planners cartesianPlanner = core.CartesianPath() @@ -14,6 +15,7 @@ # 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 ce671828d..ba4cd8587 100755 --- a/demo/scripts/fix_collision_objects.py +++ b/demo/scripts/fix_collision_objects.py @@ -2,14 +2,16 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from py_binding_tools import roscpp_init +import rclcpp import time -roscpp_init("mtc_tutorial") +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 1b2e7962b..f56a0ec95 100755 --- a/demo/scripts/fixed_state.py +++ b/demo/scripts/fixed_state.py @@ -3,12 +3,12 @@ from moveit.core import planning_scene from moveit.task_constructor import core, stages -from py_binding_tools import roscpp_init +import rclcpp from moveit.core.planning_scene import PlanningScene import time -roscpp_init("mtc_tutorial") - +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # Create a task task = core.Task() @@ -16,7 +16,7 @@ # [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 ebaf967f0..2a20d84cc 100755 --- a/demo/scripts/generate_pose.py +++ b/demo/scripts/generate_pose.py @@ -3,10 +3,11 @@ from moveit.task_constructor import core, stages from geometry_msgs.msg import PoseStamped -from py_binding_tools import roscpp_init +import rclcpp import time -roscpp_init("mtc_tutorial") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # Specify the planning group group = "panda_arm" @@ -14,6 +15,7 @@ # Create a task task = core.Task() task.name = "generate pose" +task.loadRobotModel(node) # Get the current robot state currentState = stages.CurrentState("current state") @@ -21,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 30b901a69..279d81cf9 100755 --- a/demo/scripts/merger.py +++ b/demo/scripts/merger.py @@ -2,10 +2,11 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from py_binding_tools import roscpp_init +import rclcpp import time -roscpp_init("mtc_tutorial") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # use the joint interpolation planner planner = core.JointInterpolationPlanner() @@ -13,6 +14,7 @@ # 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 39da7837a..d64ba7a46 100755 --- a/demo/scripts/modify_planning_scene.py +++ b/demo/scripts/modify_planning_scene.py @@ -5,14 +5,16 @@ from moveit_msgs.msg import CollisionObject from shape_msgs.msg import SolidPrimitive from geometry_msgs.msg import PoseStamped -from py_binding_tools import roscpp_init +import rclcpp import time -roscpp_init("mtc_tutorial") +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 index 3ee64dfe2..948dd96a3 100755 --- a/demo/scripts/multi_planner.py +++ b/demo/scripts/multi_planner.py @@ -2,21 +2,21 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from py_binding_tools import roscpp_init +import rclcpp import time -roscpp_init("mtc_tutorial") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") -ompl_planner = core.PipelinePlanner("ompl") -ompl_planner.planner = "RRTConnectkConfigDefault" -pilz_planner = core.PipelinePlanner("pilz_industrial_motion_planner") -pilz_planner.planner = "PTP" +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") diff --git a/demo/scripts/pickplace.py b/demo/scripts/pickplace.py index 5105c6d12..e33503495 100755 --- a/demo/scripts/pickplace.py +++ b/demo/scripts/pickplace.py @@ -1,13 +1,14 @@ #! /usr/bin/env python3 # -*- coding: utf-8 -*- -from py_binding_tools import roscpp_init +import rclcpp from moveit.task_constructor import core, stages from moveit_commander import PlanningSceneInterface from geometry_msgs.msg import PoseStamped, TwistStamped import time -roscpp_init("mtc_tutorial") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # [pickAndPlaceTut1] # Specify robot parameters @@ -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 index 0a6d8c94a..75d32a935 100755 --- a/demo/scripts/properties.py +++ b/demo/scripts/properties.py @@ -5,9 +5,10 @@ from geometry_msgs.msg import PoseStamped import time -from py_binding_tools import roscpp_init +import rclcpp -roscpp_init("mtc_tutorial") +rclcpp.init() +node = rclcpp.Node("mtc_tutorial") # [propertyTut1] # Create a property From 75988a4a1c08a05bda05d4f107e7e8fc0356a551 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 11 Jul 2024 15:43:49 +0200 Subject: [PATCH 49/55] Install python demo scripts --- demo/CMakeLists.txt | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) 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} ) From 5519162b400ab42b07d5e5db1d6bfab6f3e2c69e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 13 Jul 2024 02:37:58 +0200 Subject: [PATCH 50/55] CI: skip python tests - with asan - with clang builds --- .github/workflows/ci.yaml | 1 + core/python/CMakeLists.txt | 3 +-- core/python/test/test_mtc.py | 21 +++++++++++++++++++++ 3 files changed, 23 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 46c855907..845e3266b 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -22,6 +22,7 @@ jobs: # - IMAGE: rolling-source # NAME: ccov # TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage" + - IMAGE: rolling-source - IMAGE: rolling-source CLANG_TIDY: pedantic - IMAGE: jazzy-source diff --git a/core/python/CMakeLists.txt b/core/python/CMakeLists.txt index 15e0be08c..8b0616f13 100644 --- a/core/python/CMakeLists.txt +++ b/core/python/CMakeLists.txt @@ -24,8 +24,7 @@ add_subdirectory(pybind11) # C++ wrapper code add_subdirectory(bindings) - -if(BUILD_TESTING) +if(BUILD_TESTING AND NOT DEFINED ENV{PRELOAD}) find_package(ament_cmake_pytest REQUIRED) find_package(launch_testing_ament_cmake) set(_pytest_tests test/test_mtc.py) diff --git a/core/python/test/test_mtc.py b/core/python/test/test_mtc.py index bdda707ee..77a57645d 100644 --- a/core/python/test/test_mtc.py +++ b/core/python/test/test_mtc.py @@ -2,6 +2,7 @@ # -*- coding: utf-8 -*- import sys +import pytest import unittest import rclcpp from geometry_msgs.msg import Pose, PoseStamped, PointStamped, TwistStamped, Vector3Stamped @@ -13,6 +14,24 @@ 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 setUp(self): self.node = rclcpp.Node("test_mtc_props") @@ -33,6 +52,7 @@ 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(self.node) props = planner.properties @@ -115,6 +135,7 @@ def test_allow_collisions(self): class TestStages(unittest.TestCase): + @unittest.skipIf(incompatible_pybind11, incompatible_pybind11_msg) def setUp(self): self.node = rclcpp.Node("test_mtc_stages") self.planner = core.PipelinePlanner(self.node) From 2e9a223827b60b316b3d2c763ce5201e7b09d377 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 16 Jul 2024 08:24:55 +0200 Subject: [PATCH 51/55] MoveRelative: handle equal min/max distance (#593) When min_distance == max_distance > 0.0, the minimal distance might be missed due to numerical errors. To avoid this, deactivate the minimal distance check and run the full distance as given by max_distance. --- core/src/stages/move_relative.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 3f1b915ed..634c8b56f 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -185,6 +185,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; From 60ccd744435e90393a38bb987bb398f8f9760974 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 16 Jul 2024 09:18:16 -0600 Subject: [PATCH 52/55] MoveRelative: fix segfault on empty trajectory (#595) Check that at least one robot state exists in the robot trajectory before accessing it. Signed-off-by: Paul Gesel --- core/src/stages/move_relative.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 634c8b56f..7bf065f39 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -296,7 +296,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning if (!success) comment = result.message; - 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; @@ -331,7 +332,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(); From cd28bdcd1f72af5a713a823c0da287dedfc0cc48 Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Wed, 17 Jul 2024 08:56:01 -0400 Subject: [PATCH 53/55] Fix early planning preemption (#597) Calling preempt() before plan() is able to reset the preempt_requested_ flag causes the preemption request to get lost. To avoid this issue, we allow a) manual resetting of the request and b) reset the request before leaving plan(). --- .gitmodules | 3 ++ core/include/moveit/task_constructor/task.h | 3 +- core/include/moveit/task_constructor/task_p.h | 2 +- core/src/scope_guard | 1 + core/src/task.cpp | 10 ++++++- core/test/test_container.cpp | 28 +++++++++++++++++++ 6 files changed, 44 insertions(+), 3 deletions(-) create mode 160000 core/src/scope_guard 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/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 00ea354c2..d1c123740 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -127,8 +127,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/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/task.cpp b/core/src/task.cpp index f969cf9e8..dbba73890 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -46,6 +46,8 @@ #include #include +#include + #include namespace { @@ -234,6 +236,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(); @@ -245,7 +250,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)) { @@ -266,6 +270,10 @@ void Task::preempt() { pimpl()->preempt_requested_ = true; } +void Task::resetPreemptRequest() { + pimpl()->preempt_requested_ = false; +} + moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) { actionlib::SimpleActionClient ac("execute_task_solution"); if (!ac.waitForServer(ros::Duration(0.5))) { 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() +} From 763148664838f6920304d57d026db5ccfc0fee91 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 17 Jul 2024 15:10:53 +0200 Subject: [PATCH 54/55] Add unittest for #581 --- core/test/test_fallback.cpp | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp index 87f9368f7..c5b7eb90b 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" @@ -159,6 +160,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) { From 4f69a22ddb41835af6dc8578408f651b7a3e5ecc Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 18 Jul 2024 13:45:15 +0200 Subject: [PATCH 55/55] Silent error "Found empty JointState message" --- capabilities/src/execute_task_solution_capability.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 2a3afa9ed..17facf74c 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -170,8 +170,10 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr exec_traj.effect_on_success_ = [this, &scene_diff = const_cast<::moveit_msgs::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::JointState(); scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::MultiDOFJointState(); + scene_diff.robot_state.is_diff = true; // silent empty JointState msg error if (!moveit::core::isEmpty(scene_diff)) { ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description);