vrep教程（十一）通過rosInterface控制vrep中的機械臂

ros和vrep通信

ubantu 16/ros kinectic / vrep3.6.2
首先，打開一個終端，運行roscore
再打開一個終端，運行vrep

RosInterface加載成功

運行vrep自帶Demo

代碼

function sysCall_init()
        
    # 句柄
    robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)

    leftMotor=sim.getObjectHandle("rosInterfaceControlledBubbleRobLeftMotor") -- Handle of the left motor

    rightMotor=sim.getObjectHandle("rosInterfaceControlledBubbleRobRightMotor") -- Handle of the right motor

    noseSensor=sim.getObjectHandle("rosInterfaceControlledBubbleRobSensingNose") -- Handle of the proximity sensor

    -- Check if the required ROS plugin is there:
    # ros插件檢測

    moduleName=0

    moduleVersion=0

    index=0

    pluginNotFound=true

    while moduleName do

        moduleName,moduleVersion=sim.getModuleName(index)

        if (moduleName=='RosInterface') then

            pluginNotFound=false

        end

        index=index+1

    end

    -- Ok now launch the ROS client application:
    # 重要
    if (not pluginNotFound) then

        local sysTime=sim.getSystemTimeInMs(-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=simROS.advertise('/'..sensorTopicName,'std_msgs/Bool')

        simTimePub=simROS.advertise('/'..simulationTimeTopicName,'std_msgs/Float32')

        leftMotorSub=simROS.subscribe('/'..leftMotorTopicName,'std_msgs/Float32','setLeftMotorVelocity_cb')
        # 'setLeftMotorVelocity_cb'是回調函數名

        rightMotorSub=simROS.subscribe('/'..rightMotorTopicName,'std_msgs/Float32','setRightMotorVelocity_cb')

        -- Now we start the client application:

        result=sim.launchExecutable('rosBubbleRob2',leftMotorTopicName.." "..rightMotorTopicName.." "..sensorTopicName.." "..simulationTimeTopicName,0)

    else

        print("<font color='#F00'>ROS interface was not found. Cannot run.</font>@html")

    end

    
end
function setLeftMotorVelocity_cb(msg)

    -- Left motor speed subscriber callback
    # msg.data是訂閱的話題的消息

    sim.setJointTargetVelocity(leftMotor,msg.data)

end



function setRightMotorVelocity_cb(msg)

    -- Right motor speed subscriber callback

    sim.setJointTargetVelocity(rightMotor,msg.data)

end


# 一個物體相對於另一個物體的座標和四元數
function getTransformStamped(objHandle,name,relTo,relToName)

    t=sim.getSystemTime()

    p=sim.getObjectPosition(objHandle,relTo)

    o=sim.getObjectQuaternion(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



# 執行部分

function sysCall_actuation()

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

    if not pluginNotFound then

        local result=sim.readProximitySensor(noseSensor)

        local detectionTrigger={}

        detectionTrigger['data']=result>0
# 發佈消息到話題
        simROS.publish(sensorPub,detectionTrigger)

        simROS.publish(simTimePub,{data=sim.getSimulationTime()})

        -- Send the robot's transform:

        simROS.sendTransform(getTransformStamped(robotHandle,'rosInterfaceControlledBubbleRob',-1,'world'))

        -- To send several transforms at once, use simROS.sendTransforms instead

    end

end



function sysCall_cleanup()

    if not pluginNotFound then

        -- Following not really needed in a simulation script (i.e. automatically shut down at simulation end):
# shutdown 節點
        simROS.shutdownPublisher(sensorPub)

        simROS.shutdownSubscriber(leftMotorSub)

        simROS.shutdownSubscriber(rightMotorSub)

    end

end

simROS API

創建話題

往話題裏發佈數據

訂閱話題

sim.launchExecutable

對應的執行文件在這裏

leftmotor和rightmotor訂閱的話題並不是在這個script裏生成的，而是在rosBubbleRob2裏生成的

如果使用自己定義的消息類型，需要去rosInterface中修改

function sysCall_init()

        

    robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)

    armJoints={-1,-1,-1,-1,-1,-1}

    for i=1,7,1 do

        armJoints[i]=sim.getObjectHandle('j'..i)

    end

    



    moduleName=0

    moduleVersion=0

    index=0

    pluginNotFound=true

    while moduleName do

        moduleName,moduleVersion=sim.getModuleName(index)

        if (moduleName=='RosInterface') then

            pluginNotFound=false

        end

        index=index+1

    end

    

    if (not pluginNotFound) then

       

        local Joint1TopicName='JointPosition_Publisher'

        

        -- JointSub=simROS.subscribe('/'..JointTopicName,'beginner_tutorials/JointTargetPosition','setJointPosition')

#改成了std_msgs/Float32
       JointSub=simROS.subscribe('/'..Joint1TopicName,'std_msgs/Float32','setJointPosition')        

        

        -- result=sim.launchExecutable('rosBubbleRob2',leftMotorTopicName.." "..rightMotorTopicName.." "..sensorTopicName.." "..simulationTimeTopicName,0)

    else

        print("<font color='#F00'>ROS interface was not found. Cannot run.</font>@html")

    end

    

end

function setJointPosition(msg)    

    sim.setJointPosition(armJoints[1],msg.data)

end





function sysCall_actuation()

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

    if not pluginNotFound then

        

        -- simROS.publish(sensorPub,detectionTrigger)

       

    end

end



function sysCall_cleanup()

    if not pluginNotFound then

        simROS.shutdownSubscriber(JointSub)

    end

end

#!/usr/bin/env python



import rospy

import math

from std_msgs.msg import Float32



rospy.init_node('joint1targetpositionpublisher', anonymous=True)

pub=rospy.Publisher('Joint1Position_Publisher', Float32, queue_size=10)



rate = rospy.Rate(2)

count = 0



while not rospy.is_shutdown():



    msg = Float32()

    msg.data = count * math.pi/180

    rospy.loginfo(msg)

    pub.publish(msg)

    count = count + 1

    rate.sleep()