point_cloud2.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include <sensor_msgs/PointField.h>
#include <sensor_msgs/PointCloud2.h>
#include <ros/console.h>

#include "mrpt_bridge/point_cloud2.h"

#include <mrpt/version.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/maps/CColouredPointsMap.h>
using namespace mrpt::maps;

namespace mrpt_bridge
{
inline bool check_field(
        const sensor_msgs::PointField& input_field, std::string check_name,
        const sensor_msgs::PointField** output)
{
        bool coherence_error = false;
        if (input_field.name == check_name)
        {
                if (input_field.datatype != sensor_msgs::PointField::FLOAT32 &&
                        input_field.datatype != sensor_msgs::PointField::FLOAT64)
                {
                        *output = NULL;
                        coherence_error = true;
                }
                else
                {
                        *output = &input_field;
                }
        }
        return coherence_error;
}

inline void get_float_from_field(
        const sensor_msgs::PointField* field, const unsigned char* data,
        float& output)
{
        if (field != NULL)
        {
                if (field->datatype == sensor_msgs::PointField::FLOAT32)
                        output = *(reinterpret_cast<const float*>(&data[field->offset]));
                else
                        output = (float)(*(
                                reinterpret_cast<const double*>(&data[field->offset])));
        }
        else
                output = 0.0;
}

bool copy(const sensor_msgs::PointCloud2& msg, CSimplePointsMap& obj)
{
        // Copy point data
        unsigned int num_points = msg.width * msg.height;
        obj.clear();
        obj.reserve(num_points);

        bool incompatible_clouds = false;
        const sensor_msgs::PointField *x_field = NULL, *y_field = NULL,
                                                                  *z_field = NULL;

        for (unsigned int i = 0; i < msg.fields.size() && !incompatible_clouds; i++)
        {
                incompatible_clouds |= check_field(msg.fields[i], "x", &x_field);
                incompatible_clouds |= check_field(msg.fields[i], "y", &y_field);
                incompatible_clouds |= check_field(msg.fields[i], "z", &z_field);
        }

        if (incompatible_clouds ||
                (x_field == NULL && y_field == NULL && z_field == NULL))
                return false;

        // If not, memcpy each group of contiguous fields separately
        for (unsigned int row = 0; row < msg.height; ++row)
        {
                const unsigned char* row_data = &msg.data[row * msg.row_step];
                for (uint32_t col = 0; col < msg.width; ++col)
                {
                        const unsigned char* msg_data = row_data + col * msg.point_step;

                        float x, y, z;
                        get_float_from_field(x_field, msg_data, x);
                        get_float_from_field(y_field, msg_data, y);
                        get_float_from_field(z_field, msg_data, z);
                        obj.insertPoint(x, y, z);
                }
        }

        return true;
}

bool copy(
        const CSimplePointsMap& obj, const std_msgs::Header& msg_header,
        sensor_msgs::PointCloud2& msg)
{
        MRPT_TODO("Implement pointcloud2 mrpt2ros");
        throw ros::Exception("not implemented yet.");
        return true;
}

}  // namespace mrpt_bridge