这篇教程简要说明,在CoppeliaSim仿真环境中,使用ROS2接口,如果是ROS 2 Dashing,直接使用安装包中的compiledRosPlugins文件夹下的libsimExtROS2Interface.so。 如果使用其他版本的ROS 2,需要手动编译生成.so文件,否则会出错。当然也支持ROS 1参考之前博文。 重新编译ROS 2之CoppeliaSim插件的过程如下所示: 下载simExtROS2Interface,这个功能包在编译成功后会生成.so文件。更多案例参考示例和controlTypeExamples下文件。 所有文件需要复制到如ros2_ws/src的文件夹下,然后依次输入下面指令:
$ export COPPELIASIM_ROOT_DIR=~/path/to/coppeliaSim/folder
$ ulimit -s unlimited #otherwise compilation might freeze/crash
$ colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DLIBPLUGIN_DIR=$COPPELIASIM_ROOT_DIR/programming/libPlugin
OK,如上是重新编译的全部过程。 启动CoppeliaSim的指令如下:
$ ./coppeliaSim.sh
会发现如下接口启动,说明ROS 2接口一切正常。 无需担心warning…… 开启一个案例玩耍起来(ros2InterfaceTopicPublisherAndSubscriber.ttt): 使用终端查看一下消息: 添加一个仿真时间: 选择sphere,添加子代码如下:
function subscriber_callback(msg)
-- This is the subscriber callback function
sim.addStatusbarMessage('subscriber receiver following Float32: '..msg.data)
end
function getTransformStamped(objHandle,name,relTo,relToName)
-- This function retrieves the stamped transform for a specific object
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_init()
-- The child script initialization
objectHandle=sim.getObjectAssociatedWithScript(sim.handle_self)
objectName=sim.getObjectName(objectHandle)
ros2InterfacePresent=simROS2
-- Prepare the float32 publisher and subscriber (we subscribe to the topic we advertise):
if ros2InterfacePresent then
publisher=simROS2.advertise('/simulationTime','std_msgs/Float32')
subscriber=simROS2.subscribe('/simulationTime','std_msgs/Float32','subscriber_callback')
end
end
function sysCall_actuation()
-- Send an updated simulation time message, and send the transform of the object attached to this script:
if ros2InterfacePresent then
simROS2.publish(publisher,{data=sim.getSimulationTime()})
simROS2.sendTransform(getTransformStamped(objectHandle,objectName,-1,'world'))
-- To send several transforms at once, use simROS2.sendTransforms instead
end
end
function sysCall_cleanup()
-- Following not really needed in a simulation script (i.e. automatically shut down at simulation end):
if rosInterfacePresent then
simROS.shutdownPublisher(publisher)
simROS.shutdownSubscriber(subscriber)
end
end
这时,可以使用:
$ ros2 topic echo /simulationTime
查看开启仿真的时间。 换一个案例(controlledViaRos2): 更多介绍,查看官方文档。使用版本为CoppeliaSim_Edu_V4_0_0_Ubuntu18_04。