00001
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
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
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
00178 int32_t rgbai = findChannelIndex(msg, "rgb");
00179 uint32_t rgbaoff = -1;
00180 if(rgbai != -1)
00181 rgbaoff = msg->fields[rgbai].offset;
00182
00183
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
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 )