Skip to content

Commit

Permalink
Handle time source exception (#1285)
Browse files Browse the repository at this point in the history
  • Loading branch information
@mjforan
mjforan authored Oct 10, 2024
1 parent 325f272 commit d3b8d07
Show file tree
Hide file tree
Showing 5 changed files with 103 additions and 30 deletions.
27 changes: 21 additions & 6 deletions rviz_common/include/rviz_common/message_filter_display.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@

#include <tf2_ros/message_filter.h>
#include <memory>
#include <string>

#include <message_filters/subscriber.hpp>

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

++messages_received_;
rviz_common::properties::StatusProperty::Level topic_status_level =
rviz_common::properties::StatusProperty::Ok;
QString topic_str = QString::number(messages_received_) + " messages received";
// Append topic subscription frequency if we can lock rviz_ros_node_.
std::shared_ptr<ros_integration::RosNodeAbstractionIface> node_interface =
rviz_ros_node_.lock();
if (node_interface != nullptr) {
const double duration =
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
const double subscription_frequency =
static_cast<double>(messages_received_) / duration;
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
try {
const double duration =
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
const double subscription_frequency =
static_cast<double>(messages_received_) / duration;
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
} catch (const std::runtime_error & e) {
if (std::string(e.what()).find("can't subtract times with different time sources") !=
std::string::npos)
{
topic_status_level = rviz_common::properties::StatusProperty::Warn;
topic_str += ". ";
topic_str += e.what();
} else {
throw;
}
}
}
setStatus(
properties::StatusProperty::Ok,
topic_status_level,
"Topic",
topic_str);

Expand Down
26 changes: 20 additions & 6 deletions rviz_common/include/rviz_common/ros_topic_display.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -264,18 +264,32 @@ class RosTopicDisplay : public _RosTopicDisplay

++messages_received_;
QString topic_str = QString::number(messages_received_) + " messages received";
rviz_common::properties::StatusProperty::Level topic_status_level =
rviz_common::properties::StatusProperty::Ok;
// Append topic subscription frequency if we can lock rviz_ros_node_.
std::shared_ptr<ros_integration::RosNodeAbstractionIface> node_interface =
rviz_ros_node_.lock();
if (node_interface != nullptr) {
const double duration =
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
const double subscription_frequency =
static_cast<double>(messages_received_) / duration;
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
try {
const double duration =
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
const double subscription_frequency =
static_cast<double>(messages_received_) / duration;
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
} catch (const std::runtime_error & e) {
if (std::string(e.what()).find("can't subtract times with different time sources") !=
std::string::npos)
{
topic_status_level = rviz_common::properties::StatusProperty::Warn;
topic_str += ". ";
topic_str += e.what();
} else {
throw;
}
}
}
setStatus(
properties::StatusProperty::Ok,
topic_status_level,
"Topic",
topic_str);

Expand Down
27 changes: 21 additions & 6 deletions rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__IMAGE_TRANSPORT_DISPLAY_HPP_

#include <memory>
#include <string>

#include "get_transport_from_topic.hpp"
#include "image_transport/image_transport.hpp"
Expand Down Expand Up @@ -171,18 +172,32 @@ class ImageTransportDisplay : public rviz_common::_RosTopicDisplay

++messages_received_;
QString topic_str = QString::number(messages_received_) + " messages received";
rviz_common::properties::StatusProperty::Level topic_status_level =
rviz_common::properties::StatusProperty::Ok;
// Append topic subscription frequency if we can lock rviz_ros_node_.
std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_interface =
rviz_ros_node_.lock();
if (node_interface != nullptr) {
const double duration =
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
const double subscription_frequency =
static_cast<double>(messages_received_) / duration;
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
try {
const double duration =
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
const double subscription_frequency =
static_cast<double>(messages_received_) / duration;
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
} catch (const std::runtime_error & e) {
if (std::string(e.what()).find("can't subtract times with different time sources") !=
std::string::npos)
{
topic_status_level = rviz_common::properties::StatusProperty::Warn;
topic_str += ". ";
topic_str += e.what();
} else {
throw;
}
}
}
setStatus(
rviz_common::properties::StatusProperty::Ok,
topic_status_level,
"Topic",
topic_str);

Expand Down
27 changes: 21 additions & 6 deletions ...lugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_transport_display.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_TRANSPORT_DISPLAY_HPP_

#include <memory>
#include <string>

#include "get_transport_from_topic.hpp"
#include "point_cloud_transport/point_cloud_transport.hpp"
Expand Down Expand Up @@ -168,18 +169,32 @@ class PointCloud2TransportDisplay : public rviz_common::_RosTopicDisplay

++messages_received_;
QString topic_str = QString::number(messages_received_) + " messages received";
rviz_common::properties::StatusProperty::Level topic_status_level =
rviz_common::properties::StatusProperty::Ok;
// Append topic subscription frequency if we can lock rviz_ros_node_.
std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_interface =
rviz_ros_node_.lock();
if (node_interface != nullptr) {
const double duration =
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
const double subscription_frequency =
static_cast<double>(messages_received_) / duration;
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
try {
const double duration =
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
const double subscription_frequency =
static_cast<double>(messages_received_) / duration;
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
} catch (const std::runtime_error & e) {
if (std::string(e.what()).find("can't subtract times with different time sources") !=
std::string::npos)
{
topic_status_level = rviz_common::properties::StatusProperty::Warn;
topic_str += ". ";
topic_str += e.what();
} else {
throw;
}
}
}
setStatus(
rviz_common::properties::StatusProperty::Ok,
topic_status_level,
"Topic",
topic_str);

Expand Down
26 changes: 20 additions & 6 deletions rviz_default_plugins/src/rviz_default_plugins/displays/map/map_display.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -360,18 +360,32 @@ void MapDisplay::incomingUpdate(const map_msgs::msg::OccupancyGridUpdate::ConstS

++update_messages_received_;
QString topic_str = QString::number(messages_received_) + " update messages received";
rviz_common::properties::StatusProperty::Level topic_status_level =
rviz_common::properties::StatusProperty::Ok;
// Append topic subscription frequency if we can lock rviz_ros_node_.
std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_interface =
rviz_ros_node_.lock();
if (node_interface != nullptr) {
const double duration =
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
const double subscription_frequency =
static_cast<double>(messages_received_) / duration;
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
try {
const double duration =
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
const double subscription_frequency =
static_cast<double>(messages_received_) / duration;
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
} catch (const std::runtime_error & e) {
if (std::string(e.what()).find("can't subtract times with different time sources") !=
std::string::npos)
{
topic_status_level = rviz_common::properties::StatusProperty::Warn;
topic_str += ". ";
topic_str += e.what();
} else {
throw;
}
}
}
setStatus(
rviz_common::properties::StatusProperty::Ok,
topic_status_level,
"Topic",
topic_str);

Expand Down

0 comments on commit d3b8d07

Please sign in to comment.