Robotics StackExchange | Archived questions

Custom message including numpy arrays

Hi!

I want to publish a custom message including some numpy arrays using python.

I am aware of the numpy_msg and all it's examples, they do however only cover publishing just one array.

My example message is:

header 
int16[] array1 
int16[] array2

My publisher looks approximatly like this:

import rospy
from std_msgs.msg import String
from test_publisher.msg import test_msg
from rospy.numpy_msg import numpy_msg
import numpy as np


def talker():
    pub = rospy.Publisher('chatter', numpy_msg(test_msg), queue_size=10)
    rospy.init_node('talker', anonymous=True)

    rate = rospy.Rate(1) #
    while not rospy.is_shutdown():

        my_msg = test_msg()
        my_msg.array_1 = np.ones([1,4],dtype=np.int16)
        my_msg.array_2 = np.ones([4,4],dtype=np.int16)

        pub.publish(my_msg)
        rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

When i run this code and enable a listener, the following exception occurs:

rospy.exceptions.ROSSerializationException: field array1 must be a list or tuple type

If i just define one array in the message and directly publish that, it works however.

Can anyone provide me with some insight?

Asked by dgus on 2018-04-24 10:59:23 UTC

Comments

numpy arrays are not natively supported in ROS messages by default. I'm not entirely sure, but I believe that is where the error is coming from.

You could take a look at eric-wieser/ros_numpy.

Asked by gvdhoorn on 2018-04-24 11:15:24 UTC

And perhaps this tutorial on the ROS wiki: Using numpy with rospy.

Asked by gvdhoorn on 2018-04-24 11:15:47 UTC

I wonder if the issue is not that you have two arrays, but that you are trying to feed 2-dimensional arrays into the message?

Asked by jarvisschultz on 2018-04-24 11:35:04 UTC

Unfortunatly the tutorial only covers converting the whole array into one message and I am not able to do the same when the message contains other fields as well

Asked by dgus on 2018-04-24 14:53:26 UTC

As far as I can see, the library supplied by wieser converts numpy arrays into other known message types, none of them usable for my purpose.

Asked by dgus on 2018-04-24 14:53:52 UTC

I have not yet tried using 1D arrays, but I will try that in the morning.

Asked by dgus on 2018-04-24 14:55:06 UTC

Answers

It is kind of hacky, but the way I solved this problem was by using sensor_msgs.msg.Image and cvbridge instead of the numpy message. OpenCV Mats in python are just numpy matrices and I am not even sure they need to be the same size every time you publish them, so that gives you a lot of flexibility. Here is a sample code for this idea:

import rospy
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image

def talker():
    rospy.init_node('talker', anonymous=True)
    mat_pub = rospy.Publisher("matrix", Image, queue_size=1)
    mybridge = CvBridge()
    rate = rospy.Rate(1) 

    mymat = np.ones([1,4],dtype=np.int16

    while not rospy.is_shutdown():
        try:
             mat_pub.publish(mybridge.cv2_to_imgmsg(mymat))
             rate.sleep()
        except CvBridgeError as e:
             rospy.logerr(e)

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

I suppose to publish more messages you can publish them separately and then read them in synch using message_filters and read them both at the same time (see http://wiki.ros.org/message_filters). I know this is not ideal, but this is one way I managed to get around this.

@gvdhoorn pointed out that this seemed like a misuse of a sensor_msgs/Image type, and I guess I have to agree. Another solution - the one I took in the end - is to use the MultiArray definitions. I haven't found an example in python, so here is mine:

import rospy
import numpy as np

from std_msgs.msg import Float32MultiArray, MultiArrayDimension

def talker():
    rospy.init_node('talker', anonymous=True)
    scores_pub = rospy.Publisher("scores",Float32MultiArray, queue_size=1) 

    scores = np.array([[[1.,2.],[3.,4.],[5.,6.]],[[11.,12.],[13.,14.],[15.,16.]]],dtype='float32')
    #if the dimensions vary, then the the layout will need to be updated as well
    scoresmsg = Float32MultiArray()
    scoresmsg.layout.dim = []
    dims = np.array(scores.shape)
    scoresize = dims.prod()/float(scores.nbytes) # this is my attempt to normalize the strides size depending on .nbytes. not sure this is correct

    for i in range(0,len(dims)): #should be rather fast. 
        # gets the num. of dims of nparray to construct the message   
        scoresmsg.layout.dim.append(MultiArrayDimension())
        scoresmsg.layout.dim[i].size = dims[i]
        scoresmsg.layout.dim[i].stride = dims[i:].prod()/scoresize
        scoresmsg.layout.dim[i].label = 'dim_%d'%i

    while not rospy.is_shutdown():
        scoresmsg.data = np.frombuffer(scores.tobytes(),'float32') ## serializes
        scores_pub.publish(scoresmsg)

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

Attention: I haven't written a subscriber for this to see if the indexing works as intended. The data is preserved which is the most important part, but the stride sizes are still an issue. I will write an example listener and double check this soon.

The output is:

layout: 
  dim: 
    - 
      label: "dim_0"
      size: 2
      stride: 48
    - 
      label: "dim_1"
      size: 3
      stride: 24
    - 
      label: "dim_2"
      size: 2
      stride: 8
  data_offset: 0
data: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 11.0, 12.0, 13.0, 14.0, 15.0, 16.0]

Asked by fbelmonteklein on 2019-04-15 06:05:17 UTC

Comments

Do I understand it correctly that you are publishing sensor_msgs/Image messages that don't actually contain image data? If so: pedantic perhaps, but that would seem to go against the idea of typed, semantically meaningful communcation that underlies the ROS pub-sub system.

Asked by gvdhoorn on 2019-04-15 07:05:51 UTC

Fair point. The other alternative is using multiarray and that is kind of awkward. I will edit my answer to contain both ideas...

Asked by fbelmonteklein on 2019-04-15 09:17:22 UTC