Custom message including numpy arrays

edit

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]