tracsat's profile - activity

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

How to Initialize a 2D MultiArray? Hello, I am using a Raspberry Pi with Lubuntu 16.04, ros Kinetic, a Sick tim 561 and

Thanks that makes a lot of sense. I was not aware that uchar was a fundamental type.

Ok that makes a lot of sense thank you! I just have one more thing if you don't mind. I noticed that the solution uses a

Ah ok this makes a lot of sense thank you! I had come to a similar conclusion after reading through a bunch of blog post

Help with PointCloud2 Data Hello, I am using a Raspberry Pi with Lubuntu 16.04, ROS Kinetic, a tim 561 and a package ca

Help with PointCloud2 Data Hello, I am using a Raspberry Pi with Lubuntu 16.04, ROS Kinetic, a tim 561 and a package ca