mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
fixup! add name to Connecting DEBUG output
This is important to differentiate between different Connect stages.
This commit is contained in:
parent
70e32cb0b3
commit
7fcef54a0e
@ -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
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user