Merge branch 'master' into ros2

Author: rhaschke

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
 

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

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>

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());

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()

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.

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;

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;
 };
 

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); }

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