diagnostics_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 "diagnostics_display.h"
37 #include <boost/algorithm/string/predicate.hpp>
38 #include <boost/lexical_cast.hpp>
39 
40 namespace jsk_rviz_plugins
41 {
42 
44  : rviz::Display(), msg_(0)
45  {
48  "Topic", "/diagnostics_agg",
49  ros::message_traits::datatype<diagnostic_msgs::DiagnosticArray>(),
50  "diagnostic_msgs::DiagnosticArray topic to subscribe to.",
51  this, SLOT( updateRosTopic() ));
54  "the parent frame_id to visualize diagnostics",
55  this, 0, true);
57  "diagnostics namespace", "/",
58  "diagnostics namespace to visualize diagnostics",
59  this, SLOT(updateDiagnosticsNamespace()));
61  "radius", 1.0,
62  "radius of diagnostics circle",
63  this, SLOT(updateRadius()));
65  "line width", 0.03,
66  "line width",
67  this, SLOT(updateLineWidth()));
69  "axis", "x",
70  "axis",
71  this, SLOT(updateAxis()));
72  axis_property_->addOption("x", 0);
73  axis_property_->addOption("y", 1);
74  axis_property_->addOption("z", 2);
76  "font size", 0.05,
77  "font size",
78  this, SLOT(updateFontSize()));
79  }
80 
82  {
83  delete ros_topic_property_;
84  delete frame_id_property_;
86  delete radius_property_;
87  delete line_width_property_;
88  delete axis_property_;
89  delete line_;
90  delete msg_;
91  delete font_size_property_;
92  }
93 
94  void DiagnosticsDisplay::update(float wall_dt, float ros_dt)
95  {
97  updateLine();
98  }
99 
100  if (!isEnabled()) {
101  return;
102  }
103 
105 
106  const float round_trip = 10.0;
107  Ogre::Quaternion orientation;
108  Ogre::Vector3 position;
109  std::string frame_id = frame_id_property_->getFrame().toStdString();
110  if( !context_->getFrameManager()->getTransform( frame_id,
111  ros::Time(0.0),
112  position, orientation ))
113  {
114  ROS_WARN( "Error transforming from frame '%s' to frame '%s'",
115  frame_id.c_str(), qPrintable( fixed_frame_ ));
116  return;
117  }
118  scene_node_->setPosition(position);
119  scene_node_->setOrientation(orientation);
120  Ogre::Vector3 orbit_position;
121  orbit_theta_ = ros_dt / round_trip * M_PI * 2.0 + orbit_theta_;
122  while (orbit_theta_ > M_PI * 2) {
123  orbit_theta_ -= M_PI*2;
124  }
125  if (axis_ == 0) { // x
126  orbit_position.x = radius_ * cos(orbit_theta_);
127  orbit_position.y = radius_ * sin(orbit_theta_);
128  orbit_position.z = 0;
129  }
130  else if (axis_ == 1) { // y
131  orbit_position.y = radius_ * cos(orbit_theta_);
132  orbit_position.z = radius_ * sin(orbit_theta_);
133  orbit_position.x = 0;
134  }
135  else if (axis_ == 2) { // z
136  orbit_position.z = radius_ * cos(orbit_theta_);
137  orbit_position.x = radius_ * sin(orbit_theta_);
138  orbit_position.y = 0;
139  }
140 
141  orbit_node_->setPosition(orbit_position);
142  if (!isEnabled()) {
143  return;
144  }
146  }
147 
149  {
150  static int counter = 0;
151  scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
152  orbit_node_ = scene_node_->createChildSceneNode(); // ??
154  msg_ = new rviz::MovableText("not initialized", "Liberation Sans", 0.05);
155  msg_->setTextAlignment(rviz::MovableText::H_CENTER,
158  orbit_node_->attachObject(msg_);
159  msg_->setVisible(false);
160  orbit_theta_ = M_PI * 2.0 / 6 * counter++;
161  updateLineWidth();
162  updateAxis();
164  updateRadius();
165  updateRosTopic();
166  updateFontSize();
167  }
168 
170  (const diagnostic_msgs::DiagnosticArray::ConstPtr& msg)
171  {
172  if (!isEnabled()) {
173  return;
174  }
175 
176  // update namespaces_ if needed
177  std::set<std::string> new_namespaces;
178  for (size_t i = 0; i < msg->status.size(); i++) {
179  new_namespaces.insert(msg->status[i].name);
180  }
181 
182  std::set<std::string> difference_namespaces;
183  std::set_difference(namespaces_.begin(), namespaces_.end(),
184  new_namespaces.begin(), new_namespaces.end(),
185  std::inserter(difference_namespaces,
186  difference_namespaces.end()));
187  if (difference_namespaces.size() != 0) {
188  namespaces_ = new_namespaces;
190  }
191  else {
192  difference_namespaces.clear();
193  std::set_difference(new_namespaces.begin(), new_namespaces.end(),
194  namespaces_.begin(), namespaces_.end(),
195  std::inserter(difference_namespaces,
196  difference_namespaces.end()));
197  if (difference_namespaces.size() != 0) {
198  namespaces_ = new_namespaces;
200  }
201  }
202 
203  if (diagnostics_namespace_.length() == 0) {
204  return;
205  }
206 
207  const float alpha = 0.8;
208  const Ogre::ColourValue OK(0.3568627450980392, 0.7529411764705882, 0.8705882352941177, alpha);
209  const Ogre::ColourValue WARN(0.9411764705882353, 0.6784313725490196, 0.3058823529411765, alpha);
210  const Ogre::ColourValue ERROR(0.8509803921568627, 0.3254901960784314, 0.30980392156862746, 0.5);
211  const Ogre::ColourValue UNKNOWN(0.2, 0.2, 0.2, 0.5);
212  Ogre::ColourValue color;
213  std::string message;
214  bool foundp = false;
215  for (size_t i = 0; i < msg->status.size(); i++) {
216  diagnostic_msgs::DiagnosticStatus status = msg->status[i];
217  if (status.name == diagnostics_namespace_) {
218  if (status.level == diagnostic_msgs::DiagnosticStatus::OK) {
219  color = OK;
220  message = status.message;
221  }
222  else if (status.level == diagnostic_msgs::DiagnosticStatus::WARN) {
223  color = WARN;
224  message = status.message;
225  }
226  else if (status.level == diagnostic_msgs::DiagnosticStatus::ERROR) {
227  color = ERROR;
228  message = status.message;
229  }
230  else {
231  // unknwon
232  color = UNKNOWN;
233  message = "unknown";
234  }
235  foundp = true;
236  break;
237  }
238  }
239 
240  if (!foundp) {
241  color = UNKNOWN;
242  message = "stall";
243  }
244 
245  line_->setColor(color.r, color.g, color.b, color.a);
246  Ogre::ColourValue font_color(color);
247  font_color.a = 1.0;
248  msg_->setColor(font_color);
249  msg_->setCaption(diagnostics_namespace_ + "\n" + message);
251  }
252 
254  {
255  line_->clear();
257  line_->setNumLines(1);
258  line_->setMaxPointsPerLine(1024);
259  for (size_t i = 0; i < 128 + 1; i++) {
260  Ogre::Vector3 step_position;
261  const double theta = M_PI * 2.0 / 128 * i;
262  if (axis_ == 0) {
263  step_position.x = radius_ * cos(theta);
264  step_position.y = radius_ * sin(theta);
265  step_position.z = 0.0;
266  }
267  else if (axis_ == 1) {
268  step_position.y = radius_ * cos(theta);
269  step_position.z = radius_ * sin(theta);
270  step_position.x = 0;
271  }
272  else if (axis_ == 2) {
273  step_position.z = radius_ * cos(theta);
274  step_position.x = radius_ * sin(theta);
275  step_position.y = 0;
276  }
277  line_->addPoint(step_position);
278  }
279  line_update_required_ = false;
280  }
281 
283  {
284  line_update_required_ = true;
285  msg_->setVisible(true);
286  }
287 
289  {
290  unsubscribe();
291  line_->clear();
292  msg_->setVisible(false);
293  }
294 
296  {
297  sub_.shutdown();
298  }
299 
301  {
302  ros::NodeHandle n;
304  1,
306  this);
307  }
308 
310  {
311  unsubscribe();
312  subscribe();
313  }
314 
316  {
318  }
319 
321  {
323  line_update_required_ = true;
324  }
325 
327  {
329  line_update_required_ = true;
330  }
331 
332 
334  {
336  line_update_required_ = true;
337  }
338 
340  {
341  //QApplication::setOverrideCursor(QCursor(Qt::WaitCursor));
343  for (std::set<std::string>::iterator it = namespaces_.begin();
344  it != namespaces_.end();
345  it++) {
347  }
349  }
350 
352  {
354  }
355 
356 }
357 
void addPoint(const Ogre::Vector3 &point)
WARN
int counter
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)
DisplayContext * context_
virtual void clearOptions()
virtual void processMessage(const diagnostic_msgs::DiagnosticArray::ConstPtr &msg)
virtual void update(float wall_dt, float ros_dt)
void setCharacterHeight(Ogre::Real height)
void addOptionStd(const std::string &option)
#define ROS_WARN(...)
std::string getTopicStd() const
Ogre::SceneNode * scene_node_
std::string getStdString()
QString fixed_frame_
status
virtual void addOption(const QString &option, int value=0)
void setCaption(const Ogre::String &caption)
virtual FrameManager * getFrameManager() const=0
rviz::RosTopicProperty * ros_topic_property_
rviz::TfFrameProperty * frame_id_property_
void setColor(float r, float g, float b, float a) override
void setNumLines(uint32_t num)
UNKNOWN
rviz::EditableEnumProperty * diagnostics_namespace_property_
void setMaxPointsPerLine(uint32_t max)
Ogre::SceneManager * scene_manager_
OK
virtual Ogre::SceneManager * getSceneManager() const=0
virtual void queueRender()=0
bool isEnabled() const
void setColor(const Ogre::ColourValue &color)
static const QString FIXED_FRAME_STRING
virtual float getFloat() const
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
void setFrameManager(FrameManager *frame_manager)
QString getFrame() const
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
virtual int getOptionInt()
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
ERROR
void setLineWidth(float width)


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