|
34 | 34 | #define RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__IMAGE_TRANSPORT_DISPLAY_HPP_ |
35 | 35 |
|
36 | 36 | #include <memory> |
| 37 | +#include <string> |
37 | 38 |
|
38 | 39 | #include "get_transport_from_topic.hpp" |
39 | 40 | #include "image_transport/image_transport.hpp" |
@@ -171,18 +172,32 @@ class ImageTransportDisplay : public rviz_common::_RosTopicDisplay |
171 | 172 |
|
172 | 173 | ++messages_received_; |
173 | 174 | QString topic_str = QString::number(messages_received_) + " messages received"; |
| 175 | + rviz_common::properties::StatusProperty::Level topic_status_level = |
| 176 | + rviz_common::properties::StatusProperty::Ok; |
174 | 177 | // Append topic subscription frequency if we can lock rviz_ros_node_. |
175 | 178 | std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_interface = |
176 | 179 | rviz_ros_node_.lock(); |
177 | 180 | if (node_interface != nullptr) { |
178 | | - const double duration = |
179 | | - (node_interface->get_raw_node()->now() - subscription_start_time_).seconds(); |
180 | | - const double subscription_frequency = |
181 | | - static_cast<double>(messages_received_) / duration; |
182 | | - topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz."; |
| 181 | + try { |
| 182 | + const double duration = |
| 183 | + (node_interface->get_raw_node()->now() - subscription_start_time_).seconds(); |
| 184 | + const double subscription_frequency = |
| 185 | + static_cast<double>(messages_received_) / duration; |
| 186 | + topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz."; |
| 187 | + } catch (const std::runtime_error & e) { |
| 188 | + if (std::string(e.what()).find("can't subtract times with different time sources") != |
| 189 | + std::string::npos) |
| 190 | + { |
| 191 | + topic_status_level = rviz_common::properties::StatusProperty::Warn; |
| 192 | + topic_str += ". "; |
| 193 | + topic_str += e.what(); |
| 194 | + } else { |
| 195 | + throw; |
| 196 | + } |
| 197 | + } |
183 | 198 | } |
184 | 199 | setStatus( |
185 | | - rviz_common::properties::StatusProperty::Ok, |
| 200 | + topic_status_level, |
186 | 201 | "Topic", |
187 | 202 | topic_str); |
188 | 203 |
|
|
0 commit comments