38 #include <OGRE/OgreMaterialManager.h> 39 #include <OGRE/OgreTextureManager.h> 40 #include <OGRE/OgreTexture.h> 41 #include <OGRE/OgreHardwarePixelBuffer.h> 42 #include <OGRE/OgreTechnique.h> 54 : previous_state_(STALL_STATE),
Display()
57 "Topic",
"/diagnostics_agg",
58 ros::message_traits::datatype<diagnostic_msgs::DiagnosticArray>(),
59 "diagnostic_msgs::DiagnosticArray topic to subscribe to.",
62 "diagnostics namespace",
"/",
63 "diagnostics namespace to visualize diagnostics",
66 "type",
"SAC",
"Type of visualization",
this, SLOT(
updateType()));
89 "stall duration", 5.0,
90 "seconds to be regarded as stalled",
113 const diagnostic_msgs::DiagnosticArray::ConstPtr& msg)
117 std::set<std::string> new_namespaces;
118 for (
size_t i = 0; i < msg->status.size(); i++) {
119 new_namespaces.insert(msg->status[i].name);
122 std::set<std::string> difference_namespaces;
124 new_namespaces.begin(), new_namespaces.end(),
125 std::inserter(difference_namespaces,
126 difference_namespaces.end()));
127 if (difference_namespaces.size() != 0) {
132 difference_namespaces.clear();
133 std::set_difference(new_namespaces.begin(), new_namespaces.end(),
135 std::inserter(difference_namespaces,
136 difference_namespaces.end()));
137 if (difference_namespaces.size() != 0) {
147 for (
size_t i = 0; i < msg->status.size(); i++) {
148 diagnostic_msgs::DiagnosticStatus
status = msg->status[i];
150 #if ROS_VERSION_MINIMUM(1,12,0) 152 = std::make_shared<diagnostic_msgs::DiagnosticStatus>(status);
155 = boost::make_shared<diagnostic_msgs::DiagnosticStatus>(status);
169 static int count = 0;
171 ss <<
"OverlayDiagnosticDisplayObject" << count++;
197 t_ = fmod(
t_, overlay_diagnostic_animation_duration);
277 if (
latest_status_->level == diagnostic_msgs::DiagnosticStatus::OK) {
280 else if (
latest_status_->level == diagnostic_msgs::DiagnosticStatus::WARN) {
283 else if (
latest_status_->level == diagnostic_msgs::DiagnosticStatus::ERROR) {
304 if (
latest_status_->level == diagnostic_msgs::DiagnosticStatus::OK) {
308 == diagnostic_msgs::DiagnosticStatus::WARN) {
312 == diagnostic_msgs::DiagnosticStatus::ERROR) {
330 QColor ok_color(25, 255, 240,
alpha_ * 255.0);
331 QColor warn_color(240, 173, 78,
alpha_ * 255.0);
332 QColor error_color(217, 83, 79,
alpha_ * 255.0);
333 QColor stall_color(151, 151, 151,
alpha_ * 255.0);
352 QColor ok_color(40, 40, 40,
alpha_ * 255.0);
353 QColor warn_color(255, 255, 255,
alpha_ * 255.0);
354 QColor error_color(240, 173, 78,
alpha_ * 255.0);
355 QColor stall_color(240, 173, 78,
alpha_ * 255.0);
375 QColor ret (a.red() * a_rate + b.red() * (1 - a_rate),
376 a.green() * a_rate + b.green() * (1 - a_rate),
377 a.blue() * a_rate + b.blue() * (1 - a_rate),
378 a.alpha() * a_rate + b.alpha() * (1 - a_rate));
385 const double r =
size_ / 128.0;
386 QFont
font(
"Liberation Sans", font_size * r, font_size * r, QFont::Bold);
390 painter.setFont(font);
392 QFontMetrics metrics(font);
393 const int text_width = metrics.width(text.c_str());
394 const int text_height = metrics.height();
402 const double r =
size_ / 128.0;
403 QFont
font(
"Liberation Sans", font_size * r, font_size * r, QFont::Bold);
407 painter.setFont(font);
409 QFontMetrics metrics(font);
410 const int text_height = metrics.height();
419 const double font_size,
420 const std::string
text)
422 const double r =
size_ / 128.0;
423 QFont
font(
"Liberation Sans", font_size * r, font_size * r,
false);
427 pen.setColor(
blendColor(fg_color, Qt::white, 0.5));
428 painter.setFont(font);
430 painter.setBrush(fg_color);
431 QFontMetrics metrics(font);
432 const int text_width = metrics.width(text.c_str());
433 const int text_height = metrics.height();
434 if (
overlay_->getTextureWidth() > text_width) {
435 path.addText((
overlay_->getTextureWidth() - text_width) / 2.0,
440 double left = - fmod(
t_, overlay_diagnostic_animation_duration) /
441 overlay_diagnostic_animation_duration * text_width * 2 + text_width;
442 path.addText(left, height, font, text.c_str());
444 painter.drawPath(path);
449 const std::string&
text)
455 overlay_->getTextureHeight() / 3.0 + status_size,
471 + status_size + namespace_size,
480 QPainter painter( &Hud );
482 const int margin = 10;
483 const int inner_line_width = 20;
485 painter.setRenderHint(QPainter::Antialiasing,
true);
486 painter.setPen(QPen(fg_color, line_width, Qt::SolidLine));
487 painter.drawEllipse(line_width / 2, line_width / 2,
491 painter.setPen(QPen(fg_color, inner_line_width, Qt::SolidLine));
492 const double start_angle = fmod(
t_, overlay_diagnostic_animation_duration) /
493 overlay_diagnostic_animation_duration * 360;
494 const double draw_angle = 250;
495 const double inner_circle_start
496 = line_width + margin + inner_line_width / 2.0;
509 painter.setPen(QPen(color, width, Qt::SolidLine));
510 QPainterPath large_rectangle_path;
511 large_rectangle_path.moveTo(A, S - B);
512 large_rectangle_path.lineTo(A, S);
513 large_rectangle_path.lineTo(S, B);
514 large_rectangle_path.lineTo(S, 0);
515 painter.setPen(Qt::NoPen);
516 painter.fillPath(large_rectangle_path, QBrush(color));
517 QPainterPath small_rectangle_path;
518 small_rectangle_path.moveTo(A, S - B);
519 small_rectangle_path.lineTo(A, S - B + C * B);
520 small_rectangle_path.lineTo(A + (S - A) * C, S - B + C * B - (S - B) * C);
521 small_rectangle_path.lineTo(A + (S - A) * C, S - B - (S - B) * C);
522 painter.setPen(Qt::NoPen);
523 painter.fillPath(small_rectangle_path, QBrush(small_color));
537 painter.setPen(QPen(color, width, Qt::SolidLine));
538 QPainterPath large_rectangle_path;
539 large_rectangle_path.moveTo(A, S - B);
540 large_rectangle_path.lineTo(A, S);
541 double r0 = (S - A - D) / 2 / (S - A);
542 large_rectangle_path.lineTo(A + (S - A - D) / 2, S - r0 * (S - B));
543 large_rectangle_path.lineTo(A + (S - A - D) / 2, S - r0 * (S - B) - B);
544 painter.setPen(Qt::NoPen);
545 painter.fillPath(large_rectangle_path, QBrush(color));
547 QPainterPath large_rectangle_path2;
548 large_rectangle_path2.moveTo(S - (S - A - D) / 2, (S - B) * r0);
549 large_rectangle_path2.lineTo(S - (S - A - D) / 2, (S - B) * r0 + B);
550 large_rectangle_path2.lineTo(S, B);
551 large_rectangle_path2.lineTo(S, 0);
552 painter.setPen(Qt::NoPen);
553 painter.fillPath(large_rectangle_path2, QBrush(color));
555 QPainterPath small_rectangle_path;
556 small_rectangle_path.moveTo(A, S - B);
557 small_rectangle_path.lineTo(A, S - B + C * B);
558 small_rectangle_path.lineTo(A + (S - A) * C, S - B + C * B - (S - B) * C);
559 small_rectangle_path.lineTo(A + (S - A) * C, S - B - (S - B) * C);
560 painter.setPen(Qt::NoPen);
561 painter.fillPath(small_rectangle_path, QBrush(small_color));
566 QColor line_color(240, 173, 78,
alpha_ * 255.0);
568 QColor small_rectangle_color(line_color);
569 QPainter painter(&Hud);
576 double max_gap = 0.05 * S;
577 painter.setRenderHint(QPainter::Antialiasing,
true);
578 painter.setPen(QPen(line_color, line_width, Qt::SolidLine));
579 painter.drawLine(QPoint(0, 0), QPoint(0, S));
580 painter.drawLine(QPoint(0, S - B / 2), QPoint(A, S - B / 2));
596 if (state == close_state) {
603 painter.setPen(QPen(
textColor(), 2 * line_width, Qt::SolidLine));
604 painter.setFont(QFont(
"Liberation Sans", 12, QFont::Bold));
605 double theta =
atan2(S - B, S - A) / M_PI * 180;
606 double text_box_height =
cos(theta*M_PI/180) * B;
607 double text_box_width = (S - A) /
cos(theta*M_PI/180) -
sin(theta*M_PI/180) * B * 2;
610 painter.translate(A, S - B);
611 painter.rotate(-theta);
612 if (text_width > text_box_width) {
613 double text_left = - fmod(
t_, overlay_diagnostic_animation_duration) /
614 overlay_diagnostic_animation_duration * text_width;
615 painter.drawText(QRectF(text_left, 0, text_width*2, text_box_height), Qt::AlignLeft | Qt::AlignVCenter | Qt::TextSingleLine,
619 painter.drawText(QRectF(0, 0, text_box_width, text_box_height), Qt::AlignLeft | Qt::AlignVCenter | Qt::TextSingleLine,
627 QColor transparent(0, 0, 0, 0.0);
632 else if (
type_ == 1) {
641 for (std::set<std::string>::iterator it =
namespaces_.begin();
689 return (top_ < y && top_ + size_ > y &&
690 left_ < x && left_ + size_ > x);
virtual double textWidth(QPainter &painter, double font_size, const std::string &text)
virtual void updateAlpha()
virtual bool isAnimating()
virtual void processMessage(const diagnostic_msgs::DiagnosticArray::ConstPtr &msg)
virtual bool isInRegion(int x, int y)
rviz::FloatProperty * stall_duration_property_
virtual double drawAnimatingText(QPainter &painter, QColor fg_color, const double height, const double font_size, const std::string text)
const double overlay_diagnostic_animation_duration
virtual QImage getQImage(unsigned int width, unsigned int height)
virtual void fillNamespaceList()
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 void clearOptions()
boost::shared_ptr< diagnostic_msgs::DiagnosticStatus > latest_status_
virtual void updateStallDuration()
virtual void update(float wall_dt, float ros_dt)
virtual QColor blendColor(QColor a, QColor b, double a_rate)
virtual void onInitialize()
virtual ~OverlayDiagnosticDisplay()
rviz::FloatProperty * alpha_property_
void addOptionStd(const std::string &option)
std::string getTopicStd() const
virtual void drawEVANonConnectedRectangle(QPainter &painter, QColor color, QColor small_color, int width, double gap)
OverlayDiagnosticDisplay()
virtual void updateDiagnosticsNamespace()
std::string getStdString()
ros::WallTime animation_start_time_
rviz::IntProperty * top_property_
std::string diagnostics_namespace_
virtual double textHeight(QPainter &painter, double font_size)
virtual State getLatestState()
virtual double animationRate()
virtual void unsubscribe()
void addOptionStd(const std::string &option, int value=0)
rviz::RosTopicProperty * ros_topic_property_
virtual void updateRosTopic()
virtual int getInt() const
virtual std::string statusText()
rviz::EnumProperty * type_property_
virtual void drawEVAConnectedRectangle(QPainter &painter, QColor color, QColor small_color, int width)
virtual void updateType()
bool setValue(const QVariant &new_value) override
OverlayObject::Ptr overlay_
virtual void drawEVA(QImage &Hud)
virtual float getFloat() const
rviz::EditableEnumProperty * diagnostics_namespace_property_
virtual QColor textColor()
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
virtual void movePosition(int x, int y)
virtual QColor foregroundColor()
virtual void drawSAC(QImage &Hud)
virtual void drawText(QPainter &painter, QColor fg_color, const std::string &text)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
rviz::IntProperty * left_property_
virtual int getOptionInt()
rviz::IntProperty * size_property_
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
std::set< std::string > namespaces_
const double overlay_diagnostic_animation_transition_duration
virtual void updateSize()
ros::WallTime latest_message_time_
virtual void setPosition(int x, int y)
virtual void updateLeft()