Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046 #ifndef SENSOR_MSGS_POINT_FIELD_CONVERSION_H
00047 #define SENSOR_MSGS_POINT_FIELD_CONVERSION_H
00048
00055 namespace sensor_msgs{
00059 template<int> struct pointFieldTypeAsType {};
00060 template<> struct pointFieldTypeAsType<sensor_msgs::PointField::INT8> { typedef int8_t type; };
00061 template<> struct pointFieldTypeAsType<sensor_msgs::PointField::UINT8> { typedef uint8_t type; };
00062 template<> struct pointFieldTypeAsType<sensor_msgs::PointField::INT16> { typedef int16_t type; };
00063 template<> struct pointFieldTypeAsType<sensor_msgs::PointField::UINT16> { typedef uint16_t type; };
00064 template<> struct pointFieldTypeAsType<sensor_msgs::PointField::INT32> { typedef int32_t type; };
00065 template<> struct pointFieldTypeAsType<sensor_msgs::PointField::UINT32> { typedef uint32_t type; };
00066 template<> struct pointFieldTypeAsType<sensor_msgs::PointField::FLOAT32> { typedef float type; };
00067 template<> struct pointFieldTypeAsType<sensor_msgs::PointField::FLOAT64> { typedef double type; };
00068
00072 template<typename T> struct typeAsPointFieldType {};
00073 template<> struct typeAsPointFieldType<int8_t> { static const uint8_t value = sensor_msgs::PointField::INT8; };
00074 template<> struct typeAsPointFieldType<uint8_t> { static const uint8_t value = sensor_msgs::PointField::UINT8; };
00075 template<> struct typeAsPointFieldType<int16_t> { static const uint8_t value = sensor_msgs::PointField::INT16; };
00076 template<> struct typeAsPointFieldType<uint16_t> { static const uint8_t value = sensor_msgs::PointField::UINT16; };
00077 template<> struct typeAsPointFieldType<int32_t> { static const uint8_t value = sensor_msgs::PointField::INT32; };
00078 template<> struct typeAsPointFieldType<uint32_t> { static const uint8_t value = sensor_msgs::PointField::UINT32; };
00079 template<> struct typeAsPointFieldType<float> { static const uint8_t value = sensor_msgs::PointField::FLOAT32; };
00080 template<> struct typeAsPointFieldType<double> { static const uint8_t value = sensor_msgs::PointField::FLOAT64; };
00081
00090 template<int point_field_type, typename T>
00091 inline T readPointCloud2BufferValue(const unsigned char* data_ptr){
00092 typedef typename pointFieldTypeAsType<point_field_type>::type type;
00093 return static_cast<T>(*(reinterpret_cast<type const *>(data_ptr)));
00094 }
00095
00103 template<typename T>
00104 inline T readPointCloud2BufferValue(const unsigned char* data_ptr, const unsigned char datatype){
00105 switch(datatype){
00106 case sensor_msgs::PointField::INT8:
00107 return readPointCloud2BufferValue<sensor_msgs::PointField::INT8, T>(data_ptr);
00108 case sensor_msgs::PointField::UINT8:
00109 return readPointCloud2BufferValue<sensor_msgs::PointField::UINT8, T>(data_ptr);
00110 case sensor_msgs::PointField::INT16:
00111 return readPointCloud2BufferValue<sensor_msgs::PointField::INT16, T>(data_ptr);
00112 case sensor_msgs::PointField::UINT16:
00113 return readPointCloud2BufferValue<sensor_msgs::PointField::UINT16, T>(data_ptr);
00114 case sensor_msgs::PointField::INT32:
00115 return readPointCloud2BufferValue<sensor_msgs::PointField::INT32, T>(data_ptr);
00116 case sensor_msgs::PointField::UINT32:
00117 return readPointCloud2BufferValue<sensor_msgs::PointField::UINT32, T>(data_ptr);
00118 case sensor_msgs::PointField::FLOAT32:
00119 return readPointCloud2BufferValue<sensor_msgs::PointField::FLOAT32, T>(data_ptr);
00120 case sensor_msgs::PointField::FLOAT64:
00121 return readPointCloud2BufferValue<sensor_msgs::PointField::FLOAT64, T>(data_ptr);
00122 }
00123 }
00124
00133 template<int point_field_type, typename T>
00134 inline void writePointCloud2BufferValue(unsigned char* data_ptr, T value){
00135 typedef typename pointFieldTypeAsType<point_field_type>::type type;
00136 *(reinterpret_cast<type*>(data_ptr)) = static_cast<type>(value);
00137 }
00138
00147 template<typename T>
00148 inline void writePointCloud2BufferValue(unsigned char* data_ptr, const unsigned char datatype, T value){
00149 switch(datatype){
00150 case sensor_msgs::PointField::INT8:
00151 writePointCloud2BufferValue<sensor_msgs::PointField::INT8, T>(data_ptr, value);
00152 break;
00153 case sensor_msgs::PointField::UINT8:
00154 writePointCloud2BufferValue<sensor_msgs::PointField::UINT8, T>(data_ptr, value);
00155 break;
00156 case sensor_msgs::PointField::INT16:
00157 writePointCloud2BufferValue<sensor_msgs::PointField::INT16, T>(data_ptr, value);
00158 break;
00159 case sensor_msgs::PointField::UINT16:
00160 writePointCloud2BufferValue<sensor_msgs::PointField::UINT16, T>(data_ptr, value);
00161 break;
00162 case sensor_msgs::PointField::INT32:
00163 writePointCloud2BufferValue<sensor_msgs::PointField::INT32, T>(data_ptr, value);
00164 break;
00165 case sensor_msgs::PointField::UINT32:
00166 writePointCloud2BufferValue<sensor_msgs::PointField::UINT32, T>(data_ptr, value);
00167 break;
00168 case sensor_msgs::PointField::FLOAT32:
00169 writePointCloud2BufferValue<sensor_msgs::PointField::FLOAT32, T>(data_ptr, value);
00170 break;
00171 case sensor_msgs::PointField::FLOAT64:
00172 writePointCloud2BufferValue<sensor_msgs::PointField::FLOAT64, T>(data_ptr, value);
00173 break;
00174 }
00175 }
00176 }
00177
00178 #endif