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 #ifndef POINT_CLOUD_TRANSFORMERS_H
00031 #define POINT_CLOUD_TRANSFORMERS_H
00032
00033 #include <sensor_msgs/PointCloud2.h>
00034
00035 #include "point_cloud_transformer.h"
00036
00037 namespace rviz
00038 {
00039
00040 class BoolProperty;
00041 class ColorProperty;
00042 class EditableEnumProperty;
00043 class EnumProperty;
00044 class FloatProperty;
00045
00046 typedef std::vector<std::string> V_string;
00047
00048 inline int32_t findChannelIndex(const sensor_msgs::PointCloud2ConstPtr& cloud, const std::string& channel)
00049 {
00050 for (size_t i = 0; i < cloud->fields.size(); ++i)
00051 {
00052 if (cloud->fields[i].name == channel)
00053 {
00054 return i;
00055 }
00056 }
00057
00058 return -1;
00059 }
00060
00061 template<typename T>
00062 inline T valueFromCloud(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t offset, uint8_t type, uint32_t point_step, uint32_t index)
00063 {
00064 const uint8_t* data = &cloud->data[(point_step * index) + offset];
00065 T ret = 0;
00066
00067 switch (type)
00068 {
00069 case sensor_msgs::PointField::INT8:
00070 case sensor_msgs::PointField::UINT8:
00071 {
00072 uint8_t val = *reinterpret_cast<const uint8_t*>(data);
00073 ret = static_cast<T>(val);
00074 break;
00075 }
00076
00077 case sensor_msgs::PointField::INT16:
00078 case sensor_msgs::PointField::UINT16:
00079 {
00080 uint16_t val = *reinterpret_cast<const uint16_t*>(data);
00081 ret = static_cast<T>(val);
00082 break;
00083 }
00084
00085 case sensor_msgs::PointField::INT32:
00086 case sensor_msgs::PointField::UINT32:
00087 {
00088 uint32_t val = *reinterpret_cast<const uint32_t*>(data);
00089 ret = static_cast<T>(val);
00090 break;
00091 }
00092
00093 case sensor_msgs::PointField::FLOAT32:
00094 {
00095 float val = *reinterpret_cast<const float*>(data);
00096 ret = static_cast<T>(val);
00097 break;
00098 }
00099
00100 case sensor_msgs::PointField::FLOAT64:
00101 {
00102 double val = *reinterpret_cast<const double*>(data);
00103 ret = static_cast<T>(val);
00104 break;
00105 }
00106 default:
00107 break;
00108 }
00109
00110 return ret;
00111 }
00112
00113 class IntensityPCTransformer : public PointCloudTransformer
00114 {
00115 Q_OBJECT
00116 public:
00117 virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00118 virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud,
00119 uint32_t mask,
00120 const Ogre::Matrix4& transform,
00121 V_PointCloudPoint& points_out);
00122 virtual uint8_t score(const sensor_msgs::PointCloud2ConstPtr& cloud);
00123 virtual void createProperties( Property* parent_property, uint32_t mask, QList<Property*>& out_props );
00124 void updateChannels(const sensor_msgs::PointCloud2ConstPtr& cloud);
00125
00126 private Q_SLOTS:
00127 void updateUseRainbow();
00128 void updateAutoComputeIntensityBounds();
00129
00130 private:
00131 V_string available_channels_;
00132
00133 ColorProperty* min_color_property_;
00134 ColorProperty* max_color_property_;
00135 BoolProperty* auto_compute_intensity_bounds_property_;
00136 BoolProperty* use_rainbow_property_;
00137 BoolProperty* invert_rainbow_property_;
00138 FloatProperty* min_intensity_property_;
00139 FloatProperty* max_intensity_property_;
00140 EditableEnumProperty* channel_name_property_;
00141 };
00142
00143 class XYZPCTransformer : public PointCloudTransformer
00144 {
00145 Q_OBJECT
00146 public:
00147 virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00148 virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, V_PointCloudPoint& points_out);
00149 };
00150
00151
00152
00153 class RGB8PCTransformer : public PointCloudTransformer
00154 {
00155 Q_OBJECT
00156 public:
00157 virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00158 virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, V_PointCloudPoint& points_out);
00159 };
00160
00161
00162
00163 class RGBF32PCTransformer : public PointCloudTransformer
00164 {
00165 Q_OBJECT
00166 public:
00167 virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00168 virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, V_PointCloudPoint& points_out);
00169 };
00170
00171
00172
00173 class FlatColorPCTransformer : public PointCloudTransformer
00174 {
00175 Q_OBJECT
00176 public:
00177 virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00178 virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, V_PointCloudPoint& points_out);
00179 virtual void createProperties( Property* parent_property, uint32_t mask, QList<Property*>& out_props );
00180 virtual uint8_t score(const sensor_msgs::PointCloud2ConstPtr& cloud);
00181
00182 private:
00183 ColorProperty* color_property_;
00184 };
00185
00186 class AxisColorPCTransformer : public PointCloudTransformer
00187 {
00188 Q_OBJECT
00189 public:
00190 virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00191 virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, V_PointCloudPoint& points_out);
00192 virtual void createProperties( Property* parent_property, uint32_t mask, QList<Property*>& out_props );
00193 virtual uint8_t score(const sensor_msgs::PointCloud2ConstPtr& cloud);
00194
00195 enum Axis
00196 {
00197 AXIS_X,
00198 AXIS_Y,
00199 AXIS_Z
00200 };
00201
00202 private Q_SLOTS:
00203 void updateAutoComputeBounds();
00204
00205 private:
00206 BoolProperty* auto_compute_bounds_property_;
00207 FloatProperty* min_value_property_;
00208 FloatProperty* max_value_property_;
00209 EnumProperty* axis_property_;
00210 BoolProperty* use_fixed_frame_property_;
00211 };
00212
00213 }
00214
00215 #endif // POINT_CLOUD_TRANSFORMERS_H