use std::collections::BTreeSet;
use std::sync::Arc;
use std::time::{Duration, Instant};
use astrs_ros2::graph::{EndpointInfo, GraphQuery, NodeInfo};
use astrs_ros2::names::mangle::TopicKind;
use astrs_ros2::node::{ContextOptions, NodeOptions, Ros2Context, Ros2Node};
use astrs_rtps::behavior::MulticastCapability;
use astrs_rtps::discovery::RosCompat;
use astrs_rtps::structure::Locator;
use serde::Serialize;
use crate::error::CliError;
pub const DEFAULT_DOMAIN_ID: u32 = 0;
pub const ENV_DOMAIN_ID: &str = "ROS_DOMAIN_ID";
pub const DEFAULT_TIMEOUT: Duration = Duration::from_secs(2);
pub const QUIET_PERIOD: Duration = Duration::from_millis(400);
pub const POLL_INTERVAL: Duration = Duration::from_millis(25);
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct ProbeArgs {
pub domain_id: u32,
pub timeout: Duration,
pub include_hidden: bool,
pub json: bool,
}
impl Default for ProbeArgs {
fn default() -> Self {
Self {
domain_id: domain_id_from_env(),
timeout: DEFAULT_TIMEOUT,
include_hidden: false,
json: false,
}
}
}
impl ProbeArgs {
#[must_use]
pub const fn query(&self) -> GraphQuery {
let query = GraphQuery::new().remote_only();
if self.include_hidden {
query.with_hidden()
} else {
query
}
}
}
#[must_use]
pub fn domain_id_from_env() -> u32 {
std::env::var(ENV_DOMAIN_ID)
.ok()
.and_then(|raw| raw.trim().parse::<u32>().ok())
.unwrap_or(DEFAULT_DOMAIN_ID)
}
#[derive(Debug, Clone, PartialEq, Eq, Serialize)]
pub struct ParticipantSummary {
pub guid: String,
pub is_astrs: bool,
pub nodes: Vec<String>,
}
#[derive(Debug, Clone, PartialEq, Eq, Serialize)]
pub struct TopicSummary {
pub name: String,
pub types: Vec<String>,
pub publishers: usize,
pub subscribers: usize,
pub publisher_nodes: Vec<String>,
pub subscriber_nodes: Vec<String>,
}
#[derive(Debug, Clone, PartialEq, Eq, Serialize)]
pub struct ProbeReport {
pub domain_id: u32,
pub listened_ms: u64,
pub multicast: String,
pub multicast_joined: bool,
pub locator: String,
pub participants: Vec<ParticipantSummary>,
pub nodes: Vec<String>,
pub topics: Vec<TopicSummary>,
pub services: Vec<TopicSummary>,
pub actions: Vec<TopicSummary>,
pub unclaimed_endpoints: usize,
}
impl ProbeReport {
#[must_use]
pub fn endpoint_count(&self) -> usize {
self.topics
.iter()
.chain(&self.services)
.chain(&self.actions)
.map(|entry| entry.publishers.saturating_add(entry.subscribers))
.sum()
}
#[must_use]
pub fn is_empty(&self) -> bool {
self.participants.is_empty() && self.topics.is_empty()
}
}
pub fn probe(args: &ProbeArgs) -> Result<ProbeReport, CliError> {
let runtime = tokio::runtime::Builder::new_multi_thread()
.worker_threads(2)
.enable_all()
.build()
.map_err(|error| CliError::Ros2(format!("no runtime for the probe: {error}")))?;
runtime.block_on(probe_async(args))
}
pub async fn probe_async(args: &ProbeArgs) -> Result<ProbeReport, CliError> {
let started = Instant::now();
let options = ContextOptions::new(args.domain_id)
.with_compat(RosCompat::Jazzy)
.with_multicast(true);
let context = Ros2Context::new(options)
.await
.map_err(|error| CliError::Ros2(format!("the probe participant: {error}")))?;
let node = Ros2Node::new(
Arc::clone(&context),
format!("astrs_probe_{}", std::process::id()),
NodeOptions::default().with_parameter_services(false),
)
.await
.map_err(|error| CliError::Ros2(format!("the probe node: {error}")))?;
let report = listen(&context, &node, args, started).await;
if let Err(error) = node.shutdown().await {
tracing::debug!(%error, "the probe node was already shut down");
}
context.shutdown().await;
report
}
async fn listen(
context: &Arc<Ros2Context>,
node: &Ros2Node,
args: &ProbeArgs,
started: Instant,
) -> Result<ProbeReport, CliError> {
let deadline = started + args.timeout;
let graph = node.graph();
let mut seen = 0_usize;
let mut last_change = Instant::now();
while Instant::now() < deadline {
let count = graph.known_participants().await;
if count != seen {
seen = count;
last_change = Instant::now();
} else if seen > 0 && last_change.elapsed() >= QUIET_PERIOD {
break;
}
tokio::time::sleep(POLL_INTERVAL).await;
}
let query = args.query();
let nodes = graph.nodes(query).await;
let endpoints = graph.endpoints(query).await;
Ok(ProbeReport {
domain_id: args.domain_id,
listened_ms: u64::try_from(started.elapsed().as_millis()).unwrap_or(u64::MAX),
multicast: describe_multicast(context.multicast()),
multicast_joined: context.multicast().is_joined(),
locator: describe_locator(context.metatraffic_locator()),
participants: summarize_participants(&nodes, graph.known_participants().await),
nodes: node_names(&nodes),
topics: summarize(&endpoints, TopicKind::Topic),
services: summarize(&endpoints, TopicKind::Request),
actions: summarize_actions(&endpoints),
unclaimed_endpoints: endpoints
.iter()
.filter(|endpoint| endpoint.is_unclaimed())
.count(),
})
}
#[must_use]
pub fn describe_multicast(capability: &MulticastCapability) -> String {
match capability {
MulticastCapability::Joined { group } => format!("joined {group}"),
MulticastCapability::Disabled => "disabled".to_owned(),
MulticastCapability::Refused { group, reason } => {
format!("refused {group} ({reason})")
}
}
}
#[must_use]
pub fn describe_locator(locator: Locator) -> String {
locator.socket_addr().map_or_else(
|_| "unaddressable".to_owned(),
|address| address.to_string(),
)
}
fn node_names(nodes: &[NodeInfo]) -> Vec<String> {
let mut names: Vec<String> = nodes.iter().map(NodeInfo::fully_qualified).collect();
names.sort_unstable();
names.dedup();
names
}
fn summarize_participants(nodes: &[NodeInfo], known: usize) -> Vec<ParticipantSummary> {
use std::collections::BTreeMap;
let mut by_participant: BTreeMap<String, ParticipantSummary> = BTreeMap::new();
for info in nodes {
let guid = info.participant.prefix.to_string();
let entry = by_participant
.entry(guid.clone())
.or_insert_with(|| ParticipantSummary {
is_astrs: info.participant.prefix.vendor_id()
== astrs_rtps::structure::VendorId::ASTRS,
guid,
nodes: Vec::new(),
});
entry.nodes.push(info.fully_qualified());
}
for entry in by_participant.values_mut() {
entry.nodes.sort_unstable();
entry.nodes.dedup();
}
let listed = by_participant.len();
let mut summaries: Vec<ParticipantSummary> = by_participant.into_values().collect();
for index in listed..known {
summaries.push(ParticipantSummary {
guid: format!("<unannounced #{}>", index.saturating_sub(listed) + 1),
is_astrs: false,
nodes: Vec::new(),
});
}
summaries
}
fn summarize(endpoints: &[EndpointInfo], kind: TopicKind) -> Vec<TopicSummary> {
use std::collections::BTreeMap;
let mut grouped: BTreeMap<&str, Grouped> = BTreeMap::new();
for endpoint in endpoints {
let matches = match kind {
TopicKind::Topic => endpoint.kind == TopicKind::Topic,
_ => endpoint.kind.is_service(),
};
if !matches || is_action_endpoint(&endpoint.name) {
continue;
}
grouped
.entry(endpoint.name.as_str())
.or_default()
.absorb(endpoint);
}
grouped
.into_iter()
.map(|(name, group)| group.finish(name.to_owned()))
.collect()
}
fn summarize_actions(endpoints: &[EndpointInfo]) -> Vec<TopicSummary> {
use std::collections::BTreeMap;
let mut grouped: BTreeMap<String, Grouped> = BTreeMap::new();
for endpoint in endpoints {
let Some((action, _)) = astrs_ros2::names::mangle::split_action_name(&endpoint.name) else {
continue;
};
grouped
.entry(action.to_owned())
.or_default()
.absorb(endpoint);
}
grouped
.into_iter()
.map(|(name, group)| group.finish(name))
.collect()
}
fn is_action_endpoint(name: &str) -> bool {
astrs_ros2::names::mangle::split_action_name(name).is_some()
}
#[derive(Debug, Default)]
struct Grouped {
types: BTreeSet<String>,
publishers: usize,
subscribers: usize,
publisher_nodes: BTreeSet<String>,
subscriber_nodes: BTreeSet<String>,
}
impl Grouped {
fn absorb(&mut self, endpoint: &EndpointInfo) {
if !endpoint.type_name.is_empty() {
self.types.insert(endpoint.type_name.clone());
}
let node = if endpoint.is_unclaimed() {
"<unclaimed>".to_owned()
} else {
endpoint.node.clone()
};
if endpoint.is_publisher {
self.publishers = self.publishers.saturating_add(1);
self.publisher_nodes.insert(node);
} else {
self.subscribers = self.subscribers.saturating_add(1);
self.subscriber_nodes.insert(node);
}
}
fn finish(self, name: String) -> TopicSummary {
TopicSummary {
name,
types: self.types.into_iter().collect(),
publishers: self.publishers,
subscribers: self.subscribers,
publisher_nodes: self.publisher_nodes.into_iter().collect(),
subscriber_nodes: self.subscriber_nodes.into_iter().collect(),
}
}
}
#[cfg(test)]
mod tests {
#![allow(clippy::unwrap_used, clippy::expect_used, clippy::panic)]
use astrs_rtps::discovery::Gid;
use astrs_rtps::structure::{EntityId, EntityKind, Guid, GuidPrefix, VendorId};
use super::*;
fn guid(seed: u8) -> Guid {
Guid::new(
GuidPrefix::vendor_scoped(VendorId::ASTRS, [seed; 10]),
EntityId::user_defined(1, EntityKind::USER_WRITER_NO_KEY),
)
}
fn endpoint(
node: &str,
name: &str,
type_name: &str,
is_publisher: bool,
kind: TopicKind,
) -> EndpointInfo {
EndpointInfo {
node: node.to_owned(),
name: name.to_owned(),
type_name: type_name.to_owned(),
gid: Gid::new(RosCompat::Jazzy, guid(1)),
is_publisher,
is_local: false,
kind,
}
}
#[test]
fn the_default_timeout_and_domain_are_the_documented_ones() {
let args = ProbeArgs {
domain_id: 0,
..ProbeArgs::default()
};
assert_eq!(args.timeout, DEFAULT_TIMEOUT);
assert_eq!(args.domain_id, 0);
assert!(!args.json);
assert!(!args.include_hidden);
}
#[test]
fn the_query_excludes_the_probes_own_endpoints() {
let args = ProbeArgs::default();
assert!(!args.query().include_local);
assert!(!args.query().include_hidden);
let hidden = ProbeArgs {
include_hidden: true,
..ProbeArgs::default()
};
assert!(hidden.query().include_hidden);
assert!(!hidden.query().include_local);
}
#[test]
fn topics_are_grouped_by_name_with_their_endpoint_counts() {
let endpoints = vec![
endpoint(
"/talker",
"/chatter",
"std_msgs/msg/String",
true,
TopicKind::Topic,
),
endpoint(
"/listener",
"/chatter",
"std_msgs/msg/String",
false,
TopicKind::Topic,
),
endpoint(
"/other",
"/chatter",
"std_msgs/msg/String",
false,
TopicKind::Topic,
),
endpoint(
"/talker",
"/scan",
"sensor_msgs/msg/LaserScan",
true,
TopicKind::Topic,
),
];
let topics = summarize(&endpoints, TopicKind::Topic);
assert_eq!(topics.len(), 2);
assert_eq!(topics[0].name, "/chatter", "sorted by name");
assert_eq!(topics[0].publishers, 1);
assert_eq!(topics[0].subscribers, 2);
assert_eq!(topics[0].types, vec!["std_msgs/msg/String"]);
assert_eq!(topics[0].publisher_nodes, vec!["/talker"]);
assert_eq!(topics[0].subscriber_nodes, vec!["/listener", "/other"]);
assert_eq!(topics[1].name, "/scan");
}
#[test]
fn two_types_on_one_topic_are_both_reported() {
let endpoints = vec![
endpoint("/a", "/x", "std_msgs/msg/String", true, TopicKind::Topic),
endpoint("/b", "/x", "std_msgs/msg/Int32", false, TopicKind::Topic),
];
let topics = summarize(&endpoints, TopicKind::Topic);
assert_eq!(
topics[0].types,
vec!["std_msgs/msg/Int32", "std_msgs/msg/String"],
"a type disagreement is shown, not collapsed"
);
}
#[test]
fn a_services_two_halves_are_one_entry() {
let endpoints = vec![
endpoint(
"/server",
"/add_two_ints",
"example_interfaces/srv/AddTwoInts_Request",
false,
TopicKind::Request,
),
endpoint(
"/server",
"/add_two_ints",
"example_interfaces/srv/AddTwoInts_Response",
true,
TopicKind::Reply,
),
];
let services = summarize(&endpoints, TopicKind::Request);
assert_eq!(services.len(), 1);
assert_eq!(services[0].name, "/add_two_ints");
assert_eq!(services[0].publishers, 1);
assert_eq!(services[0].subscribers, 1);
}
#[test]
fn an_actions_endpoints_are_grouped_under_the_action_name() {
let endpoints = vec![
endpoint(
"/server",
"/fibonacci/_action/send_goal",
"example_interfaces/action/Fibonacci_SendGoal_Request",
false,
TopicKind::Request,
),
endpoint(
"/server",
"/fibonacci/_action/feedback",
"example_interfaces/action/Fibonacci_FeedbackMessage",
true,
TopicKind::Topic,
),
];
let actions = summarize_actions(&endpoints);
assert_eq!(actions.len(), 1);
assert_eq!(actions[0].name, "/fibonacci");
assert_eq!(actions[0].publishers, 1);
assert_eq!(actions[0].subscribers, 1);
assert!(summarize(&endpoints, TopicKind::Topic).is_empty());
assert!(summarize(&endpoints, TopicKind::Request).is_empty());
}
#[test]
fn an_unclaimed_endpoint_is_labelled_rather_than_dropped() {
let endpoints = vec![endpoint(
"",
"/mystery",
"some/msg/Type",
true,
TopicKind::Topic,
)];
let topics = summarize(&endpoints, TopicKind::Topic);
assert_eq!(topics[0].publisher_nodes, vec!["<unclaimed>"]);
}
#[test]
fn participants_are_grouped_and_unannounced_ones_are_counted() {
let nodes = vec![
NodeInfo {
name: "talker".to_owned(),
namespace: "/".to_owned(),
participant: guid(1),
is_local: false,
enclave: None,
},
NodeInfo {
name: "listener".to_owned(),
namespace: "/".to_owned(),
participant: guid(1),
is_local: false,
enclave: None,
},
];
let summaries = summarize_participants(&nodes, 3);
assert_eq!(summaries.len(), 3, "one announced, two unannounced");
assert_eq!(summaries[0].nodes, vec!["/listener", "/talker"]);
assert!(
summaries[0].is_astrs,
"the fixture uses the AstRS vendor id"
);
assert!(summaries[1].nodes.is_empty());
}
#[test]
fn the_multicast_capability_renders_every_outcome() {
let group = std::net::Ipv4Addr::new(239, 255, 0, 1);
assert_eq!(
describe_multicast(&MulticastCapability::Joined { group }),
"joined 239.255.0.1"
);
assert_eq!(
describe_multicast(&MulticastCapability::Disabled),
"disabled"
);
}
#[test]
fn an_empty_report_says_so() {
let report = ProbeReport {
domain_id: 0,
listened_ms: 10,
multicast: "joined".to_owned(),
multicast_joined: true,
locator: "127.0.0.1:7400".to_owned(),
participants: Vec::new(),
nodes: Vec::new(),
topics: Vec::new(),
services: Vec::new(),
actions: Vec::new(),
unclaimed_endpoints: 0,
};
assert!(report.is_empty());
assert_eq!(report.endpoint_count(), 0);
}
#[test]
fn a_malformed_ros_domain_id_falls_back_rather_than_failing() {
assert_eq!(DEFAULT_DOMAIN_ID, 0);
assert!(domain_id_from_env() < 233, "a legal ROS domain id");
}
}