Merge branch 'master' into ros2

Author: rhaschke

Diff for: 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 for: capabilities/CMakeLists.txt

@@ -3,6 +3,7 @@ project(moveit_task_constructor_capabilities)
 
 find_package(ament_cmake REQUIRED)
 
+find_package(fmt REQUIRED)
 find_package(Boost REQUIRED)
 find_package(moveit_common REQUIRED)
 moveit_package()
@@ -20,6 +21,7 @@ add_library(${PROJECT_NAME} SHARED
 )
 ament_target_dependencies(${PROJECT_NAME}
                           Boost
+                          fmt
                           rclcpp_action
                           moveit_core
                           moveit_ros_move_group

Diff for: capabilities/package.xml

@@ -12,6 +12,7 @@
 
 	<buildtool_depend>ament_cmake</buildtool_depend>
 
+	<depend>fmt</depend>
 	<depend>moveit_core</depend>
 	<depend>moveit_ros_move_group</depend>
 	<depend>moveit_ros_planning</depend>

Diff for: capabilities/src/execute_task_solution_capability.cpp

@@ -43,9 +43,7 @@
 #include <moveit/move_group/capability_names.h>
 #include <moveit/robot_state/conversions.h>
 #include <moveit/utils/message_checks.h>
-#include <moveit/moveit_cpp/moveit_cpp.h>
-
-#include <boost/algorithm/string/join.hpp>
+#include <fmt/format.h>
 
 namespace {
 
@@ -164,8 +162,8 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 			if (!joint_names.empty()) {
 				group = findJointModelGroup(*model, joint_names);
 				if (!group) {
-					RCLCPP_ERROR_STREAM(LOGGER, "Could not find JointModelGroup that actuates {"
-					                                << boost::algorithm::join(joint_names, ", ") << "}");
+					RCLCPP_ERROR_STREAM(LOGGER, fmt::format("Could not find JointModelGroup that actuates {{{}}}",
+					                                        fmt::join(joint_names, ", ")));
 					return false;
 				}
 				RCLCPP_DEBUG(LOGGER, "Using JointModelGroup '%s' for execution", group->getName().c_str());

Diff for: core/CMakeLists.txt

@@ -4,6 +4,7 @@ project(moveit_task_constructor_core)
 find_package(ament_cmake REQUIRED)
 
 find_package(Boost REQUIRED)
+find_package(fmt REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(moveit_common REQUIRED)
 moveit_package()

Diff for: 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 for: core/include/moveit/task_constructor/container.h

@@ -51,6 +51,10 @@ class ContainerBase : public Stage
 	PRIVATE_CLASS(ContainerBase)
 	using pointer = std::unique_ptr<ContainerBase>;
 
+	/// Explicitly enable/disable pruning
+	void setPruning(bool pruning) { setProperty("pruning", pruning); }
+	bool pruning() const { return properties().get<bool>("pruning"); }
+
 	size_t numChildren() const;
 	Stage* findChild(const std::string& name) const;
 	Stage* operator[](int index) const;

Diff for: 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<std::string> joints);
 	/// Limit measurements to given joints and use given weighting
 	PathLength(std::map<std::string, double> j) : joints(std::move(j)) {}
+
+	using TrajectoryCostTerm::operator();
 	double operator()(const SubTrajectory& s, std::string& comment) const override;
 
 	std::map<std::string, double> joints;  //< joint weights
@@ -145,6 +148,8 @@ class DistanceToReference : public TrajectoryCostTerm
 	                    std::map<std::string, double> w = std::map<std::string, double>());
 	DistanceToReference(const std::map<std::string, double>& ref, Mode m = Mode::AUTO,
 	                    std::map<std::string, double> w = std::map<std::string, double>());
+
+	using TrajectoryCostTerm::operator();
 	double operator()(const SubTrajectory& s, std::string& comment) const override;
 
 	moveit_msgs::msg::RobotState reference;
@@ -156,6 +161,7 @@ class DistanceToReference : public TrajectoryCostTerm
 class TrajectoryDuration : public TrajectoryCostTerm
 {
 public:
+	using TrajectoryCostTerm::operator();
 	double operator()(const SubTrajectory& s, std::string& comment) const override;
 };
 
@@ -167,6 +173,7 @@ class LinkMotion : public TrajectoryCostTerm
 
 	std::string link_name;
 
+	using TrajectoryCostTerm::operator();
 	double operator()(const SubTrajectory& s, std::string& comment) const override;
 };
 
@@ -191,6 +198,7 @@ class Clearance : public TrajectoryCostTerm
 
 	std::function<double(double)> distance_to_cost;
 
+	using TrajectoryCostTerm::operator();
 	double operator()(const SubTrajectory& s, std::string& comment) const override;
 };
 

Diff for: core/include/moveit/task_constructor/solvers/cartesian_path.h

@@ -52,6 +52,10 @@ class CartesianPath : public PlannerInterface
 public:
 	CartesianPath();
 
+	void setIKFrame(const geometry_msgs::msg::PoseStamped& pose) { setProperty("ik_frame", pose); }
+	void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
+	void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
+
 	void setStepSize(double step_size) { setProperty("step_size", step_size); }
 	void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); }
 	void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }

Diff for: core/include/moveit/task_constructor/solvers/multi_planner.h

@@ -71,6 +71,8 @@ class MultiPlanner : public PlannerInterface, public std::vector<solvers::Planne
 	            const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
 	            const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
 	            const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
+
+	std::string getPlannerId() const override { return "MultiPlanner"; }
 };
 }  // namespace solvers
 }  // namespace task_constructor

Diff for: core/include/moveit/task_constructor/stage_p.h

@@ -44,6 +44,7 @@
 #include <moveit/task_constructor/cost_queue.h>
 
 #include <rclcpp/rclcpp.hpp>
+#include <fmt/core.h>
 
 #include <ostream>
 #include <chrono>
@@ -144,7 +145,7 @@ class StagePrivate
 	void newSolution(const SolutionBasePtr& solution);
 	bool storeFailures() const { return introspection_ != nullptr; }
 	void runCompute() {
-		RCLCPP_DEBUG_STREAM(LOGGER, "Computing stage '" << name() << "'");
+		RCLCPP_DEBUG_STREAM(LOGGER, fmt::format("Computing stage '{}'", name()));
 		auto compute_start_time = std::chrono::steady_clock::now();
 		try {
 			compute();

Diff for: core/include/moveit/task_constructor/stages/passthrough.h

@@ -36,9 +36,6 @@
 #pragma once
 
 #include <moveit/task_constructor/container.h>
-#include <moveit/task_constructor/cost_queue.h>
-#include <geometry_msgs/msg/pose_stamped.hpp>
-#include <Eigen/Geometry>
 
 namespace moveit {
 namespace task_constructor {

Diff for: core/include/moveit/task_constructor/task.h

@@ -119,6 +119,9 @@ class Task : protected WrapperBase
 	using WrapperBase::setTimeout;
 	using WrapperBase::timeout;
 
+	using WrapperBase::pruning;
+	using WrapperBase::setPruning;
+
 	/// reset all stages
 	void reset() final;
 	/// initialize all stages with given scene

Diff for: 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 for: core/package.xml

@@ -3,9 +3,9 @@
 	<version>0.1.3</version>
 	<description>MoveIt Task Pipeline</description>
 
-	<url type="website">https://github.com/ros-planning/moveit_task_constructor</url>
-	<url type="repository">https://github.com/ros-planning/moveit_task_constructor</url>
-	<url type="bugtracker">https://github.com/ros-planning/moveit_task_constructor/issues</url>
+	<url type="website">https://github.com/moveit/moveit_task_constructor</url>
+	<url type="repository">https://github.com/moveit/moveit_task_constructor</url>
+	<url type="bugtracker">https://github.com/moveit/moveit_task_constructor/issues</url>
 
 	<license>BSD</license>
 	<maintainer email="[email protected]">Michael Goerner</maintainer>
@@ -14,6 +14,7 @@
 	<buildtool_depend>ament_cmake</buildtool_depend>
 	<buildtool_depend>ament_cmake_python</buildtool_depend>
 
+	<depend>fmt</depend>
 	<depend>geometry_msgs</depend>
 	<depend>moveit_core</depend>
 	<depend>moveit_ros_planning</depend>

Diff for: core/python/bindings/src/core.cpp

@@ -33,6 +33,7 @@
  *********************************************************************/
 
 #include "core.h"
+#include "utils.h"
 #include <pybind11/stl.h>
 #include <pybind11/functional.h>
 #include <moveit/python/task_constructor/properties.h>
@@ -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>
-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())
@@ -96,7 +80,9 @@ void setForwardedProperties(Stage& self, const py::object& names) {
 
 void export_core(pybind11::module& m) {
 	/// translate InitStageException into InitStageError
-	static py::exception<InitStageException> init_stage_error(m, "InitStageError");
+	PYBIND11_CONSTINIT static py::gil_safe_call_once_and_store<py::object> exc_storage;
+	exc_storage.call_once_and_store_result([&]() { return py::exception<InitStageException>(m, "InitStageError"); });
+
 	/// provide extended error description for InitStageException
 	py::register_exception_translator([](std::exception_ptr p) {  // NOLINT(performance-unnecessary-value-param)
 		try {
@@ -105,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 for: core/python/bindings/src/solvers.cpp

@@ -36,7 +36,9 @@
 #include <moveit/task_constructor/solvers/cartesian_path.h>
 #include <moveit/task_constructor/solvers/pipeline_planner.h>
 #include <moveit/task_constructor/solvers/joint_interpolation.h>
+#include <moveit/task_constructor/solvers/multi_planner.h>
 #include <moveit_msgs/WorkspaceParameters.h>
+#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 {
@@ -114,6 +117,27 @@ void export_solvers(py::module& m) {
 	        "This values specifies the fraction of mean acceptable joint motion per step.")
 	    .property<double>("min_fraction", "float: Fraction of overall distance required to succeed.")
 	    .def(py::init<>());
+
+	properties::class_<MultiPlanner, PlannerInterface>(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<MultiPlanner>)
+	    .def(
+	        "add",
+	        [](MultiPlanner& self, const py::args& args) {
+		        for (auto it = args.begin(), end = args.end(); it != end; ++it)
+			        self.push_back(it->cast<PlannerInterfacePtr>());
+	        },
+	        "Insert one or more planners")
+	    .def(
+	        "clear", [](MultiPlanner& self) { self.clear(); }, "Remove all planners")
+	    .def(py::init<>());
 }
 }  // namespace python
 }  // namespace moveit

Diff for: 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<uint32_t>("max_ik_solutions", "uint: max number of solutions to return")
 	    .property<bool>("ignore_collisions", R"(
 			bool: Specify if collisions with other members of
 			the planning scene are allowed.
 		)")
+	    .property<double>("min_solution_distance", "reject solution that are closer than this to previously found solutions")
+	    .property<moveit_msgs::msg::Constraints>("constraints", "additional constraints to obey")
 	    .property<geometry_msgs::msg::PoseStamped>("ik_frame", R"(
 			PoseStamped_: Specify the frame with respect
 			to which the inverse kinematics

Diff for: core/python/bindings/src/utils.h

@@ -0,0 +1,26 @@
+#include <pybind11/pybind11.h>
+
+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>
+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 for: core/python/pybind11

@@ -1,6 +1,5 @@
-#! /usr/bin/env python
+#! /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)