normal_display.cpp
Go to the documentation of this file.
00001 // -*- mode: C++ -*-
00002 #include "normal_display.h"
00003 
00004 using namespace rviz;
00005 
00006 namespace jsk_rviz_plugins
00007 {
00008 
00009   NormalDisplay::NormalDisplay():skip_rate_(1),scale_(0.3),alpha_(1.0)
00010   {
00011     skip_rate_property_
00012       = new FloatProperty("Display Rate (%)", 1,
00013                                             "Skip the display normals for speed up. Around 1% is recommended",
00014                                             this, SLOT( updateSkipRate() ));
00015     skip_rate_property_->setMax(100.0);
00016     skip_rate_property_->setMin(  0.0);
00017 
00018     scale_property_
00019       = new rviz::FloatProperty("Scale", 0.3,
00020                                 "set the scale of arrow",
00021                                 this, SLOT(updateScale()));
00022 
00023     scale_property_->setMin(0.0);
00024 
00025     alpha_property_
00026       = new rviz::FloatProperty("Alpha", 1,
00027                                 "set the alpha of arrow",
00028                                 this, SLOT(updateAlpha()));
00029 
00030     alpha_property_->setMax(1.0);
00031     alpha_property_->setMin(0.0);
00032 
00033     style_property_ = new EnumProperty( "Style", "PointsColor",
00034                                         "Rendering mode to use, in order of computational complexity.",
00035                                         this, SLOT( updateStyle() ), this);
00036     style_property_->addOption( "PointsColor", NormalDisplay::POINTS_COLOR );
00037     style_property_->addOption( "FlatColor", NormalDisplay::FLAT_COLOR );
00038     style_property_->addOption( "DirectionColor", NormalDisplay::DIRECTION_COLOR );
00039     style_property_->addOption( "CurvatureColor", NormalDisplay::CURVATURE_COLOR );
00040 
00041     color_property_ = new ColorProperty( "Color", Qt::white,
00042                                          "Color to assign to every point.",this);
00043     color_property_->hide();
00044 
00045     rainbow_property_ = new BoolProperty( "Use Rainbow", true, "Set rainbow range", this, SLOT( updateRainbow() ), this);
00046     rainbow_property_->hide();
00047 
00048     min_color_property_ = new ColorProperty( "MinColor", Qt::green,
00049                                          "Min color.",this);
00050     min_color_property_->hide();
00051     max_color_property_ = new ColorProperty( "Max Color", Qt::red,
00052                                              "Max color.",this);
00053     max_color_property_->hide();
00054   }
00055 
00056   void NormalDisplay::getRainbow(float value , float& rf, float& gf, float& bf){
00057     value = std::min(value, 1.0f);
00058     value = std::max(value, 0.0f);
00059     float h = value * 5.0f + 1.0f;
00060     int i = floor(h);
00061     float f = h - i;
00062     if ( !(i&1) ) f = 1 - f;
00063     float n = 1 - f;
00064     if      (i <= 1) rf = n, gf = 0, bf = 1;
00065     else if (i == 2) rf = 0, gf = n, bf = 1;
00066     else if (i == 3) rf = 0, gf = 1, bf = n;
00067     else if (i == 4) rf = n, gf = 1, bf = 0;
00068     else if (i >= 5) rf = 1, gf = n, bf = 0;
00069   }
00070 
00071   void NormalDisplay::updateRainbow(){
00072     if(rainbow_property_->getBool()){
00073       min_color_property_->hide();
00074       max_color_property_->hide();
00075     }else{
00076       min_color_property_->show();
00077       max_color_property_->show();
00078     }
00079   }
00080 
00081   void NormalDisplay::updateScale(){
00082     scale_ = scale_property_->getFloat();
00083   };
00084 
00085   void NormalDisplay::updateAlpha(){
00086     alpha_ = alpha_property_->getFloat();
00087   };
00088 
00089   void NormalDisplay::updateSkipRate(){
00090     skip_rate_ = skip_rate_property_->getFloat();
00091   }
00092 
00093   void NormalDisplay::updateStyle()
00094   {
00095     NormalDisplay::ColorTypes mode = (NormalDisplay::ColorTypes) style_property_->getOptionInt();
00096     if( mode != NormalDisplay::FLAT_COLOR )
00097       {
00098         color_property_->hide();
00099       }
00100     else
00101       {
00102         color_property_->show();
00103       }
00104 
00105     if( mode != NormalDisplay::CURVATURE_COLOR)
00106       {
00107         min_color_property_->hide();
00108         max_color_property_->hide();
00109         rainbow_property_->hide();
00110       }
00111     else
00112       {
00113         rainbow_property_->show();
00114         if(rainbow_property_->getBool()){
00115           min_color_property_->hide();
00116           max_color_property_->hide();
00117         }else{
00118           min_color_property_->show();
00119           max_color_property_->show();
00120         }
00121       }
00122   }
00123 
00124   void NormalDisplay::onInitialize()
00125   {
00126     MFDClass::onInitialize();
00127   }
00128 
00129   NormalDisplay::~NormalDisplay()
00130   {
00131     delete style_property_;
00132     delete color_property_;
00133     visuals_.clear();
00134   }
00135 
00136   void NormalDisplay::reset()
00137   {
00138     MFDClass::reset();
00139     visuals_.clear();
00140   }
00141 
00142 
00143   void NormalDisplay::processMessage( const sensor_msgs::PointCloud2::ConstPtr& msg )
00144   {
00145     //check x,y,z
00146     int32_t xi = findChannelIndex(msg, "x");
00147     int32_t yi = findChannelIndex(msg, "y");
00148     int32_t zi = findChannelIndex(msg, "z");
00149 
00150     if (xi == -1 || yi == -1 || zi == -1)
00151       {
00152         ROS_ERROR("doesn't have x, y, z");
00153         return;
00154       }
00155 
00156     const uint32_t xoff = msg->fields[xi].offset;
00157     const uint32_t yoff = msg->fields[yi].offset;
00158     const uint32_t zoff = msg->fields[zi].offset;
00159 
00160     //check normals x,y,z
00161     int32_t normal_xi = findChannelIndex(msg, "normal_x");
00162     int32_t normal_yi = findChannelIndex(msg, "normal_y");
00163     int32_t normal_zi = findChannelIndex(msg, "normal_z");
00164     int32_t curvature_i = findChannelIndex(msg, "curvature");
00165 
00166     if (normal_xi == -1 || normal_yi == -1 || normal_zi == -1 || curvature_i == -1)
00167       {
00168         ROS_ERROR("doesn't have normal_x, normal_y, normal_z, curvature");
00169         return;
00170       }
00171 
00172     const uint32_t normal_xoff = msg->fields[normal_xi].offset;
00173     const uint32_t normal_yoff = msg->fields[normal_yi].offset;
00174     const uint32_t normal_zoff = msg->fields[normal_zi].offset;
00175     const uint32_t curvature_off = msg->fields[curvature_i].offset;
00176 
00177     //check rgba color
00178     int32_t rgbai = findChannelIndex(msg, "rgb");
00179     uint32_t rgbaoff = -1;
00180     if(rgbai != -1)
00181       rgbaoff = msg->fields[rgbai].offset;
00182 
00183     //check other option values
00184     const uint32_t point_step = msg->point_step;
00185     const size_t point_count = msg->width * msg->height;
00186 
00187     if (point_count == 0)
00188       {
00189         ROS_ERROR("doesn't have point_count > 0");
00190         return;
00191       }
00192 
00193     Ogre::Quaternion orientation;
00194     Ogre::Vector3 position;
00195     if( !context_->getFrameManager()->getTransform( msg->header.frame_id,
00196                                                     msg->header.stamp,
00197                                                     position, orientation ))
00198       {
00199         ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
00200                    msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00201         return;
00202       }
00203 
00204     int skip_time = int( 100 / skip_rate_ );
00205     skip_time = std::max(skip_time, 1);
00206     skip_time = std::min(skip_time, int(point_count / 2));
00207     visuals_.rset_capacity(int(point_count/skip_time));
00208     const uint8_t* ptr = &msg->data.front();
00209 
00210     //Use Prev Curvature max, min
00211     static float prev_max_curvature = 0.0;
00212     static float prev_min_curvature = 1.0;
00213     float max_curvature = 0.0;
00214     float min_curvature = 1.0;
00215     bool use_rainbow = rainbow_property_->getBool();
00216     Ogre::ColourValue max_color = max_color_property_->getOgreColor();
00217     Ogre::ColourValue min_color = min_color_property_->getOgreColor();
00218     for (size_t i = 0; i < point_count; ++i)
00219       {
00220         if(i % skip_time != 0){
00221           ptr += point_step;
00222           continue;
00223         }
00224         float x = *reinterpret_cast<const float*>(ptr + xoff);
00225         float y = *reinterpret_cast<const float*>(ptr + yoff);
00226         float z = *reinterpret_cast<const float*>(ptr + zoff);
00227         float normal_x = *reinterpret_cast<const float*>(ptr + normal_xoff);
00228         float normal_y = *reinterpret_cast<const float*>(ptr + normal_yoff);
00229         float normal_z = *reinterpret_cast<const float*>(ptr + normal_zoff);
00230         float curvature = *reinterpret_cast<const float*>(ptr + curvature_off);
00231         int r=1,g=0,b=0;
00232 
00233         if (validateFloats(Ogre::Vector3(x, y, z)) && validateFloats(Ogre::Vector3(normal_x, normal_y, normal_z)))
00234           {
00235             boost::shared_ptr<NormalVisual> visual;
00236             if(visuals_.full()){
00237               visual = visuals_.front();
00238             }else{
00239               visual.reset(new NormalVisual( context_->getSceneManager(), scene_node_ ));
00240             }
00241             visual->setValues( x, y, z, normal_x, normal_y, normal_z );
00242             visual->setFramePosition( position );
00243             visual->setFrameOrientation( orientation );
00244             visual->setScale( scale_ );
00245 
00246             QColor color = color_property_->getColor();
00247             Ogre::Vector3 dir_vec(normal_x, normal_y, normal_z);
00248             switch((NormalDisplay::ColorTypes) style_property_->getOptionInt()){
00249             case (NormalDisplay::POINTS_COLOR):
00250               {
00251                 int r=1,g=0,b=0;
00252                 if(rgbai != -1){
00253                   b = *reinterpret_cast<const uint8_t*>(ptr + rgbaoff);
00254                   g = *reinterpret_cast<const uint8_t*>(ptr + rgbaoff + 1*sizeof(uint8_t));
00255                   r = *reinterpret_cast<const uint8_t*>(ptr + rgbaoff + 2*sizeof(uint8_t));
00256                 }
00257                 visual->setColor( r/256.0, g/256.0, b/256.0, alpha_ );
00258               }
00259               break;
00260             case (NormalDisplay::FLAT_COLOR):
00261               visual->setColor(color.redF(), color.greenF(), color.blueF(), alpha_);
00262               break;
00263             case (NormalDisplay::DIRECTION_COLOR):
00264               visual->setColor( dir_vec.dotProduct(Ogre::Vector3(-1,0,0)), dir_vec.dotProduct(Ogre::Vector3(0,1,0)), dir_vec.dotProduct(Ogre::Vector3(0,0,-1)), alpha_ );
00265               break;
00266             case (NormalDisplay::CURVATURE_COLOR):
00267               if(use_rainbow){
00268                 float prev_diff = prev_max_curvature - prev_min_curvature;
00269                 float value = 1 - (curvature - prev_min_curvature)/prev_diff;
00270                 float rf,gf,bf;
00271                 getRainbow(value ,rf, gf, bf);
00272                 visual->setColor( rf, gf, bf, alpha_);
00273               }else{
00274                 float value  = curvature/(prev_max_curvature - prev_min_curvature);
00275                 value = std::min(value, 1.0f);
00276                 value = std::max(value, 0.0f);
00277 
00278                 float rf = max_color.r * value + min_color.r * ( 1 - value );
00279                 float gf = max_color.g * value + min_color.g * ( 1 - value );
00280                 float bf = max_color.b * value + min_color.b * ( 1 - value );
00281                 visual->setColor( rf, gf, bf, alpha_);
00282               }
00283               max_curvature = std::max(max_curvature, curvature);
00284               min_curvature = std::min(min_curvature, curvature);
00285 
00286               break;
00287             }
00288             visuals_.push_back(visual);
00289           }
00290 
00291         ptr += point_step;
00292       }
00293     prev_min_curvature = min_curvature;
00294     prev_max_curvature = max_curvature;
00295 
00296   }
00297 }
00298 
00299 #include <pluginlib/class_list_macros.h>
00300 PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::NormalDisplay,rviz::Display )


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sun Sep 13 2015 22:29:03