task3.py 文件源码

def find_repair_button(macro):
    image, details = macro.fc.zarj.eyes.get_cloud_image_with_details(macro.fc.cloud)
    things = Things(image, details, 3)
    if things.repair_button is not None and things.repair_button.computed_center is not None:
        point = PointStamped()
        point.header = macro.fc.cloud.header
        det = things.repair_button.computed_center
        point.point.x = det[0]
        point.point.y = det[1]
        point.point.z = det[2]
        button = macro.fc.zarj.transform.tf_buffer.transform(point, macro.fc.zarj.walk.lfname)
        log("button found at: {}/{}/{}".format(button.point.x, button.point.y, button.point.z))

        return button.point

    return None