Class RosGraphMonitor

Nested Relationships

Nested Types

Class Documentation

class RosGraphMonitor

Monitors the ROS application graph, providing diagnostics about its health.

Public Functions

RosGraphMonitor(rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, std::function<rclcpp::Time()> now_fn, rclcpp::Logger logger, QueryParams query_params, GraphMonitorConfiguration config = GraphMonitorConfiguration{})

Constructor.

Parameters:
  • node_graph – Interface from owning Node to retrieve information about the ROS graph

  • now_fn – Function to fetch the current time as defined in the owning context

  • logger

  • config – Includes/excludes the entities to care about in diagnostic reporting

  • query_params – Function to query parameters of a node by name

virtual ~RosGraphMonitor()
void evaluate(std::vector<diagnostic_msgs::msg::DiagnosticStatus> &status)

Return diagnostics of latest graph understanding.

Returns:

A message filled with all current conditions. May be empty array, but never nullptr

bool wait_for_update(std::chrono::milliseconds timeout)

Wait until next graph update is integrated into the monitor.

Parameters:

timeout

Returns:

True if there were graph updates detected, or false on timeout

GraphMonitorConfiguration &config()
Returns:

Mutable reference to the configuration, for updating

const GraphMonitorConfiguration &config() const
Returns:

Const reference to configuration

void on_topic_statistics(const rosgraph_monitor_msgs::msg::TopicStatistics &statistics)

Integrate new topic statistics input to determine if topics are meeting contracts.

Parameters:

statistics – Incoming statistics list

void fill_rosgraph_msg(rosgraph_monitor_msgs::msg::Graph &msg)

Fill a Graph message containing current graph state.

void set_graph_change_callback(std::function<void(rosgraph_monitor_msgs::msg::Graph&)> callback)

Set callback function to be called when graph changes.

Parameters:

callback – Function to call when graph updates occur

Protected Types

typedef std::map<std::string, std::vector<std::string>> TopicsToTypes
typedef std::unordered_map<RosRmwGid, EndpointTracking> EndpointTrackingMap
typedef std::unordered_set<RosRmwGid> EndpointSet
typedef std::pair<std::string, std::string> NodeAndTopic

Protected Functions

void update_graph()

Update internal graph representation and detect changes.

void watch_for_updates()

Called in thread to watch the graph in infinite loop and rebuild tracking on changes.

bool ignore_node(const std::string &node_name)

Should we skip tracking this node?

Parameters:

node_name

Returns:

Whether to ignore tracking the node

void track_node_updates(const std::vector<std::string> &observed_node_names)

Check current observed state against our tracked state, updating tracking info.

Parameters:

observed_node_names

void track_endpoint_updates(const TopicsToTypes &observed_topics_and_types)

Check current observed state against our tracked state, updating tracking info.

Parameters:

observed_topics_and_types

std::optional<EndpointTrackingMap::iterator> add_publisher(const std::string &topic_name, const rclcpp::TopicEndpointInfo &info)
Returns:

Iterator to existing or added publisher, or nullopt if node ignored

std::optional<RosRmwGid> lookup_publisher(const std::string &node_name, const std::string &topic_name) const
Returns:

GID of a publisher if found, else nullopt if such an endpoint not tracked.

std::optional<EndpointTrackingMap::iterator> add_subscription(const std::string &topic_name, const rclcpp::TopicEndpointInfo &info)
Returns:

Iterator to existing or added publisher, or nullopt if node ignored

std::optional<RosRmwGid> lookup_subscription(const std::string &node_name, const std::string &topic_name) const
Returns:

GID of a publisher if found, else nullopt if such an endpoint not tracked.

bool topic_period_ok(const rosgraph_monitor_msgs::msg::TopicStatistic &stat, const rclcpp::Duration &deadline) const
void statusWrapper(diagnostic_updater::DiagnosticStatusWrapper &msg, uint8_t level, const std::string &message, const std::string &subname) const
void query_node_parameters(const std::string &node_name)

Query parameters for a newly discovered node.

Parameters:

node_name – The name of the node to query parameters for

Protected Attributes

GraphMonitorConfiguration config_
std::function<rclcpp::Time()> now_fn_
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_
rclcpp::Logger logger_
std::atomic_bool shutdown_ = false
rclcpp::Event::SharedPtr graph_change_event_
std::thread watch_thread_
Event update_event_
std::function<void()> graph_change_callback_
QueryParams query_params_
std::unordered_map<std::string, NodeTracking> nodes_
EndpointTrackingMap publishers_
EndpointTrackingMap subscriptions_
std::unordered_map<NodeAndTopic, RosRmwGid> publisher_lookup_
std::unordered_map<NodeAndTopic, RosRmwGid> subscription_lookup_
std::unordered_set<std::string> ignored_nodes_
std::unordered_set<std::string> returned_nodes_
std::unordered_map<std::string, TopicTracking> topic_endpoint_counts_
std::unordered_set<std::string> pubs_with_no_subs_
std::unordered_set<std::string> subs_with_no_pubs_
std::unordered_map<std::string, std::shared_future<void>> params_futures
struct EndpointTracking

Keeps information and flags about observed Publishers/Subscriptions over time.

Public Functions

rosgraph_monitor_msgs::msg::Topic to_msg()
EndpointTracking(const std::string &topic_name, const rclcpp::TopicEndpointInfo &info, const rclcpp::Time &now)

Public Members

bool stale = false
const std::string topic_name
const std::string node_name
const rclcpp::TopicEndpointInfo info
rclcpp::Time last_stats_timestamp
std::optional<rosgraph_monitor_msgs::msg::TopicStatistic> period_stat
struct NodeTracking

Keeps flags for tracking observed nodes over time.

Public Functions

explicit NodeTracking(const std::string &name)

Public Members

std::string name
bool missing = false
bool stale = false
std::vector<ParameterTracking> params
struct ParameterTracking

Public Functions

rcl_interfaces::msg::ParameterDescriptor to_msg() const

Public Members

std::string name
uint8_t type
struct TopicTracking

Keeps aggregate info about a topic as a whole over time.

Public Members

size_t pubs = 0
size_t subs = 0