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_plugin
00048 {
00049
00050 PieChartDisplay::PieChartDisplay()
00051 : rviz::Display()
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_color_property_
00088 = new rviz::ColorProperty("text color",
00089 QColor(25, 255, 240),
00090 "text color",
00091 this, SLOT(updateTextColor()));
00092 text_alpha_property_
00093 = new rviz::FloatProperty("text alpha", 1.0,
00094 "alpha belnding value for text",
00095 this, SLOT(updateTextAlpha()));
00096 text_size_property_
00097 = new rviz::IntProperty("text size", 14,
00098 "text size",
00099 this, SLOT(updateTextSize()));
00100 show_caption_property_
00101 = new rviz::BoolProperty("show caption", true,
00102 "show caption",
00103 this, SLOT(updateShowCaption()));
00104 max_value_property_
00105 = new rviz::FloatProperty("max value", 1.0,
00106 "max value of pie chart",
00107 this, SLOT(updateMaxValue()));
00108 min_value_property_
00109 = new rviz::FloatProperty("min value", 0.0,
00110 "min value of pie chart",
00111 this, SLOT(updateMinValue()));
00112 auto_color_change_property_
00113 = new rviz::BoolProperty("auto color change",
00114 false,
00115 "change the color automatically",
00116 this, SLOT(updateAutoColorChange()));
00117 max_color_property_
00118 = new rviz::ColorProperty("max color",
00119 QColor(255, 0, 0),
00120 "only used if auto color change is set to True.",
00121 this, SLOT(updateMaxColor()));
00122
00123 }
00124
00125 PieChartDisplay::~PieChartDisplay()
00126 {
00127 if (overlay_->isVisible()) {
00128 overlay_->hide();
00129 }
00130 delete update_topic_property_;
00131 delete fg_color_property_;
00132 delete bg_color_property_;
00133 delete text_color_property_;
00134 delete fg_alpha_property_;
00135 delete fg_alpha2_property_;
00136 delete bg_alpha_property_;
00137 delete text_alpha_property_;
00138 delete top_property_;
00139 delete left_property_;
00140 delete size_property_;
00141 delete min_value_property_;
00142 delete max_value_property_;
00143 delete text_size_property_;
00144 delete show_caption_property_;
00145 }
00146
00147 void PieChartDisplay::onInitialize()
00148 {
00149 static int count = 0;
00150 rviz::UniformStringStream ss;
00151 ss << "PieChartDisplayObject" << count++;
00152 overlay_.reset(new OverlayObject(ss.str()));
00153 onEnable();
00154 updateSize();
00155 updateLeft();
00156 updateTop();
00157 updateFGColor();
00158 updateBGColor();
00159 updateFGAlpha();
00160 updateFGAlpha2();
00161 updateBGAlpha();
00162 updateMinValue();
00163 updateMaxValue();
00164 updateTextColor();
00165 updateTextAlpha();
00166 updateTextSize();
00167 updateShowCaption();
00168 updateAutoColorChange();
00169 updateMaxColor();
00170 overlay_->updateTextureSize(texture_size_, texture_size_ + caption_offset_);
00171 overlay_->hide();
00172 }
00173
00174 void PieChartDisplay::processMessage(const std_msgs::Float32::ConstPtr& msg)
00175 {
00176 boost::mutex::scoped_lock lock(mutex_);
00177
00178 if (!overlay_->isVisible()) {
00179 return;
00180 }
00181
00182 overlay_->updateTextureSize(texture_size_, texture_size_ + caption_offset_);
00183 drawPlot(msg->data);
00184 overlay_->setPosition(left_, top_);
00185 overlay_->setDimensions(overlay_->getTextureWidth(),
00186 overlay_->getTextureHeight());
00187 }
00188
00189 void PieChartDisplay::drawPlot(double val)
00190 {
00191 QColor fg_color(fg_color_);
00192
00193 if (auto_color_change_) {
00194 double r
00195 = std::min(1.0, fabs((val - min_value_) / (max_value_ - min_value_)));
00196 fg_color.setRed((max_color_.red() - fg_color_.red()) * r
00197 + fg_color_.red());
00198 fg_color.setGreen((max_color_.green() - fg_color_.green()) * r
00199 + fg_color_.green());
00200 fg_color.setBlue((max_color_.blue() - fg_color_.blue()) * r
00201 + fg_color_.blue());
00202 }
00203
00204
00205 QColor fg_color2(fg_color);
00206 QColor bg_color(bg_color_);
00207 QColor text_color(text_color_);
00208 fg_color.setAlpha(fg_alpha_);
00209 fg_color2.setAlpha(fg_alpha2_);
00210 bg_color.setAlpha(bg_alpha_);
00211 text_color.setAlpha(text_alpha_);
00212 int width = overlay_->getTextureWidth();
00213 int height = overlay_->getTextureHeight();
00214 {
00215 ScopedPixelBuffer buffer = overlay_->getBuffer();
00216 QImage Hud = buffer.getQImage(*overlay_, bg_color);
00217 QPainter painter( &Hud );
00218 painter.setRenderHint(QPainter::Antialiasing, true);
00219
00220 const int outer_line_width = 5;
00221 const int value_line_width = 10;
00222 const int value_indicator_line_width = 2;
00223 const int value_padding = 5;
00224
00225 const int value_aabb_offset
00226 = outer_line_width + value_padding + value_line_width / 2;
00227
00228 painter.setPen(QPen(fg_color, outer_line_width, Qt::SolidLine));
00229
00230 painter.drawEllipse(outer_line_width / 2, outer_line_width / 2,
00231 width - outer_line_width ,
00232 height - outer_line_width - caption_offset_);
00233
00234 painter.setPen(QPen(fg_color2, value_indicator_line_width, Qt::SolidLine));
00235 painter.drawEllipse(value_aabb_offset, value_aabb_offset,
00236 width - value_aabb_offset * 2,
00237 height - value_aabb_offset * 2 - caption_offset_);
00238
00239 const double ratio = (val - min_value_) / (max_value_ - min_value_);
00240 const double ratio_angle = ratio * 360.0;
00241 const double start_angle_offset = -90;
00242 painter.setPen(QPen(fg_color, value_line_width, Qt::SolidLine));
00243 painter.drawArc(QRectF(value_aabb_offset, value_aabb_offset,
00244 width - value_aabb_offset * 2,
00245 height - value_aabb_offset * 2 - caption_offset_),
00246 start_angle_offset * 16 ,
00247 ratio_angle * 16);
00248 QFont font = painter.font();
00249 font.setPointSize(text_size_);
00250 font.setBold(true);
00251 painter.setFont(font);
00252 painter.setPen(QPen(text_color, value_line_width, Qt::SolidLine));
00253 std::ostringstream s;
00254 s << std::fixed << std::setprecision(2) << val;
00255 painter.drawText(0, 0, width, height - caption_offset_,
00256 Qt::AlignCenter | Qt::AlignVCenter,
00257 s.str().c_str());
00258
00259
00260 if (show_caption_) {
00261 painter.drawText(0, height - caption_offset_, width, caption_offset_,
00262 Qt::AlignCenter | Qt::AlignVCenter,
00263 getName());
00264 }
00265
00266
00267 painter.end();
00268
00269 }
00270 }
00271
00272
00273 void PieChartDisplay::subscribe()
00274 {
00275 std::string topic_name = update_topic_property_->getTopicStd();
00276 if (topic_name.length() > 0 && topic_name != "/") {
00277 ros::NodeHandle n;
00278 sub_ = n.subscribe(topic_name, 1, &PieChartDisplay::processMessage, this);
00279 }
00280 }
00281
00282
00283 void PieChartDisplay::unsubscribe()
00284 {
00285 sub_.shutdown();
00286 }
00287
00288 void PieChartDisplay::onEnable()
00289 {
00290 subscribe();
00291 overlay_->show();
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::updateTextColor()
00353 {
00354 text_color_ = text_color_property_->getColor();
00355 }
00356
00357 void PieChartDisplay::updateTextAlpha()
00358 {
00359 text_alpha_ = text_alpha_property_->getFloat() * 255;
00360 }
00361
00362 void PieChartDisplay::updateTextSize()
00363 {
00364 boost::mutex::scoped_lock lock(mutex_);
00365 text_size_ = text_size_property_->getInt();
00366 QFont font;
00367 font.setPointSize(text_size_);
00368 caption_offset_ = QFontMetrics(font).height();
00369
00370 }
00371
00372 void PieChartDisplay::updateShowCaption()
00373 {
00374 show_caption_ = show_caption_property_->getBool();
00375 }
00376
00377
00378 void PieChartDisplay::updateTopic()
00379 {
00380 unsubscribe();
00381 subscribe();
00382 }
00383
00384 void PieChartDisplay::updateAutoColorChange()
00385 {
00386 auto_color_change_ = auto_color_change_property_->getBool();
00387 }
00388
00389 void PieChartDisplay::updateMaxColor()
00390 {
00391 max_color_ = max_color_property_->getColor();
00392 }
00393
00394
00395 }
00396
00397 #include <pluginlib/class_list_macros.h>
00398 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugin::PieChartDisplay, rviz::Display )