QoS (#225)

* Sensor and Reliable QoS

Signed-off-by: Francisco Martín Rico <[email protected]>

Author: fmrico

webots_ros2_core/webots_ros2_core/devices/camera_device.py

@@ -52,12 +52,15 @@ def __init__(self, node, device_key, wb_device, params=None):
         self._recognition_webots_publisher = None
         self._image_publisher = None
 
+        qos_sensor_reliable = qos_profile_sensor_data
+        qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE
+
         # Create topics
         if not self._disable:
             self._image_publisher = self._node.create_publisher(
                 Image,
                 self._topic_name + '/image_raw',
-                qos_profile_sensor_data
+                qos_sensor_reliable
             )
             self._camera_info_publisher = self._node.create_publisher(
                 CameraInfo,
@@ -73,12 +76,12 @@ def __init__(self, node, device_key, wb_device, params=None):
                 self._recognition_publisher = self._node.create_publisher(
                     Detection2DArray,
                     self._topic_name + '/recognitions',
-                    qos_profile_sensor_data
+                    qos_sensor_reliable
                 )
                 self._recognition_webots_publisher = self._node.create_publisher(
                     WbCameraRecognitionObjects,
                     self._topic_name + '/recognitions/webots',
-                    qos_profile_sensor_data
+                    qos_sensor_reliable
                 )
 
             # CameraInfo data

webots_ros2_core/webots_ros2_core/devices/distance_sensor_device.py

@@ -14,7 +14,7 @@
 
 """Webots DistanceSensor device wrapper for ROS2."""
 
-from rclpy.qos import qos_profile_sensor_data
+from rclpy.qos import QoSReliabilityPolicy, qos_profile_sensor_data
 from sensor_msgs.msg import Range
 from webots_ros2_core.math.interpolation import interpolate_lookup_table
 from .sensor_device import SensorDevice
@@ -47,8 +47,11 @@ def __init__(self, node, device_key, wb_device, params=None):
 
         # Create topics
         if not self._disable:
+            qos_sensor_reliable = qos_profile_sensor_data
+            qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE
+
             self._publisher = self._node.create_publisher(Range, self._topic_name,
-                                                          qos_profile_sensor_data)
+                                                          qos_sensor_reliable)
 
     def __get_max_value(self):
         table = self._wb_device.getLookupTable()

webots_ros2_core/webots_ros2_core/devices/gps_device.py

@@ -14,7 +14,7 @@
 
 """Webots GPS device wrapper for ROS2."""
 
-from rclpy.qos import qos_profile_sensor_data
+from rclpy.qos import QoSReliabilityPolicy, qos_profile_sensor_data
 from std_msgs.msg import Float32
 from sensor_msgs.msg import NavSatFix, NavSatStatus
 from geometry_msgs.msg import PointStamped
@@ -60,16 +60,19 @@ def __init__(self, node, device_key, wb_device, params=None):
         # Change default timestep
         self._timestep = 128
 
+        qos_sensor_reliable = qos_profile_sensor_data
+        qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE
+
         # Create topics
         self.__speed_publisher = node.create_publisher(
-            Float32, self._topic_name + '/speed', qos_profile_sensor_data)
+            Float32, self._topic_name + '/speed', qos_sensor_reliable)
 
         if self.__coordinate_system == GPS.WGS84:
             self.__gps_publisher = node.create_publisher(
-                NavSatFix, self._topic_name + '/gps', qos_profile_sensor_data)
+                NavSatFix, self._topic_name + '/gps', qos_sensor_reliable)
         else:
             self.__gps_publisher = node.create_publisher(
-                PointStamped, self._topic_name + '/gps', qos_profile_sensor_data)
+                PointStamped, self._topic_name + '/gps', qos_sensor_reliable)
 
     def step(self):
         stamp = super().step()

webots_ros2_core/webots_ros2_core/devices/imu_device.py

@@ -14,7 +14,7 @@
 
 """Webots Accelerometer, Gyro and InertialUnit devices wrapper for ROS2."""
 
-from rclpy.qos import qos_profile_sensor_data
+from rclpy.qos import QoSReliabilityPolicy, qos_profile_sensor_data
 from sensor_msgs.msg import Imu
 from webots_ros2_core.math.interpolation import interpolate_lookup_table
 from .sensor_device import SensorDevice
@@ -53,8 +53,11 @@ def __init__(self, node, device_key, wb_device, params=None):
         # Create topics
         self._publisher = None
         if not self._disable:
+            qos_sensor_reliable = qos_profile_sensor_data
+            qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE
+
             self._publisher = self._node.create_publisher(Imu, self._topic_name,
-                                                          qos_profile_sensor_data)
+                                                          qos_sensor_reliable)
 
     def __enable_imu(self):
         for wb_device in self._wb_device:

webots_ros2_core/webots_ros2_core/devices/led_device.py

@@ -15,7 +15,7 @@
 """LED device."""
 
 from std_msgs.msg import Int32
-from rclpy.qos import qos_profile_sensor_data
+from rclpy.qos import QoSReliabilityPolicy, qos_profile_sensor_data
 from .device import Device
 
 
@@ -48,12 +48,15 @@ def __init__(self, node, device_key, wb_device, params=None):
         # Determine default params
         self._topic_name = self._get_param('topic_name', self._create_topic_name(wb_device))
 
+        qos_sensor_reliable = qos_profile_sensor_data
+        qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE
+
         # Create subscribers
         self.__led_subscriber = self._node.create_subscription(
             Int32,
             self._topic_name,
             self.__callback,
-            qos_profile_sensor_data
+            qos_sensor_reliable
         )
 
     def __callback(self, msg):

webots_ros2_core/webots_ros2_core/devices/lidar_device.py

@@ -14,7 +14,7 @@
 
 """Lidar device."""
 
-from rclpy.qos import qos_profile_sensor_data
+from rclpy.qos import QoSReliabilityPolicy, qos_profile_sensor_data
 from sensor_msgs.msg import LaserScan, PointCloud2, PointField
 from tf2_ros import StaticTransformBroadcaster
 from geometry_msgs.msg import TransformStamped
@@ -60,14 +60,17 @@ def __init__(self, node, device_key, wb_device, params=None):
         # Change default timestep
         self._timestep = 128
 
+        qos_sensor_reliable = qos_profile_sensor_data
+        qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE
+
         # Create topics
         if wb_device.getNumberOfLayers() > 1:
             wb_device.enablePointCloud()
             self.__publisher = node.create_publisher(PointCloud2, self._topic_name,
-                                                     qos_profile_sensor_data)
+                                                     qos_sensor_reliable)
         else:
             self.__publisher = node.create_publisher(LaserScan, self._topic_name,
-                                                     qos_profile_sensor_data)
+                                                     qos_sensor_reliable)
             self.__static_broadcaster = StaticTransformBroadcaster(node)
             transform_stamped = TransformStamped()
             transform_stamped.header.frame_id = self._frame_id

webots_ros2_core/webots_ros2_core/devices/light_sensor_device.py

@@ -14,7 +14,7 @@
 
 """Webots LightSensor device wrapper for ROS2."""
 
-from rclpy.qos import qos_profile_sensor_data
+from rclpy.qos import QoSReliabilityPolicy, qos_profile_sensor_data
 from sensor_msgs.msg import Illuminance
 from webots_ros2_core.math.interpolation import interpolate_lookup_table
 from .sensor_device import SensorDevice
@@ -49,8 +49,11 @@ def __init__(self, node, device_key, wb_device, params=None):
         # Create topics
         self._publisher = None
         if not self._disable:
+            qos_sensor_reliable = qos_profile_sensor_data
+            qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE
+
             self._publisher = self._node.create_publisher(Illuminance, self._topic_name,
-                                                          qos_profile_sensor_data)
+                                                          qos_sensor_reliable)
 
     def __get_variance(self, raw_value):
         table = self._wb_device.getLookupTable()

webots_ros2_core/webots_ros2_core/devices/range_finder_device.py

@@ -16,7 +16,7 @@
 
 import numpy as np
 from sensor_msgs.msg import Image
-from rclpy.qos import qos_profile_sensor_data
+from rclpy.qos import QoSReliabilityPolicy, qos_profile_sensor_data
 from .sensor_device import SensorDevice
 
 
@@ -44,12 +44,15 @@ def __init__(self, node, device_key, wb_device, params=None):
         super().__init__(node, device_key, wb_device, params)
         self._image_publisher = None
 
+        qos_sensor_reliable = qos_profile_sensor_data
+        qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE
+
         # Create topics
         if not self._disable:
             self._image_publisher = self._node.create_publisher(
                 Image,
                 self._topic_name + '/image_depth',
-                qos_profile_sensor_data
+                qos_sensor_reliable
             )
 
             # Load parameters