How-to hotplug a joystick into a running ROS teleop node using libudev

I’ve yet to see a software stack for a mobile robot that does not support teleoperation. Game-controller-based interfaces are ubiquitous, but I’ve also seen teleoperation implemented via mouse/GUI or even as simply as with a keyboard.

Usually, the presence of an input device is verified at system startup. If no input device is found, the teleop driver node typically exits with an error. More sophisticated implementations print a warning and keep running, albeit without teleoperation support. This concept is known as graceful degradation.

In this article, however, I’d like to suggest a different approach. Users can’t always be bothered to keep the input device connected to the computer running the teleoperation code. In all likelihood, the input device is only plugged in when the robot is stuck in a situation it cannot get itself out of.

Restarting the whole software stack, or even just the parts related to teleoperation, is an unnecessary chore. It makes much more sense to receive a notification when a new input device is added, then open that device and stream its data via ROS. Throughout this article I’ll assume the input device is a joystick that shows up as /dev/input/js0 when connected.

libudev allows monitoring hardware events on Linux. First, we set up a monitor that listens for device events of the input subsystem:

1
2
3
4

struct udev * udev = udev_new();
struct udev_monitor * udev_mon = udev_monitor_new_from_netlink(udev, "udev");
udev_monitor_filter_add_match_subsystem_devtype(udev_mon, "input", nullptr);
udev_monitor_enable_receiving(udev_mon);

Next, a dedicated thread continuously polls the monitor’s file descriptor to check whether a device event has occurred:

1
2
3
4
5

struct pollfd pfd;
pfd.fd = udev_monitor_get_fd(udev_mon);
pfd.events = POLLIN;

int const rc = poll(&pfd, 1 /* number of fds */, 50 /* timeout in ms */);

If an event for the input subsystem is pending, rc is greater than 0. By inspecting the event’s action and devnode properties, we can determine whether our desired input device has been added:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18

struct udev_device * udev_dev = udev_monitor_receive_device(udev_mon);
char const * action  = udev_device_get_action(udev_dev);
char const * devnode = udev_device_get_devnode(udev_dev);

/* Not every input event carries a device node, hence the NULL checks. */
if (action && devnode && !strcmp(action, "add") && !strcmp(devnode, "/dev/input/js0"))
{
  /* The desired input device, a joystick available under
   * /dev/input/js0, has been added and can now be opened
   * and read from.
   */
  int fd = open(devnode, O_RDONLY);
  /* Read joystick events, transform them into teleoperation
   * commands and publish them as ROS topics.
   * ...
   */
}
udev_device_unref(udev_dev);

To use libudev in your ROS package, add the following lines to your CMakeLists.txt:

1
2
3
4

find_package(PkgConfig REQUIRED)
pkg_check_modules(UDEV REQUIRED libudev)
target_include_directories(${MY_TELEOP_NODE_TARGET} PRIVATE ${UDEV_INCLUDE_DIRS})
target_link_libraries(${MY_TELEOP_NODE_TARGET} ${UDEV_LIBRARIES})

The same mechanism can be extended to detect when the input device is unplugged during operation (action equals "remove"). The teleop node can then close the device and wait for it to reappear, degrading gracefully instead of crashing.

PS: Don’t forget to free allocated resources ;) :

1
2

udev_monitor_unref(udev_mon);
udev_unref(udev);