anzu_ros_operations.h and anzu_ros_operations.cc snippet

anzu_ros_operations.cc

// License: same as https://github.com/RobotLocomotion/drake-ros

#include <ifaddrs.h>
#include <sched.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <net/if.h>
#include <net/route.h>
#include <sys/ioctl.h>
#include <sys/types.h>

/// Creates linux namespaces suitable for isolating network traffic (for both
/// ROS 2 and LCM), and configures the network namespace to enable that traffic.
///
/// This is most typically used to isolate a test program from any other network
/// traffic on the same machine.
///
/// The new namespaces are:
/// - A new user namespace to avoid needing CAP_SYS_ADMIN to create network and
///   IPC namespaces.
/// - A new network namespace to prevent cross-talk via the network.
/// - A new IPC namespaces to prevent cross-talk via shared memory.
///
/// Upon return, the current process will be in the created namespaces. Any
/// future child processes that are subsequently launched will be in the same
/// namespace, so they will be able to talk amongst themselves and this process.
///
/// This function relies on updating environment variables to help do its job.
/// If code launches new subprocesses, be sure that the environment variables
/// are preserved when doing so. (This usually happens correctly by default;
/// you'd need to go out of your way to turn it off.)
///
/// @throws std::exception if this process has already launched any threads.
void CreateLinuxNetworkNamespaces() {
  // If we've already been called once (e.g., by a parent process) there's no
  // need to do the work again (and trying to do so would fail). Our *.py file
  // repeats the same constant, so be sure to keep both copies in sync.
  constexpr char kFinishedMarker[] =
      "_ANZU_ROS_OPERATIONS_CREATED_LINUX_NETWORK_NAMESPACES";
  if (getenv(kFinishedMarker) != nullptr) {
    return;
  }

  // Enter the namespaces.
  int result = unshare(CLONE_NEWUSER | CLONE_NEWNET | CLONE_NEWIPC);
  if (result != 0 && errno == EINVAL) {
    throw std::runtime_error(
        "CreateLinuxNetworkNamespaces failed to unshare(). One common cause "
        "of this is if any background threads have already been started.");
  }
  ANZU_CHECK_SYSCALL(result);

  // Assert there is exactly one network interface.
  struct ifaddrs* ifaddr = nullptr;
  result = getifaddrs(&ifaddr);
  ANZU_CHECK_SYSCALL(result);
  DRAKE_THROW_UNLESS(ifaddr != nullptr);
  drake::ScopeExit ifaddr_cleanup([&]() { freeifaddrs(ifaddr); });
  DRAKE_THROW_UNLESS(ifaddr->ifa_next == nullptr);

  // Create a socket to do ioctl stuff on.
  int fd = socket(AF_INET, SOCK_DGRAM, 0);
  DRAKE_THROW_UNLESS(fd >= 0);
  drake::ScopeExit fd_cleanup([&]() { close(fd); });

  // Check what flags are set on the interface.
  struct ifreq ioctl_request = {};
  strncpy(ioctl_request.ifr_name, ifaddr->ifa_name, IFNAMSIZ);
  result = ioctl(fd, SIOCGIFFLAGS, &ioctl_request);
  ANZU_CHECK_SYSCALL(result);

  // Expect a loopback interface.
  DRAKE_THROW_UNLESS(ioctl_request.ifr_flags & IFF_LOOPBACK);

  // Enable multicast and bring up interface.
  ioctl_request.ifr_flags |= IFF_MULTICAST;
  ioctl_request.ifr_flags |= IFF_UP;
  result = ioctl(fd, SIOCSIFFLAGS, &ioctl_request);
  ANZU_CHECK_SYSCALL(result);

  // For programs that use both LCM and ROS, we need an LCM route ala
  //  sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev lo
  struct rtentry route = {};
  auto* dest = reinterpret_cast<struct sockaddr_in*>(&route.rt_dst);
  dest->sin_family = AF_INET;
  dest->sin_addr.s_addr = inet_addr("224.0.0.0");
  auto* mask = reinterpret_cast<struct sockaddr_in*>(&route.rt_genmask);
  mask->sin_family = AF_INET;
  mask->sin_addr.s_addr = inet_addr("240.0.0.0");
  std::string device{"lo"};
  route.rt_dev = device.data();
  route.rt_flags = RTF_UP;
  result = ioctl(fd, SIOCADDRT, &route);
  ANZU_CHECK_SYSCALL(result);

  // Success!
  result = setenv(kFinishedMarker, "1", 1);
  ANZU_CHECK_SYSCALL(result);
}