Connecting: also check that attached objects match

This commit is contained in:
Robert Haschke 2019-02-21 00:17:20 +01:00
parent e885c272fa
commit 579c336558
2 changed files with 122 additions and 2 deletions

View File

@ -41,6 +41,7 @@
#include <moveit/planning_scene/planning_scene.h>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <ros/console.h>
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<const moveit::core::AttachedBody*> from_attached;
std::vector<const moveit::core::AttachedBody*> 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;
}

View File

@ -10,9 +10,10 @@
#include <gtest/gtest.h>
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<const char*>{"bar", "boom"}, true);
s->allowCollisions("foo", std::set<const char*>{"ab", "abc"}, false);
}
void spawnObject(PlanningScene& scene, const std::string& name, int type,
const std::vector<double>& 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<PlanningScene>(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";
}