Rosanswers logo


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?



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

cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
  //Create container for data
  sensor_msgs::PointCloud2 output;
  //Data Processing
  output = *input;
  std_msgs::Float32MultiArray arr;
  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

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

Originally posted by tracsat on ROS Answers with karma: 13 on 2019-03-25

Post score: 1


1 Answer 1


Rosanswers logo

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

Originally posted by jarvisschultz with karma: 9031 on 2020-05-14

This answer was ACCEPTED on the original site

Post score: 0


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.