def table_callback(self, msg):
try:
table_array = TableArray()
table_array.header.frame_id = "world"
highest_table = None
for table in msg.tables:
new_table = Table()
new_table.header.frame_id = "world"
table_pose = PoseStamped(pose=table.pose, header=table.header)
table_pose.header.stamp = self.transformer.getLatestCommonTime(
"world", table.header.frame_id)
new_table.pose = self.transformer.transformPose(
"world", table_pose).pose
new_table.convex_hull = table.convex_hull
table_array.tables.append(new_table)
if highest_table is None or new_table.pose.position.z > highest_table.pose.position.z:
highest_table = new_table
self.publisher.publish(table_array)
position = highest_table.pose.position
#position.z -= 0.1
MoveHelper.add_table(position=position)
# rospy.sleep(10)
self.is_finished = True
except:
self.is_finished = False
