task2.py 文件源码

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

        if things.choke_outer:
            point = PointStamped()
            point.header = macro.fc.cloud.header
            det = details[things.choke_outer.position[1], things.choke_outer.position[0]]
            point.point.x = det[0]
            point.point.y = det[1]
            point.point.z = det[2]
            outer = macro.fc.zarj.transform.tf_buffer.transform(point, macro.fc.zarj.walk.lfname)
            log("outer choke found at: {}/{}/{}".format(outer.point.x, outer.point.y, outer.point.z))

            macro.fc.points[0] = [outer.point.x, outer.point.y, outer.point.z]
            macro.fc.points[1] = [inner.point.x, inner.point.y, inner.point.z]

            return True

    return False