fixup! add name to Connecting DEBUG output

This is important to differentiate between different Connect stages.
This commit is contained in:
v4hn 2018-10-22 17:28:52 +02:00
parent 70e32cb0b3
commit 7fcef54a0e

View File

@ -696,7 +696,7 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
const planning_scene::PlanningSceneConstPtr& to = to_state.scene();
if (from->getWorld()->size() != to->getWorld()->size()) {
ROS_DEBUG_NAMED("Connecting", "different number of collision objects");
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different number of collision objects");
return false;
}
@ -705,18 +705,18 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
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);
if (!to_object) {
ROS_DEBUG_STREAM_NAMED("Connecting", "object missing: " << from_object_pair.first);
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object_pair.first);
return false;
}
if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", "different object shapes: " << from_object_pair.first);
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object_pair.first);
return false; // shapes not matching
}
for (auto from_it = from_object->shape_poses_.cbegin(), from_end = from_object->shape_poses_.cend(),
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", "different object shape transforms: " << from_object_pair.first);
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object_pair.first);
return false; // transforms do not match
}
}