Custom ROS2 service to get Image

[juhel]

Hi,

With the help of responses from this topic (see https://forum.coppeliarobotics.com/view ... hp?t=10791), I can now create a AddFourInts.srv service message and use it in a Coppelia python threaded script.

Code: Select all

def sysCall_init():
    sim = require('sim')
    simROS2 = require('simROS2')
    global srv
    sim.addLog(sim.verbosity_msgs, 'Thread started')
    # Create the ROS2 service
    srv = simROS2.createService('/add_four_ints', 'tutorial_interfaces/srv/AddFourInts', srv_callback)

def sysCall_thread():
    while not sim.getSimulationStopping():
        # Keep the thread alive
        sim.wait(0.01)  # Wait for 10ms before the next iteration

def srv_callback(req):
    sim.addLog(sim.verbosity_msgs, f'Service called: {req}')
    # Create the response
    resp = simROS2.createInterface('tutorial_interfaces/srv/AddFourIntsResponse')
    resp['sum'] = req['a'] + req['b'] + req['c'] + req['d']
    return resp

def sysCall_cleanup():
    # Shutdown the ROS2 service
    simROS2.shutdownService(srv)
    sim.addLog(sim.verbosity_msgs, 'Thread finished')

and from a terminal, I can use this service :

Code: Select all

ros2 service call /add_four_ints tutorial_interfaces/srv/AddFourInts "{a: 2, b: 3, c: 4, d: 5}"
requester: making request: tutorial_interfaces.srv.AddFourInts_Request(a=2, b=3, c=4, d=5)

response:
tutorial_interfaces.srv.AddFourInts_Response(sum=14)

OK, so now I want to create another service to get an image from a vision sensor.
So,I've added, in the tutorial_interfaces/src folder, the file MyImage.srv :

Code: Select all

---
sensor_msgs/Image image

In the CMakefile.txt, I've added this line :

Code: Select all

find_package(sensor_msgs REQUIRED)

and in the package.xml file, I've added :

Code: Select all

<depend>sensor_msgs</depend>

After compilation, I can see that this new interface is OK :

Code: Select all

ros2 interface show tutorial_interfaces/srv/MyImage 
---
sensor_msgs/Image image
	std_msgs/Header header #
		builtin_interfaces/Time stamp
			int32 sec
			uint32 nanosec
		string frame_id
	                             # Header frame_id should be optical frame of camera
	                             # origin of frame should be optical center of cameara
	                             # +x should point to the right in the image
	                             # +y should point down in the image
	                             # +z should point into to plane of the image
	                             # If the frame_id here and the frame_id of the CameraInfo
	                             # message associated with the image conflict
	                             # the behavior is undefined
	uint32 height                #
	uint32 width                 #
	string encoding       #
	                      # taken from the list of strings in include/sensor_msgs/image_encodings.hpp
	uint8 is_bigendian    #
	uint32 step           #
	uint8[] data          #

Now, I modified the sim_ros_interface/meta/interfaces.txt file to add this line :

Code: Select all

tutorial_interfaces/srv/MyImage

After compiling this package, I've got a new libsimROS2.so in the Coppelia folder.

Now, I've created a new threaded python script with a service called

Code: Select all

/vision_sensor

to get the image from visionSensor and send it as reply.

Code: Select all

# Ensure that this script is added as a threaded child script in CoppeliaSim
# Set the script's interpreter to Python
from sensor_msgs.msg import Image

def sysCall_init():
    sim = require('sim')
    simROS2 = require('simROS2')
    self.visionSensor = sim.getObject('/visionSensor')
    global srv
    sim.addLog(sim.verbosity_msgs, 'Thread started')
    # Create the ROS2 service
    srv = simROS2.createService('/vision_sensor', 'tutorial_interfaces/srv/MyImage', srv_callback)

def sysCall_thread():
    while not sim.getSimulationStopping():
        # Keep the thread alive
        sim.wait(0.01)  # Wait for 10ms before the next iteration

def srv_callback(req):
    sim.addLog(sim.verbosity_msgs, f'Service called: {req}')
    # Create the response
    resp = simROS2.createInterface('tutorial_interfaces/srv/MyImageResponse')
    print('==================')
    data, resolution = sim.getVisionSensorImg(self.visionSensor)
    resp['image'] = Image()
    #resp['image'].header.stamp = ??
    resp['image'].height = resolution[1]
    resp['image'].width = resolution[0]
    resp['image'].encoding = 'rgb8'
    resp['image'].is_bigendian = 1
    resp['image'].step = resolution[0] * 3
    #resp['image'].data = data
    print('image generated')
    return resp

def sysCall_cleanup():
    # Shutdown the ROS2 service
    simROS2.shutdownService(srv)
    sim.addLog(sim.verbosity_msgs, 'Thread finished')

To test this service, in a terminal, I type :

Code: Select all

ros2 service call /vision_sensor tutorial_interfaces/srv/MyImage 

and it print this (in the terminal) :

Code: Select all

requester: making request: tutorial_interfaces.srv.MyImage_Request()

response:
tutorial_interfaces.srv.MyImage_Response(image=sensor_msgs.msg.Image(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), height=0, width=0, encoding='', is_bigendian=0, step=0, data=[]))

===> all data are empty (or = 0)

but on the Coppelia console, I can see :

Code: Select all

[/scriptCamera:info] Thread started
[/scriptCamera:info] Service called: {}
==================
image generated
[/scriptCamera:warning] Received non-serializable data: {'image': sensor_msgs.msg.Image(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), height=256, width=256, encoding='rgb8', is_bigendian=1, step=768, data=[])}

===> data are not empty (i.e height = 256).

Why?
Do you have any example of how to use service with custom message with Coppelia?


Philippe

[fferri]

Does it happen with a Lua script as well?

[juhel]

Hi,

I don't know the LUA language but I try to translate the python script in LUA.

Here's the LUA script :

Code: Select all

simROS2 = require('simROS2')
sim=require'sim'

function sysCall_init()
    visionSensor=sim.getObject('/visionSensor')
    print(visionSensor)
    corout=coroutine.create(coroutineMain)
end

function sysCall_actuation()
    if coroutine.status(corout)~='dead' then
        local ok,errorMsg=coroutine.resume(corout)
        if errorMsg then
            error(debug.traceback(corout,errorMsg),2)
        end
    end
end

function srv_callback(req)
    sim.addLog(sim.verbosity_msgs,'service called: '..table.tostring(req))
    local resp=simROS2.createInterface('tutorial_interfaces/srv/MyImageResponse')
    
    local data,resolution=sim.getVisionSensorImg(visionSensor)
    print(resolution)
    print('width = '..resolution[1])
    print('height = '..resolution[2])
    print(data)
    
    d={}
    d.header={stamp=simROS2.getTime(), frame_id='a'}
    d.height=resolution[2]
    d.width=resolution[1]
    d.encoding='rgb8'
    d.is_bigendian=1
    d.step=resolution[1]*3
    d.data=data
    resp.image = d
    return resp
end

function coroutineMain()
    require 'utils' -- for table.tostring
    sim.addLog(sim.verbosity_msgs,'thread started')
    srv=simROS2.createService('/vision_sensor','tutorial_interfaces/srv/MyImage',srv_callback)
    while true do
        sim.switchThread()
    end
end

function sysCall_cleanup()
    simROS2.shutdownService(srv)
    sim.addLog(sim.verbosity_msgs,'thread finished')
end

It seems to work (service called with no error) but the result is strange.
In the terminal where I've called the service, no data and only the width is filled, not the height.

Code: Select all

ros2 service call /vision_sensor tutorial_interfaces/srv/MyImage 
requester: making request: tutorial_interfaces.srv.MyImage_Request()

response:
tutorial_interfaces.srv.MyImage_Response(image=sensor_msgs.msg.Image(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), height=0, width=256, encoding='', is_bigendian=0, step=0, data=[]))

But in the Coppelia console, the data seems to have some value ([buffer (196608 bytes)]) and the width AND the height have a value of 256.

Code: Select all

[/scriptCameraLUA:info] service called: {}
{256, 256}
width = 256
height = 256
[buffer (196608 bytes)]

Philippe

[juhel]

Hi,

Using ROS2 service with Coppelia is a bit complicated (edition of interfaces.txt, compilation of libsimROS2.so, ...).
Once the get image service is up and running, I propose to write a tutorial about how to write a ROS2 service for COppelia.
Is there any recommendations for this (format : HTML or PDF, can you (Coppelia) host it or should I have to host it myself, ...)

Philippe

[coppelia]

Hello Philippe,

you always have 2 ways of using ROS / ROS2 in CoppeliaSim:

• either you use the simROS /simROS2 plugins and functions from a Lua script. See the example in scenes/messaging/ros2InterfaceTopicPublisherAndSubscriber-lua.ttt
• either you use rclpy (the Python client library for ROS 2) from a Python script. See the example in scenes/messaging/ros2InterfaceTopicPublisherAndSubscriber-python.ttt

But if you are willing to write a tutorial that would be great, we would probably rework it slightly to include it into the CoppeliaSim user manual. HTML would probably be the best!

Cheers

[juhel]

OK, perfect.

The only problem is that my scripts don't work at the moment.
Most of the information (data, with, height, ...) are empty or equal to 0 so it means that my scripts (python and LUA) have errors. But I can't see what is wrong.
The LUA script is an 'improvisation', I don't know this language. If you can take a look at it because I don't want to give wrong information in a tutorial (a bad formed LUA script).
HEre's the scene with LUA script : https://drive.google.com/file/d/1G-6gRD ... sp=sharing
Here's the scene with the python script : https://drive.google.com/file/d/1l9iiEA ... sp=sharing

Philippe

[juhel]

Hi,

Today, I manage to solve my problem of writing a ROS2 service wich use a custom image message (to ask the service and receive an Image from a vision sensor).

I've written a tutorial about ROS2 services and Coppelia. Here's the link https://docs.google.com/document/d/11fR ... sp=sharing. Don't hesitate to use it and send me any improvements.

Just I explain at the end of this document, I've written 2 python scripts.
This one :

Code: Select all

# Ensure that this script is added as a threaded child script in CoppeliaSim
# Set the script's interpreter to Python
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from tutorial_interfaces.srv import MyImage
import numpy as np


class ImageService(Node):
    def __init__(self):
        super().__init__('image_service')
        self.visionSensor = sim.getObject('/visionSensor')
        # Create a ROS2 service 
        self.srv = self.create_service(MyImage, 'vision_sensor', self.image_callback)
        
    def image_callback(self, request, response):
        data, resolution = sim.getVisionSensorImg(self.visionSensor)
        response.image.width = resolution[0]
        response.image.height = resolution[1]
        response.image.encoding = 'rgb8'
        response.image.is_bigendian = 1
        response.image.step = resolution[0] * 3
        response.image.data = data
        return response

def sysCall_init():
    sim = require('sim')
    simROS2 = require('simROS2')
    rclpy.init(args=None)
    sim.addLog(sim.verbosity_scriptinfos, "init ...")
    sim.setStepping(True) # enabling stepping mode
    
def sysCall_thread():
    service = ImageService()
    while not sim.getSimulationStopping():
        sim.addLog(sim.verbosity_scriptinfos, "thread ...")
        rclpy.spin_once(service)
        #sim.wait(0.01)  # Wait for 10ms before the next iteration
        sim.step()

def sysCall_cleanup():
    # Shutdown the ROS2 service
    simROS2.shutdownService(srv)
    sim.addLog(sim.verbosity_msgs, 'Thread finished')

which works, and this one :

Code: Select all

# Ensure that this script is added as a threaded child script in CoppeliaSim
# Set the script's interpreter to Python
from sensor_msgs.msg import Image
import numpy as np

def sysCall_init():
    sim = require('sim')
    simROS2 = require('simROS2')
    self.visionSensor = sim.getObject('/visionSensor')
    global srv
    sim.addLog(sim.verbosity_msgs, 'Thread started')
    # Create the ROS2 service
    srv = simROS2.createService('/vision_sensor', 'tutorial_interfaces/srv/MyImage', srv_callback)

def sysCall_thread():
    while not sim.getSimulationStopping():
        # Keep the thread alive
        sim.wait(0.01)  # Wait for 10ms before the next iteration

def srv_callback(req):
    sim.addLog(sim.verbosity_msgs, f'Service called: {req}')
    # Create the response
    resp = simROS2.createInterface('tutorial_interfaces/srv/MyImageResponse')
    data, resolution = sim.getVisionSensorImg(self.visionSensor)
    img = Image()
    img.height = resolution[1]
    img.width = resolution[0]
    img.encoding = 'rgb8'
    img.is_bigendian = 1
    img.step = resolution[0] * 3
    img.data = data
    resp['image'] = img
    return resp

def sysCall_cleanup():
    # Shutdown the ROS2 service
    simROS2.shutdownService(srv)
    sim.addLog(sim.verbosity_msgs, 'Thread finished')

which doesn't work and I don't know why?

Other questions :

• when this script is running, Coppelia uses 100% of CPU. How can I correct this?

• the first time the service is called, it returns a black image but the next calls return a good image. Why?

Regards,

Philippe

[fferri]

This error:

Code: Select all

[/scriptCamera:warning] Received non-serializable data: {'image': sensor_msgs.msg.Image(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), height=256, width=256, encoding='rgb8', is_bigendian=1, step=768, data=[])}

happens because you are returning a rclpy Python object, e.g. sensor_msgs.msg.Image (which is not serializable).
The correct usage of simROS2 plugin requires returning a plain table, just like you would do in Lua.

To work with rclpy msg/srv objects, use rclpy for everything, and not simROS2.

You should NOT mix simROS2 and rclpy.