plotter_2d_display.cpp
Go to the documentation of this file.
1 // -*- mode: c++; -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 #include "plotter_2d_display.h"
37 #include <OGRE/OgreHardwarePixelBuffer.h>
39 #include <rviz/display_context.h>
40 #include <QPainter>
41 
42 namespace jsk_rviz_plugins
43 {
45  : rviz::Display(), min_value_(0.0), max_value_(0.0)
46  {
48  "Topic", "",
49  ros::message_traits::datatype<std_msgs::Float32>(),
50  "std_msgs::Float32 topic to subscribe to.",
51  this, SLOT(updateTopic()));
53  "Show Value", true,
54  "Show value on plotter",
55  this, SLOT(updateShowValue()));
57  "Buffer length", 100,
58  ros::message_traits::datatype<std_msgs::Float32>(),
59  this, SLOT(updateBufferSize()));
60  width_property_ = new rviz::IntProperty("width", 128,
61  "width of the plotter window",
62  this, SLOT(updateWidth()));
64  width_property_->setMax(2000);
65  height_property_ = new rviz::IntProperty("height", 128,
66  "height of the plotter window",
67  this, SLOT(updateHeight()));
69  height_property_->setMax(2000);
70  left_property_ = new rviz::IntProperty("left", 128,
71  "left of the plotter window",
72  this, SLOT(updateLeft()));
74  top_property_ = new rviz::IntProperty("top", 128,
75  "top of the plotter window",
76  this, SLOT(updateTop()));
79  "auto scale", true,
80  "enable auto scale",
81  this, SLOT(updateAutoScale()));
83  "max value", 1.0,
84  "max value, used only if auto scale is disabled",
85  this, SLOT(updateMaxValue()));
87  "min value", -1.0,
88  "min value, used only if auto scale is disabled",
89  this, SLOT(updateMinValue()));
91  "foreground color", QColor(25, 255, 240),
92  "color to draw line",
93  this, SLOT(updateFGColor()));
95  "foreground alpha", 0.7,
96  "alpha belnding value for foreground",
97  this, SLOT(updateFGAlpha()));
101  "background color", QColor(0, 0, 0),
102  "background color",
103  this, SLOT(updateBGColor()));
105  "backround alpha", 0.0,
106  "alpha belnding value for background",
107  this, SLOT(updateBGAlpha()));
110  line_width_property_ = new rviz::IntProperty("linewidth", 1,
111  "linewidth of the plot",
112  this, SLOT(updateLineWidth()));
116  "border", true,
117  "show border or not",
118  this, SLOT(updateShowBorder()));
119  text_size_property_ = new rviz::IntProperty("text size", 12,
120  "text size of the caption",
121  this, SLOT(updateTextSize()));
125  "show caption", true,
126  "show caption or not",
127  this, SLOT(updateShowCaption()));
129  "update interval", 0.04,
130  "update interval of the plotter",
131  this, SLOT(updateUpdateInterval()));
135  = new rviz::BoolProperty("auto color change",
136  false,
137  "change the color automatically",
138  this, SLOT(updateAutoColorChange()));
140  = new rviz::ColorProperty(
141  "max color",
142  QColor(255, 0, 0),
143  "only used if auto color change is set to True.",
144  this, SLOT(updateMaxColor()));
145  }
146 
148  {
149  onDisable();
150  // delete update_topic_property_;
151  // delete buffer_length_property_;
152  // delete fg_color_property_;
153  // delete bg_color_property_;
154  // delete fg_alpha_property_;
155  // delete bg_alpha_property_;
156  // delete top_property_;
157  // delete left_property_;
158  // delete width_property_;
159  // delete height_property_;
160  // delete line_width_property_;
161  // delete show_border_property_;
162  // delete auto_color_change_property_;
163  // delete max_color_property_;
164  // delete update_interval_property_;
165  // delete show_caption_property_;
166  // delete text_size_property_;
167  // delete min_value_property_;
168  // delete max_value_property_;
169  // delete auto_color_change_property_;
170  }
171 
173  {
174  buffer_.resize(buffer_length_);
175  if (min_value_ == 0.0 && max_value_ == 0.0) {
176  min_value_ = -1.0;
177  max_value_ = 1.0;
178  }
179  for (size_t i = 0; i < buffer_length_; i++) {
180  buffer_[i] = 0.0;
181  }
182  }
183 
185  {
186  static int count = 0;
188  ss << "Plotter2DDisplayObject" << count++;
189  overlay_.reset(new OverlayObject(ss.str()));
191  onEnable();
192  updateShowValue();
193  updateWidth();
194  updateHeight();
195  updateLeft();
196  updateTop();
197  updateFGColor();
198  updateBGColor();
199  updateFGAlpha();
200  updateBGAlpha();
201  updateLineWidth();
205  updateMaxColor();
207  updateTextSize();
208  updateAutoScale();
209  updateMinValue();
210  updateMaxValue();
211  overlay_->updateTextureSize(width_property_->getInt(),
213  }
214 
216  {
217  QColor fg_color(fg_color_);
218  QColor bg_color(bg_color_);
219 
220  fg_color.setAlpha(fg_alpha_);
221  bg_color.setAlpha(bg_alpha_);
222 
223  if (auto_color_change_) {
224  double r
225  = std::min(std::max((buffer_[buffer_.size() - 1] - min_value_) / (max_value_ - min_value_),
226  0.0), 1.0);
227  if (r > 0.3) {
228  double r2 = (r - 0.3) / 0.7;
229  fg_color.setRed((max_color_.red() - fg_color_.red()) * r2
230  + fg_color_.red());
231  fg_color.setGreen((max_color_.green() - fg_color_.green()) * r2
232  + fg_color_.green());
233  fg_color.setBlue((max_color_.blue() - fg_color_.blue()) * r2
234  + fg_color_.blue());
235  }
236  }
237 
238  {
239  ScopedPixelBuffer buffer = overlay_->getBuffer();
240  QImage Hud = buffer.getQImage(*overlay_);
241  // initilize by the background color
242  for (int i = 0; i < overlay_->getTextureWidth(); i++) {
243  for (int j = 0; j < overlay_->getTextureHeight(); j++) {
244  Hud.setPixel(i, j, bg_color.rgba());
245  }
246  }
247  // paste in HUD speedometer. I resize the image and offset it by 8 pixels from
248  // the bottom left edge of the render window
249  QPainter painter( &Hud );
250  painter.setRenderHint(QPainter::Antialiasing, true);
251  painter.setPen(QPen(fg_color, line_width_, Qt::SolidLine));
252 
253  uint16_t w = overlay_->getTextureWidth();
254  uint16_t h = overlay_->getTextureHeight() - caption_offset_;
255 
256  double margined_max_value = max_value_ + (max_value_ - min_value_) / 2;
257  double margined_min_value = min_value_ - (max_value_ - min_value_) / 2;
258 
259  for (size_t i = 1; i < buffer_length_; i++) {
260  double v_prev = (margined_max_value - buffer_[i - 1]) / (margined_max_value - margined_min_value);
261  double v = (margined_max_value - buffer_[i]) / (margined_max_value - margined_min_value);
262  double u_prev = (i - 1) / (float)buffer_length_;
263  double u = i / (float)buffer_length_;
264 
265  // chop within 0 ~ 1
266  v_prev = std::max(std::min(v_prev, 1.0), 0.0);
267  u_prev = std::max(std::min(u_prev, 1.0), 0.0);
268  v = std::max(std::min(v, 1.0), 0.0);
269  u = std::max(std::min(u, 1.0), 0.0);
270 
271  uint16_t x_prev = (int)(u_prev * w);
272  uint16_t x = (int)(u * w);
273  uint16_t y_prev = (int)(v_prev * h);
274  uint16_t y = (int)(v * h);
275  painter.drawLine(x_prev, y_prev, x, y);
276  }
277  // draw border
278  if (show_border_) {
279  painter.drawLine(0, 0, 0, h);
280  painter.drawLine(0, h, w, h);
281  painter.drawLine(w, h, w, 0);
282  painter.drawLine(w, 0, 0, 0);
283  }
284  // draw caption
285  if (show_caption_) {
286  QFont font = painter.font();
287  font.setPointSize(text_size_);
288  font.setBold(true);
289  painter.setFont(font);
290  painter.drawText(0, h, w, caption_offset_,
291  Qt::AlignCenter | Qt::AlignVCenter,
292  getName());
293  }
294  if (show_value_) {
295  QFont font = painter.font();
296  font.setPointSize(w / 4);
297  font.setBold(true);
298  painter.setFont(font);
299  std::ostringstream ss;
300  ss << std::fixed << std::setprecision(2) << buffer_[buffer_.size() - 1];
301  painter.drawText(0, 0, w, h,
302  Qt::AlignCenter | Qt::AlignVCenter,
303  ss.str().c_str());
304  }
305 
306  // done
307  painter.end();
308  }
309  }
310 
311  void Plotter2DDisplay::processMessage(const std_msgs::Float32::ConstPtr& msg)
312  {
313  boost::mutex::scoped_lock lock(mutex_);
314 
315  if (!isEnabled()) {
316  return;
317  }
318  // add the message to the buffer
319  double min_value = buffer_[0];
320  double max_value = buffer_[0];
321  for (size_t i = 0; i < buffer_length_ - 1; i++) {
322  buffer_[i] = buffer_[i + 1];
323  if (min_value > buffer_[i]) {
324  min_value = buffer_[i];
325  }
326  if (max_value < buffer_[i]) {
327  max_value = buffer_[i];
328  }
329  }
330  buffer_[buffer_length_ - 1] = msg->data;
331  if (min_value > msg->data) {
332  min_value = msg->data;
333  }
334  if (max_value < msg->data) {
335  max_value = msg->data;
336  }
337  if (auto_scale_) {
338  min_value_ = min_value;
339  max_value_ = max_value;
340  if (min_value_ == max_value_) {
341  min_value_ = min_value_ - 0.5;
342  max_value_ = max_value_ + 0.5;
343  }
344  }
345  if (!overlay_->isVisible()) {
346  return;
347  }
348 
349  draw_required_ = true;
350  }
351 
352  void Plotter2DDisplay::update(float wall_dt, float ros_dt)
353  {
354  if (draw_required_) {
355  if (wall_dt + last_time_ > update_interval_) {
356  overlay_->updateTextureSize(texture_width_,
358  overlay_->setPosition(left_, top_);
359  overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
360  last_time_ = 0;
361  drawPlot();
362  draw_required_ = false;
363  }
364  else {
365  last_time_ = last_time_ + wall_dt;
366  }
367  }
368  }
369 
371  {
373  std::string topic_name = update_topic_property_->getTopicStd();
374  if (topic_name.length() > 0 && topic_name != "/") {
375  ros::NodeHandle n;
376  sub_ = n.subscribe(topic_name, 1, &Plotter2DDisplay::processMessage, this);
377  }
378  }
379 
381  {
382  sub_.shutdown();
383  }
384 
386  {
387  last_time_ = 0;
388  draw_required_ = false;
389  subscribe();
390  overlay_->show();
391  }
392 
394  {
395  unsubscribe();
396  overlay_->hide();
397  }
398 
400  {
401  boost::mutex::scoped_lock lock(mutex_);
403  }
404 
406  {
407  boost::mutex::scoped_lock lock(mutex_);
409  }
410 
412  {
414  }
415 
417  {
419  }
420 
422  {
424  }
425 
427  {
429  }
430 
432  {
434  }
435 
437  {
439  }
440 
442  {
443  unsubscribe();
444  subscribe();
445  }
446 
448  {
450  }
451 
453  {
455  }
456 
458  {
460  }
461 
463  {
466  }
467 
469  {
471  if (auto_color_change_) {
473  }
474  else {
476  }
477  }
478 
480  {
482  }
483 
485  {
487  }
488 
490  {
492  QFont font;
493  font.setPointSize(text_size_);
494  caption_offset_ = QFontMetrics(font).height();
495  }
496 
498  {
500  if (show_caption_) {
502  }
503  else {
505  }
506  }
507 
509  {
510  if (!auto_scale_) {
512  }
513  }
514 
516  {
517  if (!auto_scale_) {
519  }
520  }
521 
523  {
525  if (auto_scale_) {
528  }
529  else {
532  }
533  updateMinValue();
534  updateMaxValue();
535  }
536 
537  bool Plotter2DDisplay::isInRegion(int x, int y)
538  {
539  return (top_ < y && top_ + texture_height_ > y &&
540  left_ < x && left_ + texture_width_ > x);
541  }
542 
544  {
545  top_ = y;
546  left_ = x;
547  }
548 
549  void Plotter2DDisplay::setPosition(int x, int y)
550  {
553  }
554 
555 
556 }
557 
void setMin(float min)
rviz::FloatProperty * fg_alpha_property_
rviz::BoolProperty * auto_color_change_property_
void setMax(float max)
rviz::BoolProperty * show_caption_property_
virtual void processMessage(const std_msgs::Float32::ConstPtr &msg)
virtual QImage getQImage(unsigned int width, unsigned int height)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
virtual QColor getColor() const
virtual void setPosition(int x, int y)
rviz::FloatProperty * update_interval_property_
rviz::ColorProperty * bg_color_property_
rviz::FloatProperty * max_value_property_
data
void setMin(int min)
std::string getTopicStd() const
void setMax(int max)
float
rviz::BoolProperty * show_border_property_
virtual void update(float wall_dt, float ros_dt)
virtual QString getName() const
rviz::FloatProperty * min_value_property_
rviz::IntProperty * buffer_length_property_
x
y
rviz::RosTopicProperty * update_topic_property_
virtual int getInt() const
rviz::FloatProperty * bg_alpha_property_
rviz::ColorProperty * max_color_property_
rviz::ColorProperty * fg_color_property_
bool isEnabled() const
bool setValue(const QVariant &new_value) override
w
virtual float getFloat() const
rviz::BoolProperty * show_value_property_
virtual void movePosition(int x, int y)
virtual bool isInRegion(int x, int y)
virtual bool getBool() const


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Thu Jun 1 2023 02:45:58