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
00045 #ifndef PCL_REGISTER_POINT_STRUCT_H_
00046 #define PCL_REGISTER_POINT_STRUCT_H_
00047
00048 #include "pcl/ros/point_traits.h"
00049
00050 #include <boost/mpl/vector.hpp>
00051 #include <boost/mpl/for_each.hpp>
00052 #include <boost/mpl/assert.hpp>
00053 #include <boost/preprocessor/seq/enum.hpp>
00054 #include <boost/preprocessor/seq/for_each.hpp>
00055 #include <boost/preprocessor/seq/transform.hpp>
00056 #include <boost/preprocessor/cat.hpp>
00057 #include <boost/preprocessor/repetition/repeat_from_to.hpp>
00058 #include <boost/type_traits/is_pod.hpp>
00059 #include <stddef.h>
00060
00061
00062 #define POINT_CLOUD_REGISTER_POINT_STRUCT(name, fseq) \
00063 POINT_CLOUD_REGISTER_POINT_STRUCT_I(name, \
00064 BOOST_PP_CAT(POINT_CLOUD_REGISTER_POINT_STRUCT_X fseq, 0)) \
00065
00066
00067 #define POINT_CLOUD_REGISTER_POINT_WRAPPER(wrapper, pod) \
00068 BOOST_MPL_ASSERT_MSG(sizeof(wrapper) == sizeof(pod), POINT_WRAPPER_AND_POD_TYPES_HAVE_DIFFERENT_SIZES, (wrapper, pod)); \
00069 namespace pcl { \
00070 namespace traits { \
00071 template<> struct POD<wrapper> { typedef pod type; }; \
00072 } \
00073 } \
00074
00075
00076
00077
00078 #define POINT_CLOUD_REGISTER_POINT_STRUCT_X(type, name, tag) \
00079 ((type, name, tag)) POINT_CLOUD_REGISTER_POINT_STRUCT_Y
00080 #define POINT_CLOUD_REGISTER_POINT_STRUCT_Y(type, name, tag) \
00081 ((type, name, tag)) POINT_CLOUD_REGISTER_POINT_STRUCT_X
00082 #define POINT_CLOUD_REGISTER_POINT_STRUCT_X0
00083 #define POINT_CLOUD_REGISTER_POINT_STRUCT_Y0
00084
00085
00086
00087
00088 #define POINT_CLOUD_REGISTER_POINT_STRUCT_I(name, seq) \
00089 namespace pcl \
00090 { \
00091 namespace fields \
00092 { \
00093 BOOST_PP_SEQ_FOR_EACH(POINT_CLOUD_REGISTER_FIELD_TAG, name, seq) \
00094 } \
00095 namespace traits \
00096 { \
00097 BOOST_PP_SEQ_FOR_EACH(POINT_CLOUD_REGISTER_FIELD_NAME, name, seq) \
00098 BOOST_PP_SEQ_FOR_EACH(POINT_CLOUD_REGISTER_FIELD_OFFSET, name, seq) \
00099 BOOST_PP_SEQ_FOR_EACH(POINT_CLOUD_REGISTER_FIELD_DATATYPE, name, seq) \
00100 POINT_CLOUD_REGISTER_POINT_FIELD_LIST(name, POINT_CLOUD_EXTRACT_TAGS(seq)) \
00101 } \
00102 } \
00103
00104
00105 #define POINT_CLOUD_REGISTER_FIELD_TAG(r, name, elem) \
00106 struct BOOST_PP_TUPLE_ELEM(3, 2, elem); \
00107
00108
00109 #define POINT_CLOUD_REGISTER_FIELD_NAME(r, point, elem) \
00110 template<int dummy> \
00111 struct name<point, pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem), dummy> \
00112 { \
00113 static const char value[]; \
00114 }; \
00115 \
00116 template<int dummy> \
00117 const char name<point, \
00118 pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem), \
00119 dummy>::value[] = \
00120 BOOST_PP_STRINGIZE(BOOST_PP_TUPLE_ELEM(3, 2, elem)); \
00121
00122
00123 #define POINT_CLOUD_REGISTER_FIELD_OFFSET(r, name, elem) \
00124 template<> struct offset<name, pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem)> \
00125 { \
00126 static const size_t value = offsetof(name, BOOST_PP_TUPLE_ELEM(3, 1, elem)); \
00127 }; \
00128
00129
00130
00131
00132
00133
00134 #define POINT_CLOUD_REGISTER_FIELD_DATATYPE(r, name, elem) \
00135 template<> struct datatype<name, pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem)> \
00136 { \
00137 typedef boost::mpl::identity<BOOST_PP_TUPLE_ELEM(3, 0, elem)>::type type; \
00138 typedef decomposeArray<type> decomposed; \
00139 static const uint8_t value = asEnum<decomposed::type>::value; \
00140 static const uint32_t size = decomposed::value; \
00141 }; \
00142
00143
00144 #define POINT_CLOUD_TAG_OP(s, data, elem) pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem)
00145
00146 #define POINT_CLOUD_EXTRACT_TAGS(seq) BOOST_PP_SEQ_TRANSFORM(POINT_CLOUD_TAG_OP, _, seq)
00147
00148 #define POINT_CLOUD_REGISTER_POINT_FIELD_LIST(name, seq) \
00149 template<> struct fieldList<name> \
00150 { \
00151 typedef boost::mpl::vector<BOOST_PP_SEQ_ENUM(seq)> type; \
00152 }; \
00153
00154
00155
00156 #if 0
00157 #define POINT_CLOUD_EXPAND_TAG_OP(s, data, elem) \
00158 (boost::mpl::identity<BOOST_PP_TUPLE_ELEM(3, 0, elem)>::type, \
00159 BOOST_PP_TUPLE_ELEM(3, 1, elem), \
00160 pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem)) \
00161
00162
00163 #define POINT_CLOUD_EXPAND_TAGS(seq) BOOST_PP_SEQ_TRANSFORM(POINT_CLOUD_EXPAND_TAG_OP, _, seq)
00164
00165 #define POINT_CLOUD_REGISTER_WITH_FUSION(name, seq) \
00166 BOOST_FUSION_ADAPT_ASSOC_STRUCT_I(name, POINT_CLOUD_EXPAND_TAGS(seq)) \
00167
00168 #endif
00169
00170 #endif //#ifndef PCL_REGISTER_POINT_STRUCT_H_