task2.py 文件源码

def find_button(macro):
    macro.fc.send_stereo_camera()
    image, details = macro.fc.zarj.eyes.get_cloud_image_with_details(macro.fc.cloud)
    things = Things(image, details, 2)
    if things.array_button is not None:
        point = PointStamped()
        point.header = macro.fc.cloud.header
        det = things.array_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))

        macro.fc.points[0] = [ button.point.x, button.point.y, button.point.z ]
        macro.fc.points[1] = None

        return True

    return False