/* * Software License Agreement (BSD License) * * Robot Operating System code by the University of Osnabrück * Copyright (c) 2015, University of Osnabrück * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above * copyright notice, this list of conditions and the following * disclaimer. * * 2. Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * 3. Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * * * * point_field_conversion.h * * Created on: 16.07.2015 * Authors: Sebastian Pütz <spuetz@uni-osnabrueck.de> */ #ifndef SENSOR_MSGS_POINT_FIELD_CONVERSION_H #define SENSOR_MSGS_POINT_FIELD_CONVERSION_H /** * \brief This file provides a type to enum mapping for the different * PointField types and methods to read and write in * a PointCloud2 buffer for the different PointField types. * \author Sebastian Pütz */ namespace sensor_msgs{ /*! * \Enum to type mapping. */ template<int> struct pointFieldTypeAsType {}; template<> struct pointFieldTypeAsType<sensor_msgs::PointField::INT8> { typedef int8_t type; }; template<> struct pointFieldTypeAsType<sensor_msgs::PointField::UINT8> { typedef uint8_t type; }; template<> struct pointFieldTypeAsType<sensor_msgs::PointField::INT16> { typedef int16_t type; }; template<> struct pointFieldTypeAsType<sensor_msgs::PointField::UINT16> { typedef uint16_t type; }; template<> struct pointFieldTypeAsType<sensor_msgs::PointField::INT32> { typedef int32_t type; }; template<> struct pointFieldTypeAsType<sensor_msgs::PointField::UINT32> { typedef uint32_t type; }; template<> struct pointFieldTypeAsType<sensor_msgs::PointField::FLOAT32> { typedef float type; }; template<> struct pointFieldTypeAsType<sensor_msgs::PointField::FLOAT64> { typedef double type; }; /*! * \Type to enum mapping. */ template<typename T> struct typeAsPointFieldType {}; template<> struct typeAsPointFieldType<int8_t> { static const uint8_t value = sensor_msgs::PointField::INT8; }; template<> struct typeAsPointFieldType<uint8_t> { static const uint8_t value = sensor_msgs::PointField::UINT8; }; template<> struct typeAsPointFieldType<int16_t> { static const uint8_t value = sensor_msgs::PointField::INT16; }; template<> struct typeAsPointFieldType<uint16_t> { static const uint8_t value = sensor_msgs::PointField::UINT16; }; template<> struct typeAsPointFieldType<int32_t> { static const uint8_t value = sensor_msgs::PointField::INT32; }; template<> struct typeAsPointFieldType<uint32_t> { static const uint8_t value = sensor_msgs::PointField::UINT32; }; template<> struct typeAsPointFieldType<float> { static const uint8_t value = sensor_msgs::PointField::FLOAT32; }; template<> struct typeAsPointFieldType<double> { static const uint8_t value = sensor_msgs::PointField::FLOAT64; }; /*! * \Converts a value at the given pointer position, interpreted as the datatype * specified by the given template argument point_field_type, to the given * template type T and returns it. * \param data_ptr pointer into the point cloud 2 buffer * \tparam point_field_type sensor_msgs::PointField datatype value * \tparam T return type */ template<int point_field_type, typename T> inline T readPointCloud2BufferValue(const unsigned char* data_ptr){ typedef typename pointFieldTypeAsType<point_field_type>::type type; return static_cast<T>(*(reinterpret_cast<type const *>(data_ptr))); } /*! * \Converts a value at the given pointer position interpreted as the datatype * specified by the given datatype parameter to the given template type and returns it. * \param data_ptr pointer into the point cloud 2 buffer * \param datatype sensor_msgs::PointField datatype value * \tparam T return type */ template<typename T> inline T readPointCloud2BufferValue(const unsigned char* data_ptr, const unsigned char datatype){ switch(datatype){ case sensor_msgs::PointField::INT8: return readPointCloud2BufferValue<sensor_msgs::PointField::INT8, T>(data_ptr); case sensor_msgs::PointField::UINT8: return readPointCloud2BufferValue<sensor_msgs::PointField::UINT8, T>(data_ptr); case sensor_msgs::PointField::INT16: return readPointCloud2BufferValue<sensor_msgs::PointField::INT16, T>(data_ptr); case sensor_msgs::PointField::UINT16: return readPointCloud2BufferValue<sensor_msgs::PointField::UINT16, T>(data_ptr); case sensor_msgs::PointField::INT32: return readPointCloud2BufferValue<sensor_msgs::PointField::INT32, T>(data_ptr); case sensor_msgs::PointField::UINT32: return readPointCloud2BufferValue<sensor_msgs::PointField::UINT32, T>(data_ptr); case sensor_msgs::PointField::FLOAT32: return readPointCloud2BufferValue<sensor_msgs::PointField::FLOAT32, T>(data_ptr); case sensor_msgs::PointField::FLOAT64: return readPointCloud2BufferValue<sensor_msgs::PointField::FLOAT64, T>(data_ptr); } } /*! * \Inserts a given value at the given point position interpreted as the datatype * specified by the template argument point_field_type. * \param data_ptr pointer into the point cloud 2 buffer * \param value the value to insert * \tparam point_field_type sensor_msgs::PointField datatype value * \tparam T type of the value to insert */ template<int point_field_type, typename T> inline void writePointCloud2BufferValue(unsigned char* data_ptr, T value){ typedef typename pointFieldTypeAsType<point_field_type>::type type; *(reinterpret_cast<type*>(data_ptr)) = static_cast<type>(value); } /*! * \Inserts a given value at the given point position interpreted as the datatype * specified by the given datatype parameter. * \param data_ptr pointer into the point cloud 2 buffer * \param datatype sensor_msgs::PointField datatype value * \param value the value to insert * \tparam T type of the value to insert */ template<typename T> inline void writePointCloud2BufferValue(unsigned char* data_ptr, const unsigned char datatype, T value){ switch(datatype){ case sensor_msgs::PointField::INT8: writePointCloud2BufferValue<sensor_msgs::PointField::INT8, T>(data_ptr, value); break; case sensor_msgs::PointField::UINT8: writePointCloud2BufferValue<sensor_msgs::PointField::UINT8, T>(data_ptr, value); break; case sensor_msgs::PointField::INT16: writePointCloud2BufferValue<sensor_msgs::PointField::INT16, T>(data_ptr, value); break; case sensor_msgs::PointField::UINT16: writePointCloud2BufferValue<sensor_msgs::PointField::UINT16, T>(data_ptr, value); break; case sensor_msgs::PointField::INT32: writePointCloud2BufferValue<sensor_msgs::PointField::INT32, T>(data_ptr, value); break; case sensor_msgs::PointField::UINT32: writePointCloud2BufferValue<sensor_msgs::PointField::UINT32, T>(data_ptr, value); break; case sensor_msgs::PointField::FLOAT32: writePointCloud2BufferValue<sensor_msgs::PointField::FLOAT32, T>(data_ptr, value); break; case sensor_msgs::PointField::FLOAT64: writePointCloud2BufferValue<sensor_msgs::PointField::FLOAT64, T>(data_ptr, value); break; } } } #endif /* point_field_conversion.h */