diff --git a/cob_generic_states/src/generic_manipulation_states.py b/cob_generic_states/src/generic_manipulation_states.py index bf20566..c47deb4 100644 --- a/cob_generic_states/src/generic_manipulation_states.py +++ b/cob_generic_states/src/generic_manipulation_states.py @@ -249,13 +249,16 @@ def execute(self, userdata): print "Service call failed: %s"%e self.retries = 0 return 'failed' + + #print userdata.object + #return 'failed' # transform object_pose to middle object_pose_in = userdata.object.pose - object_pose_in.header.stamp = self.listener.getLatestCommonTime("/base_link",object_pose_in.header.frame_id) - object_pose_in.pose.position.x += userdata.object.bounding_box.x/2.0 - object_pose_in.pose.position.y += userdata.object.bounding_box.y/2.0 - object_pose_in.pose.position.z += userdata.object.bounding_box.z/2.0 + #object_pose_in.header.stamp = self.listener.getLatestCommonTime("/base_link",object_pose_in.header.frame_id) + #object_pose_in.pose.position.x += userdata.object.bounding_box_lwh.x/2.0 + #object_pose_in.pose.position.y += userdata.object.bounding_box_lwh.y/2.0 + object_pose_in.pose.position.z += userdata.object.bounding_box_lwh.z/2.0 req = GetPoseStampedTransformedRequest()