179 lines
8.7 KiB
C
179 lines
8.7 KiB
C
|
/*
|
||
|
|
* 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 */
|