Added-methods-to-numpify-and-msgify-LaserScan-msg-and-corresponding-unittest

Author: aseelam1996

CMakeLists.txt

@@ -18,6 +18,7 @@ if(BUILD_TESTING)
   ament_add_pytest_test(occupancygrids test/test_occupancygrids.py)
   ament_add_pytest_test(geometry test/test_geometry.py)
   ament_add_pytest_test(quaternions test/test_quat.py)
+  ament_add_pytest_test(laserscan test/test_laserscan.py)
 endif()
 
 ##############

README.md

@@ -54,6 +54,7 @@ Currently supports:
    data = ros2_numpy.numpify(msg)
    ```
 
+* `sensor_msgs.msg.LaserScan` ↔ structured `np.array`
 * `sensor_msgs.msg.Image` ↔ 2/3-D `np.array`, similar to the function of `cv_bridge`, but without the dependency on `cv2`
 * `nav_msgs.msg.OccupancyGrid` ↔ `np.ma.array`
 * `geometry.msg.Vector3` ↔ 1-D `np.array`. `hom=True` gives `[x, y, z, 0]`

ros2_numpy/__init__.py

@@ -7,3 +7,4 @@
 from . import image
 from . import occupancy_grid
 from . import geometry
+from . import laser_scan

ros2_numpy/laser_scan.py

@@ -0,0 +1,159 @@
+#!/usr/bin/env python3
+"""
+Methods to numpify LaserScan message.
+"""
+
+from .registry import converts_from_numpy, converts_to_numpy
+
+import numpy as np
+from sensor_msgs.msg import LaserScan
+
+
+@converts_to_numpy(LaserScan)
+def laserscan_to_array(
+    scan, remove_invalid_ranges=False, include_ranges_and_intensities=False
+):
+    """
+    Takes a sensor_msgs/msg/LaserScan msg and returns a structered array with
+    fields x, y and z that correspond to cartesian position data. Optionally,
+    ranges and intensities fields that correspond to the range and intensity
+    of a point are also included if include_ranges_and_intensities is True.
+
+    Parameters
+    ----------
+    scan : ROS2 LaserScan message
+        Input laser scan message to get numpyed
+    remove_invalid_ranges : bool, optional
+        whether to remove invalid ranges from the input scan, by default False
+    include_ranges_and_intensities : bool, optional
+        whether to also return the ranges & intensities along with the cartesian
+        position.
+
+    Returns
+    -------
+    pts : numpy k-array where each element will be a structured record that
+        contains either 3 or 5 fields. If include_ranges_and_intensities is
+        False, each element of  output array is a structured record that has
+        3 fields ['x', 'y', 'z'] of type float 32. Else, it has 5 fields ['x',
+        'y', 'z', 'ranges', 'intensities']. Since output is a structured array,
+        all the x-coordinates of the points can be accessed as out_array['x'].
+        Similarly, the y and z coodinates can be accessed as out_array['y']
+        and out_array['z'] respectively.
+    """
+    n_points = len(scan.ranges)
+    angles = np.linspace(
+        scan.angle_min,
+        scan.angle_max,
+        n_points,
+    )
+    ranges = np.array(scan.ranges, dtype="f4")
+    intensities = np.array(scan.intensities, dtype="f4")
+    if remove_invalid_ranges:
+        indices_invalid_range = (
+            np.isinf(ranges)
+            | (ranges < scan.range_min)
+            | (ranges > scan.range_max)
+        )
+        ranges = ranges[~indices_invalid_range]
+        angles = angles[~indices_invalid_range]
+        intensities = intensities[~indices_invalid_range]
+
+    x = np.array(ranges * np.cos(angles), dtype="f4")
+    y = np.array(ranges * np.sin(angles), dtype="f4")
+    z = np.zeros(ranges.shape[0], dtype="f4")
+    if include_ranges_and_intensities:
+        dtype = np.dtype(
+            [
+                ("x", "f4"),
+                ("y", "f4"),
+                ("z", "f4"),
+                ("ranges", "f4"),
+                ("intensities", "f4"),
+            ]
+        )
+        out_array = np.empty(len(x), dtype=dtype)
+        out_array["x"] = x
+        out_array["y"] = y
+        out_array["z"] = z
+        out_array["ranges"] = ranges
+        out_array["intensities"] = intensities
+        return out_array
+    else:
+        dtype = np.dtype([("x", "f4"), ("y", "f4"), ("z", "f4")])
+        out_array = np.empty(len(x), dtype=dtype)
+        out_array["x"] = x
+        out_array["y"] = y
+        out_array["z"] = z
+        return out_array
+
+
+@converts_from_numpy(LaserScan)
+def array_to_laserscan(arr, header, scan_time=0.0, time_increment=0.0):
+    """
+    Takes a structured array(created from LaserScan msg) and returns a
+    LaserScan message. Fields that cannot be determined from the numpy
+    array are provided as inputs. Since the LaserScan message relies
+    on a consistent angular increment, structured array in which the
+    points have been omitted will result in a LaserScan message that is
+    not correct.
+
+    Parameters
+    ----------
+    arr : Structured numpy array with fields x, y and z.
+        Input numpy array that was created from LaserScan message.
+    header : std_msgs::msg::Header,
+        The header to be written to the output LaserScan message.
+    scan_time : float, optional
+        time between scans [seconds] of LaserScan
+    time_increment : float, optional
+        time between measurements [seconds] - if lidar is moving,
+        this will be used in interpolating position
+
+    Returns
+    -------
+    scan_msg : sensor_msgs::msg::LaserScan message
+        If the input array does not contain an `intensities` field,
+        this message has the intensities list filled to zeros.
+
+    """
+    n_points = arr.shape[0]
+
+    if "intensities" in arr.dtype.names:
+        intensities = arr["intensities"]
+    else:
+        intensities = np.zeros(n_points).astype(float)
+
+    if "ranges" in arr.dtype.names:
+        ranges = arr["ranges"]
+    else:
+        ranges = np.sqrt(arr["x"] ** 2 + arr["y"] ** 2).astype(float)
+
+    angles = np.arctan2(arr["y"], arr["x"]).astype(float)
+
+    # Create a LaserScan message
+    scan_msg = LaserScan()
+
+    scan_msg.header = header
+
+    scan_msg.intensities = intensities.tolist()
+    scan_msg.ranges = ranges.tolist()
+
+    # Compute min and max of the ranges
+    scan_msg.range_min = np.min(ranges)
+    scan_msg.range_max = np.max(ranges)
+
+    # Compute min and max of the angles
+    scan_msg.angle_min = np.min(angles)
+    scan_msg.angle_max = np.max(angles)
+
+    # Use the time_increment and scan_time from input arguments
+    scan_msg.time_increment = time_increment
+    scan_msg.scan_time = scan_time
+
+    # Compute angle increment. Since the angle_max is not exclusive, omit
+    # the last point.
+    scan_msg.angle_increment = (scan_msg.angle_max - scan_msg.angle_min) / (
+        n_points - 1
+    )
+
+    return scan_msg

test/test_laserscan.py

@@ -0,0 +1,68 @@
+import unittest
+import numpy as np
+import ros2_numpy as rnp
+
+from sensor_msgs.msg import LaserScan
+from rclpy.clock import Clock
+
+
+class TestLaserScan(unittest.TestCase):
+    def test_to_and_from_laserscan(self):
+        # Create a dummy LaserScan message
+        scan = LaserScan()
+        scan.header.stamp = Clock().now().to_msg()
+        scan.header.frame_id = "lidar_2d"
+        scan.range_min = 0.01
+        scan.range_max = 200.0
+        scan.angle_increment = np.radians(0.1)
+        scan.angle_min = -np.pi
+        scan.angle_max = np.pi - scan.angle_increment
+        scan.scan_time = 0.0
+        scan.time_increment = 0.0
+        scan.ranges = np.full(3600, 10.0, dtype="f4").tolist()
+        scan.intensities = np.full(3600, 5, dtype="f4").tolist()
+        laserscan_array = rnp.numpify(
+            scan,
+            remove_invalid_ranges=False,
+            include_ranges_and_intensities=True,
+        )
+
+        self.assertEqual(laserscan_array.shape[0], 3600)
+        np.testing.assert_array_equal(
+            laserscan_array["ranges"], np.array(scan.ranges)
+        )
+        np.testing.assert_array_equal(
+            laserscan_array["intensities"], np.array(scan.intensities)
+        )
+
+        laserscan_array_without_ranges_and_intensities = rnp.numpify(scan)
+
+        laserscan_msg = rnp.msgify(
+            LaserScan,
+            laserscan_array_without_ranges_and_intensities,
+            scan.header,
+            scan.scan_time,
+            scan.time_increment,
+        )
+        self.assertAlmostEqual(
+            laserscan_msg.angle_min, scan.angle_min, places=5
+        )
+        self.assertAlmostEqual(
+            laserscan_msg.angle_increment, scan.angle_increment
+        )
+        self.assertAlmostEqual(laserscan_msg.angle_max, scan.angle_max)
+        self.assertAlmostEqual(laserscan_msg.scan_time, scan.scan_time)
+        self.assertAlmostEqual(
+            laserscan_msg.time_increment, scan.time_increment
+        )
+        self.assertAlmostEqual(laserscan_msg.header.stamp, scan.header.stamp)
+        self.assertEqual(laserscan_msg.header.frame_id, scan.header.frame_id)
+        self.assertEqual(len(laserscan_msg.ranges), 3600)
+        self.assertEqual(len(laserscan_msg.intensities), 3600)
+        np.testing.assert_array_equal(
+            laserscan_array["ranges"], np.array(scan.ranges)
+        )
+
+
+if __name__ == "__main__":
+    unittest.main()