|
43 | 43 | * a PointCloud2 buffer for the different PointField types. |
44 | 44 | * \author Sebastian Pütz |
45 | 45 | */ |
46 | | -namespace sensor_msgs{ |
47 | | - /*! |
48 | | - * \Enum to type mapping. |
49 | | - */ |
50 | | - template<int> struct pointFieldTypeAsType {}; |
51 | | - template<> struct pointFieldTypeAsType<sensor_msgs::PointField::INT8> { typedef int8_t type; }; |
52 | | - template<> struct pointFieldTypeAsType<sensor_msgs::PointField::UINT8> { typedef uint8_t type; }; |
53 | | - template<> struct pointFieldTypeAsType<sensor_msgs::PointField::INT16> { typedef int16_t type; }; |
54 | | - template<> struct pointFieldTypeAsType<sensor_msgs::PointField::UINT16> { typedef uint16_t type; }; |
55 | | - template<> struct pointFieldTypeAsType<sensor_msgs::PointField::INT32> { typedef int32_t type; }; |
56 | | - template<> struct pointFieldTypeAsType<sensor_msgs::PointField::UINT32> { typedef uint32_t type; }; |
57 | | - template<> struct pointFieldTypeAsType<sensor_msgs::PointField::FLOAT32> { typedef float type; }; |
58 | | - template<> struct pointFieldTypeAsType<sensor_msgs::PointField::FLOAT64> { typedef double type; }; |
| 46 | +namespace sensor_msgs |
| 47 | +{ |
| 48 | +/*! |
| 49 | + * \Enum to type mapping. |
| 50 | + */ |
| 51 | +template<int> |
| 52 | +struct pointFieldTypeAsType {}; |
| 53 | +template<> |
| 54 | +struct pointFieldTypeAsType<sensor_msgs::PointField::INT8>{ typedef int8_t type; }; |
| 55 | +template<> |
| 56 | +struct pointFieldTypeAsType<sensor_msgs::PointField::UINT8>{ typedef uint8_t type; }; |
| 57 | +template<> |
| 58 | +struct pointFieldTypeAsType<sensor_msgs::PointField::INT16>{ typedef int16_t type; }; |
| 59 | +template<> |
| 60 | +struct pointFieldTypeAsType<sensor_msgs::PointField::UINT16>{ typedef uint16_t type; }; |
| 61 | +template<> |
| 62 | +struct pointFieldTypeAsType<sensor_msgs::PointField::INT32>{ typedef int32_t type; }; |
| 63 | +template<> |
| 64 | +struct pointFieldTypeAsType<sensor_msgs::PointField::UINT32>{ typedef uint32_t type; }; |
| 65 | +template<> |
| 66 | +struct pointFieldTypeAsType<sensor_msgs::PointField::FLOAT32>{ typedef float type; }; |
| 67 | +template<> |
| 68 | +struct pointFieldTypeAsType<sensor_msgs::PointField::FLOAT64>{ typedef double type; }; |
59 | 69 |
|
60 | | - /*! |
61 | | - * \Type to enum mapping. |
62 | | - */ |
63 | | - template<typename T> struct typeAsPointFieldType {}; |
64 | | - template<> struct typeAsPointFieldType<int8_t> { static const uint8_t value = sensor_msgs::PointField::INT8; }; |
65 | | - template<> struct typeAsPointFieldType<uint8_t> { static const uint8_t value = sensor_msgs::PointField::UINT8; }; |
66 | | - template<> struct typeAsPointFieldType<int16_t> { static const uint8_t value = sensor_msgs::PointField::INT16; }; |
67 | | - template<> struct typeAsPointFieldType<uint16_t> { static const uint8_t value = sensor_msgs::PointField::UINT16; }; |
68 | | - template<> struct typeAsPointFieldType<int32_t> { static const uint8_t value = sensor_msgs::PointField::INT32; }; |
69 | | - template<> struct typeAsPointFieldType<uint32_t> { static const uint8_t value = sensor_msgs::PointField::UINT32; }; |
70 | | - template<> struct typeAsPointFieldType<float> { static const uint8_t value = sensor_msgs::PointField::FLOAT32; }; |
71 | | - template<> struct typeAsPointFieldType<double> { static const uint8_t value = sensor_msgs::PointField::FLOAT64; }; |
| 70 | +/*! |
| 71 | + * \Type to enum mapping. |
| 72 | + */ |
| 73 | +template<typename T> |
| 74 | +struct typeAsPointFieldType {}; |
| 75 | +template<> |
| 76 | +struct typeAsPointFieldType<int8_t>{ static const uint8_t value = sensor_msgs::PointField::INT8; }; |
| 77 | +template<> |
| 78 | +struct typeAsPointFieldType<uint8_t>{ static const uint8_t value = sensor_msgs::PointField::UINT8; |
| 79 | +}; |
| 80 | +template<> |
| 81 | +struct typeAsPointFieldType<int16_t>{ static const uint8_t value = sensor_msgs::PointField::INT16; |
| 82 | +}; |
| 83 | +template<> |
| 84 | +struct typeAsPointFieldType<uint16_t>{ static const uint8_t value = sensor_msgs::PointField::UINT16; |
| 85 | +}; |
| 86 | +template<> |
| 87 | +struct typeAsPointFieldType<int32_t>{ static const uint8_t value = sensor_msgs::PointField::INT32; |
| 88 | +}; |
| 89 | +template<> |
| 90 | +struct typeAsPointFieldType<uint32_t>{ static const uint8_t value = sensor_msgs::PointField::UINT32; |
| 91 | +}; |
| 92 | +template<> |
| 93 | +struct typeAsPointFieldType<float>{ static const uint8_t value = sensor_msgs::PointField::FLOAT32; |
| 94 | +}; |
| 95 | +template<> |
| 96 | +struct typeAsPointFieldType<double>{ static const uint8_t value = sensor_msgs::PointField::FLOAT64; |
| 97 | +}; |
72 | 98 |
|
73 | | - /*! |
74 | | - * \Converts a value at the given pointer position, interpreted as the datatype |
75 | | - * specified by the given template argument point_field_type, to the given |
76 | | - * template type T and returns it. |
77 | | - * \param data_ptr pointer into the point cloud 2 buffer |
78 | | - * \tparam point_field_type sensor_msgs::PointField datatype value |
79 | | - * \tparam T return type |
80 | | - */ |
81 | | - template<int point_field_type, typename T> |
82 | | - inline T readPointCloud2BufferValue(const unsigned char* data_ptr){ |
83 | | - typedef typename pointFieldTypeAsType<point_field_type>::type type; |
84 | | - return static_cast<T>(*(reinterpret_cast<type const *>(data_ptr))); |
85 | | - } |
| 99 | +/*! |
| 100 | + * \Converts a value at the given pointer position, interpreted as the datatype |
| 101 | + * specified by the given template argument point_field_type, to the given |
| 102 | + * template type T and returns it. |
| 103 | + * \param data_ptr pointer into the point cloud 2 buffer |
| 104 | + * \tparam point_field_type sensor_msgs::PointField datatype value |
| 105 | + * \tparam T return type |
| 106 | + */ |
| 107 | +template<int point_field_type, typename T> |
| 108 | +inline T readPointCloud2BufferValue(const unsigned char * data_ptr) |
| 109 | +{ |
| 110 | + typedef typename pointFieldTypeAsType<point_field_type>::type type; |
| 111 | + return static_cast<T>(*(reinterpret_cast<type const *>(data_ptr))); |
| 112 | +} |
86 | 113 |
|
87 | | - /*! |
88 | | - * \Converts a value at the given pointer position interpreted as the datatype |
89 | | - * specified by the given datatype parameter to the given template type and returns it. |
90 | | - * \param data_ptr pointer into the point cloud 2 buffer |
91 | | - * \param datatype sensor_msgs::PointField datatype value |
92 | | - * \tparam T return type |
93 | | - */ |
94 | | - template<typename T> |
95 | | - inline T readPointCloud2BufferValue(const unsigned char* data_ptr, const unsigned char datatype){ |
96 | | - switch(datatype){ |
97 | | - case sensor_msgs::PointField::INT8: |
98 | | - return readPointCloud2BufferValue<sensor_msgs::PointField::INT8, T>(data_ptr); |
99 | | - case sensor_msgs::PointField::UINT8: |
100 | | - return readPointCloud2BufferValue<sensor_msgs::PointField::UINT8, T>(data_ptr); |
101 | | - case sensor_msgs::PointField::INT16: |
102 | | - return readPointCloud2BufferValue<sensor_msgs::PointField::INT16, T>(data_ptr); |
103 | | - case sensor_msgs::PointField::UINT16: |
104 | | - return readPointCloud2BufferValue<sensor_msgs::PointField::UINT16, T>(data_ptr); |
105 | | - case sensor_msgs::PointField::INT32: |
106 | | - return readPointCloud2BufferValue<sensor_msgs::PointField::INT32, T>(data_ptr); |
107 | | - case sensor_msgs::PointField::UINT32: |
108 | | - return readPointCloud2BufferValue<sensor_msgs::PointField::UINT32, T>(data_ptr); |
109 | | - case sensor_msgs::PointField::FLOAT32: |
110 | | - return readPointCloud2BufferValue<sensor_msgs::PointField::FLOAT32, T>(data_ptr); |
111 | | - case sensor_msgs::PointField::FLOAT64: |
112 | | - return readPointCloud2BufferValue<sensor_msgs::PointField::FLOAT64, T>(data_ptr); |
113 | | - } |
114 | | - // This should never be reached, but return statement added to avoid compiler warning. (#84) |
115 | | - return T(); |
116 | | - } |
| 114 | +/*! |
| 115 | + * \Converts a value at the given pointer position interpreted as the datatype |
| 116 | + * specified by the given datatype parameter to the given template type and returns it. |
| 117 | + * \param data_ptr pointer into the point cloud 2 buffer |
| 118 | + * \param datatype sensor_msgs::PointField datatype value |
| 119 | + * \tparam T return type |
| 120 | + */ |
| 121 | +template<typename T> |
| 122 | +inline T readPointCloud2BufferValue(const unsigned char * data_ptr, const unsigned char datatype) |
| 123 | +{ |
| 124 | + switch (datatype) { |
| 125 | + case sensor_msgs::PointField::INT8: |
| 126 | + return readPointCloud2BufferValue<sensor_msgs::PointField::INT8, T>(data_ptr); |
| 127 | + case sensor_msgs::PointField::UINT8: |
| 128 | + return readPointCloud2BufferValue<sensor_msgs::PointField::UINT8, T>(data_ptr); |
| 129 | + case sensor_msgs::PointField::INT16: |
| 130 | + return readPointCloud2BufferValue<sensor_msgs::PointField::INT16, T>(data_ptr); |
| 131 | + case sensor_msgs::PointField::UINT16: |
| 132 | + return readPointCloud2BufferValue<sensor_msgs::PointField::UINT16, T>(data_ptr); |
| 133 | + case sensor_msgs::PointField::INT32: |
| 134 | + return readPointCloud2BufferValue<sensor_msgs::PointField::INT32, T>(data_ptr); |
| 135 | + case sensor_msgs::PointField::UINT32: |
| 136 | + return readPointCloud2BufferValue<sensor_msgs::PointField::UINT32, T>(data_ptr); |
| 137 | + case sensor_msgs::PointField::FLOAT32: |
| 138 | + return readPointCloud2BufferValue<sensor_msgs::PointField::FLOAT32, T>(data_ptr); |
| 139 | + case sensor_msgs::PointField::FLOAT64: |
| 140 | + return readPointCloud2BufferValue<sensor_msgs::PointField::FLOAT64, T>(data_ptr); |
| 141 | + } |
| 142 | + // This should never be reached, but return statement added to avoid compiler warning. (#84) |
| 143 | + return T(); |
| 144 | +} |
117 | 145 |
|
118 | | - /*! |
119 | | - * \Inserts a given value at the given point position interpreted as the datatype |
120 | | - * specified by the template argument point_field_type. |
121 | | - * \param data_ptr pointer into the point cloud 2 buffer |
122 | | - * \param value the value to insert |
123 | | - * \tparam point_field_type sensor_msgs::PointField datatype value |
124 | | - * \tparam T type of the value to insert |
125 | | - */ |
126 | | - template<int point_field_type, typename T> |
127 | | - inline void writePointCloud2BufferValue(unsigned char* data_ptr, T value){ |
128 | | - typedef typename pointFieldTypeAsType<point_field_type>::type type; |
129 | | - *(reinterpret_cast<type*>(data_ptr)) = static_cast<type>(value); |
130 | | - } |
| 146 | +/*! |
| 147 | + * \Inserts a given value at the given point position interpreted as the datatype |
| 148 | + * specified by the template argument point_field_type. |
| 149 | + * \param data_ptr pointer into the point cloud 2 buffer |
| 150 | + * \param value the value to insert |
| 151 | + * \tparam point_field_type sensor_msgs::PointField datatype value |
| 152 | + * \tparam T type of the value to insert |
| 153 | + */ |
| 154 | +template<int point_field_type, typename T> |
| 155 | +inline void writePointCloud2BufferValue(unsigned char * data_ptr, T value) |
| 156 | +{ |
| 157 | + typedef typename pointFieldTypeAsType<point_field_type>::type type; |
| 158 | + *(reinterpret_cast<type *>(data_ptr)) = static_cast<type>(value); |
| 159 | +} |
131 | 160 |
|
132 | | - /*! |
133 | | - * \Inserts a given value at the given point position interpreted as the datatype |
134 | | - * specified by the given datatype parameter. |
135 | | - * \param data_ptr pointer into the point cloud 2 buffer |
136 | | - * \param datatype sensor_msgs::PointField datatype value |
137 | | - * \param value the value to insert |
138 | | - * \tparam T type of the value to insert |
139 | | - */ |
140 | | - template<typename T> |
141 | | - inline void writePointCloud2BufferValue(unsigned char* data_ptr, const unsigned char datatype, T value){ |
142 | | - switch(datatype){ |
143 | | - case sensor_msgs::PointField::INT8: |
144 | | - writePointCloud2BufferValue<sensor_msgs::PointField::INT8, T>(data_ptr, value); |
145 | | - break; |
146 | | - case sensor_msgs::PointField::UINT8: |
147 | | - writePointCloud2BufferValue<sensor_msgs::PointField::UINT8, T>(data_ptr, value); |
148 | | - break; |
149 | | - case sensor_msgs::PointField::INT16: |
150 | | - writePointCloud2BufferValue<sensor_msgs::PointField::INT16, T>(data_ptr, value); |
151 | | - break; |
152 | | - case sensor_msgs::PointField::UINT16: |
153 | | - writePointCloud2BufferValue<sensor_msgs::PointField::UINT16, T>(data_ptr, value); |
154 | | - break; |
155 | | - case sensor_msgs::PointField::INT32: |
156 | | - writePointCloud2BufferValue<sensor_msgs::PointField::INT32, T>(data_ptr, value); |
157 | | - break; |
158 | | - case sensor_msgs::PointField::UINT32: |
159 | | - writePointCloud2BufferValue<sensor_msgs::PointField::UINT32, T>(data_ptr, value); |
160 | | - break; |
161 | | - case sensor_msgs::PointField::FLOAT32: |
162 | | - writePointCloud2BufferValue<sensor_msgs::PointField::FLOAT32, T>(data_ptr, value); |
163 | | - break; |
164 | | - case sensor_msgs::PointField::FLOAT64: |
165 | | - writePointCloud2BufferValue<sensor_msgs::PointField::FLOAT64, T>(data_ptr, value); |
166 | | - break; |
167 | | - } |
168 | | - } |
| 161 | +/*! |
| 162 | + * \Inserts a given value at the given point position interpreted as the datatype |
| 163 | + * specified by the given datatype parameter. |
| 164 | + * \param data_ptr pointer into the point cloud 2 buffer |
| 165 | + * \param datatype sensor_msgs::PointField datatype value |
| 166 | + * \param value the value to insert |
| 167 | + * \tparam T type of the value to insert |
| 168 | + */ |
| 169 | +template<typename T> |
| 170 | +inline void writePointCloud2BufferValue(unsigned char * data_ptr, const unsigned char datatype, |
| 171 | + T value) |
| 172 | +{ |
| 173 | + switch (datatype) { |
| 174 | + case sensor_msgs::PointField::INT8: |
| 175 | + writePointCloud2BufferValue<sensor_msgs::PointField::INT8, T>(data_ptr, value); |
| 176 | + break; |
| 177 | + case sensor_msgs::PointField::UINT8: |
| 178 | + writePointCloud2BufferValue<sensor_msgs::PointField::UINT8, T>(data_ptr, value); |
| 179 | + break; |
| 180 | + case sensor_msgs::PointField::INT16: |
| 181 | + writePointCloud2BufferValue<sensor_msgs::PointField::INT16, T>(data_ptr, value); |
| 182 | + break; |
| 183 | + case sensor_msgs::PointField::UINT16: |
| 184 | + writePointCloud2BufferValue<sensor_msgs::PointField::UINT16, T>(data_ptr, value); |
| 185 | + break; |
| 186 | + case sensor_msgs::PointField::INT32: |
| 187 | + writePointCloud2BufferValue<sensor_msgs::PointField::INT32, T>(data_ptr, value); |
| 188 | + break; |
| 189 | + case sensor_msgs::PointField::UINT32: |
| 190 | + writePointCloud2BufferValue<sensor_msgs::PointField::UINT32, T>(data_ptr, value); |
| 191 | + break; |
| 192 | + case sensor_msgs::PointField::FLOAT32: |
| 193 | + writePointCloud2BufferValue<sensor_msgs::PointField::FLOAT32, T>(data_ptr, value); |
| 194 | + break; |
| 195 | + case sensor_msgs::PointField::FLOAT64: |
| 196 | + writePointCloud2BufferValue<sensor_msgs::PointField::FLOAT64, T>(data_ptr, value); |
| 197 | + break; |
| 198 | + } |
169 | 199 | } |
| 200 | +} // namespace sensor_msgs |
170 | 201 | #endif // SENSOR_MSGS__POINT_FIELD_CONVERSION_HPP_ |
171 | | - |
|
0 commit comments