Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Simple Float32MultiArray +Eigen Demo
cmake_minimum_required(VERSION 2.8.3)
project(matrix_demo)
find_package(catkin REQUIRED
rospy
roscpp
std_msgs
cmake_modules
)
include_directories(
${catkin_INCLUDE_DIRS}
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES matrix_demo
CATKIN_DEPENDS rospy roscpp std_msgs
DEPENDS eigen
)
find_package(Eigen REQUIRED)
include_directories(${EIGEN_INCLUDE_DIRS})
add_definitions(${EIGEN_DEFINITIONS})
add_executable(matrix_receiver matrix_receiver.cpp)
add_dependencies(matrix_receiver ${catkin_EXPORTED_TARGETS})
target_link_libraries(matrix_receiver ${catkin_LIBRARIES})
#include <ros/ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/MultiArrayDimension.h>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
void matrixcb(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
float dstride0 = msg->layout.dim[0].stride;
float dstride1 = msg->layout.dim[1].stride;
float h = msg->layout.dim[0].size;
float w = msg->layout.dim[1].size;
ROS_INFO("mat(0,0) = %f",msg->data[0 + dstride1*0]);
ROS_INFO("mat(0,1) = %f",msg->data[0 + dstride1*1]);
ROS_INFO("mat(1,1) = %f\r\n",msg->data[1 + dstride1*1]);
// Below are a few basic Eigen demos:
std::vector<float> data = msg->data;
Eigen::Map<Eigen::MatrixXf> mat(data.data(), h, w);
std::cout << "I received = " << std::endl << mat << std::endl;
std::cout << "Its inverse is = " << std::endl << mat.inverse() << std::endl;
std::cout << "Multiplying by itself = " << std::endl << mat*mat << std::endl;
Eigen::EigenSolver<Eigen::MatrixXf> es(mat);
std::cout << "Its eigenvalues are = " << std::endl << es.eigenvalues() << std::endl;
Eigen::MatrixXf newmat = mat;
// Now let's do the nested for loop computation:
for (int i=0; i<h-1; i++)
for (int j=0; j<w; j++)
newmat(i,j) = mat(i+1,j)+2;
std::cout << "newmat = " << std::endl << newmat << std::endl;
return;
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, "subscriber");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("sent_matrix", 1, matrixcb);
ros::spin();
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
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:
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 = 3
mat.layout.dim[1].size = 3
mat.layout.dim[0].stride = 3*3
mat.layout.dim[1].stride = 3
mat.layout.data_offset = 0
mat.data = [0]*9
# 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():
tmpmat = np.zeros((3,3))
for i in range(3):
for j in range(3):
num = random.randrange(0,10)
mat.data[offset + i + dstride1*j] = num
tmpmat[i,j] = num
pub.publish(mat)
rospy.loginfo("I'm sending:")
print tmpmat,"\r\n"
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>libeigen3-dev</build_depend>
<build_depend>cmake_modules</build_depend>
<run_depend>rospy</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>cmake_modules</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
@ZazAa1812
Copy link

ZazAa1812 commented Jun 12, 2019

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

@jarvisschultz
Copy link
Author

jarvisschultz commented Jun 12, 2019

@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

ZazAa1812 commented Jun 12, 2019

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

jarvisschultz commented Jun 12, 2019

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

jarvisschultz commented Jun 12, 2019

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

ZazAa1812 commented Jun 12, 2019

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

ZazAa1812 commented Jun 12, 2019

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

jarvisschultz commented Jun 12, 2019

Glad to help!

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