Skip to content

Instantly share code, notes, and snippets.

@jarvisschultz
Last active January 19, 2024 23:25
Show Gist options
  • Star 14 You must be signed in to star a gist
  • Fork 5 You must be signed in to fork a gist
  • Save jarvisschultz/7a886ed2714fac9f5226 to your computer and use it in GitHub Desktop.
Save jarvisschultz/7a886ed2714fac9f5226 to your computer and use it in GitHub Desktop.
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>
@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.

@mhdadk
Copy link

mhdadk commented Jun 20, 2023

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

@Charifou
Copy link

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.

@jarvisschultz
Copy link
Author

jarvisschultz commented Jan 18, 2024

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

@Charifou
Copy link

@jarvisschultz you are right. Thanks !

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