Ask Your Question
1

How to Initialize a 2D MultiArray?

asked 2019-03-25 14:05:27 -0600

tracsat gravatar image

Hello,

I am using a Raspberry Pi with Lubuntu 16.04, ros Kinetic, a Sick tim 561 and the package sick_scan to get data from the tim561. Currently my goal is to extract the x and y data from the sick_scan package which is published to the topic cloud as a PointCloud2. I can do this but not exactly in the way that I want. I have coded a subsriber to the cloud topic which then publishes out whatever by following a tutorial. So right now I create a Float32MultiArray and store the distance values that I get (so I do sqrt(x^2 + y^2)) and can successfully publish this.

This works fine, and the code below is what I am using. However, I would prefer to be publishing the x and y data. Unfortunately, I am rather unfamiliar with Float32MultiArray, and I am unsure as how to create a 2D array using this data type. It would also be ideal if I could give it a set size for memory allocation purposes.

I am more used to standard arrays, so what I am looking to do is float array[2][length] where length is the number of data points I am receiving (output.data.size() / 16). Is there any way to do this with a 2D Float32MultiArray?

Thanks!

Code:

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

#include <iostream>
#include <cmath>
#include <std_msgs/MultiArrayLayout.h>
#include <std_msgs/MultiArrayDimension.h>
#include <std_msgs/Float32MultiArray.h>

ros::Publisher pub;
typedef unsigned char uchar;


float bytesToFloat(uchar b0, uchar b1, uchar b2, uchar b3)
{
  float f;
  uchar b[] = {b0, b1, b2, b3};
  memcpy(&f, &b, sizeof(f));
  return f;
}

void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  //Create container for data
  sensor_msgs::PointCloud2 output;

  //Data Processing
  output = *input;

  std_msgs::Float32MultiArray arr;
  arr.data.clear();

  for (int i = 0; i < output.data.size(); i += 16)
  {
      arr.data.push_back(sqrt(pow(bytesToFloat(output.data[i], output.data[i + 1], output.data[i + 2], output.data[i + 3]), 2) +  pow(bytesToFloat(output.data[i + 4], output.data[i + 5], output.data[i + 6], output.data[i + 7$
  }

  //Publish data
  pub.publish(arr);
}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("input", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<std_msgs::Float32MultiArray> ("arr", 1);

  // Spin
  ros::spin ();
}
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2020-05-14 13:09:11 -0600

You could certainly use a std_msgs::Float32MultiArray to publish the data you are trying to publish. See the following answers for some examples on how to use:

The better question is if you actually _need_ to be publishing a multidimensional array. It might actually be much easier to work with a custom message that had 2 arrays (one for x and one for y).

edit flag offensive delete link more
0

answered 2020-05-14 11:43:50 -0600

emielke gravatar image

MultiArrays, unfortunately, cannot be multidimensional. So a 2D array could be flattened to a 1D array for this purpose, with the 2nd dimension tacked on to the end of the first row/column of data.

edit flag offensive delete link more

Comments

The data in a MultiArray message is stored as one long vector, but the std_msgs::MultiArrayDimension[] dim that is part of each is used to instruct a receiver how to unpack that array back into it's original multi-dimensional shape. Whether or not the user cares about the shape of the data they are transmitting is a different story.

jarvisschultz gravatar image jarvisschultz  ( 2020-05-14 13:01:32 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2019-03-25 14:05:27 -0600

Seen: 200 times

Last updated: May 14