CoppeliaSim视觉传感器颜色识别与物体位置检测

function sysCall_init()
    -- 初始化函数，在脚本加载时调用
    camera = sim.getObjectAssociatedWithScript(sim.handle_self)
    sensor = sim.getObjectHandle('conveyorBelt_sensor#0')
    view = sim.floatingViewAdd(0.9, 0.55, 0.2, 0.1, 0)
    sim.adjustView(view, camera, 64)
    res = sim.getVisionSensorResolution(camera)
    cam_pos = sim.getObjectPosition(camera, -1)
    cam_x = 1 -- 视角宽度
    cam_y = 0.25
    ProximitySensor = 0
    ProximitySensorprevious = 0
    tick = 0
end


function sysCall_actuation()
    -- 执行函数，在每个仿真步骤中调用，本脚本中未使用
end


function sysCall_vision(inData)
    -- 视觉处理函数，在每个仿真步骤中调用
    local data = {}
    ProximitySensorprevious = ProximitySensor
    ProximitySensor = sim.readProximitySensor(sensor)
    local handler = inData.handle
    if (ProximitySensorprevious <= 0) and (ProximitySensor > 0) then
        -- 当传感器检测到物体时
        local packet = {}
        local data = {}
        simVision.sensorImgToWorkImg(handler)  -- 传感器图像转换到工作图像
        -- 颜色选择
        simVision.selectiveColorOnWorkImg(inData.handle, {1.00, 0.29, 0.21}, {0.15, 0.15, 0.15}, true, false, true)
        simVision.selectiveColorOnWorkImg(inData.handle, {0.28, 1, 0.21}, {0.15, 0.15, 0.15}, true, true, false)
        simVision.addBuffer1ToWorkImg(inData.handle) -- 将缓存1添加到工作图像
        -- 边缘检测
        local _, ans = simVision.blobDetectionOnWorkImg(handler, 0.1, 0, false)
        packet = sim.unpackFloatTable(ans)
        local blobCount = packet[1]  -- 检测到的物体数量
        local dataSizePerBlob = packet[2]  -- 每个物体的数据大小
        -- 提取物体信息
        for i = 1, blobCount, 1 do
            local blobRelativeSize = packet[2 + (i - 1) * dataSizePerBlob + 1]
            local blobOrientation = packet[2 + (i - 1) * dataSizePerBlob + 2]
            local blobRelativePos = {packet[2 + (i - 1) * dataSizePerBlob + 3], packet[2 + (i - 1) * dataSizePerBlob + 4]}
            local realposX = cam_pos[1] + cam_x / 2 - cam_x * packet[2 + (i - 1) * dataSizePerBlob + 3] - 0.012
            local realposY = cam_pos[2] + cam_y / 2 - cam_y * packet[2 + (i - 1) * dataSizePerBlob + 4]
            local blobBoxRelativeDimensions = {packet[2 + (i - 1) * dataSizePerBlob + 5], packet[2 + (i - 1) * dataSizePerBlob + 6]}
            local blobCol = sim.getVisionSensorImage(camera, res[1] * blobRelativePos[1], res[2] * blobRelativePos[2], 1, 1, 0)
            local colorCode = 'unknown'
            if (blobCol[1] > blobCol[2]) and (blobCol[1] > blobCol[3]) then
                colorCode = 'red'
            end
            if (blobCol[2] > blobCol[1]) and (blobCol[2] > blobCol[3]) then
                colorCode = 'green'
            end
            data[i] = {realposX, realposY, colorCode} -- 存储物体位置和颜色信息
        end
        -- 对物体位置进行排序
        for i = 1, #data - 1, 1 do
            for j = 1, #data - i, 1 do
                if (data[j][1] > data[j + 1][1]) then
                    local relay = {}
                    relay = data[j]
                    data[j] = data[j + 1]
                    data[j + 1] = relay
                end
            end
        end
        -- 将数据传递给其他脚本
        sim.writeCustomDataBlock(handler, 'data3', sim.packTable(data))
        sim.setIntegerSignal('getdata', 1)
        simVision.workImgToSensorImg(handler) -- 将工作图像转换回传感器图像
    end
    -- 处理完成信号
    if (sim.getIntegerSignal('finishDone') == 1) then
        data = {}
        sim.writeCustomDataBlock(handler, 'data3', sim.packTable(data))
        sim.clearIntegerSignal('finishDone')
    end
end