From 8b3f6b8e03432651e2b788128e17603f0972c677 Mon Sep 17 00:00:00 2001
From: Anthony Deschamps <anthony.j.deschamps@gmail.com>
Date: Tue, 26 Oct 2021 11:32:55 -0400
Subject: [PATCH] Make joint_states subscription QoS configurable; default to
 SensorDataQoS. (#179)

This changes the subscription default from `reliable` to
`best_effort`. This is helpful when visualizing a robot that's running
on a wifi-connected device, and eliminates a warning when starting up
drivers on a robot that publishes with `best_effort`.

The subscription defaults to `best_effort`, but can be overridden with
the following config:

```yaml
robot_state_publisher:
  ros__parameters:
    qos_overrides:
      /joint_states:
        subscription:
          reliability: reliable
```
---
 src/robot_state_publisher.cpp | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/robot_state_publisher.cpp b/src/robot_state_publisher.cpp
index 5fbc86c..4c2724b 100644
--- a/src/robot_state_publisher.cpp
+++ b/src/robot_state_publisher.cpp
@@ -136,11 +136,16 @@ RobotStatePublisher::RobotStatePublisher(const rclcpp::NodeOptions & options)
 
   setupURDF(urdf_xml);
 
+  auto subscriber_options = rclcpp::SubscriptionOptions();
+  subscriber_options.qos_overriding_options =
+    rclcpp::QosOverridingOptions::with_default_policies();
+
   // subscribe to joint state
   joint_state_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
-    "joint_states", 10, std::bind(
-      &RobotStatePublisher::callbackJointState, this,
-      std::placeholders::_1));
+    "joint_states",
+    rclcpp::SensorDataQoS(),
+    std::bind(&RobotStatePublisher::callbackJointState, this, std::placeholders::_1),
+    subscriber_options);
 
   publishFixedTransforms();