00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include "pie_chart_display.h"
00037
00038 #include <OGRE/OgreMaterialManager.h>
00039 #include <OGRE/OgreTextureManager.h>
00040 #include <OGRE/OgreTexture.h>
00041 #include <OGRE/OgreTechnique.h>
00042 #include <OGRE/OgreHardwarePixelBuffer.h>
00043 #include <rviz/uniform_string_stream.h>
00044 #include <rviz/display_context.h>
00045 #include <QPainter>
00046
00047 namespace jsk_rviz_plugins
00048 {
00049
00050 PieChartDisplay::PieChartDisplay()
00051 : rviz::Display(), update_required_(false), first_time_(true), data_(0.0)
00052 {
00053 update_topic_property_ = new rviz::RosTopicProperty(
00054 "Topic", "",
00055 ros::message_traits::datatype<std_msgs::Float32>(),
00056 "std_msgs::Float32 topic to subscribe to.",
00057 this, SLOT( updateTopic() ));
00058 size_property_ = new rviz::IntProperty("size", 128,
00059 "size of the plotter window",
00060 this, SLOT(updateSize()));
00061 left_property_ = new rviz::IntProperty("left", 128,
00062 "left of the plotter window",
00063 this, SLOT(updateLeft()));
00064 top_property_ = new rviz::IntProperty("top", 128,
00065 "top of the plotter window",
00066 this, SLOT(updateTop()));
00067 fg_color_property_ = new rviz::ColorProperty("foreground color",
00068 QColor(25, 255, 240),
00069 "color to draw line",
00070 this, SLOT(updateFGColor()));
00071 fg_alpha_property_
00072 = new rviz::FloatProperty("foreground alpha", 0.7,
00073 "alpha belnding value for foreground",
00074 this, SLOT(updateFGAlpha()));
00075 fg_alpha2_property_
00076 = new rviz::FloatProperty("foreground alpha 2", 0.4,
00077 "alpha belnding value for foreground for indicator",
00078 this, SLOT(updateFGAlpha2()));
00079 bg_color_property_ = new rviz::ColorProperty("background color",
00080 QColor(0, 0, 0),
00081 "background color",
00082 this, SLOT(updateBGColor()));
00083 bg_alpha_property_
00084 = new rviz::FloatProperty("backround alpha", 0.0,
00085 "alpha belnding value for background",
00086 this, SLOT(updateBGAlpha()));
00087 text_size_property_
00088 = new rviz::IntProperty("text size", 14,
00089 "text size",
00090 this, SLOT(updateTextSize()));
00091 show_caption_property_
00092 = new rviz::BoolProperty("show caption", true,
00093 "show caption",
00094 this, SLOT(updateShowCaption()));
00095 max_value_property_
00096 = new rviz::FloatProperty("max value", 1.0,
00097 "max value of pie chart",
00098 this, SLOT(updateMaxValue()));
00099 min_value_property_
00100 = new rviz::FloatProperty("min value", 0.0,
00101 "min value of pie chart",
00102 this, SLOT(updateMinValue()));
00103 auto_color_change_property_
00104 = new rviz::BoolProperty("auto color change",
00105 false,
00106 "change the color automatically",
00107 this, SLOT(updateAutoColorChange()));
00108 max_color_property_
00109 = new rviz::ColorProperty("max color",
00110 QColor(255, 0, 0),
00111 "only used if auto color change is set to True.",
00112 this, SLOT(updateMaxColor()));
00113
00114 }
00115
00116 PieChartDisplay::~PieChartDisplay()
00117 {
00118 if (overlay_->isVisible()) {
00119 overlay_->hide();
00120 }
00121 delete update_topic_property_;
00122 delete fg_color_property_;
00123 delete bg_color_property_;
00124 delete fg_alpha_property_;
00125 delete fg_alpha2_property_;
00126 delete bg_alpha_property_;
00127 delete top_property_;
00128 delete left_property_;
00129 delete size_property_;
00130 delete min_value_property_;
00131 delete max_value_property_;
00132 delete text_size_property_;
00133 delete show_caption_property_;
00134 }
00135
00136 void PieChartDisplay::onInitialize()
00137 {
00138 static int count = 0;
00139 rviz::UniformStringStream ss;
00140 ss << "PieChartDisplayObject" << count++;
00141 overlay_.reset(new OverlayObject(ss.str()));
00142 onEnable();
00143 updateSize();
00144 updateLeft();
00145 updateTop();
00146 updateFGColor();
00147 updateBGColor();
00148 updateFGAlpha();
00149 updateFGAlpha2();
00150 updateBGAlpha();
00151 updateMinValue();
00152 updateMaxValue();
00153 updateTextSize();
00154 updateShowCaption();
00155 updateAutoColorChange();
00156 updateMaxColor();
00157 overlay_->updateTextureSize(texture_size_, texture_size_ + caption_offset_);
00158 overlay_->hide();
00159 }
00160
00161 void PieChartDisplay::update(float wall_dt, float ros_dt)
00162 {
00163 if (update_required_) {
00164 update_required_ = false;
00165 overlay_->updateTextureSize(texture_size_, texture_size_ + caption_offset_);
00166 drawPlot(data_);
00167 overlay_->setPosition(left_, top_);
00168 overlay_->setDimensions(overlay_->getTextureWidth(),
00169 overlay_->getTextureHeight());
00170 }
00171 }
00172
00173 void PieChartDisplay::processMessage(const std_msgs::Float32::ConstPtr& msg)
00174 {
00175 boost::mutex::scoped_lock lock(mutex_);
00176
00177 if (!overlay_->isVisible()) {
00178 return;
00179 }
00180 if (data_ != msg->data || first_time_) {
00181 first_time_ = false;
00182 data_ = msg->data;
00183 update_required_ = true;
00184 }
00185 }
00186
00187 void PieChartDisplay::drawPlot(double val)
00188 {
00189 QColor fg_color(fg_color_);
00190
00191 if (auto_color_change_) {
00192 double r
00193 = std::min(1.0, fabs((val - min_value_) / (max_value_ - min_value_)));
00194 if (r > 0.6) {
00195 double r2 = (r - 0.6) / 0.4;
00196 fg_color.setRed((max_color_.red() - fg_color_.red()) * r2
00197 + fg_color_.red());
00198 fg_color.setGreen((max_color_.green() - fg_color_.green()) * r2
00199 + fg_color_.green());
00200 fg_color.setBlue((max_color_.blue() - fg_color_.blue()) * r2
00201 + fg_color_.blue());
00202 }
00203 }
00204
00205
00206 QColor fg_color2(fg_color);
00207 QColor bg_color(bg_color_);
00208 fg_color.setAlpha(fg_alpha_);
00209 fg_color2.setAlpha(fg_alpha2_);
00210 bg_color.setAlpha(bg_alpha_);
00211 int width = overlay_->getTextureWidth();
00212 int height = overlay_->getTextureHeight();
00213 {
00214 ScopedPixelBuffer buffer = overlay_->getBuffer();
00215 QImage Hud = buffer.getQImage(*overlay_, bg_color);
00216 QPainter painter( &Hud );
00217 painter.setRenderHint(QPainter::Antialiasing, true);
00218
00219 const int outer_line_width = 5;
00220 const int value_line_width = 10;
00221 const int value_indicator_line_width = 2;
00222 const int value_padding = 5;
00223
00224 const int value_aabb_offset
00225 = outer_line_width + value_padding + value_line_width / 2;
00226
00227 painter.setPen(QPen(fg_color, outer_line_width, Qt::SolidLine));
00228
00229 painter.drawEllipse(outer_line_width / 2, outer_line_width / 2,
00230 width - outer_line_width ,
00231 height - outer_line_width - caption_offset_);
00232
00233 painter.setPen(QPen(fg_color2, value_indicator_line_width, Qt::SolidLine));
00234 painter.drawEllipse(value_aabb_offset, value_aabb_offset,
00235 width - value_aabb_offset * 2,
00236 height - value_aabb_offset * 2 - caption_offset_);
00237
00238 const double ratio = (val - min_value_) / (max_value_ - min_value_);
00239 const double ratio_angle = ratio * 360.0;
00240 const double start_angle_offset = -90;
00241 painter.setPen(QPen(fg_color, value_line_width, Qt::SolidLine));
00242 painter.drawArc(QRectF(value_aabb_offset, value_aabb_offset,
00243 width - value_aabb_offset * 2,
00244 height - value_aabb_offset * 2 - caption_offset_),
00245 start_angle_offset * 16 ,
00246 ratio_angle * 16);
00247 QFont font = painter.font();
00248 font.setPointSize(text_size_);
00249 font.setBold(true);
00250 painter.setFont(font);
00251 painter.setPen(QPen(fg_color, value_line_width, Qt::SolidLine));
00252 std::ostringstream s;
00253 s << std::fixed << std::setprecision(2) << val;
00254 painter.drawText(0, 0, width, height - caption_offset_,
00255 Qt::AlignCenter | Qt::AlignVCenter,
00256 s.str().c_str());
00257
00258
00259 if (show_caption_) {
00260 painter.drawText(0, height - caption_offset_, width, caption_offset_,
00261 Qt::AlignCenter | Qt::AlignVCenter,
00262 getName());
00263 }
00264
00265
00266 painter.end();
00267
00268 }
00269 }
00270
00271
00272 void PieChartDisplay::subscribe()
00273 {
00274 std::string topic_name = update_topic_property_->getTopicStd();
00275 if (topic_name.length() > 0 && topic_name != "/") {
00276 ros::NodeHandle n;
00277 sub_ = n.subscribe(topic_name, 1, &PieChartDisplay::processMessage, this);
00278 }
00279 }
00280
00281
00282 void PieChartDisplay::unsubscribe()
00283 {
00284 sub_.shutdown();
00285 }
00286
00287 void PieChartDisplay::onEnable()
00288 {
00289 subscribe();
00290 overlay_->show();
00291 first_time_ = true;
00292 }
00293
00294 void PieChartDisplay::onDisable()
00295 {
00296 unsubscribe();
00297 overlay_->hide();
00298 }
00299
00300 void PieChartDisplay::updateSize()
00301 {
00302 boost::mutex::scoped_lock lock(mutex_);
00303 texture_size_ = size_property_->getInt();
00304 }
00305
00306 void PieChartDisplay::updateTop()
00307 {
00308 top_ = top_property_->getInt();
00309 }
00310
00311 void PieChartDisplay::updateLeft()
00312 {
00313 left_ = left_property_->getInt();
00314 }
00315
00316 void PieChartDisplay::updateBGColor()
00317 {
00318 bg_color_ = bg_color_property_->getColor();
00319 }
00320
00321 void PieChartDisplay::updateFGColor()
00322 {
00323 fg_color_ = fg_color_property_->getColor();
00324 }
00325
00326 void PieChartDisplay::updateFGAlpha()
00327 {
00328 fg_alpha_ = fg_alpha_property_->getFloat() * 255.0;
00329 }
00330
00331 void PieChartDisplay::updateFGAlpha2()
00332 {
00333 fg_alpha2_ = fg_alpha2_property_->getFloat() * 255.0;
00334 }
00335
00336
00337 void PieChartDisplay::updateBGAlpha()
00338 {
00339 bg_alpha_ = bg_alpha_property_->getFloat() * 255.0;
00340 }
00341
00342 void PieChartDisplay::updateMinValue()
00343 {
00344 min_value_ = min_value_property_->getFloat();
00345 }
00346
00347 void PieChartDisplay::updateMaxValue()
00348 {
00349 max_value_ = max_value_property_->getFloat();
00350 }
00351
00352 void PieChartDisplay::updateTextSize()
00353 {
00354 boost::mutex::scoped_lock lock(mutex_);
00355 text_size_ = text_size_property_->getInt();
00356 QFont font;
00357 font.setPointSize(text_size_);
00358 caption_offset_ = QFontMetrics(font).height();
00359
00360 }
00361
00362 void PieChartDisplay::updateShowCaption()
00363 {
00364 show_caption_ = show_caption_property_->getBool();
00365 }
00366
00367
00368 void PieChartDisplay::updateTopic()
00369 {
00370 unsubscribe();
00371 subscribe();
00372 }
00373
00374 void PieChartDisplay::updateAutoColorChange()
00375 {
00376 auto_color_change_ = auto_color_change_property_->getBool();
00377 if (auto_color_change_) {
00378 max_color_property_->show();
00379 }
00380 else {
00381 max_color_property_->hide();
00382 }
00383 }
00384
00385 void PieChartDisplay::updateMaxColor()
00386 {
00387 max_color_ = max_color_property_->getColor();
00388 }
00389
00390 bool PieChartDisplay::isInRegion(int x, int y)
00391 {
00392 return (top_ < y && top_ + texture_size_ > y &&
00393 left_ < x && left_ + texture_size_ > x);
00394 }
00395
00396 void PieChartDisplay::movePosition(int x, int y)
00397 {
00398 top_ = y;
00399 left_ = x;
00400 }
00401
00402 void PieChartDisplay::setPosition(int x, int y)
00403 {
00404 top_property_->setValue(y);
00405 left_property_->setValue(x);
00406 }
00407 }
00408
00409 #include <pluginlib/class_list_macros.h>
00410 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugins::PieChartDisplay, rviz::Display )