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 <moveit/planning_scene/planning_scene.h>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <iomanip>
|
#include <iomanip>
|
||||||
|
#include <algorithm>
|
||||||
#include <ros/console.h>
|
#include <ros/console.h>
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
@ -741,6 +742,46 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
|
|||||||
return false; // transforms do not match
|
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;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -10,9 +10,10 @@
|
|||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
|
using namespace planning_scene;
|
||||||
|
|
||||||
class GeneratorMockup : public Generator {
|
class GeneratorMockup : public Generator {
|
||||||
planning_scene::PlanningScenePtr ps;
|
PlanningScenePtr ps;
|
||||||
InterfacePtr prev;
|
InterfacePtr prev;
|
||||||
InterfacePtr next;
|
InterfacePtr next;
|
||||||
|
|
||||||
@ -25,7 +26,7 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr &robot_model) {
|
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; }
|
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) {
|
TEST(Stage, registerCallbacks) {
|
||||||
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
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::vector<const char*>{"bar", "boom"}, true);
|
||||||
s->allowCollisions("foo", std::set<const char*>{"ab", "abc"}, false);
|
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