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>

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 !