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 you are right. Thanks !