rviz: rviz: point_cloud_transformers.h Source File

00001 /*
00002  * Copyright (c) 2010, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #ifndef POINT_CLOUD_TRANSFORMERS_H
00031 #define POINT_CLOUD_TRANSFORMERS_H
00032 
00033 #include "point_cloud_transformer.h"
00034 #include <rviz/helpers/color.h>
00035 #include <sensor_msgs/PointCloud2.h>
00036 
00037 namespace rviz
00038 {
00039 
00040 typedef std::vector<std::string> V_string; 
00041 
00042 inline int32_t findChannelIndex(const sensor_msgs::PointCloud2ConstPtr& cloud, const std::string& channel)
00043 {
00044   for (size_t i = 0; i < cloud->fields.size(); ++i)
00045   {
00046     if (cloud->fields[i].name == channel)
00047     {
00048       return i;
00049     }
00050   }
00051 
00052   return -1;
00053 }
00054 
00055 template<typename T>
00056 inline T valueFromCloud(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t offset, uint8_t type, uint32_t point_step, uint32_t index)
00057 {
00058   const uint8_t* data = &cloud->data[(point_step * index) + offset];
00059   T ret = 0;
00060 
00061   switch (type)
00062   {
00063   case sensor_msgs::PointField::INT8:
00064   case sensor_msgs::PointField::UINT8:
00065     {
00066       uint8_t val = *reinterpret_cast<const uint8_t*>(data);
00067       ret = static_cast<T>(val);
00068       break;
00069     }
00070 
00071   case sensor_msgs::PointField::INT16:
00072   case sensor_msgs::PointField::UINT16:
00073     {
00074       uint16_t val = *reinterpret_cast<const uint16_t*>(data);
00075       ret = static_cast<T>(val);
00076       break;
00077     }
00078 
00079   case sensor_msgs::PointField::INT32:
00080   case sensor_msgs::PointField::UINT32:
00081     {
00082       uint32_t val = *reinterpret_cast<const uint32_t*>(data);
00083       ret = static_cast<T>(val);
00084       break;
00085     }
00086 
00087   case sensor_msgs::PointField::FLOAT32:
00088     {
00089       float val = *reinterpret_cast<const float*>(data);
00090       ret = static_cast<T>(val);
00091       break;
00092     }
00093 
00094   case sensor_msgs::PointField::FLOAT64:
00095     {
00096       double val = *reinterpret_cast<const double*>(data);
00097       ret = static_cast<T>(val);
00098       break;
00099     }
00100   default:
00101     break;
00102   }
00103 
00104   return ret;
00105 }
00106 
00107 class IntensityPCTransformer : public PointCloudTransformer
00108 {
00109 public:
00110   IntensityPCTransformer()
00111   : min_color_( 0.0f, 0.0f, 0.0f )
00112   , max_color_( 1.0f, 1.0f, 1.0f )
00113   , min_intensity_(0.0f)
00114   , max_intensity_(4096.0f)
00115   , use_full_rgb_colors_(false) 
00116   , selected_channel_("intensity") 
00117   {
00118     setAutoComputeIntensityBounds(true);
00119   }
00120 
00121   virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00122   virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out);
00123   virtual uint8_t score(const sensor_msgs::PointCloud2ConstPtr& cloud);
00124   virtual void reset();
00125   virtual void createProperties(PropertyManager* property_man, const CategoryPropertyWPtr& parent, const std::string& prefix, uint32_t mask, V_PropertyBaseWPtr& out_props);
00126 
00127   void setMinColor( const Color& color );
00128   void setMaxColor( const Color& color );
00129   const Color& getMaxColor() { return max_color_; }
00130   const Color& getMinColor() { return min_color_; }
00131   void setMinIntensity(float val);
00132   void setMaxIntensity(float val);
00133   float getMinIntensity() { return min_intensity_; }
00134   float getMaxIntensity() { return max_intensity_; }
00135   void setAutoComputeIntensityBounds(bool compute);
00136   bool getAutoComputeIntensityBounds() { return auto_compute_intensity_bounds_; }
00137   void setUseFullRGBColors(bool full_rgb); 
00138   bool getUseFullRGBColors() { return use_full_rgb_colors_; } 
00139   const std::string& getChannelName() { return selected_channel_; } 
00140   void setChannelName(const std::string& channel); 
00141   void updateChannels(const sensor_msgs::PointCloud2ConstPtr& cloud); 
00142 
00143 private:
00144   Color min_color_;
00145   Color max_color_;
00146   float min_intensity_;
00147   float max_intensity_;
00148   bool auto_compute_intensity_bounds_;
00149   bool use_full_rgb_colors_;
00150   bool intensity_bounds_changed_;
00151   std::string selected_channel_;
00152   V_string available_channels_;
00153 
00154   ColorPropertyWPtr min_color_property_;
00155   ColorPropertyWPtr max_color_property_;
00156   BoolPropertyWPtr auto_compute_intensity_bounds_property_;
00157   BoolPropertyWPtr use_full_rgb_colors_property_;
00158   FloatPropertyWPtr min_intensity_property_;
00159   FloatPropertyWPtr max_intensity_property_;
00160   EditEnumPropertyWPtr channel_name_property_;
00161 
00162   RetransformFunc retransform_func_;
00163 };
00164 
00165 class XYZPCTransformer : public PointCloudTransformer
00166 {
00167 public:
00168   XYZPCTransformer()
00169   {}
00170 
00171   virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00172   virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out);
00173 };
00174 
00175 
00176 
00177 class RGB8PCTransformer : public PointCloudTransformer
00178 {
00179 public:
00180   virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00181   virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out);
00182 };
00183 
00184 
00185 
00186 class RGBF32PCTransformer : public PointCloudTransformer
00187 {
00188 public:
00189   virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00190   virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out);
00191 };
00192 
00193 
00194 
00195 class FlatColorPCTransformer : public PointCloudTransformer
00196 {
00197 public:
00198   FlatColorPCTransformer()
00199   : color_(1.0, 1.0, 1.0)
00200   {}
00201 
00202   virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00203   virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out);
00204   virtual void createProperties(PropertyManager* property_man, const CategoryPropertyWPtr& parent, const std::string& prefix, uint32_t mask, V_PropertyBaseWPtr& out_props);
00205   virtual uint8_t score(const sensor_msgs::PointCloud2ConstPtr& cloud);
00206 
00207   void setColor(const Color& color);
00208   const Color& getColor() { return color_; }
00209 
00210 private:
00211   Color color_;
00212   ColorPropertyWPtr color_property_;
00213 };
00214 
00215 class AxisColorPCTransformer : public PointCloudTransformer
00216 {
00217 public:
00218   AxisColorPCTransformer()
00219     : min_value_(-10.0f)
00220     , max_value_(10.0f)
00221     , use_fixed_frame_(true)
00222     , axis_(AXIS_Z)
00223     {
00224       setAutoComputeBounds(true);
00225     }
00226 
00227   virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00228   virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out);
00229   virtual void createProperties(PropertyManager* property_man, const CategoryPropertyWPtr& parent, const std::string& prefix, uint32_t mask, V_PropertyBaseWPtr& out_props);
00230   virtual uint8_t score(const sensor_msgs::PointCloud2ConstPtr& cloud);
00231 
00232   void setMinValue(float val);
00233   void setMaxValue(float val);
00234   float getMinValue() { return min_value_; }
00235   float getMaxValue() { return max_value_; }
00236   void setAutoComputeBounds(bool compute);
00237   bool getAutoComputeBounds() { return auto_compute_bounds_; }
00238 
00239   void setUseFixedFrame(bool use);
00240   bool getUseFixedFrame() { return use_fixed_frame_; }
00241 
00242   enum Axis
00243   {
00244     AXIS_X,
00245     AXIS_Y,
00246     AXIS_Z
00247   };
00248 
00249   void setAxis(int axis);
00250   int getAxis() { return axis_; }
00251 
00252 private:
00253 
00254   float min_value_;
00255   float max_value_;
00256 
00257   bool auto_compute_bounds_;
00258   bool use_fixed_frame_;
00259 
00260   int axis_;
00261 
00262   BoolPropertyWPtr auto_compute_bounds_property_;
00263   FloatPropertyWPtr min_value_property_;
00264   FloatPropertyWPtr max_value_property_;
00265   EnumPropertyWPtr axis_property_;
00266   BoolPropertyWPtr use_fixed_frame_property_;
00267 };
00268 
00269 
00270 }
00271 
00272 #endif // POINT_CLOUD_TRANSFORMERS_H