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/stage_p.h>
#include <moveit/task_constructor/container_p.h> #include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h> #include <moveit/task_constructor/introspection.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <ros/console.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 // both scenes should have the same set of collision objects, at the same location
for (const auto& from_object_pair : *from->getWorld()) { 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::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) { 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; 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()) { 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 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(); to_it = to_object->shape_poses_.cbegin();
from_it != from_end; ++from_it, ++to_it) from_it != from_end; ++from_it, ++to_it)
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) { 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 return false; // transforms do not match
} }
} }
@ -857,16 +868,24 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
<< to_object->getAttachedLinkName()); << to_object->getAttachedLinkName());
return false; // links not matching 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()); ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object->getName());
return false; // shapes not matching return false; // shapes not matching
} }
for (auto from_it = from_object->getFixedTransforms().cbegin(), #if MOVEIT_HAS_OBJECT_POSE
from_end = from_object->getFixedTransforms().cend(), to_it = to_object->getFixedTransforms().cbegin(); auto from_it = from_object->getShapePosesInLinkFrame().cbegin();
from_it != from_end; ++from_it, ++to_it) 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)) { 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 return false; // transforms do not match
} }
} }

View File

@ -37,6 +37,7 @@
#include <moveit/task_constructor/stages/compute_ik.h> #include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/storage.h> #include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/marker_tools.h> #include <moveit/task_constructor/marker_tools.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.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); ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown frame: " << ik_pose_msg.header.frame_id);
return; 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()) { if (tf.empty()) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Attached body doesn't have shapes."); ROS_WARN_STREAM_NAMED("ComputeIK", "Attached body doesn't have shapes.");
return; return;

View File

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