mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Connecting: also check that attached objects match
This commit is contained in:
parent
e885c272fa
commit
579c336558
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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";
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user