diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 7d382b58..9631107e 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include namespace moveit { namespace task_constructor { @@ -741,6 +742,46 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta return false; // transforms do not match } } + + // Also test for attached objects which have a different storage + std::vector from_attached; + 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; + } + + 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; + } + 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->getFixedTransforms().size() != to_object->getFixedTransforms().size()) { + ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object->getName()); + return false; // shapes not matching + } + + for (auto from_it = from_object->getFixedTransforms().cbegin(), from_end = from_object->getFixedTransforms().cend(), + to_it = to_object->getFixedTransforms().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 object pose: " << from_object->getName()); + return false; // transforms do not match + } + } return true; } diff --git a/core/test/test_stage.cpp b/core/test/test_stage.cpp index a73504f9..7a5a8073 100644 --- a/core/test/test_stage.cpp +++ b/core/test/test_stage.cpp @@ -10,9 +10,10 @@ #include using namespace moveit::task_constructor; +using namespace planning_scene; class GeneratorMockup : public Generator { - planning_scene::PlanningScenePtr ps; + PlanningScenePtr ps; InterfacePtr prev; InterfacePtr next; @@ -25,7 +26,7 @@ public: } void init(const moveit::core::RobotModelConstPtr &robot_model) { - ps.reset((new planning_scene::PlanningScene(robot_model))); + ps.reset((new PlanningScene(robot_model))); } bool canCompute() const override { return true; } @@ -36,6 +37,12 @@ public: } }; +class ConnectMockup : public Connecting { +public: + using Connecting::compatible; + void compute(const InterfaceState& from, const InterfaceState& to) override {} +}; + TEST(Stage, registerCallbacks) { ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); @@ -98,3 +105,75 @@ TEST(ModifyPlanningScene, allowCollisions) { s->allowCollisions("foo", std::vector{"bar", "boom"}, true); s->allowCollisions("foo", std::set{"ab", "abc"}, false); } + +void spawnObject(PlanningScene& scene, const std::string& name, int type, + const std::vector& pos = {0,0,0}) { + moveit_msgs::CollisionObject o; + o.id= name; + o.header.frame_id= scene.getPlanningFrame(); + o.primitive_poses.resize(1); + o.primitive_poses[0].position.x = pos[0]; + o.primitive_poses[0].position.y = pos[1]; + o.primitive_poses[0].position.z = pos[2]; + o.primitive_poses[0].orientation.w = 1.0; + + o.primitives.resize(1); + o.primitives[0].type= type; + + switch (type) { + case shape_msgs::SolidPrimitive::CYLINDER: + o.primitives[0].dimensions = {0.1, 0.02}; + break; + case shape_msgs::SolidPrimitive::BOX: + o.primitives[0].dimensions = {0.1, 0.2, 0.3}; + break; + case shape_msgs::SolidPrimitive::SPHERE: + o.primitives[0].dimensions = {0.05}; + break; + } + scene.processCollisionObjectMsg(o); +} + +void attachObject(PlanningScene& scene, + const std::string& object, const std::string& link, bool attach) +{ + moveit_msgs::AttachedCollisionObject obj; + obj.link_name = link; + obj.object.operation = attach ? (int8_t) moveit_msgs::CollisionObject::ADD + : (int8_t) moveit_msgs::CollisionObject::REMOVE; + obj.object.id = object; + scene.processAttachedCollisionObjectMsg(obj); +} + +TEST(Connect, compatible) { + ConnectMockup connect; + auto scene = std::make_shared(getModel()); + auto& state = scene->getCurrentStateNonConst(); + state.setToDefaultValues(); + spawnObject(*scene, "object", shape_msgs::SolidPrimitive::CYLINDER); + state.update(); + + auto other = scene->diff(); + EXPECT_TRUE(connect.compatible(scene, other)) << "identical scenes"; + + spawnObject(*other, "object", shape_msgs::SolidPrimitive::BOX); + // EXPECT_FALSE(connect.compatible(scene, other)) << "different shapes"; + + spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER, {0.1,0,0}); + EXPECT_FALSE(connect.compatible(scene, other)) << "different pose"; + + spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER); + EXPECT_TRUE(connect.compatible(scene, other)) << "same objects"; + + // attached objects + other = scene->diff(); + attachObject(*scene, "object", "base_link", true); + EXPECT_FALSE(connect.compatible(scene, other)) << "detached and attached object"; + + other = scene->diff(); + EXPECT_TRUE(connect.compatible(scene, other)) << "identical scenes, attached object"; + + spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER, {0.1,0,0}); + attachObject(*other, "object", "base_link", true); + EXPECT_FALSE(connect.compatible(scene, other)) << "different pose"; +}