Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Simple Float32MultiArray +Eigen Demo
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})
#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;
}
#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;
}
#!/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()
<?xml version="1.0"?>
<package>
<name>matrix_demo</name>
<version>1.0.0</version>
<description>The matrix_demo package</description>
<maintainer email="tmp@tmp.com">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>
@ZazAa1812
Copy link

What should I do if I'm not using the stride parameters ? Is there a way to properly define it?

@jarvisschultz
Copy link
Author

@ZazAa1812 I'm not really sure how to interpret your question. The stride field is important for defining the shape of your multi-dimensional array. Note that in the C++ example, I can use the stride parameter of the MultiArrayDimension to access the row and column entries of the 3x3 matrix.

@ZazAa1812
Copy link

Oh sorry, I was thinking that stride is just for when you interpreting image data. I got confuse in that. From what I understand, you publish a row of array (that was originally 3x3 matrix) and subscriber receive the data and reconstruct the array into matrix. Is that correct or I missing a fundamental knowledge here?

@jarvisschultz
Copy link
Author

Regardless of the shape of the array, the actual data in the array is stored as a std::vector. You use the stride parameter of each entry in the MultiArrayLayout.dim field to reconstruct the shape of the original array. Probably the best documentation for this is in the definition of the MultiArrayLayout message: http://docs.ros.org/melodic/api/std_msgs/html/msg/MultiArrayLayout.html

@jarvisschultz
Copy link
Author

I'll also point out that your assertion of the subscriber receiving the 1D array and reconstructing the 3x3 matrix is correct. I'm using the Eigen::Map function to do that.

@ZazAa1812
Copy link

I read the documentation a few times but I can't really comprehend the brief documentation. I always thought that it could send a matrix by matrix of data just like that. Need to deconstruct and reconstruct it. If it is not too troublesome for your, can you elaborate more on label, size and stride? Is height and width refers to the original rows and columns of your intended matrix (3x3) which is why you set the size into 3 for both of them? Regarding the stride, is dim[0].stride the total vector size of the the data and what is dim[1]?

Screenshot 2019-06-13 07:10:35

If I want to publish 2D array of amplitudes and time, can I just use your method or I need to include dim[2]?

Screenshot 2019-06-13 07:14:01

@jarvisschultz
Copy link
Author

jarvisschultz commented Jun 12, 2019

I have a 2D array. Thus I have two entries in mat.layout.dim -- one defines the rows, and one defines the columns. The stride of the zeroth dimension is indeed the total number of entries, and stride of the first dimension is the number of columns. The label field is just a convenience. You also have a 2D array, and you'll need the same information (of course, you may have different labels and values for stride).

If you really only have two vectors (time and amplitude), it might be much easier to have a custom message that contains two separate arrays. E.g. a possible message definition would be:

# time:
float32[] time
# amplitude:
float32[] amplitude

Then they could be accessed with msg.time and msg.amplitude.

As a side note, it's generally poor form to insert images of text. Now you can't copy/paste or search. The text was originally much more friendly for readers.

@ZazAa1812
Copy link

Thank you so much for explaining this to me. I understand now about the concept. Sorry for the images though. Haha

@jarvisschultz
Copy link
Author

Glad to help!

@Mechazo11
Copy link

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

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

@jarvisschultz
Copy link
Author

@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.

@Mechazo11
Copy link

@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

@Mechazo11
Copy link

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

@jarvisschultz
Copy link
Author

@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

@jarvisschultz
Copy link
Author

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.

@Mechazo11
Copy link

Mechazo11 commented Oct 18, 2022

@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

@kunalkumthekar
Copy link

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?!

@Mechazo11
Copy link

@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?

@jarvisschultz
Copy link
Author

@Mechazo11 Added a simple example in C++

@Mechazo11
Copy link

@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.

@mhdadk
Copy link

mhdadk commented Jun 5, 2023

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.

@jarvisschultz
Copy link
Author

@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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment