V-rep学习笔记：ROSInterface

function subscriber_callback(msg)
    -- This is the subscriber callback function
    simAddStatusbarMessage(‘subscriber receiver following Float32: ‘..msg.data)
end

function getTransformStamped(objHandle,name,relTo,relToName)
    -- This function retrieves the stamped transform for a specific object
    t=simGetSystemTime()
    p=simGetObjectPosition(objHandle,relTo)
    o=simGetObjectQuaternion(objHandle,relTo)
    return {
        header={
            stamp=t,
            frame_id=relToName
        },
        child_frame_id=name,
        transform={
            translation={x=p[1],y=p[2],z=p[3]},
            rotation={x=o[1],y=o[2],z=o[3],w=o[4]}
        }
    }
end

if (sim_call_type==sim_childscriptcall_initialization) then
    -- The child script initialization
    objectHandle=simGetObjectAssociatedWithScript(sim_handle_self)
    objectName=simGetObjectName(objectHandle)
    -- Check if the required RosInterface is there:
    moduleName=0
    index=0
    rosInterfacePresent=false
    while moduleName do
        moduleName=simGetModuleName(index)
        if (moduleName==‘RosInterface‘) then
            rosInterfacePresent=true
        end
        index=index+1
    end

    -- Prepare the float32 publisher and subscriber (we subscribe to the topic we advertise):
    if rosInterfacePresent then
        publisher=simExtRosInterface_advertise(‘/simulationTime‘,‘std_msgs/Float32‘)
        subscriber=simExtRosInterface_subscribe(‘/simulationTime‘,‘std_msgs/Float32‘,‘subscriber_callback‘)
    end
end

if (sim_call_type==sim_childscriptcall_actuation) then
    -- Send an updated simulation time message, and send the transform of the object attached to this script:
    if rosInterfacePresent then
        simExtRosInterface_publish(publisher,{data=simGetSimulationTime()})
        simExtRosInterface_sendTransform(getTransformStamped(objectHandle,objectName,-1,‘world‘))
        -- To send several transforms at once, use simExtRosInterface_sendTransforms instead
    end
end

if (sim_call_type==sim_childscriptcall_cleanup) then
    -- Following not really needed in a simulation script (i.e. automatically shut down at simulation end):
    if rosInterfacePresent then
        simExtRosInterface_shutdownPublisher(publisher)
        simExtRosInterface_shutdownSubscriber(subscriber)
    end
end

-- This illustrates how to publish and subscribe to an image using the ROS Interface.
-- An alternate version using image transport can be created with following functions:
--
-- simExtRosInterface_imageTransportAdvertise
-- simExtRosInterface_imageTransportPublish
-- simExtRosInterface_imageTransportShutdownPublisher
-- simExtRosInterface_imageTransportShutdownSubscriber
-- simExtRosInterface_imageTransportSubscribe

function imageMessage_callback(msg)
    -- Apply the received image to the passive vision sensor that acts as an image container
    simSetVisionSensorCharImage(passiveVisionSensor,msg.data)
end

if (sim_call_type==sim_childscriptcall_initialization) then
    -- Get some handles:
    activeVisionSensor=simGetObjectHandle(‘Vision_sensor‘)
    passiveVisionSensor=simGetObjectHandle(‘PassiveVision_sensor‘)

    -- Enable an image publisher and subscriber:
    pub=simExtRosInterface_advertise(‘/image‘, ‘sensor_msgs/Image‘)
    --After calling this function, this publisher will treat uint8 arrays as string. Using strings should be in general much faster that using int arrays in Lua.
    simExtRosInterface_publisherTreatUInt8ArrayAsString(pub) -- treat uint8 arrays as strings (much faster, tables/arrays are kind of slow in Lua)

    sub=simExtRosInterface_subscribe(‘/image‘, ‘sensor_msgs/Image‘, ‘imageMessage_callback‘)
    simExtRosInterface_subscriberTreatUInt8ArrayAsString(sub) -- treat uint8 arrays as strings (much faster, tables/arrays are kind of slow in Lua)
end

if (sim_call_type==sim_childscriptcall_sensing) then
    -- Publish the image of the active vision sensor:
    local data,w,h=simGetVisionSensorCharImage(activeVisionSensor)
    d={}
    d[‘header‘]={seq=0,stamp=simExtRosInterface_getTime(), frame_id="a"}
    d[‘height‘]=h
    d[‘width‘]=w
    d[‘encoding‘]=‘rgb8‘
    d[‘is_bigendian‘]=1
    d[‘step‘]=w*3
    d[‘data‘]=data
    simExtRosInterface_publish(pub,d)
end

if (sim_call_type==sim_childscriptcall_cleanup) then
    -- Shut down publisher and subscriber. Not really needed from a simulation script (automatic shutdown)
    simExtRosInterface_shutdownPublisher(pub)
    simExtRosInterface_shutdownSubscriber(sub)
end

function setLeftMotorVelocity_cb(msg)

    -- Left motor speed subscriber callback

    simSetJointTargetVelocity(leftMotor,msg.data)

end



function setRightMotorVelocity_cb(msg)

    -- Right motor speed subscriber callback

    simSetJointTargetVelocity(rightMotor,msg.data)

end



function getTransformStamped(objHandle,name,relTo,relToName)

    t=simGetSystemTime()

    p=simGetObjectPosition(objHandle,relTo)

    o=simGetObjectQuaternion(objHandle,relTo)

    return {

        header={

            stamp=t,

            frame_id=relToName

        },

        child_frame_id=name,

        transform={

            translation={x=p[1],y=p[2],z=p[3]},

            rotation={x=o[1],y=o[2],z=o[3],w=o[4]}

        }

    }

end



if (sim_call_type==sim_childscriptcall_initialization) then

    robotHandle=simGetObjectAssociatedWithScript(sim_handle_self)

    leftMotor=simGetObjectHandle("rosInterfaceControlledBubbleRobLeftMotor") -- Handle of the left motor

    rightMotor=simGetObjectHandle("rosInterfaceControlledBubbleRobRightMotor") -- Handle of the right motor

    noseSensor=simGetObjectHandle("rosInterfaceControlledBubbleRobSensingNose") -- Handle of the proximity sensor



    -- Check if the required ROS plugin is there:

    moduleName=0

    moduleVersion=0

    index=0

    pluginNotFound=true

    while moduleName do

        moduleName,moduleVersion=simGetModuleName(index)

        if (moduleName==‘RosInterface‘) then

            pluginNotFound=false

        end

        index=index+1

    end



    -- Add a banner:

    if (pluginNotFound) then

        bannerText="I cannot run! (I couldn‘t find my RosInterface plugin)"

    else

        bannerText="I am controlled via a ROS node and the RosInterface! (‘rosBubbleRob2‘ controlls me)"

    end

    black={0,0,0,0,0,0,0,0,0,0,0,0}

    red={0,0,0,0,0,0,0,0,0,1,0.2,0.2}

    simAddBanner(bannerText,0,sim_banner_bitmapfont+sim_banner_overlay,nil,simGetObjectAssociatedWithScript(sim_handle_self),black,red)



    -- Ok now launch the ROS client application:

    if (not pluginNotFound) then

        local sysTime=simGetSystemTimeInMs(-1) 

        local leftMotorTopicName=‘leftMotorSpeed‘..sysTime -- we add a random component so that we can have several instances of this robot running

        local rightMotorTopicName=‘rightMotorSpeed‘..sysTime -- we add a random component so that we can have several instances of this robot running

        local sensorTopicName=‘sensorTrigger‘..sysTime -- we add a random component so that we can have several instances of this robot running

        local simulationTimeTopicName=‘simTime‘..sysTime -- we add a random component so that we can have several instances of this robot running



        -- Prepare the sensor publisher and the motor speed subscribers:

        sensorPub=simExtRosInterface_advertise(‘/‘..sensorTopicName,‘std_msgs/Bool‘)

        simTimePub=simExtRosInterface_advertise(‘/‘..simulationTimeTopicName,‘std_msgs/Float32‘)

        leftMotorSub=simExtRosInterface_subscribe(‘/‘..leftMotorTopicName,‘std_msgs/Float32‘,‘setLeftMotorVelocity_cb‘)

        rightMotorSub=simExtRosInterface_subscribe(‘/‘..rightMotorTopicName,‘std_msgs/Float32‘,‘setRightMotorVelocity_cb‘)



        -- Now we start the client application:

        result=simLaunchExecutable(‘rosBubbleRob2‘,leftMotorTopicName.." "..rightMotorTopicName.." "..sensorTopicName.." "..simulationTimeTopicName,0)

    end

end



if (sim_call_type==sim_childscriptcall_actuation) then

    -- Send an updated sensor and simulation time message, and send the transform of the robot:

    if not pluginNotFound then

        local result=simReadProximitySensor(noseSensor)

        local detectionTrigger={}

        detectionTrigger[‘data‘]=result>0

        simExtRosInterface_publish(sensorPub,detectionTrigger)

        simExtRosInterface_publish(simTimePub,{data=simGetSimulationTime()})

        -- Send the robot‘s transform:

        simExtRosInterface_sendTransform(getTransformStamped(robotHandle,‘rosInterfaceControlledBubbleRob‘,-1,‘world‘))

        -- To send several transforms at once, use simExtRosInterface_sendTransforms instead

    end

end



if (sim_call_type==sim_childscriptcall_cleanup) then

    if not pluginNotFound then

        -- Following not really needed in a simulation script (i.e. automatically shut down at simulation end):

        simExtRosInterface_shutdownPublisher(sensorPub)

        simExtRosInterface_shutdownSubscriber(leftMotorSub)

        simExtRosInterface_shutdownSubscriber(rightMotorSub)

    end

end