address interface changes for object poses in MoveIt

Also include a check for the new object pose field in `Connecting::compatible()`.
This commit is contained in:
v4hn 2021-08-23 00:28:34 +02:00
parent 0273aec83e
commit f51f6eb982
3 changed files with 38 additions and 11 deletions

View File

@ -38,6 +38,8 @@
#include <moveit/task_constructor/stage_p.h>
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
#include <ros/console.h>
@ -811,14 +813,23 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
// both scenes should have the same set of collision objects, at the same location
for (const auto& from_object_pair : *from->getWorld()) {
const std::string& from_object_name = from_object_pair.first;
const collision_detection::World::ObjectPtr& from_object = from_object_pair.second;
const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_pair.first);
const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_name);
if (!to_object) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object_pair.first);
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object_name);
return false;
}
#if MOVEIT_HAS_OBJECT_POSE
if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4)) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object_name);
return false; // transforms do not match
}
#endif
if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object_pair.first);
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object_name);
return false; // shapes not matching
}
@ -826,7 +837,7 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
to_it = to_object->shape_poses_.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_pair.first);
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different shape pose: " << from_object_name);
return false; // transforms do not match
}
}
@ -857,16 +868,24 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
<< to_object->getAttachedLinkName());
return false; // links not matching
}
if (from_object->getFixedTransforms().size() != to_object->getFixedTransforms().size()) {
if (from_object->getShapes().size() != to_object->getShapes().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 MOVEIT_HAS_OBJECT_POSE
auto from_it = from_object->getShapePosesInLinkFrame().cbegin();
auto from_end = from_object->getShapePosesInLinkFrame().cend();
auto to_it = to_object->getShapePosesInLinkFrame().cbegin();
#else
auto from_it = from_object->getFixedTransforms().cbegin();
auto from_end = from_object->getFixedTransforms().cend();
auto to_it = to_object->getFixedTransforms().cbegin();
#endif
for (; 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());
ROS_DEBUG_STREAM_NAMED("Connecting",
name() << ": different pose of attached object shape: " << from_object->getName());
return false; // transforms do not match
}
}

View File

@ -37,6 +37,7 @@
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/marker_tools.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
@ -295,7 +296,12 @@ void ComputeIK::compute() {
ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown frame: " << ik_pose_msg.header.frame_id);
return;
}
const EigenSTL::vector_Isometry3d& tf = attached->getFixedTransforms();
const EigenSTL::vector_Isometry3d& tf =
#if MOVEIT_HAS_OBJECT_POSE
attached->getShapePosesInLinkFrame();
#else
attached->getFixedTransforms();
#endif
if (tf.empty()) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Attached body doesn't have shapes.");
return;

View File

@ -37,7 +37,9 @@
#include <moveit/task_constructor/stages/generate_place_pose.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/marker_tools.h>
#include <rviz_marker_tools/marker_creation.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/attached_body.h>
@ -63,7 +65,7 @@ void GeneratePlacePose::onNewSolution(const SolutionBase& s) {
std::string msg;
if (!scene->getCurrentState().hasAttachedBody(object))
msg = "'" + object + "' is not an attached object";
if (scene->getCurrentState().getAttachedBody(object)->getFixedTransforms().empty())
if (scene->getCurrentState().getAttachedBody(object)->getShapes().empty())
msg = "'" + object + "' has no associated shapes";
if (!msg.empty()) {
if (storeFailures()) {