ROS InterfacesThere is a ROS interface and a ROS2 interface available for CoppeliaSim, both duplicate the C++ ROS/ROS2 API with good fidelity. They are implemented via the simROS plugin and simROS2 plugin. Use the catkin tools to build those packages, otherwise you might run into difficulties. As an example, a vision sensor ROS2 publisher could look like: #python
def sysCall_init():
sim = require('sim')
simROS = require('simROS')
self.visionSensor = sim.getObject('/Vision_sensor')
# Enable an image publisher:
self.pub = simROS2.createPublisher('/image', 'sensor_msgs/msg/Image')
simROS2.publisherTreatUInt8ArrayAsString(self.pub)
def sysCall_sensing():
# Publish the image of the vision sensor:
img, resolution = sim.getVisionSensorImg(self.visionSensor)
data = {}
data['header'] = {'stamp': simROS2.getTime(), 'frame_id': 'a'}
data['height'] = resolution[1]
data['width'] = resolution[0]
data['encoding'] = 'rgb8'
data['is_bigendian'] = 1
data['step'] = resolution[0] * 3
data['data'] = img
simROS2.publish(self.pub, data)
def sysCall_cleanup():
simROS2.shutdownPublisher(self.pub)
--lua
function sysCall_init()
sim = require('sim')
simROS = require('simROS')
visionSensor = sim.getObject('/Vision_sensor')
-- Enable an image publisher:
pub = simROS2.createPublisher('/image', 'sensor_msgs/msg/Image')
simROS2.publisherTreatUInt8ArrayAsString(pub)
end
function sysCall_sensing()
-- Publish the image of the vision sensor:
local img, resolution = sim.getVisionSensorImg(visionSensor)
data = {}
data.header = {stamp = simROS2.getTime(), frame_id = 'a'}
data.height = resolution[2]
data.width = resolution[1]
data.encoding = 'rgb8'
data.is_bigendian = 1
data.step = resolution[1] * 3
data.data = img
simROS2.publish(pub, data)
end
function sysCall_cleanup()
simROS2.shutdownPublisher(pub)
end
The subscriber on the other hand could look like: #python
def sysCall_init():
sim = require('sim')
simROS = require('simROS')
# Enable an image subscriber:
self.sub = simROS2.createSubscription('/image', 'sensor_msgs/msg/Image', image_callback)
simROS2.subscriptionTreatUInt8ArrayAsString(self.sub)
def image_callback(msg):
# Here we have received an image
pass
def sysCall_cleanup():
simROS2.shutdownSubscription(self.sub)
--lua
function sysCall_init()
sim = require('sim')
simROS = require('simROS')
-- Enable an image subscriber:
sub = simROS2.createSubscription('/image', 'sensor_msgs/msg/Image', 'image_callback')
simROS2.subscriptionTreatUInt8ArrayAsString(sub)
end
function image_callback(msg)
-- Here we have received an image
end
function sysCall_cleanup()
simROS2.shutdownSubscription(sub)
end
Also have a look at the ROS tutorial and the external controller tutorial. |