CoppeliaSim:发布视觉传感器信息CameraInfo

http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html

---
--- Generated by EmmyLua(https://github.com/EmmyLua)
--- Created by chrisliu.
--- DateTime: 2020/2/28 ??9:01
---

function sysCall_init()

    -- Get some handles:
    activeVisionSensor=sim.getObjectHandle('Vision_sensor')

    -- Enable an image publisher and subscriber:
    if simROS then
		print("ROS interface was found.@html")

        clockpub=simROS.advertise('/clock','rosgraph_msgs/Clock')
        simROS.publisherTreatUInt8ArrayAsString(clockpub) -- treat uint8 arrays as strings (much faster, tables/arrays are kind of slow in Lua)
        
        pubCamera=simROS.advertise('/Camera', 'sensor_msgs/Image')
        simROS.publisherTreatUInt8ArrayAsString(pubCamera) -- treat uint8 arrays as strings (much faster, tables/arrays are kind of slow in Lua)
        
        pubCameraInfo=simROS.advertise('/Camera/camera_info', 'sensor_msgs/CameraInfo')
        simROS.publisherTreatUInt8ArrayAsString(pubCameraInfo)
    
    else
        print("ROS interface was not found. Cannot run.@html")
    end
end

function sysCall_sensing()
    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(pubCamera,d)

        view_angle = 40*math.pi/180
        --sim.visionfloatparam_perspective_angle (1004): float parameter : perspective projection angle
        viewing_angle_id = 1004
        simGetObjectFloatParameter(activeVisionSensor, viewing_angle_id, view_angle)
        f_x = (w/2)/math.tan(view_angle/2);
        f_y = f_x;
        CameraInfo={}
        CameraInfo['header']={seq=0,stamp=sim.getSystemTime(), frame_id="floorCamera_link"}
        CameraInfo['height']=h
        CameraInfo['width']=w
        CameraInfo['distortion_model']="plumb_bob"
        CameraInfo['D']={0, 0, 0, 0, 0}
        CameraInfo['K']={f_x, 0, w/2, 0, f_y, h/2, 0, 0, 1}
        CameraInfo['R']={1, 0, 0, 0, 1, 0, 0, 0, 1}
        CameraInfo['P']={f_x, 0, w/2, 0, 0, f_y, h/2, 0, 0, 0, 1, 0}
        CameraInfo['binning_x']= 1
        CameraInfo['binning_y']= 1
        CameraInfo['roi']= {x_offset=0, y_offset=0, width = 0, height = 0, do_rectify= false}
        simROS.publish(pubCameraInfo,CameraInfo)
        
    end
end

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