point_cloud_transformers.cpp
Go to the documentation of this 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 #include "point_cloud_transformers.h"
00031 #include "rviz/properties/property.h"
00032 #include "rviz/properties/property_manager.h"
00033 #include "rviz/validate_floats.h"
00034 #include <OGRE/OgreColourValue.h>
00035 #include <OGRE/OgreVector3.h>
00036 #include <OGRE/OgreMatrix4.h>
00037 
00038 namespace rviz
00039 {
00040 
00041 static void getRainbowColor(float value, Ogre::ColourValue& color)
00042 {
00043   value = std::min(value, 1.0f);
00044   value = std::max(value, 0.0f);
00045 
00046   float h = value * 5.0f + 1.0f;
00047   int i = floor(h);
00048   float f = h - i;
00049   if ( !(i&1) ) f = 1 - f; // if i is even
00050   float n = 1 - f;
00051 
00052   if      (i <= 1) color[0] = n, color[1] = 0, color[2] = 1;
00053   else if (i == 2) color[0] = 0, color[1] = n, color[2] = 1;
00054   else if (i == 3) color[0] = 0, color[1] = 1, color[2] = n;
00055   else if (i == 4) color[0] = n, color[1] = 1, color[2] = 0;
00056   else if (i >= 5) color[0] = 1, color[1] = n, color[2] = 0;
00057 }
00058 
00059 uint8_t IntensityPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00060 {
00061   updateChannels(cloud);
00062   return Support_Color;
00063 }
00064 
00065 uint8_t IntensityPCTransformer::score(const sensor_msgs::PointCloud2ConstPtr& cloud)
00066 {
00067   return 255;
00068 }
00069 
00070 bool IntensityPCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out)
00071 {
00072   if (!(mask & Support_Color))
00073   {
00074     return false;
00075   }
00076 
00077   int32_t index = findChannelIndex(cloud, selected_channel_);
00078 
00079   if (index == -1)
00080   {
00081     if (selected_channel_ == "intensity")
00082     {
00083       index = findChannelIndex(cloud, "intensities");
00084       if (index == -1)
00085       {
00086         return false;
00087       }
00088     }
00089     else
00090     {
00091       return false;
00092     }
00093   }
00094 
00095   const uint32_t offset = cloud->fields[index].offset;
00096   const uint8_t type = cloud->fields[index].datatype;
00097   const uint32_t point_step = cloud->point_step;
00098   const uint32_t num_points = cloud->width * cloud->height;
00099 
00100   float min_intensity = 999999.0f;
00101   float max_intensity = -999999.0f;
00102   if (auto_compute_intensity_bounds_)
00103   {
00104     for (uint32_t i = 0; i < num_points; ++i)
00105     {
00106       float val = valueFromCloud<float>(cloud, offset, type, point_step, i);
00107       min_intensity = std::min(val, min_intensity);
00108       max_intensity = std::max(val, max_intensity);
00109     }
00110 
00111     min_intensity = std::max(-999999.0f, min_intensity);
00112     max_intensity = std::min(999999.0f, max_intensity);
00113     min_intensity_ = min_intensity;
00114     max_intensity_ = max_intensity;
00115   }
00116   else
00117   {
00118     min_intensity = min_intensity_;
00119     max_intensity = max_intensity_;
00120   }
00121   float diff_intensity = max_intensity - min_intensity;
00122   Color max_color = max_color_;
00123   Color min_color = min_color_;
00124 
00125   for (uint32_t i = 0; i < num_points; ++i)
00126   {
00127     float val = valueFromCloud<float>(cloud, offset, type, point_step, i);
00128 
00129     if (use_full_rgb_colors_) {
00130       float range = std::max(diff_intensity, 0.001f);
00131       float value = 1.0 - (val - min_intensity)/range;
00132       getRainbowColor(value, out.points[i].color);
00133     }
00134     else {
00135       float normalized_intensity = diff_intensity > 0.0f ? ( val - min_intensity ) / diff_intensity : 1.0f;
00136       normalized_intensity = std::min(1.0f, std::max(0.0f, normalized_intensity));
00137       out.points[i].color.r = max_color.r_*normalized_intensity + min_color.r_*(1.0f - normalized_intensity);
00138       out.points[i].color.g = max_color.g_*normalized_intensity + min_color.g_*(1.0f - normalized_intensity);
00139       out.points[i].color.b = max_color.b_*normalized_intensity + min_color.b_*(1.0f - normalized_intensity);
00140     }
00141   }
00142 
00143   return true;
00144 }
00145 
00146 void IntensityPCTransformer::reset()
00147 {
00148   min_intensity_ = 0.0f;
00149   max_intensity_ = 4096.0f;
00150   selected_channel_ = std::string("intensity");
00151   available_channels_.clear();
00152 }
00153 
00154 void IntensityPCTransformer::createProperties(PropertyManager* property_man, const CategoryPropertyWPtr& parent, const std::string& prefix, uint32_t mask, V_PropertyBaseWPtr& out_props)
00155 {
00156   if (mask & Support_Color)
00157   {
00158     channel_name_property_ = property_man->createProperty<EditEnumProperty>("Channel Name", prefix, boost::bind( &IntensityPCTransformer::getChannelName, this),
00159                                                                             boost::bind( &IntensityPCTransformer::setChannelName, this, _1), parent, this);
00160     setPropertyHelpText(channel_name_property_, "Select the channel to use to compute the intensity");
00161 
00162     use_full_rgb_colors_property_ = property_man->createProperty<BoolProperty>( "Use full RGB spectrum",
00163                                                                                 prefix, boost::bind( &IntensityPCTransformer::getUseFullRGBColors, this),
00164                                                                                 boost::bind( &IntensityPCTransformer::setUseFullRGBColors, this, _1),
00165                                                                                 parent, this);
00166     setPropertyHelpText(use_full_rgb_colors_property_, "Whether to interpolate strictly between min and max color or use the full RGB color spectrum for intensities");
00167 
00168     min_color_property_ = property_man->createProperty<ColorProperty>( "Min Color", prefix, boost::bind( &IntensityPCTransformer::getMinColor, this ),
00169                                                                        boost::bind( &IntensityPCTransformer::setMinColor, this, _1 ), parent, this );
00170     setPropertyHelpText(min_color_property_, "Color to assign the points with the minimum intensity.  Actual color is interpolated between this and Max Color.");
00171     max_color_property_ = property_man->createProperty<ColorProperty>( "Max Color", prefix, boost::bind( &IntensityPCTransformer::getMaxColor, this ),
00172                                                                        boost::bind( &IntensityPCTransformer::setMaxColor, this, _1 ), parent, this );
00173     setPropertyHelpText(max_color_property_, "Color to assign the points with the maximum intensity.  Actual color is interpolated between this and Min Color.");
00174     ColorPropertyPtr color_prop = max_color_property_.lock();
00175     // legacy "Color" support... convert it to max color
00176     color_prop->addLegacyName("Color");
00177 
00178     auto_compute_intensity_bounds_property_ = property_man->createProperty<BoolProperty>( "Autocompute Intensity Bounds", prefix, boost::bind( &IntensityPCTransformer::getAutoComputeIntensityBounds, this ),
00179                                                                                           boost::bind( &IntensityPCTransformer::setAutoComputeIntensityBounds, this, _1 ), parent, this );
00180     setPropertyHelpText(auto_compute_intensity_bounds_property_, "Whether to automatically compute the intensity min/max values.");
00181     min_intensity_property_ = property_man->createProperty<FloatProperty>( "Min Intensity", prefix, boost::bind( &IntensityPCTransformer::getMinIntensity, this ),
00182                                                                            boost::bind( &IntensityPCTransformer::setMinIntensity, this, _1 ), parent, this );
00183     setPropertyHelpText(min_intensity_property_, "Minimum possible intensity value, used to interpolate from Min Color to Max Color for a point.");
00184     max_intensity_property_ = property_man->createProperty<FloatProperty>( "Max Intensity", prefix, boost::bind( &IntensityPCTransformer::getMaxIntensity, this ),
00185                                                                            boost::bind( &IntensityPCTransformer::setMaxIntensity, this, _1 ), parent, this );
00186     setPropertyHelpText(max_intensity_property_, "Maximum possible intensity value, used to interpolate from Min Color to Max Color for a point.");
00187 
00188     out_props.push_back(channel_name_property_);
00189     out_props.push_back(use_full_rgb_colors_property_);
00190     out_props.push_back(min_color_property_);
00191     out_props.push_back(max_color_property_);
00192     out_props.push_back(auto_compute_intensity_bounds_property_);
00193     out_props.push_back(min_intensity_property_);
00194     out_props.push_back(max_intensity_property_);
00195 
00196     if (auto_compute_intensity_bounds_)
00197     {
00198       hideProperty(min_intensity_property_);
00199       hideProperty(max_intensity_property_);
00200     }
00201     else
00202     {
00203       showProperty(min_intensity_property_);
00204       showProperty(max_intensity_property_);
00205     }
00206 
00207     if (use_full_rgb_colors_)
00208     {
00209       hideProperty(min_color_property_);
00210       hideProperty(max_color_property_);
00211     }
00212     else
00213     {
00214       showProperty(min_color_property_);
00215       showProperty(max_color_property_);
00216     }
00217   }
00218 }
00219 
00220 void IntensityPCTransformer::setChannelName(const std::string& channel)
00221 {
00222   // If we validate channel here to be in the list of
00223   // available_channels_ it will always fail at load time, since
00224   // available_channels_ is populated dynamically as point cloud
00225   // messages arrive.  Therefore we don't validate it and we live with
00226   // the consequences at runtime.
00227   selected_channel_ = channel;
00228 
00229   propertyChanged(channel_name_property_);
00230 
00231   causeRetransform();
00232 }
00233 
00234 void IntensityPCTransformer::updateChannels(const sensor_msgs::PointCloud2ConstPtr& cloud)
00235 {
00236   V_string channels;
00237   for(size_t i = 0; i < cloud->fields.size(); ++i)
00238   {
00239     channels.push_back(cloud->fields[i].name);
00240   }
00241   std::sort(channels.begin(), channels.end());
00242 
00243   EditEnumPropertyPtr channel_prop = channel_name_property_.lock();
00244   ROS_ASSERT(channel_prop);
00245 
00246   if (channels != available_channels_)
00247   {
00248     channel_prop->clear();
00249     for(V_string::const_iterator it = channels.begin(); it != channels.end(); ++it)
00250     {
00251       const std::string& channel = *it;
00252       if (channel.empty())
00253       {
00254         continue;
00255       }
00256       channel_prop->addOption(channel);
00257     }
00258     available_channels_ = channels;
00259   }
00260 }
00261 
00262 void IntensityPCTransformer::setMaxColor( const Color& color )
00263 {
00264   max_color_ = color;
00265 
00266   propertyChanged(max_color_property_);
00267 
00268   causeRetransform();
00269 }
00270 
00271 void IntensityPCTransformer::setMinColor( const Color& color )
00272 {
00273   min_color_ = color;
00274 
00275   propertyChanged(min_color_property_);
00276 
00277   causeRetransform();
00278 }
00279 
00280 void IntensityPCTransformer::setMinIntensity( float val )
00281 {
00282   min_intensity_ = val;
00283   if (min_intensity_ > max_intensity_)
00284   {
00285     min_intensity_ = max_intensity_;
00286   }
00287 
00288   propertyChanged(min_intensity_property_);
00289 
00290   causeRetransform();
00291 }
00292 
00293 void IntensityPCTransformer::setMaxIntensity( float val )
00294 {
00295   max_intensity_ = val;
00296   if (max_intensity_ < min_intensity_)
00297   {
00298     max_intensity_ = min_intensity_;
00299   }
00300 
00301   propertyChanged(max_intensity_property_);
00302 
00303   causeRetransform();
00304 }
00305 
00306 void IntensityPCTransformer::setAutoComputeIntensityBounds(bool compute)
00307 {
00308   auto_compute_intensity_bounds_ = compute;
00309 
00310   if (auto_compute_intensity_bounds_)
00311   {
00312     hideProperty(min_intensity_property_);
00313     hideProperty(max_intensity_property_);
00314   }
00315   else
00316   {
00317     showProperty(min_intensity_property_);
00318     showProperty(max_intensity_property_);
00319   }
00320 
00321   propertyChanged(auto_compute_intensity_bounds_property_);
00322 
00323   causeRetransform();
00324 }
00325 
00326 void IntensityPCTransformer::setUseFullRGBColors(bool full_rgb)
00327 {
00328   use_full_rgb_colors_ = full_rgb;
00329 
00330   if (use_full_rgb_colors_)
00331   {
00332     hideProperty(min_color_property_);
00333     hideProperty(max_color_property_);
00334   }
00335   else
00336   {
00337     showProperty(min_color_property_);
00338     showProperty(max_color_property_);
00339   }
00340 
00341   propertyChanged(use_full_rgb_colors_property_);
00342 
00343   causeRetransform();
00344 }
00345 
00346 uint8_t XYZPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00347 {
00348   int32_t xi = findChannelIndex(cloud, "x");
00349   int32_t yi = findChannelIndex(cloud, "y");
00350   int32_t zi = findChannelIndex(cloud, "z");
00351 
00352   if (xi == -1 || yi == -1 || zi == -1)
00353   {
00354     return Support_None;
00355   }
00356 
00357   if (cloud->fields[xi].datatype == sensor_msgs::PointField::FLOAT32)
00358   {
00359     return Support_XYZ;
00360   }
00361 
00362   return Support_None;
00363 }
00364 
00365 bool XYZPCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out)
00366 {
00367   if (!(mask & Support_XYZ))
00368   {
00369     return false;
00370   }
00371 
00372   int32_t xi = findChannelIndex(cloud, "x");
00373   int32_t yi = findChannelIndex(cloud, "y");
00374   int32_t zi = findChannelIndex(cloud, "z");
00375 
00376   const uint32_t xoff = cloud->fields[xi].offset;
00377   const uint32_t yoff = cloud->fields[yi].offset;
00378   const uint32_t zoff = cloud->fields[zi].offset;
00379   const uint32_t point_step = cloud->point_step;
00380   const uint32_t num_points = cloud->width * cloud->height;
00381   uint8_t const* point = &cloud->data.front();
00382   for (uint32_t i = 0; i < num_points; ++i, point += point_step)
00383   {
00384     float x = *reinterpret_cast<const float*>(point + xoff);
00385     float y = *reinterpret_cast<const float*>(point + yoff);
00386     float z = *reinterpret_cast<const float*>(point + zoff);
00387 
00388     Ogre::Vector3 pos(x, y, z);
00389     pos = transform * pos;
00390     out.points[i].position = pos;
00391   }
00392 
00393   return true;
00394 }
00395 
00396 uint8_t RGB8PCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00397 {
00398   int32_t index = findChannelIndex(cloud, "rgb");
00399   if (index == -1)
00400   {
00401     return Support_None;
00402   }
00403 
00404   if (cloud->fields[index].datatype == sensor_msgs::PointField::INT32 ||
00405       cloud->fields[index].datatype == sensor_msgs::PointField::FLOAT32)
00406   {
00407     return Support_Color;
00408   }
00409 
00410   return Support_None;
00411 }
00412 
00413 bool RGB8PCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out)
00414 {
00415   if (!(mask & Support_Color))
00416   {
00417     return false;
00418   }
00419 
00420   int32_t index = findChannelIndex(cloud, "rgb");
00421 
00422   const uint32_t off = cloud->fields[index].offset;
00423   const uint32_t point_step = cloud->point_step;
00424   const uint32_t num_points = cloud->width * cloud->height;
00425   uint8_t const* point = &cloud->data.front();
00426   for (uint32_t i = 0; i < num_points; ++i, point += point_step)
00427   {
00428     uint32_t rgb = *reinterpret_cast<const uint32_t*>(point + off);
00429     float r = ((rgb >> 16) & 0xff) / 255.0f;
00430     float g = ((rgb >> 8) & 0xff) / 255.0f;
00431     float b = (rgb & 0xff) / 255.0f;
00432     out.points[i].color = Ogre::ColourValue(r, g, b);
00433   }
00434 
00435   return true;
00436 }
00437 
00438 uint8_t RGBF32PCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00439 {
00440   int32_t ri = findChannelIndex(cloud, "r");
00441   int32_t gi = findChannelIndex(cloud, "g");
00442   int32_t bi = findChannelIndex(cloud, "b");
00443   if (ri == -1 || gi == -1 || bi == -1)
00444   {
00445     return Support_None;
00446   }
00447 
00448   if (cloud->fields[ri].datatype == sensor_msgs::PointField::FLOAT32)
00449   {
00450     return Support_Color;
00451   }
00452 
00453   return Support_None;
00454 }
00455 
00456 bool RGBF32PCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out)
00457 {
00458   if (!(mask & Support_Color))
00459   {
00460     return false;
00461   }
00462 
00463   int32_t ri = findChannelIndex(cloud, "r");
00464   int32_t gi = findChannelIndex(cloud, "g");
00465   int32_t bi = findChannelIndex(cloud, "b");
00466 
00467   const uint32_t roff = cloud->fields[ri].offset;
00468   const uint32_t goff = cloud->fields[gi].offset;
00469   const uint32_t boff = cloud->fields[bi].offset;
00470   const uint32_t point_step = cloud->point_step;
00471   const uint32_t num_points = cloud->width * cloud->height;
00472   uint8_t const* point = &cloud->data.front();
00473   for (uint32_t i = 0; i < num_points; ++i, point += point_step)
00474   {
00475     float r = *reinterpret_cast<const float*>(point + roff);
00476     float g = *reinterpret_cast<const float*>(point + goff);
00477     float b = *reinterpret_cast<const float*>(point + boff);
00478     out.points[i].color = Ogre::ColourValue(r, g, b);
00479   }
00480 
00481   return true;
00482 }
00483 
00484 uint8_t FlatColorPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00485 {
00486   return Support_Color;
00487 }
00488 
00489 uint8_t FlatColorPCTransformer::score(const sensor_msgs::PointCloud2ConstPtr& cloud)
00490 {
00491   return 0;
00492 }
00493 
00494 bool FlatColorPCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out)
00495 {
00496   if (!(mask & Support_Color))
00497   {
00498     return false;
00499   }
00500 
00501   const uint32_t num_points = cloud->width * cloud->height;
00502   for (uint32_t i = 0; i < num_points; ++i)
00503   {
00504     out.points[i].color = Ogre::ColourValue(color_.r_, color_.g_, color_.b_);
00505   }
00506 
00507   return true;
00508 }
00509 
00510 void FlatColorPCTransformer::setColor(const Color& c)
00511 {
00512   color_ = c;
00513   propertyChanged(color_property_);
00514   causeRetransform();
00515 }
00516 
00517 void FlatColorPCTransformer::createProperties(PropertyManager* property_man, const CategoryPropertyWPtr& parent, const std::string& prefix, uint32_t mask, V_PropertyBaseWPtr& out_props)
00518 {
00519   if (mask & Support_Color)
00520   {
00521     color_property_ = property_man->createProperty<ColorProperty>("Color", prefix, boost::bind( &FlatColorPCTransformer::getColor, this ),
00522                                                                   boost::bind( &FlatColorPCTransformer::setColor, this, _1 ), parent, this);
00523     setPropertyHelpText(color_property_, "Color to assign to every point.");
00524 
00525     out_props.push_back(color_property_);
00526   }
00527 }
00528 
00529 uint8_t AxisColorPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
00530 {
00531   return Support_Color;
00532 }
00533 
00534 uint8_t AxisColorPCTransformer::score(const sensor_msgs::PointCloud2ConstPtr& cloud)
00535 {
00536   return 255;
00537 }
00538 
00539 bool AxisColorPCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t mask, const Ogre::Matrix4& transform, PointCloud& out)
00540 {
00541   if (!(mask & Support_Color))
00542   {
00543     return false;
00544   }
00545 
00546   int32_t xi = findChannelIndex(cloud, "x");
00547   int32_t yi = findChannelIndex(cloud, "y");
00548   int32_t zi = findChannelIndex(cloud, "z");
00549 
00550   const uint32_t xoff = cloud->fields[xi].offset;
00551   const uint32_t yoff = cloud->fields[yi].offset;
00552   const uint32_t zoff = cloud->fields[zi].offset;
00553   const uint32_t point_step = cloud->point_step;
00554   const uint32_t num_points = cloud->width * cloud->height;
00555   uint8_t const* point = &cloud->data.front();
00556 
00557   // compute bounds
00558 
00559   float min_value_current = 9999.0f;
00560   float max_value_current = -9999.0f;
00561   std::vector<float> values;
00562   values.reserve(num_points);
00563 
00564   for (uint32_t i = 0; i < num_points; ++i, point += point_step)
00565   {
00566     float x = *reinterpret_cast<const float*>(point + xoff);
00567     float y = *reinterpret_cast<const float*>(point + yoff);
00568     float z = *reinterpret_cast<const float*>(point + zoff);
00569 
00570     Ogre::Vector3 pos(x, y, z);
00571 
00572     if (use_fixed_frame_)
00573     {
00574       pos = transform * pos;
00575     }
00576 
00577     float val = pos[axis_];
00578     min_value_current = std::min(min_value_current, val);
00579     max_value_current = std::max(max_value_current, val);
00580 
00581     values.push_back(val);
00582   }
00583 
00584   if (auto_compute_bounds_)
00585   {
00586     min_value_ = min_value_current;
00587     max_value_ = max_value_current;
00588   }
00589 
00590   for (uint32_t i = 0; i < num_points; ++i)
00591   {
00592     float range = std::max(max_value_ - min_value_, 0.001f);
00593     float value = 1.0 - (values[i] - min_value_)/range;
00594     getRainbowColor(value, out.points[i].color);
00595   }
00596 
00597   return true;
00598 }
00599 
00600 void AxisColorPCTransformer::createProperties(PropertyManager* property_man, const CategoryPropertyWPtr& parent, const std::string& prefix, uint32_t mask, V_PropertyBaseWPtr& out_props)
00601 {
00602   if (mask & Support_Color)
00603   {
00604     axis_property_ = property_man->createProperty<EnumProperty>("Axis", prefix, boost::bind(&AxisColorPCTransformer::getAxis, this), boost::bind(&AxisColorPCTransformer::setAxis, this, _1),
00605                                                                 parent, this);
00606     EnumPropertyPtr prop = axis_property_.lock();
00607     prop->addOption("X", AXIS_X);
00608     prop->addOption("Y", AXIS_Y);
00609     prop->addOption("Z", AXIS_Z);
00610     setPropertyHelpText(axis_property_, "The axis to interpolate the color along.");
00611     auto_compute_bounds_property_ = property_man->createProperty<BoolProperty>( "Autocompute Value Bounds", prefix, boost::bind( &AxisColorPCTransformer::getAutoComputeBounds, this ),
00612                                                                               boost::bind( &AxisColorPCTransformer::setAutoComputeBounds, this, _1 ), parent, this );
00613 
00614     setPropertyHelpText(auto_compute_bounds_property_, "Whether to automatically compute the value min/max values.");
00615     min_value_property_ = property_man->createProperty<FloatProperty>( "Min Value", prefix, boost::bind( &AxisColorPCTransformer::getMinValue, this ),
00616                                                                               boost::bind( &AxisColorPCTransformer::setMinValue, this, _1 ), parent, this );
00617     setPropertyHelpText(min_value_property_, "Minimum value value, used to interpolate the color of a point.");
00618     max_value_property_ = property_man->createProperty<FloatProperty>( "Max Value", prefix, boost::bind( &AxisColorPCTransformer::getMaxValue, this ),
00619                                                                             boost::bind( &AxisColorPCTransformer::setMaxValue, this, _1 ), parent, this );
00620     setPropertyHelpText(max_value_property_, "Maximum value value, used to interpolate the color of a point.");
00621 
00622     use_fixed_frame_property_ = property_man->createProperty<BoolProperty>( "Use Fixed Frame", prefix, boost::bind( &AxisColorPCTransformer::getUseFixedFrame, this ),
00623                                                                             boost::bind( &AxisColorPCTransformer::setUseFixedFrame, this, _1 ), parent, this );
00624     setPropertyHelpText(use_fixed_frame_property_, "Whether to color the cloud based on its fixed frame position or its local frame position.");
00625 
00626     out_props.push_back(axis_property_);
00627     out_props.push_back(auto_compute_bounds_property_);
00628     out_props.push_back(min_value_property_);
00629     out_props.push_back(max_value_property_);
00630     out_props.push_back(use_fixed_frame_property_);
00631 
00632     if (auto_compute_bounds_)
00633     {
00634       hideProperty(min_value_property_);
00635       hideProperty(max_value_property_);
00636     }
00637     else
00638     {
00639       showProperty(min_value_property_);
00640       showProperty(max_value_property_);
00641     }
00642   }
00643 }
00644 
00645 void AxisColorPCTransformer::setUseFixedFrame(bool use)
00646 {
00647   use_fixed_frame_ = use;
00648   propertyChanged(use_fixed_frame_property_);
00649   causeRetransform();
00650 }
00651 
00652 void AxisColorPCTransformer::setAxis(int axis)
00653 {
00654   axis_ = axis;
00655   propertyChanged(axis_property_);
00656   causeRetransform();
00657 }
00658 
00659 void AxisColorPCTransformer::setMinValue( float val )
00660 {
00661   min_value_ = val;
00662   if (min_value_ > max_value_)
00663   {
00664     min_value_ = max_value_;
00665   }
00666 
00667   propertyChanged(min_value_property_);
00668 
00669   causeRetransform();
00670 }
00671 
00672 void AxisColorPCTransformer::setMaxValue( float val )
00673 {
00674   max_value_ = val;
00675   if (max_value_ < min_value_)
00676   {
00677     max_value_ = min_value_;
00678   }
00679 
00680   propertyChanged(max_value_property_);
00681 
00682   causeRetransform();
00683 }
00684 
00685 void AxisColorPCTransformer::setAutoComputeBounds(bool compute)
00686 {
00687   auto_compute_bounds_ = compute;
00688 
00689   if (auto_compute_bounds_)
00690   {
00691     hideProperty(min_value_property_);
00692     hideProperty(max_value_property_);
00693   }
00694   else
00695   {
00696     showProperty(min_value_property_);
00697     showProperty(max_value_property_);
00698   }
00699 
00700   propertyChanged(auto_compute_bounds_property_);
00701 
00702   causeRetransform();
00703 }
00704 
00705 }


rviz_qt
Author(s): Dave Hershberger
autogenerated on Fri Dec 6 2013 20:56:53