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()