Hi!

This is more or less a follow-up to a previous question of mine.

In short: I used the pcdtopointcloud tool to create a PointCloud publisher.

Now I have extended my publisher to work with the PMD Nano camboard I'm using. The cloud is displayed in RViz however it looks weird:

If I look at it from the side I get this peculiar view:

I work with the rgbd-grabber library and if I use sample provided in it I get this:

I had great difficulties figuring out how to publish and display the cloud inside RViz in the first and the way it looks like tells me I am doing some configuration wrong. You can see on the top of this...pyramid...turned upside down I get a view of my hand (though it looks pretty bad). It seems that the depth image I'm getting is somehow projected onto the bottom of this pyramid (bottom here is the "top" side of the cloud) and in 2D. All I'm doing is use the pcl::toROSMsg(...) to convert my pcl::PointCloudpcl::PointXYZ::Ptr to a ros::sensor_msgs::PointCloud2 message, which a publish to the ROS network. I do not do any sort of transformation to its points.

Any advice how to correct this behaviour? I can also provide the code though as I said I just take the points from the camera, convert them to a ROS-compatible message and display them inside RViz.

Thanks!

EDIT: The code is as follows:

#include <ros/ros.h>
#include <string>
#include <memory>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl_ros/point_cloud.h>
//#include <sensor_msgs/point_cloud_conversion.h>
#include <pcl/point_types.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include "../include/pmd_nano/PMDNano.h"
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl_ros/publisher.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/publisher.h>

using std::string;

class PMDPublisher
{
protected:
  string ppp_file, pap_file;
  ros::NodeHandle nh;
  std::shared_ptr<DepthCamera> camera;
  string cloud_topic;
  string imgDepth_topic, imgAmplitude_topic;
  string tf_frame;
  ros::NodeHandle private_nh;
  pcl_ros::Publisher<sensor_msgs::PointCloud2> pubCloud;
  image_transport::Publisher pubDepth, pubAmplitude;
public:
  sensor_msgs::PointCloud2 cloud;
  //sensor_msgs::PointCloud2Ptr cloud;

  PMDPublisher(string _ppp_file, string _pap_file, string _cloud_topic="pmd_cloud", string _imgDepth_topic="pmd_depth", string _imgAmplitude_topic="pmd_amplitude")
    : ppp_file(_ppp_file), pap_file(_pap_file),
      cloud_topic(_cloud_topic),
      imgDepth_topic(_imgDepth_topic), imgAmplitude_topic(_imgAmplitude_topic),
      tf_frame("/base_link"),
      private_nh("~")
  {
    ROS_INFO("Initializing PMD Publisher");
    if(ppp_file == "" || pap_file == "")
      exit(EXIT_FAILURE);

    ROS_INFO_STREAM("Following PMD configuration files were loaded:\n"
                    << "PPP file: \"" << ppp_file << "\"\n"
                    << "PAP file: \"" << pap_file << "\"\n");

    camera = std::shared_ptr<DepthCamera>(new PMDNano(pap_file, ppp_file));

    pubCloud.advertise(nh, cloud_topic, 1);
    image_transport::ImageTransport imgTransportDepth(nh), imgTransportAmplitude(nh);
    pubDepth = imgTransportDepth.advertise(imgDepth_topic, 1);
    pubAmplitude = imgTransportAmplitude.advertise(imgAmplitude_topic, 1);

    private_nh.param("frame_id", tf_frame, string("/base_link"));
    ROS_INFO_STREAM("Publishing data on topic \"" << nh.resolveName(cloud_topic) << "\" with frame_id \"" << tf_frame << "\"");
  }

  int start()
  {
    try {
     camera->start();
    }
    catch(std::exception e) {
      ROS_ERROR("Node unable to start camera");
      return -1;
    }
    ROS_INFO_STREAM("Depth: " << camera->depthSize().width << " by " << camera->depthSize().height);
    return 0;
  }

  bool spin ()
  {
    ROS_INFO("Allocating memory");
    cv::Mat depth = cv::Mat::zeros(camera->depthSize(), CV_32F);
    cv::Mat amplitude = cv::Mat::zeros(camera->depthSize(), CV_32F);
    cv::Mat a;
    int nr_points;
    string fields_list;
    sensor_msgs::ImagePtr depthMsg, amplitudeMsg;
    // Reserve memory for the cloud based on the dimension parameters retrieved from the PMD
    PointCloud::Ptr cloudPcl(new PointCloud(camera->depthSize().width, camera->depthSize().height));

    ros::Rate r(50);

    ROS_INFO("Entering loop");
    while(nh.ok ())
    {
      ROS_INFO("Capturing new data");
      camera->captureDepth(depth);
      camera->captureAmplitude(amplitude);
      camera->capturePointCloud(cloudPcl);
      pcl::toROSMsg(*cloudPcl, cloud);  // Convert the PointCloud to ROS message PointCloud2
      cloud.header.frame_id = tf_frame;
      nr_points = cloud.width * cloud.height;
      fields_list = pcl::getFieldsList(cloud);

      ROS_INFO("Flipping retrieved data and converting colour space (amplitude only!)");
      cv::flip(depth, depth, 0);
      cv::flip(amplitude, amplitude, 0);
      amplitude.convertTo(a, CV_8U, 255.0 / 1000.0);

      depthMsg = cv_bridge::CvImage(std_msgs::Header(), "mono16", depth).toImageMsg();
      amplitudeMsg = cv_bridge::CvImage(std_msgs::Header(), "mono8", amplitude).toImageMsg();

      //ROS_DEBUG_STREAM_ONCE
      ROS_INFO_STREAM("Publishing data with " << nr_points
                            << " points " << fields_list
                            << " on topic \"" << nh.resolveName(cloud_topic)
                            << "\" in frame \"" << cloud.header.frame_id << "\"");

      cloud.header.stamp = ros::Time::now();
      pubCloud.publish(cloud);
      pubDepth.publish(depthMsg);
      pubAmplitude.publish(amplitudeMsg);

      r.sleep();
    }
    return (true);
  }
};

int main(int argc, char* argv[])
{
  ros::init (argc, argv, "pmd_nano_publisher");
  ros::NodeHandle nh;
  string ppp_file = "", pap_file = "";
  ROS_INFO("Retrieveing parameters");
  nh.getParam("ppp", ppp_file);
  nh.getParam("pap", pap_file);

  if(argc == 3 && (ppp_file == "" || pap_file == "")) {
    ROS_INFO("Overloading launch parameters with command line arguments");
    ppp_file = argv[1];
    pap_file = argv[2];
  }

  PMDPublisher pmdp(ppp_file, pap_file);

  if (pmdp.start () == -1)
  {
    ROS_ERROR("Woops!");
    return -1;
  }
  /*ROS_INFO_STREAM("Loaded a point cloud with " << pmdp.cloud.width * pmdp.cloud.height
                  << " points (total size is " << pmdp.cloud.data.size() << ") and the following channels: " << pcl::getFieldsList(pmdp.cloud));*/
  pmdp.spin();

  return 0;
}

Code is still pretty rough on the edges (though at least I haven't discovered any memory leaks). As you can see here is what happens with the point cloud data:

1. sensor_msgs::PointCloud2 cloud - stores the message that I want to publish
2. PointCloud::Ptr cloudPcl(new PointCloud(camera->depthSize().width, camera->depthSize().height)) - the memory block that will hold the pcl::PointCloudpcl::PointXYZ that I get from my PMD (again - this can be seen in the last screenshot)
3. camera->capturePointCloud(cloudPcl) - populates my pcl::PointCloudpcl::PointXYZ memory block with data from the PMD (last screenshot)
4. cloud.header.frameid = tfframe - sets the frame of the point cloud; any object inside RViz has to have some sort of a frame and respectively transformation between that frame and some fixed frame unless both are the same (which I did in my case)
5. cloud.header.stamp = ros::Time::now() - add time stamp using ROS time
6. pubCloud.publish(cloud) - publish the sensor_msgs::PointCloud2

I can't see what I'm doing wrong here to be honest. :-/

EDIT2: As requested the log files: https://github.com/aleksandaratanasov/PMDNano_ROS/tree/master/pcd_data (I have created both ascii and binary compressed plus a ZIP archive containing all the samples) Using my PCL Visualizer widget I have discovered that the problem starts BEFORE the conversion to ROS message even starts. Inside my publisher I created a sequence of PCD files using the "raw" data from the PMDNano (using the rgbd-grabber library). Then I viewed the generated samples and it looks the same way as in RViz hence the issue lies somewhere in the way I'm using the PMDNano class inside my node. I have no idea what can cause such strange results but at least now I have found that the problem is not in the conversion step which I'm really thankful for, @William . If you or anyone else has any advice where I can start looking next I would be extremely thankful!