Range and qos (#198)

* Range Finder

Signed-off-by: Francisco Martin Rico <[email protected]>

* Fix QoS

Signed-off-by: Francisco Martin Rico <[email protected]>

* Unused import

* Add docs

* Fix deep code errors and avoid unnecessary call

Signed-off-by: Francisco Martin Rico <[email protected]>

* Fix deep code errors and avoid unnecessary call

Signed-off-by: Francisco Martin Rico <[email protected]>

* Geometric wheels values

Signed-off-by: Francisco Martin Rico <[email protected]>

* CI fix

Co-authored-by: Darko Lukic <[email protected]>

Author: fmrico

docs/source/API-Devices.rst

@@ -37,6 +37,13 @@ We created a collection of Python modules that read a Webots node description an
    :show-inheritance:
 
 
+### RangeFinder Device
+
+.. automodule:: webots_ros2_core.devices.range_finder_device
+   :members:
+   :show-inheritance:
+
+
 ### Distance Sensor Device
 
 .. automodule:: webots_ros2_core.devices.distance_sensor_device

webots_ros2_core/webots_ros2_core/devices/camera_device.py

@@ -16,7 +16,7 @@
 
 from sensor_msgs.msg import Image, CameraInfo
 from rclpy.time import Time
-from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile
+from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, QoSReliabilityPolicy, qos_profile_sensor_data
 from .sensor_device import SensorDevice
 
 
@@ -50,36 +50,41 @@ def __init__(self, node, device_key, wb_device, params=None):
             self._image_publisher = self._node.create_publisher(
                 Image,
                 self._topic_name + '/image_raw',
-                1
+                qos_profile_sensor_data
             )
             self._camera_info_publisher = self._node.create_publisher(
                 CameraInfo,
                 self._topic_name + '/camera_info',
                 QoSProfile(
                     depth=1,
+                    reliability=QoSReliabilityPolicy.RELIABLE,
                     durability=DurabilityPolicy.TRANSIENT_LOCAL,
                     history=HistoryPolicy.KEEP_LAST,
                 )
             )
 
             # CameraInfo data
-            msg = CameraInfo()
-            msg.header.stamp = Time(seconds=self._node.robot.getTime()).to_msg()
-            msg.height = self._wb_device.getHeight()
-            msg.width = self._wb_device.getWidth()
-            msg.distortion_model = 'plumb_bob'
-            msg.d = [0.0, 0.0, 0.0, 0.0, 0.0]
-            msg.k = [
-                self._wb_device.getFocalLength(), 0.0, self._wb_device.getWidth() / 2,
-                0.0, self._wb_device.getFocalLength(), self._wb_device.getHeight() / 2,
+            self.__message_info = CameraInfo()
+            self.__message_info.header.stamp = Time(seconds=self._node.robot.getTime()).to_msg()
+            self.__message_info.height = self._wb_device.getHeight()
+            self.__message_info.width = self._wb_device.getWidth()
+            self.__message_info.distortion_model = 'plumb_bob'
+            focal_length = self._wb_device.getFocalLength()
+            if focal_length == 0:
+                focal_length = 570.34  # Identical to Orbbec Astra
+            self.__message_info.d = [0.0, 0.0, 0.0, 0.0, 0.0]
+            self.__message_info.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+            self.__message_info.k = [
+                focal_length, 0.0, self._wb_device.getWidth() / 2,
+                0.0, focal_length, self._wb_device.getHeight() / 2,
                 0.0, 0.0, 1.0
             ]
-            msg.p = [
-                self._wb_device.getFocalLength(), 0.0, self._wb_device.getWidth() / 2, 0.0,
-                0.0, self._wb_device.getFocalLength(), self._wb_device.getHeight() / 2, 0.0,
+            self.__message_info.p = [
+                focal_length, 0.0, self._wb_device.getWidth() / 2, 0.0,
+                0.0, focal_length, self._wb_device.getHeight() / 2, 0.0,
                 0.0, 0.0, 1.0, 0.0
             ]
-            self._camera_info_publisher.publish(msg)
+            self._camera_info_publisher.publish(self.__message_info)
 
             # Load parameters
             camera_period_param = node.declare_parameter(wb_device.getName() + '_period', self._timestep)
@@ -89,10 +94,13 @@ def step(self):
         stamp = super().step()
         if not stamp:
             return
-
         # Publish camera data
         if self._image_publisher.get_subscription_count() > 0 or self._always_publish:
             self._wb_device.enable(self._timestep)
+            image = self._wb_device.getImage()
+
+            if image is None:
+                return
 
             # Image data
             msg = Image()
@@ -107,8 +115,11 @@ def step(self):
             # Both, `bytearray` and `array.array`, implement Python buffer protocol, so we should not see unpredictable
             # behavior.
             # deepcode ignore W0212: Avoid conversion from `bytearray` to `array.array`.
-            msg._data = self._wb_device.getImage()
+            msg._data = image
             msg.encoding = 'bgra8'
             self._image_publisher.publish(msg)
+
+            self.__message_info.header.stamp = Time(seconds=self._node.robot.getTime()).to_msg()
+            self._camera_info_publisher.publish(self.__message_info)
         else:
             self._wb_device.disable()

webots_ros2_core/webots_ros2_core/devices/device_manager.py

@@ -17,6 +17,7 @@
 """Auto discover Webots devices and publish suitable ROS2 topics."""
 
 from .camera_device import CameraDevice
+from .range_finder_device import RangeFinderDevice
 from .led_device import LEDDevice
 from .lidar_device import LidarDevice
 from .distance_sensor_device import DistanceSensorDevice
@@ -45,6 +46,8 @@ def __init__(self, node, config=None):
             # Create ROS2 wrapped device
             if wb_device.getNodeType() == Node.CAMERA:
                 device = CameraDevice(node, device_key, wb_device, self.__config.get(device_key, None))
+            if wb_device.getNodeType() == Node.RANGE_FINDER:
+                device = RangeFinderDevice(node, device_key, wb_device, self.__config.get(device_key, None))
             elif wb_device.getNodeType() == Node.LED:
                 device = LEDDevice(node, device_key, wb_device, self.__config.get(device_key, None))
             elif wb_device.getNodeType() == Node.LIDAR:

webots_ros2_core/webots_ros2_core/devices/distance_sensor_device.py

@@ -14,6 +14,7 @@
 
 """Webots DistanceSensor device wrapper for ROS2."""
 
+from rclpy.qos import 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
@@ -46,7 +47,8 @@ def __init__(self, node, device_key, wb_device, params=None):
 
         # Create topics
         if not self._disable:
-            self._publisher = self._node.create_publisher(Range, self._topic_name, 1)
+            self._publisher = self._node.create_publisher(Range, self._topic_name,
+                                                          qos_profile_sensor_data)
 
     def __get_max_value(self):
         table = self._wb_device.getLookupTable()

webots_ros2_core/webots_ros2_core/devices/imu_device.py

@@ -14,6 +14,7 @@
 
 """Webots Accelerometer, Gyro and InertialUnit devices wrapper for ROS2."""
 
+from rclpy.qos import 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
@@ -52,7 +53,8 @@ def __init__(self, node, device_key, wb_device, params=None):
         # Create topics
         self._publisher = None
         if not self._disable:
-            self._publisher = self._node.create_publisher(Imu, self._topic_name, 1)
+            self._publisher = self._node.create_publisher(Imu, self._topic_name,
+                                                          qos_profile_sensor_data)
 
     def __enable_imu(self):
         for wb_device in self._wb_device:

webots_ros2_core/webots_ros2_core/devices/lidar_device.py

@@ -14,6 +14,7 @@
 
 """Lidar device."""
 
+from rclpy.qos import qos_profile_sensor_data
 from sensor_msgs.msg import LaserScan, PointCloud2, PointField
 from tf2_ros import StaticTransformBroadcaster
 from geometry_msgs.msg import TransformStamped
@@ -62,9 +63,11 @@ def __init__(self, node, device_key, wb_device, params=None):
         # Create topics
         if wb_device.getNumberOfLayers() > 1:
             wb_device.enablePointCloud()
-            self.__publisher = node.create_publisher(PointCloud2, self._topic_name, 1)
+            self.__publisher = node.create_publisher(PointCloud2, self._topic_name,
+                                                     qos_profile_sensor_data)
         else:
-            self.__publisher = node.create_publisher(LaserScan, self._topic_name, 1)
+            self.__publisher = node.create_publisher(LaserScan, self._topic_name,
+                                                     qos_profile_sensor_data)
             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,6 +14,7 @@
 
 """Webots LightSensor device wrapper for ROS2."""
 
+from rclpy.qos import 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
@@ -48,7 +49,8 @@ def __init__(self, node, device_key, wb_device, params=None):
         # Create topics
         self._publisher = None
         if not self._disable:
-            self._publisher = self._node.create_publisher(Illuminance, self._topic_name, 1)
+            self._publisher = self._node.create_publisher(Illuminance, self._topic_name,
+                                                          qos_profile_sensor_data)
 
     def __get_variance(self, raw_value):
         table = self._wb_device.getLookupTable()

webots_ros2_core/webots_ros2_core/devices/range_finder_device.py

@@ -0,0 +1,91 @@
+# Copyright 2021 Intelligent Robotics Labs URJC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""RangeFinder device."""
+
+import numpy as np
+from sensor_msgs.msg import Image
+from rclpy.qos import qos_profile_sensor_data
+from .sensor_device import SensorDevice
+
+
+class RangeFinderDevice(SensorDevice):
+    """
+    ROS2 wrapper for Webots RangeFinder node.
+
+    Creates suitable ROS2 interface based on Webots
+    [RangeFinder](https://cyberbotics.com/doc/reference/rangefinder) node instance:
+
+    It allows the following functinalities:
+    - Publishes raw depth image of type `sensor_msgs/Image`
+
+    Args:
+        node (WebotsNode): The ROS2 node.
+        device_key (str): Unique identifier of the device used for configuration.
+        wb_device (RangeFinder): Webots node of type RangeFinder.
+
+    Kwargs:
+        params (dict): Inherited from `SensorDevice`
+
+    """
+
+    def __init__(self, node, device_key, wb_device, params=None):
+        super().__init__(node, device_key, wb_device, params)
+        self._image_publisher = None
+
+        # Create topics
+        if not self._disable:
+            self._image_publisher = self._node.create_publisher(
+                Image,
+                self._topic_name + '/image_depth',
+                qos_profile_sensor_data
+            )
+
+            # Load parameters
+            camera_period_param = node.declare_parameter(wb_device.getName() + '_period', self._timestep)
+            self._camera_period = camera_period_param.value
+
+    def step(self):
+        stamp = super().step()
+        if not stamp:
+            return
+        # Publish camera data
+        if self._image_publisher.get_subscription_count() > 0 or self._always_publish:
+            self._wb_device.enable(self._timestep)
+
+            # Image data
+            msg = Image()
+            msg.header.stamp = stamp
+            msg.header.frame_id = self._frame_id
+            msg.height = self._wb_device.getHeight()
+            msg.width = self._wb_device.getWidth()
+            msg.is_bigendian = False
+            msg.step = self._wb_device.getWidth() * 4
+
+            if self._wb_device.getRangeImage() is None:
+                return
+            image_array = np.array(self._wb_device.getRangeImage(), dtype="float32")
+
+            # We pass `data` directly to we avoid using `data` setter.
+            # Otherwise ROS2 converts data to `array.array` which slows down the simulation as it copies memory internally.
+            # Both, `bytearray` and `array.array`, implement Python buffer protocol, so we should not see unpredictable
+            # behavior.
+            # deepcode ignore W0212: Avoid conversion from `bytearray` to `array.array`.
+            msg._data = image_array.tobytes()
+            msg.encoding = '32FC1'
+
+            self._image_publisher.publish(msg)
+
+        else:
+            self._wb_device.disable()

webots_ros2_tiago/resource/tiago.yaml

@@ -7,5 +7,5 @@ webots_driver:
     robot_base_frame: base_link
     synchronization: false
     use_joint_state_publisher: true
-    wheel_distance: 0.404
-    wheel_radius: 0.1955
+    wheel_distance: 0.5
+    wheel_radius: 0.12