task2.py 文件源码

def find_plug(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.power_plug:
        point = PointStamped()
        point.header = macro.fc.cloud.header
        det = details[things.power_plug.position[1], things.power_plug.position[0]]
        point.point.x = det[0]
        point.point.y = det[1]
        point.point.z = det[2]
        plug = macro.fc.zarj.transform.tf_buffer.transform(point, macro.fc.zarj.walk.lfname)
        log("plug at: {}/{}/{}".format(plug.point.x, plug.point.y, plug.point.z))
        macro.fc.points[0] = [plug.point.x, plug.point.y, plug.point.z]
        macro.fc.points[1] = None

        return True

    return False