normal_display.cpp
Go to the documentation of this file.
1 // -*- mode: C++ -*-
2 #include "normal_display.h"
3 
4 using namespace rviz;
5 
6 namespace jsk_rviz_plugins
7 {
8 
9  NormalDisplay::NormalDisplay():skip_rate_(1),scale_(0.3),alpha_(1.0)
10  {
12  = new FloatProperty("Display Rate (%)", 1,
13  "Skip the display normals for speed up. Around 1% is recommended",
14  this, SLOT( updateSkipRate() ));
17 
19  = new rviz::FloatProperty("Scale", 0.3,
20  "set the scale of arrow",
21  this, SLOT(updateScale()));
22 
23  scale_property_->setMin(0.0);
24 
26  = new rviz::FloatProperty("Alpha", 1,
27  "set the alpha of arrow",
28  this, SLOT(updateAlpha()));
29 
30  alpha_property_->setMax(1.0);
31  alpha_property_->setMin(0.0);
32 
33  style_property_ = new EnumProperty( "Style", "PointsColor",
34  "Rendering mode to use, in order of computational complexity.",
35  this, SLOT( updateStyle() ), this);
40 
41  color_property_ = new ColorProperty( "Color", Qt::white,
42  "Color to assign to every point.",this);
44 
45  rainbow_property_ = new BoolProperty( "Use Rainbow", true, "Set rainbow range", this, SLOT( updateRainbow() ), this);
47 
48  min_color_property_ = new ColorProperty( "MinColor", Qt::green,
49  "Min color.",this);
51  max_color_property_ = new ColorProperty( "Max Color", Qt::red,
52  "Max color.",this);
54  }
55 
56  void NormalDisplay::getRainbow(float value , float& rf, float& gf, float& bf){
57  value = std::min(value, 1.0f);
58  value = std::max(value, 0.0f);
59  float h = value * 5.0f + 1.0f;
60  int i = floor(h);
61  float f = h - i;
62  if ( !(i&1) ) f = 1 - f;
63  float n = 1 - f;
64  if (i <= 1) rf = n, gf = 0, bf = 1;
65  else if (i == 2) rf = 0, gf = n, bf = 1;
66  else if (i == 3) rf = 0, gf = 1, bf = n;
67  else if (i == 4) rf = n, gf = 1, bf = 0;
68  else if (i >= 5) rf = 1, gf = n, bf = 0;
69  }
70 
75  }else{
78  }
79  }
80 
83  };
84 
87  };
88 
91  }
92 
94  {
96  if( mode != NormalDisplay::FLAT_COLOR )
97  {
99  }
100  else
101  {
103  }
104 
105  if( mode != NormalDisplay::CURVATURE_COLOR)
106  {
110  }
111  else
112  {
114  if(rainbow_property_->getBool()){
117  }else{
120  }
121  }
122  }
123 
125  {
127  }
128 
130  {
131  delete style_property_;
132  delete color_property_;
133  visuals_.clear();
134  }
135 
137  {
138  MFDClass::reset();
139  visuals_.clear();
140  }
141 
142 
143  void NormalDisplay::processMessage( const sensor_msgs::PointCloud2::ConstPtr& msg )
144  {
145  //check x,y,z
146  int32_t xi = findChannelIndex(msg, "x");
147  int32_t yi = findChannelIndex(msg, "y");
148  int32_t zi = findChannelIndex(msg, "z");
149 
150  if (xi == -1 || yi == -1 || zi == -1)
151  {
152  ROS_ERROR("doesn't have x, y, z");
153  return;
154  }
155 
156  const uint32_t xoff = msg->fields[xi].offset;
157  const uint32_t yoff = msg->fields[yi].offset;
158  const uint32_t zoff = msg->fields[zi].offset;
159 
160  //check normals x,y,z
161  int32_t normal_xi = findChannelIndex(msg, "normal_x");
162  int32_t normal_yi = findChannelIndex(msg, "normal_y");
163  int32_t normal_zi = findChannelIndex(msg, "normal_z");
164  int32_t curvature_i = findChannelIndex(msg, "curvature");
165 
166  if (normal_xi == -1 || normal_yi == -1 || normal_zi == -1 || curvature_i == -1)
167  {
168  ROS_ERROR("doesn't have normal_x, normal_y, normal_z, curvature");
169  return;
170  }
171 
172  const uint32_t normal_xoff = msg->fields[normal_xi].offset;
173  const uint32_t normal_yoff = msg->fields[normal_yi].offset;
174  const uint32_t normal_zoff = msg->fields[normal_zi].offset;
175  const uint32_t curvature_off = msg->fields[curvature_i].offset;
176 
177  //check rgba color
178  int32_t rgbai = findChannelIndex(msg, "rgb");
179  uint32_t rgbaoff = -1;
180  if(rgbai != -1)
181  rgbaoff = msg->fields[rgbai].offset;
182 
183  //check other option values
184  const uint32_t point_step = msg->point_step;
185  const size_t point_count = msg->width * msg->height;
186 
187  if (point_count == 0)
188  {
189  ROS_ERROR("doesn't have point_count > 0");
190  return;
191  }
192 
193  Ogre::Quaternion orientation;
194  Ogre::Vector3 position;
195  if( !context_->getFrameManager()->getTransform( msg->header.frame_id,
196  msg->header.stamp,
197  position, orientation ))
198  {
199  ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
200  msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
201  return;
202  }
203 
204  int skip_time = int( 100 / skip_rate_ );
205  skip_time = std::max(skip_time, 1);
206  skip_time = std::min(skip_time, int(point_count / 2));
207  visuals_.rset_capacity(int(point_count/skip_time));
208  const uint8_t* ptr = &msg->data.front();
209 
210  //Use Prev Curvature max, min
211  static float prev_max_curvature = 0.0;
212  static float prev_min_curvature = 1.0;
213  float max_curvature = 0.0;
214  float min_curvature = 1.0;
215  bool use_rainbow = rainbow_property_->getBool();
216  Ogre::ColourValue max_color = max_color_property_->getOgreColor();
217  Ogre::ColourValue min_color = min_color_property_->getOgreColor();
218  for (size_t i = 0; i < point_count; ++i)
219  {
220  if(i % skip_time != 0){
221  ptr += point_step;
222  continue;
223  }
224  float x = *reinterpret_cast<const float*>(ptr + xoff);
225  float y = *reinterpret_cast<const float*>(ptr + yoff);
226  float z = *reinterpret_cast<const float*>(ptr + zoff);
227  float normal_x = *reinterpret_cast<const float*>(ptr + normal_xoff);
228  float normal_y = *reinterpret_cast<const float*>(ptr + normal_yoff);
229  float normal_z = *reinterpret_cast<const float*>(ptr + normal_zoff);
230  float curvature = *reinterpret_cast<const float*>(ptr + curvature_off);
231  int r=1,g=0,b=0;
232 
233  if (validateFloats(Ogre::Vector3(x, y, z)) && validateFloats(Ogre::Vector3(normal_x, normal_y, normal_z)))
234  {
235 #if ROS_VERSION_MINIMUM(1,12,0)
236  std::shared_ptr<NormalVisual> visual;
237 #else
239 #endif
240  if(visuals_.full()){
241  visual = visuals_.front();
242  }else{
243  visual.reset(new NormalVisual( context_->getSceneManager(), scene_node_ ));
244  }
245  visual->setValues( x, y, z, normal_x, normal_y, normal_z );
246  visual->setFramePosition( position );
247  visual->setFrameOrientation( orientation );
248  visual->setScale( scale_ );
249 
250  QColor color = color_property_->getColor();
251  Ogre::Vector3 dir_vec(normal_x, normal_y, normal_z);
254  {
255  int r=1,g=0,b=0;
256  if(rgbai != -1){
257  b = *reinterpret_cast<const uint8_t*>(ptr + rgbaoff);
258  g = *reinterpret_cast<const uint8_t*>(ptr + rgbaoff + 1*sizeof(uint8_t));
259  r = *reinterpret_cast<const uint8_t*>(ptr + rgbaoff + 2*sizeof(uint8_t));
260  }
261  visual->setColor( r/256.0, g/256.0, b/256.0, alpha_ );
262  }
263  break;
265  visual->setColor(color.redF(), color.greenF(), color.blueF(), alpha_);
266  break;
268  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_ );
269  break;
271  if(use_rainbow){
272  float prev_diff = prev_max_curvature - prev_min_curvature;
273  float value = 1 - (curvature - prev_min_curvature)/prev_diff;
274  float rf,gf,bf;
275  getRainbow(value ,rf, gf, bf);
276  visual->setColor( rf, gf, bf, alpha_);
277  }else{
278  float value = curvature/(prev_max_curvature - prev_min_curvature);
279  value = std::min(value, 1.0f);
280  value = std::max(value, 0.0f);
281 
282  float rf = max_color.r * value + min_color.r * ( 1 - value );
283  float gf = max_color.g * value + min_color.g * ( 1 - value );
284  float bf = max_color.b * value + min_color.b * ( 1 - value );
285  visual->setColor( rf, gf, bf, alpha_);
286  }
287  max_curvature = std::max(max_curvature, curvature);
288  min_curvature = std::min(min_curvature, curvature);
289 
290  break;
291  }
292  visuals_.push_back(visual);
293  }
294 
295  ptr += point_step;
296  }
297  prev_min_curvature = min_curvature;
298  prev_max_curvature = max_curvature;
299 
300  }
301 }
302 
virtual QColor getColor() const
rviz::FloatProperty * skip_rate_property_
f
void setMin(float min)
void setMax(float max)
rviz::FloatProperty * alpha_property_
Ogre::ColourValue getOgreColor() const
rviz::EnumProperty * style_property_
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
DisplayContext * context_
rviz::ColorProperty * color_property_
rviz::BoolProperty * rainbow_property_
virtual float getFloat() const
rviz::FloatProperty * scale_property_
Ogre::SceneNode * scene_node_
rviz::ColorProperty * min_color_property_
virtual bool getBool() const
QString fixed_frame_
void getRainbow(float value, float &rf, float &gf, float &bf)
virtual void addOption(const QString &option, int value=0)
bool validateFloats(const sensor_msgs::CameraInfo &msg)
virtual FrameManager * getFrameManager() const =0
virtual Ogre::SceneManager * getSceneManager() const =0
int32_t findChannelIndex(const sensor_msgs::PointCloud2ConstPtr &cloud, const std::string &channel)
void processMessage(const sensor_msgs::PointCloud2::ConstPtr &msg)
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
boost::circular_buffer< boost::shared_ptr< NormalVisual > > visuals_
virtual int getOptionInt()
#define ROS_ERROR(...)
BoolProperty(const QString &name=QString(), bool default_value=false, const QString &description=QString(), Property *parent=0, const char *changed_slot=0, QObject *receiver=0)
rviz::ColorProperty * max_color_property_
#define ROS_DEBUG(...)


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sat Mar 20 2021 03:03:18