はじめに

ROSでpythonを使う際，numpy arrayをpublish/subscribeしたいときに変換する関数をまとめておきました．

numpy ⇔ MultiArray変換

ポイントとしては，std_msgs/MultiArrayLayoutを変換したnumpy.ndarrayにあわせて変更する必要がある点です．

converter.py

# !/usr/bin/env python

from functools import partial

from std_msgs.msg import MultiArrayDimension, Float32MultiArray

def _numpy2multiarray(multiarray_type, np_array):
    """Convert numpy.ndarray to multiarray"""
    multiarray = multiarray_type()
    multiarray.layout.dim = [MultiArrayDimension("dim%d" % i, np_array.shape[i], np_array.shape[i] * np_array.dtype.itemsize) for i in range(np_array.ndim)]
    multiarray.data = np_array.reshape(1, -1)[0].tolist()
    return multiarray

def _multiarray2numpy(pytype, dtype, multiarray):
    """Convert multiarray to numpy.ndarray"""
    dims = map(lambda x: x.size, multiarray.layout.dim)
    return np.array(multiarray.data, dtype=pytype).reshape(dims).astype(dtype)

numpy2f32multi = partial(_numpy2multiarray, Float32MultiArray)
f32multi2numpy = partial(_multiarray2numpy, float, np.float32)

partial()の部分をInt8MultiArray等で定義することで他のデータ型にも対応させることができます．

Publish/Subscribe

talker.py

# !/usr/bin/env python

import numpy as np
import rospy
from std_msgs.msg import Float32MultiArray

from converter import numpy2f32multi


def talker():
    pub = rospy.Publisher('chatter', Float32MultiArray, queue_size=10)
    rospy.init_node('talker', anonymous=True)
    r = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        arr = np.random.rand(2, 2, 2).astype(np.float32)
        arr_msg = numpy2f32multi(arr)
        pub.publish(arr_msg)
        r.sleep()
 
if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException: pass

listener.py

# !/usr/bin/env python

import numpy as np
import rospy
from std_msgs.msg import Float32MultiArray

from converter import f32multi2numpy


def callback(msg):
    arr = f32multi2numpy(msg)
    rospy.loginfo(arr)
    rospy.loginfo(type(arr))
    rospy.loginfo(arr.shape)


def listener():
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber('chatter', Float32MultiArray, callback)
    rospy.spin()

if __name__ == '__main__':
    listener()