From f51f6eb982f90f3c31d9adfe1fc6e041d467c5bb Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 23 Aug 2021 00:28:34 +0200 Subject: [PATCH] address interface changes for object poses in MoveIt Also include a check for the new object pose field in `Connecting::compatible()`. --- core/src/stage.cpp | 37 +++++++++++++++++++------ core/src/stages/compute_ik.cpp | 8 +++++- core/src/stages/generate_place_pose.cpp | 4 ++- 3 files changed, 38 insertions(+), 11 deletions(-) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 3a3601bd..1d0f1d36 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -38,6 +38,8 @@ #include #include #include +#include + #include #include @@ -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 } } diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 3e5e1f14..4954bd5e 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -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; diff --git a/core/src/stages/generate_place_pose.cpp b/core/src/stages/generate_place_pose.cpp index 8c8cd84a..dc94e235 100644 --- a/core/src/stages/generate_place_pose.cpp +++ b/core/src/stages/generate_place_pose.cpp @@ -37,7 +37,9 @@ #include #include #include + #include + #include #include #include @@ -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()) {