Simple Float32MultiArray +Eigen Demo

CMakeLists.txt

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

matrix_receiver.cpp

#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;
}

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

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

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