def detect_tabletop_objects(self):
DETECT_ERROR = 0.
cbbf = ClusterBoundingBoxFinder(tf_listener=self.gman.cm.tf_listener)
object_detector = rospy.ServiceProxy("/object_detection", TabletopSegmentation)
try:
detects = object_detector()
object_detector.close()
except ServiceException as se:
rospy.logerr("Tabletop segmentation crashed")
return []
if detects.result != 4:
rospy.logerr("Detection failed (err %d)" % (detects.result))
return []
table_z = detects.table.pose.pose.position.z
objects = []
for cluster in detects.clusters:
(object_points,
object_bounding_box_dims,
object_bounding_box,
object_to_base_link_frame,
object_to_cluster_frame) = cbbf.find_object_frame_and_bounding_box(cluster)
# rospy.loginfo("bounding box:", object_bounding_box)
(object_pos, object_quat) = cf.mat_to_pos_and_quat(object_to_cluster_frame)
angs = euler_from_quaternion(object_quat)
# position is half of height
object_pos[2] = table_z + object_bounding_box[1][2] / 2. + DETECT_ERROR
objects += [[object_pos, angs[2]]]
return objects
def get_transform(self, tool_frame, base_frame):
try:
self.tf_listener.waitForTransform(\
tool_frame, base_frame, rospy.Time(0), rospy.Duration(2))
(trans,rot) = self.tf_listener.lookupTransform(\
tool_frame, base_frame,rospy.Time())
mat = np.matrix(tf.transformations.quaternion_matrix(rot))
mat[0:3,3] = np.matrix(trans).T
new_pose = mat*self.offset
trans,rot = mat_to_pos_and_quat(new_pose)
return trans,rot
except:
return False, False
"""
def point_transformed( T, pos):
offset = blank_T
offset[0:3,3] = np.matrix(pos).T
new_pose = T * offset
trans, rot = mat_to_pos_and_quat(new_pose)
return trans,rot
