Apriltag:在Coppeliasim中无法通过视觉传感器/摄像头识别tag

原因在于Coppeliasim输出的图像时关于y轴镜像的,所以导致Apriltag无法识别
可以在视觉传感器的脚本使用simVision插件进行镜像

function sysCall_init()

    -- Get some handles:
    activeVisionSensor=sim.getObjectHandle('Vision_sensor')
    visionSensor=sim.getObjectAssociatedWithScript(sim.handle_self)
    -- Enable an image publisher and subscriber:
    if simROS then
    
        print("ROS interface was found.@html")
        pub=simROS.advertise('/floorCamera', 'sensor_msgs/Image')
        simROS.publisherTreatUInt8ArrayAsString(pub) -- treat uint8 arrays as strings (much faster, tables/arrays are kind of slow in Lua)
        
    else
        print("ROS interface was not found. Cannot run.@html")
    end
end

function sysCall_sensing()
	--触发视觉处理回调函数
    local trigger,packet1,packet2=sim.handleVisionSensor(visionSensor)

    if simROS then
        --set /use_sim_time to true
        simROS.publish(clockpub,{
     clock=sim.getSystemTime()})
        -- Publish the image of the active vision sensor:
        local data,w,h=sim.getVisionSensorCharImage(activeVisionSensor)
        d={
     }
        d['header']={
     stamp=sim.getSystemTime(), frame_id="floorCamera_link"}
        d['height']=h
        d['width']=w
        d['encoding']='rgb8'
        d['is_bigendian']=1
        d['step']=w*3
        d['data']=data
        simROS.publish(pub,d)       
    end
end

function sysCall_cleanup()
    if simROS then
        -- Shut down publisher and subscriber. Not really needed from a simulation script (automatic shutdown)
        simROS.shutdownPublisher(pub)
    end
end

function sysCall_vision(inData)

    simVision.sensorImgToWorkImg(inData.handle)
    simVision.horizontalFlipWorkImg(inData.handle)
    simVision.workImgToSensorImg(inData.handle)

end


你可能感兴趣的:(Apriltag,CoppeliaSim)