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