[ROS1/TCPROS] Improve self-ip heuristics.

roslibrust_ros1/Cargo.toml

@@ -32,6 +32,7 @@ regex = { version = "1.9" }
 byteorder = "1.4"
 thiserror = "2.0"
 anyhow = "1.0"
+if-addrs = "0.14.0"
 
 [dev-dependencies]
 # Used for message definitions in tests

roslibrust_ros1/src/node/handle.rs

@@ -32,7 +32,7 @@ impl NodeHandle {
         let _ = Name::new("test").unwrap().resolve_to_global(&name);
 
         // Follow ROS rules and determine our IP and hostname
-        let (addr, hostname) = super::determine_addr().await?;
+        let (addr, hostname) = super::determine_addr(master_uri).await?;
 
         let node = Node::new(master_uri, &hostname, &name, addr).await?;
         let nh = NodeHandle { inner: node };

roslibrust_ros1/src/node/mod.rs

@@ -1,6 +1,7 @@
 //! This module contains the top level Node and NodeHandle classes.
 //! These wrap the lower level management of a ROS Node connection into a higher level and thread safe API.
 
+use if_addrs::Interface;
 use roslibrust_common::Error;
 
 use super::{names::InvalidNameError, RosMasterError};
@@ -29,8 +30,8 @@ pub struct ProtocolParams {
 /// Following ROS's idiomatic address rules uses ROS_HOSTNAME and ROS_IP to determine the address that server should be hosted at.
 /// Returns both the resolved IpAddress of the host (used for actually opening the socket), and the String "hostname" which should
 /// be used in the URI.
-async fn determine_addr() -> Result<(Ipv4Addr, String), RosMasterError> {
-    // If ROS_IP is set that trumps anything else
+async fn determine_addr(master_uri: &str) -> Result<(Ipv4Addr, String), RosMasterError> {
+    // If ROS_IP is set, that trumps anything else
     if let Ok(ip_str) = std::env::var("ROS_IP") {
         let ip = ip_str.parse().map_err(|e| {
             RosMasterError::HostIpResolutionFailure(format!(
@@ -39,41 +40,85 @@ async fn determine_addr() -> Result<(Ipv4Addr, String), RosMasterError> {
         })?;
         return Ok((ip, ip_str));
     }
-    // If ROS_HOSTNAME is set that is next highest precedent
+    // If ROS_HOSTNAME is set, that is next highest precedent
     if let Ok(name) = std::env::var("ROS_HOSTNAME") {
         let ip = hostname_to_ipv4(&name).await?;
         return Ok((ip, name));
     }
+
     // If neither env var is set, use the computers "hostname"
     let name = gethostname::gethostname();
     let name = name.into_string().map_err(|e| {
             RosMasterError::HostIpResolutionFailure(format!("This host's hostname is a string that cannot be validly converted into a Rust type, and therefore we cannot convert it into an IpAddrv4: {e:?}"))
         })?;
+
+    // Try to find an IP in the same subnet as the ROS master
+    if let Some(master_ip) = try_get_master_ip(master_uri) {
+        if let Ok(local_interfaces) = if_addrs::get_if_addrs() {
+            if let Some(ip) = try_find_addr_in_same_subnet(master_ip, &local_interfaces) {
+                return Ok((ip, name));
+            }
+        }
+    }
+
+    // Fallback to just use the first ip we can find
     let ip = hostname_to_ipv4(&name).await?;
     Ok((ip, name))
 }
 
+fn try_find_addr_in_same_subnet(
+    master_ip: Ipv4Addr,
+    local_interfaces: &Vec<Interface>,
+) -> Option<Ipv4Addr> {
+    for iface in local_interfaces {
+        if let if_addrs::IfAddr::V4(ifv4) = &iface.addr {
+            if is_in_same_subnet(ifv4.ip, master_ip, ifv4.netmask) {
+                return Some(ifv4.ip);
+            }
+        }
+    }
+    None
+}
+
+fn try_get_master_ip(master_uri: &str) -> Option<Ipv4Addr> {
+    let s = master_uri
+        .strip_prefix("http://")
+        .or_else(|| master_uri.strip_prefix("https://"))
+        .unwrap_or(master_uri);
+    let host = s.split(':').next()?;
+    host.parse::<Ipv4Addr>().ok()
+}
+
+fn is_in_same_subnet(ip1: Ipv4Addr, ip2: Ipv4Addr, mask: Ipv4Addr) -> bool {
+    let ip1_octets = ip1.octets();
+    let ip2_octets = ip2.octets();
+    let mask_octets = mask.octets();
+
+    for i in 0..4 {
+        if (ip1_octets[i] & mask_octets[i]) != (ip2_octets[i] & mask_octets[i]) {
+            return false;
+        }
+    }
+    true
+}
+
 /// Given a the name of a host use's std::net::ToSocketAddrs to perform a DNS lookup and return the resulting IP address.
 /// This function is intended to be used to determine the correct IP host the socket for the xmlrpc server on.
 async fn hostname_to_ipv4(name: &str) -> Result<Ipv4Addr, RosMasterError> {
     let name_with_port = &format!("{name}:0");
-    let mut i = tokio::net::lookup_host(name_with_port).await.map_err(|e| {
+    let i = tokio::net::lookup_host(name_with_port).await.map_err(|e| {
         RosMasterError::HostIpResolutionFailure(format!(
             "Failure while attempting to lookup ROS_HOSTNAME: {e:?}"
         ))
     })?;
-    if let Some(addr) = i.next() {
-        match addr.ip() {
-                IpAddr::V4(ip) => Ok(ip),
-                IpAddr::V6(ip) => {
-                    Err(RosMasterError::HostIpResolutionFailure(format!("ROS_HOSTNAME resolved to an IPv6 address which is not support by ROS/roslibrust: {ip:?}")))
-                }
-            }
-    } else {
-        Err(RosMasterError::HostIpResolutionFailure(format!(
-            "ROS_HOSTNAME did not resolve any address: {name:?}"
-        )))
+    for addr in i {
+        if let IpAddr::V4(ip) = addr.ip() {
+            return Ok(ip);
+        }
     }
+    Err(RosMasterError::HostIpResolutionFailure(format!(
+        "ROS_HOSTNAME resolved to no IPv4 addresses: {name:?}"
+    )))
 }
 
 #[derive(thiserror::Error, Debug)]