Skip to content

Commit d3b8d07

Browse files
mjforanmjforan
authored
Handle time source exception (#1285)
1 parent 325f272 commit d3b8d07

File tree

5 files changed

+103
-30
lines changed

5 files changed

+103
-30
lines changed

‎rviz_common/include/rviz_common/message_filter_display.hpp‎

Lines changed: 21 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@
3333

3434
#include <tf2_ros/message_filter.h>
3535
#include <memory>
36+
#include <string>
3637

3738
#include <message_filters/subscriber.hpp>
3839

@@ -207,19 +208,33 @@ class MessageFilterDisplay : public _RosTopicDisplay
207208
auto msg = std::static_pointer_cast<const MessageType>(type_erased_msg);
208209

209210
++messages_received_;
211+
rviz_common::properties::StatusProperty::Level topic_status_level =
212+
rviz_common::properties::StatusProperty::Ok;
210213
QString topic_str = QString::number(messages_received_) + " messages received";
211214
// Append topic subscription frequency if we can lock rviz_ros_node_.
212215
std::shared_ptr<ros_integration::RosNodeAbstractionIface> node_interface =
213216
rviz_ros_node_.lock();
214217
if (node_interface != nullptr) {
215-
const double duration =
216-
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
217-
const double subscription_frequency =
218-
static_cast<double>(messages_received_) / duration;
219-
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
218+
try {
219+
const double duration =
220+
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
221+
const double subscription_frequency =
222+
static_cast<double>(messages_received_) / duration;
223+
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
224+
} catch (const std::runtime_error & e) {
225+
if (std::string(e.what()).find("can't subtract times with different time sources") !=
226+
std::string::npos)
227+
{
228+
topic_status_level = rviz_common::properties::StatusProperty::Warn;
229+
topic_str += ". ";
230+
topic_str += e.what();
231+
} else {
232+
throw;
233+
}
234+
}
220235
}
221236
setStatus(
222-
properties::StatusProperty::Ok,
237+
topic_status_level,
223238
"Topic",
224239
topic_str);
225240

‎rviz_common/include/rviz_common/ros_topic_display.hpp‎

Lines changed: 20 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -264,18 +264,32 @@ class RosTopicDisplay : public _RosTopicDisplay
264264

265265
++messages_received_;
266266
QString topic_str = QString::number(messages_received_) + " messages received";
267+
rviz_common::properties::StatusProperty::Level topic_status_level =
268+
rviz_common::properties::StatusProperty::Ok;
267269
// Append topic subscription frequency if we can lock rviz_ros_node_.
268270
std::shared_ptr<ros_integration::RosNodeAbstractionIface> node_interface =
269271
rviz_ros_node_.lock();
270272
if (node_interface != nullptr) {
271-
const double duration =
272-
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
273-
const double subscription_frequency =
274-
static_cast<double>(messages_received_) / duration;
275-
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
273+
try {
274+
const double duration =
275+
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
276+
const double subscription_frequency =
277+
static_cast<double>(messages_received_) / duration;
278+
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
279+
} catch (const std::runtime_error & e) {
280+
if (std::string(e.what()).find("can't subtract times with different time sources") !=
281+
std::string::npos)
282+
{
283+
topic_status_level = rviz_common::properties::StatusProperty::Warn;
284+
topic_str += ". ";
285+
topic_str += e.what();
286+
} else {
287+
throw;
288+
}
289+
}
276290
}
277291
setStatus(
278-
properties::StatusProperty::Ok,
292+
topic_status_level,
279293
"Topic",
280294
topic_str);
281295

‎rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp‎

Lines changed: 21 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@
3434
#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__IMAGE_TRANSPORT_DISPLAY_HPP_
3535

3636
#include <memory>
37+
#include <string>
3738

3839
#include "get_transport_from_topic.hpp"
3940
#include "image_transport/image_transport.hpp"
@@ -171,18 +172,32 @@ class ImageTransportDisplay : public rviz_common::_RosTopicDisplay
171172

172173
++messages_received_;
173174
QString topic_str = QString::number(messages_received_) + " messages received";
175+
rviz_common::properties::StatusProperty::Level topic_status_level =
176+
rviz_common::properties::StatusProperty::Ok;
174177
// Append topic subscription frequency if we can lock rviz_ros_node_.
175178
std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_interface =
176179
rviz_ros_node_.lock();
177180
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+
}
183198
}
184199
setStatus(
185-
rviz_common::properties::StatusProperty::Ok,
200+
topic_status_level,
186201
"Topic",
187202
topic_str);
188203

‎rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_transport_display.hpp‎

Lines changed: 21 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@
3232
#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_TRANSPORT_DISPLAY_HPP_
3333

3434
#include <memory>
35+
#include <string>
3536

3637
#include "get_transport_from_topic.hpp"
3738
#include "point_cloud_transport/point_cloud_transport.hpp"
@@ -168,18 +169,32 @@ class PointCloud2TransportDisplay : public rviz_common::_RosTopicDisplay
168169

169170
++messages_received_;
170171
QString topic_str = QString::number(messages_received_) + " messages received";
172+
rviz_common::properties::StatusProperty::Level topic_status_level =
173+
rviz_common::properties::StatusProperty::Ok;
171174
// Append topic subscription frequency if we can lock rviz_ros_node_.
172175
std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_interface =
173176
rviz_ros_node_.lock();
174177
if (node_interface != nullptr) {
175-
const double duration =
176-
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
177-
const double subscription_frequency =
178-
static_cast<double>(messages_received_) / duration;
179-
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
178+
try {
179+
const double duration =
180+
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
181+
const double subscription_frequency =
182+
static_cast<double>(messages_received_) / duration;
183+
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
184+
} catch (const std::runtime_error & e) {
185+
if (std::string(e.what()).find("can't subtract times with different time sources") !=
186+
std::string::npos)
187+
{
188+
topic_status_level = rviz_common::properties::StatusProperty::Warn;
189+
topic_str += ". ";
190+
topic_str += e.what();
191+
} else {
192+
throw;
193+
}
194+
}
180195
}
181196
setStatus(
182-
rviz_common::properties::StatusProperty::Ok,
197+
topic_status_level,
183198
"Topic",
184199
topic_str);
185200

‎rviz_default_plugins/src/rviz_default_plugins/displays/map/map_display.cpp‎

Lines changed: 20 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -360,18 +360,32 @@ void MapDisplay::incomingUpdate(const map_msgs::msg::OccupancyGridUpdate::ConstS
360360

361361
++update_messages_received_;
362362
QString topic_str = QString::number(messages_received_) + " update messages received";
363+
rviz_common::properties::StatusProperty::Level topic_status_level =
364+
rviz_common::properties::StatusProperty::Ok;
363365
// Append topic subscription frequency if we can lock rviz_ros_node_.
364366
std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_interface =
365367
rviz_ros_node_.lock();
366368
if (node_interface != nullptr) {
367-
const double duration =
368-
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
369-
const double subscription_frequency =
370-
static_cast<double>(messages_received_) / duration;
371-
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
369+
try {
370+
const double duration =
371+
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
372+
const double subscription_frequency =
373+
static_cast<double>(messages_received_) / duration;
374+
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
375+
} catch (const std::runtime_error & e) {
376+
if (std::string(e.what()).find("can't subtract times with different time sources") !=
377+
std::string::npos)
378+
{
379+
topic_status_level = rviz_common::properties::StatusProperty::Warn;
380+
topic_str += ". ";
381+
topic_str += e.what();
382+
} else {
383+
throw;
384+
}
385+
}
372386
}
373387
setStatus(
374-
rviz_common::properties::StatusProperty::Ok,
388+
topic_status_level,
375389
"Topic",
376390
topic_str);
377391

0 commit comments

Comments
 (0)