Simple Float32MultiArray +Eigen Demo

CMakeLists.txt

cmake_minimum_required(VERSION 3.5.1)
project(matrix_demo)

find_package(catkin REQUIRED
  rospy
  roscpp
  std_msgs
  )

include_directories(
  ${catkin_INCLUDE_DIRS}
  )


catkin_package(
 CATKIN_DEPENDS rospy roscpp std_msgs
 )

find_package(Eigen3 REQUIRED)

include_directories(${catkin_INCLUDE_DIRS})

add_executable(matrix_receiver matrix_receiver.cpp)
add_dependencies(matrix_receiver ${catkin_EXPORTED_TARGETS})
target_link_libraries(matrix_receiver ${catkin_LIBRARIES})

add_executable(matrix_sender matrix_sender.cpp)
add_dependencies(matrix_sender ${catkin_EXPORTED_TARGETS})
target_link_libraries(matrix_sender ${catkin_LIBRARIES})

matrix_receiver.cpp

#include <ros/ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/MultiArrayDimension.h>
#include <iostream>
#include <eigen3/Eigen/Dense>

void matrixcb(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
  const int dstride0 = msg->layout.dim[0].stride;
  const int dstride1 = msg->layout.dim[1].stride;
  const float h = msg->layout.dim[0].size;
  const float w = msg->layout.dim[1].size;
  // ROS_INFO_STREAM("msg = " << *msg);
  ROS_INFO("mat(0,0) = %f", msg->data[dstride1 * 0 + 0]);
  ROS_INFO("mat(0,1) = %f", msg->data[dstride1 * 0 + 1]);
  ROS_INFO("mat(1,1) = %f\r\n", msg->data[dstride1 * 1 + 1]);

  // Below are a few basic Eigen demos:
  std::vector<float> data = msg->data;
  Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > mat(data.data(), h, w);
  std::cout << "I received = " << std::endl << mat << std::endl;
  return;
}

int main(int argc, char* argv[])
{
  ros::init(argc, argv, "matrix_receiver");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("sent_matrix", 1, matrixcb);
  ros::spin();
  return 0;
}

matrix_sender.cpp

#include <ros/ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/MultiArrayDimension.h>
#include <eigen3/Eigen/Dense>

#include <iostream>
#include <vector>

static constexpr std::uint32_t WIDTH(3);
static constexpr std::uint32_t HEIGHT(3);

void generate_and_pub_matrix(ros::Publisher& pub, const std::uint8_t count)
{
  Eigen::Matrix<float, HEIGHT, WIDTH, Eigen::RowMajor> mat;
  mat.setRandom();
  ROS_INFO_STREAM("For loop " << +count << " generated the following matrix: " << std::endl << mat);

  // Now we can convert to a message
  std_msgs::Float32MultiArray msg;
  msg.layout.dim.push_back(std_msgs::MultiArrayDimension());
  msg.layout.dim.push_back(std_msgs::MultiArrayDimension());
  msg.layout.dim[0].label = "height";
  msg.layout.dim[0].size = HEIGHT;
  msg.layout.dim[0].stride = HEIGHT*WIDTH;
  msg.layout.dim[1].label = "width";
  msg.layout.dim[1].size = WIDTH;
  msg.layout.dim[1].stride = WIDTH;
  msg.layout.data_offset = 0;
  std::vector<float> vec;
  vec.resize(mat.size());
  Eigen::Map<Eigen::VectorXf> mvec(mat.data(), mat.size());
  Eigen::VectorXf::Map(&vec[0], mvec.size()) = mvec;
  msg.data = vec;
  pub.publish(msg);
}

int main(int argc, char* argv[])
{
  ros::init(argc, argv, "matrix_sender");
  ros::NodeHandle n;
  ros::Publisher pub = n.advertise<std_msgs::Float32MultiArray>("sent_matrix", 1);

  ros::Duration(1.0).sleep();

  ros::Rate loop(0.5);
  std::uint8_t count = 0;
  while (ros::ok())
  {
    ++count;
    generate_and_pub_matrix(pub, count);
    ros::spinOnce();
    loop.sleep();
  }

  return 0;
}

matrix_sender.py

#!/usr/bin/env python
import rospy
from std_msgs.msg import Float32MultiArray
from std_msgs.msg import MultiArrayDimension
import random

import numpy as np

def matrix_cb(msg):
    mat = np.array(msg.data)
    mat = mat.reshape(msg.layout.dim[0].size, msg.layout.dim[1].size)
    rospy.loginfo("Received: \n%s\n", str(mat))
    return


if __name__ =="__main__":
    rospy.init_node("publisher")
    pub = rospy.Publisher('sent_matrix', Float32MultiArray, queue_size=1)
    r = rospy.Rate(0.5)
    # let's build a 3x3 matrix:
    width = 3
    height = 3
    mat = Float32MultiArray()
    mat.layout.dim.append(MultiArrayDimension())
    mat.layout.dim.append(MultiArrayDimension())
    mat.layout.dim[0].label = "height"
    mat.layout.dim[1].label = "width"
    mat.layout.dim[0].size = height
    mat.layout.dim[1].size = width
    mat.layout.dim[0].stride = width*height
    mat.layout.dim[1].stride = width
    mat.layout.data_offset = 0
    mat.data = [0]*width*height

    # let's create a subscriber to illustrate how to convert from a received
    # Float32MultiArray to a numpy array
    sub = rospy.Subscriber('sent_matrix', Float32MultiArray, matrix_cb, queue_size=1)

    # save a few dimensions:
    dstride0 = mat.layout.dim[0].stride
    dstride1 = mat.layout.dim[1].stride
    offset = mat.layout.data_offset
    while not rospy.is_shutdown():
        # create a numpy array that we will use just for logging purposes
        tmpmat = np.zeros((height,width))
        # for each instance of the while loop, we will generate random numbers
        # to go into a matrix that we will publish
        for i in range(height):
            for j in range(width):
                # for each (i,j) entry generate the random number
                num = random.randrange(0,10)
                # store the random number in the message we publish
                mat.data[offset + i*dstride1 + j] = num
                # also store the number in our numpy array for logging
                tmpmat[i,j] = num
        pub.publish(mat)
        rospy.loginfo("I'm sending: \r\n %s\r\n", str(tmpmat))
        r.sleep()

 

package.xml

<?xml version="1.0"?>
<package>
  <name>matrix_demo</name>
  <version>1.0.0</version>
  <description>The matrix_demo package</description>
  <maintainer email="[email protected]">tmp</maintainer>
  <license>MIT</license>
  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>rospy</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>eigen</build_depend>
  <build_depend>cmake_modules</build_depend>

  <run_depend>rospy</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>std_msgs</run_depend>

</package>

@jarvisschultz Thank you very much for posting this tutorial. Could you elaborate on the purpose of this line

mat.layout.dim.append(MultiArrayDimension()) ?

@Mechazo11 In Python, the mat.layout.dim field is a List that is initialized as an empty list when we construct mat with mat = Float32MultiArray(). I call the mat.layout.dim.append(MultiArrayDimension()) line twice to create a list of length 2 (because I'm creating a 2-dimensional matrix) with each entry in the list being a default-constructed MultiArrayDimension object. Then in the chunk below, I can use the [] operator to access each of those entries and fill out appropriate details about the MultiArrayDimension entries.

@jarvisschultz Thanks for the prompt response. I was trying to replicate sending a 2D rectangular matrix which in python list in list would look like

[[Cls_id, x1,y1,x2,y2], [....], .....]. My question is can we use Multiarray to replicate this idea? I am really confused with how mat.data[offset + i + dstride1*j] is working, when I try to access element by element of each row (each row is a 1 x5 row vector), it throws an error list range out of index when I tried to access the i,j th element

I think what my comment above simplifies to what is the syntax to access (i,j)th element from both Python and C++ using MultiArray msgs

@Mechazo11 The Float32MultiArray message is definitely appropriate for sending/receiving a 2D matrix like you are describing. In this message, the actual matrix data will be stored as a vector/list that has a length equal to the matrix width multiplied by the matrix height. The stride and dimensions fields contained in the .layout.dim field describe the "shape" of the matrix even though the actual data is stored in a single-dimensional vector. The mat.data[offset + i + dstride1*j] is accessing the entry in the vector corresponding to the (i,j) index. This is described in the MultiArrayLayout message comments.

So in the Python script, inside of the nested for-loop I am doing the following:

1. Generating a random number to be the (i,j) entry in a random matrix that I'll publish
2. Setting the corresponding entry in the mat.data list
3. Setting the (i,j) entry of tmpmat just so I can log the matrix that I will publish with an appropriate shape.

If you were to run this node and at the same time run rostopic echo /sent_matrix you should be able to see how the (i,j) entries in the log messages map to the list entries in the published message.

If you are subscribing to a Float32MultiArray, in general, I think you can either:

• Convert the vector data to some data structure representing a matrix in whatever language you are subscribing in. This is what I did in the C++ example above where I converted to an Eigen::MatrixXf. In that file you can see I can then access data using Eigen's (i,j) syntax.
• Continue storing the data in a single vector and then always access the (i,j) entries using a calculation like offset + i + dstride1*j

Just updated the gist with this commit to clean up a few things. Also added an example of how to convert from a ROS Float32MultiArray to a numpy array in Python since that was missing before.

@jarvisschultz Thank you very much for updating the tutorial. I got your code to work with my python script to send out a python list in list to a C++ receiver which converts the data message into a usable rectangular/square matrix

I am trying to access members of MultiArrayDimension() within MultiArrayLayout which is within Float64MultiArray
i cannot access the members of MultiArrayDimension()

When I initiate the dim as an empty araay: msg.dim = []
Later, I append it with MulitArrayDimension : msg.dim.append(MulitArrayDimension())
Later, I access the members of it and assign values to it: msg.dim[0].label = ''
msg.dim[0].size = 36.

However, I get the error saying: The label field mult be of type str.

I dont understand it can you help me out?!

@jarvisschultz Hi. Thank you for your help last year in explaining how to use the Float32MultiArray to send and receive 2D matrices between Python and C++. Can you kindly show an example that sends an Eigen [m x n] 2D matrix from a C++ node?

@Mechazo11 Added a simple example in C++

@jarvisschultz thank you very much, really appreciate your prompt response. Using your example, I was able to transfer an Eigen matrix both ways between two C++ nodes.

In case anyone is curious, as this was not immediately clear to me from the documentation, the Float32MultiArray::data property is of type std::vector<float> (with a custom allocator), as described here and here.

@mhdadk that's definitely correct! Thanks for the tip!

This is in agreement with the ROS Message Description Specification documentation. Note, data as a field type of float32[], which according to the previous link should be a std::vector<float> in C++ and a tuple of float in Python.

@jarvisschultz thanks a lot for the info! I'm reproducing below, for convenience to future readers, the "Array Handling" section below taken from the link in Jarvis's comment:

Array handling

Array Type	Serialization	C++	Python2	Python3
fixed-length	no extra serialization	0.11+: boost::array<T, length>, otherwise: std::vector	tuple (1)	tuple (1)
variable-length	uint32 length prefix	std::vector	tuple (1)	tuple (1)
uint8[]	see above	as above	str	bytes (2)
bool[]	see above	std::vector<uint8_t>	list of bool	list of bool

As an example for using this table, the points property in the sensor_msgs/PointCloud message is of type std::vector<geometry_msgs/Point32>.

Hi, I want to use these Float32MultiArray to create Grid map message and I got this error in rviz "isRowMajor() failed because layout label is not set correctly.".
I just pass them as data to grid_map_msgs/GridMap message. Obviously there is a problem in the way to set the labels. I tried to first create a layout and assign it to the array but I have the same error raised. Can someone help me? Thanks.

@Charifou glancing at the grid_map source code it seems like they only accept Float32MultiArray messages with very particular labels for the layout. See the source for isRowMajor where message.layout.dim[0].label must be either "column_index" or "row_index" (see the definition of storageIndexNames)

@jarvisschultz you are right. Thanks !