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 inline int32_t findChannelIndex(const sensor_msgs::PointCloud2ConstPtr& cloud, const std::string& channel)
00041 {
00042   for (size_t i = 0; i < cloud->fields.size(); ++i)
00043   {
00044     if (cloud->fields[i].name == channel)
00045     {
00046       return i;
00047     }
00048   }
00049 
00050   return -1;
00051 }
00052 
00053 template<typename T>
00054 inline T valueFromCloud(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t offset, uint8_t type, uint32_t point_step, uint32_t index)
00055 {
00056   const uint8_t* data = &cloud->data[(point_step * index) + offset];
00057   T ret = 0;
00058 
00059   switch (type)
00060   {
00061   case sensor_msgs::PointField::INT8:
00062   case sensor_msgs::PointField::UINT8:
00063     {
00064       uint8_t val = *reinterpret_cast<const uint8_t*>(data);
00065       ret = static_cast<T>(val);
00066       break;
00067     }
00068 
00069   case sensor_msgs::PointField::INT16:
00070   case sensor_msgs::PointField::UINT16:
00071     {
00072       uint16_t val = *reinterpret_cast<const uint16_t*>(data);
00073       ret = static_cast<T>(val);
00074       break;
00075     }
00076 
00077   case sensor_msgs::PointField::INT32:
00078   case sensor_msgs::PointField::UINT32:
00079     {
00080       uint32_t val = *reinterpret_cast<const uint32_t*>(data);
00081       ret = static_cast<T>(val);
00082       break;
00083     }
00084 
00085   case sensor_msgs::PointField::FLOAT32:
00086     {
00087       float val = *reinterpret_cast<const float*>(data);
00088       ret = static_cast<T>(val);
00089       break;
00090     }
00091 
00092   case sensor_msgs::PointField::FLOAT64:
00093     {
00094       double val = *reinterpret_cast<const double*>(data);
00095       ret = static_cast<T>(val);
00096       break;
00097     }
00098   default:
00099     break;
00100   }
00101 
00102   return ret;
00103 }
00104 
00105 class IntensityPCTransformer : public PointCloudTransformer
00106 {
00107 public:
00108   IntensityPCTransformer()
00109   : min_color_( 0.0f, 0.0f, 0.0f )
00110   , max_color_( 1.0f, 1.0f, 1.0f )
00111   , min_intensity_(0.0f)
00112   , max_intensity_(4096.0f)
00113   {
00114     setAutoComputeIntensityBounds(true);
00115   }
00116 
00117   virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00118   virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out);
00119   virtual uint8_t score(const sensor_msgs::PointCloud2ConstPtr& cloud);
00120   virtual void reset();
00121   virtual void createProperties(PropertyManager* property_man, const CategoryPropertyWPtr& parent, const std::string& prefix, uint32_t mask, V_PropertyBaseWPtr& out_props);
00122 
00123   void setMinColor( const Color& color );
00124   void setMaxColor( const Color& color );
00125   const Color& getMaxColor() { return max_color_; }
00126   const Color& getMinColor() { return min_color_; }
00127   void setMinIntensity(float val);
00128   void setMaxIntensity(float val);
00129   float getMinIntensity() { return min_intensity_; }
00130   float getMaxIntensity() { return max_intensity_; }
00131   void setAutoComputeIntensityBounds(bool compute);
00132   bool getAutoComputeIntensityBounds() { return auto_compute_intensity_bounds_; }
00133 
00134 private:
00135   Color min_color_;
00136   Color max_color_;
00137   float min_intensity_;
00138   float max_intensity_;
00139   bool auto_compute_intensity_bounds_;
00140   bool intensity_bounds_changed_;
00141 
00142   ColorPropertyWPtr min_color_property_;
00143   ColorPropertyWPtr max_color_property_;
00144   BoolPropertyWPtr auto_compute_intensity_bounds_property_;
00145   FloatPropertyWPtr min_intensity_property_;
00146   FloatPropertyWPtr max_intensity_property_;
00147 
00148   RetransformFunc retransform_func_;
00149 };
00150 
00151 class XYZPCTransformer : public PointCloudTransformer
00152 {
00153 public:
00154   XYZPCTransformer()
00155   {}
00156 
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, PointCloud& out);
00159 };
00160 
00161 
00162 
00163 class RGB8PCTransformer : public PointCloudTransformer
00164 {
00165 public:
00166   virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00167   virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out);
00168 };
00169 
00170 
00171 
00172 class RGBF32PCTransformer : public PointCloudTransformer
00173 {
00174 public:
00175   virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00176   virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out);
00177 };
00178 
00179 
00180 
00181 class FlatColorPCTransformer : public PointCloudTransformer
00182 {
00183 public:
00184   FlatColorPCTransformer()
00185   : color_(1.0, 1.0, 1.0)
00186   {}
00187 
00188   virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00189   virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out);
00190   virtual void createProperties(PropertyManager* property_man, const CategoryPropertyWPtr& parent, const std::string& prefix, uint32_t mask, V_PropertyBaseWPtr& out_props);
00191   virtual uint8_t score(const sensor_msgs::PointCloud2ConstPtr& cloud);
00192 
00193   void setColor(const Color& color);
00194   const Color& getColor() { return color_; }
00195 
00196 private:
00197   Color color_;
00198   ColorPropertyWPtr color_property_;
00199 };
00200 
00201 class AxisColorPCTransformer : public PointCloudTransformer
00202 {
00203 public:
00204   AxisColorPCTransformer()
00205     : min_value_(-10.0f)
00206     , max_value_(10.0f)
00207     , axis_(AXIS_Z)
00208     , use_fixed_frame_(true)
00209     {
00210       setAutoComputeBounds(true);
00211     }
00212 
00213   virtual uint8_t supports(const sensor_msgs::PointCloud2ConstPtr& cloud);
00214   virtual bool transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out);
00215   virtual void createProperties(PropertyManager* property_man, const CategoryPropertyWPtr& parent, const std::string& prefix, uint32_t mask, V_PropertyBaseWPtr& out_props);
00216   virtual uint8_t score(const sensor_msgs::PointCloud2ConstPtr& cloud);
00217 
00218   void setMinValue(float val);
00219   void setMaxValue(float val);
00220   float getMinValue() { return min_value_; }
00221   float getMaxValue() { return max_value_; }
00222   void setAutoComputeBounds(bool compute);
00223   bool getAutoComputeBounds() { return auto_compute_bounds_; }
00224 
00225   void setUseFixedFrame(bool use);
00226   bool getUseFixedFrame() { return use_fixed_frame_; }
00227 
00228   enum Axis
00229   {
00230     AXIS_X,
00231     AXIS_Y,
00232     AXIS_Z
00233   };
00234 
00235   void setAxis(int axis);
00236   int getAxis() { return axis_; }
00237 
00238 private:
00239 
00240   float min_value_;
00241   float max_value_;
00242 
00243   bool auto_compute_bounds_;
00244   bool use_fixed_frame_;
00245 
00246   int axis_;
00247 
00248   BoolPropertyWPtr auto_compute_bounds_property_;
00249   FloatPropertyWPtr min_value_property_;
00250   FloatPropertyWPtr max_value_property_;
00251   EnumPropertyWPtr axis_property_;
00252   BoolPropertyWPtr use_fixed_frame_property_;
00253 
00254   static void getColor(float value, Ogre::ColourValue& color);
00255 };
00256 
00257 
00258 }
00259 
00260 #endif // POINT_CLOUD_TRANSFORMERS_H