[WIP] Move Relative with Pilz does not reject trajectory with collision

core/test/move_relative.test

@@ -1,5 +1,5 @@
 <launch>
-  <include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch">
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/move_group.launch">
     <arg name="load_robot_description" value="true"/>
   </include>
 

core/test/test_move_relative.cpp

@@ -4,6 +4,7 @@
 #include <moveit/task_constructor/stages/move_relative.h>
 #include <moveit/task_constructor/stages/fixed_state.h>
 #include <moveit/task_constructor/solvers/cartesian_path.h>
+#include <moveit/task_constructor/solvers/pipeline_planner.h>
 
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/robot_trajectory/robot_trajectory.h>
@@ -13,23 +14,44 @@
 
 #include <gtest/gtest.h>
 
+#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;
 
 constexpr double TAU{ 2 * M_PI };
 constexpr double EPS{ 5e-5 };
 
+template <typename Planner>
+std::shared_ptr<Planner> create();
+template <>
+solvers::CartesianPathPtr create<solvers::CartesianPath>() {
+	return std::make_shared<solvers::CartesianPath>();
+}
+template <>
+solvers::PipelinePlannerPtr create<solvers::PipelinePlanner>() {
+	auto p = std::make_shared<solvers::PipelinePlanner>("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 <typename Planner = solvers::CartesianPath>
 struct PandaMoveRelative : public testing::Test
 {
 	Task t;
 	stages::MoveRelative* move;
 	PlanningScenePtr scene;
+	std::shared_ptr<Planner> planner;
 
 	const JointModelGroup* group;
 
-	PandaMoveRelative() {
+	PandaMoveRelative() : planner(create<Planner>()) {
 		t.setRobotModel(loadModel());
 
 		group = t.getRobotModel()->getJointModelGroup("panda_arm");
@@ -39,31 +61,44 @@ struct PandaMoveRelative : public testing::Test
 		scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready");
 		t.add(std::make_unique<stages::FixedState>("start", scene));
 
-		auto move_relative = std::make_unique<stages::MoveRelative>("move", std::make_shared<solvers::CartesianPath>());
+		auto move_relative = std::make_unique<stages::MoveRelative>("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<solvers::CartesianPath>;
+
+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);
 		p.dimensions[p.SPHERE_RADIUS] = 0.01;
 		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<solvers::CartesianPath, solvers::PipelinePlanner>;
+TYPED_TEST_SUITE(PandaMoveRelative, PlannerTypes);
+TYPED_TEST(PandaMoveRelative, cartesianCollisionMinMaxDistance) {
+	if (std::is_same<TypeParam, solvers::PipelinePlanner>::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<const SubTrajectory>(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");