Fix type checks and typos

ipm_image_node/ipm_image_node/ipm.py

@@ -23,7 +23,10 @@
 from rclpy.duration import Duration
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
-from sensor_msgs.msg import CameraInfo, Image, PointCloud2, PointField
+from sensor_msgs.msg import CameraInfo
+from sensor_msgs.msg import Image
+from sensor_msgs.msg import PointCloud2
+from sensor_msgs.msg import PointField
 from sensor_msgs_py.point_cloud2 import create_cloud
 from std_msgs.msg import Header
 import tf2_ros as tf2
@@ -36,7 +39,7 @@ class IPMImageNode(Node):
     def __init__(self) -> None:
         super().__init__('ipm_image_node')
         # We need to create a tf buffer
-        self.tf_buffer = tf2.Buffer(cache_time=Duration(seconds=30.0))
+        self.tf_buffer = tf2.Buffer(cache_time=Duration(seconds=30))
         self.tf_listener = tf2.TransformListener(self.tf_buffer, self)
 
         # Create an IPM instance
@@ -53,19 +56,20 @@ def __init__(self) -> None:
         # Create publisher
         self.result_publisher = self.create_publisher(PointCloud2, 'projected_point_cloud', 1)
 
-        # Create subsciber
+        # Create subscriber
         self.create_subscription(Image, 'input', self.map_message, 1)
 
-    def map_message(self, msg: Image) -> PointCloud2:
+    def map_message(self, msg: Image) -> None:
         """
         Map a mask or complete rgb image as a pointcloud on the field plane.
 
+        Publishes the projected point cloud.
+
         :param msg: Message containing a mask of pixels or complete
                     rgb image that should be projected
-        :returns: The projected point cloud
         """
         # Get params
-        scale = self.get_parameter('scale').value
+        scale: float = self.get_parameter('scale').value  # type: ignore
         output_frame = self.get_parameter('output_frame').value
 
         # Get field plane
@@ -75,8 +79,11 @@ def map_message(self, msg: Image) -> PointCloud2:
 
         # Convert subsampled image
         image = cv2.resize(
-            cv_bridge.imgmsg_to_cv2(msg),
-            (0, 0), fx=scale, fy=scale, interpolation=cv2.INTER_NEAREST)
+            src=cv_bridge.imgmsg_to_cv2(msg),
+            dsize=(0, 0),
+            fx=scale,  # type: ignore  # This is in fact a float, but the type stubs are wrong
+            fy=scale,  # type: ignore  # This is in fact a float, but the type stubs are wrong
+            interpolation=cv2.INTER_NEAREST)
 
         # Check if we have a mask or full image
         image_type = self.get_parameter('type').value
@@ -108,7 +115,7 @@ def map_message(self, msg: Image) -> PointCloud2:
         except CameraInfoNotSetException:
             self.get_logger().warn(
                 'Inverse perspective mapping should be performed, '
-                'but no camera info was recived yet!',
+                'but no camera info was received yet!',
                 throttle_duration_sec=5)
             return
 

ipm_image_node/test/test_ipm.py

@@ -22,8 +22,11 @@
 import rclpy
 from rclpy.node import Node
 from rclpy.parameter import Parameter
-from sensor_msgs.msg import CameraInfo, Image, PointCloud2
-from sensor_msgs_py.point_cloud2 import read_points, read_points_numpy
+from sensor_msgs.msg import CameraInfo
+from sensor_msgs.msg import Image
+from sensor_msgs.msg import PointCloud2
+from sensor_msgs_py.point_cloud2 import read_points
+from sensor_msgs_py.point_cloud2 import read_points_numpy
 from std_msgs.msg import Header
 from tf2_msgs.msg import TFMessage
 
@@ -53,7 +56,7 @@ def standard_ipm_image_test_case(
     rclpy.init()
     # Create IPM node
     node = IPMImageNode()
-    # Create test node which comunicates with the IPM node
+    # Create test node which communicates with the IPM node
     test_node = Node('test_handler')
     # Create publishers to send data to the IPM node
     ball_pub = test_node.create_publisher(
@@ -63,7 +66,7 @@ def standard_ipm_image_test_case(
     tf_pub = test_node.create_publisher(
         TFMessage, 'tf', 10)
 
-    # Create a shared reference to the recived message in the local scope
+    # Create a shared reference to the received message in the local scope
     received_msg: List[Optional[PointCloud2]] = [None]
 
     # Create a callback with sets this reference
@@ -109,10 +112,10 @@ def callback(msg):
     # Spin the IPM to process the new data
     rclpy.spin_once(node, timeout_sec=0.1)
 
-    # Spin the test__node to recive the results from the IPM
+    # Spin the test__node to receive the results from the IPM
     rclpy.spin_once(test_node, timeout_sec=0.1)
 
-    # Assert that we recived a message
+    # Assert that we received a message
     assert received_msg[0] is not None
 
     # Clean shutdown of the nodes
@@ -144,7 +147,7 @@ def test_ipm_mask():
     # Convert point cloud to numpy
     output_np = read_points_numpy(out)
 
-    # Assert that we recived the correct message
+    # Assert that we received the correct message
     assert len(output_np) == 2, 'Wrong number of pixels'
     assert out.header.stamp == inp.header.stamp, 'Time stamp got changed by the ipm'
     assert out.header.frame_id == 'base_footprint', \
@@ -186,7 +189,7 @@ def test_ipm_image():
     # Get center pixel
     center_output = output_np[int(img_center_y), int(img_center_x)]
 
-    # Assert that we recived the correct message
+    # Assert that we received the correct message
     assert image_np.shape[0] * image_np.shape[1] == image_np.shape[0] * image_np.shape[1], \
         'Wrong number of pixels'
     assert out.header.stamp == inp.header.stamp, 'Time stamp got changed by the ipm'

ipm_library/ipm_library/ipm.py

@@ -14,13 +14,12 @@
 
 from typing import Optional, Tuple
 
-from builtin_interfaces.msg import Time
 from ipm_library import utils
-from ipm_library.exceptions import (
-    CameraInfoNotSetException,
-    InvalidPlaneException,
-    NoIntersectionError)
+from ipm_library.exceptions import CameraInfoNotSetException
+from ipm_library.exceptions import InvalidPlaneException
+from ipm_library.exceptions import NoIntersectionError
 import numpy as np
+from rclpy.time import Time
 from sensor_msgs.msg import CameraInfo
 from shape_msgs.msg import Plane
 from std_msgs.msg import Header
@@ -43,21 +42,21 @@ def __init__(
         :param camera_info: `CameraInfo` Message containing the
             camera intrinsics, camera frame, ...
             The camera info can be updated later on using the setter or
-            provided directly if it is unlikly to change
+            provided directly if it is unlikely to change
         """
         # TF needs a listener that is init in the node context, so we need a reference
         self._tf_buffer = tf_buffer
         self.set_camera_info(camera_info)
 
-    def set_camera_info(self, camera_info: CameraInfo) -> None:
+    def set_camera_info(self, camera_info: Optional[CameraInfo]) -> None:
         """
         Set a new `CameraInfo` message.
 
         :param camera_info: The updated camera info message.
         """
         self._camera_info = camera_info
 
-    def get_camera_info(self):
+    def get_camera_info(self) -> Optional[CameraInfo]:
         """
         Return the latest `CameraInfo` message.
 
@@ -93,7 +92,7 @@ def map_point(
             assumed that the plane is in CameraInfo's frame.
         :param output_frame_id: TF2 frame in which the output should be provided. If not provided,
             the returned point will be in CameraInfo's frame.
-        :rasies CameraInfoNotSetException if camera info has not been provided
+        :raises CameraInfoNotSetException if camera info has not been provided
         :raise: InvalidPlaneException if the plane is invalid
         :raise: NoIntersectionError if the point is not on the plane
         :returns: The point mapped onto the given plane in the output frame
@@ -139,11 +138,12 @@ def map_points(
         :param output_frame_id: TF2 frame in which the output should be provided. If not provided,
             the returned points will be in CameraInfo's frame.
         :returns: The points mapped onto the given plane in the output frame
-        :rasies CameraInfoNotSetException if camera info has not been provided
+        :raises CameraInfoNotSetException if camera info has not been provided
         :raises InvalidPlaneException if the plane is invalid
         """
         if not self.camera_info_received():
             raise CameraInfoNotSetException
+        assert self._camera_info is not None  # Necessary for type checker
 
         if not np.any(plane_msg.coef[:3]):
             raise InvalidPlaneException
@@ -169,7 +169,7 @@ def map_points(
             time=time,
             buffer=self._tf_buffer)
 
-        # Convert points to float if they aren't allready
+        # Convert points to float if they aren't already
         if points.dtype.char not in np.typecodes['AllFloat']:
             points = points.astype(np.float32)
 
@@ -183,9 +183,9 @@ def map_points(
         # Transform output point if output frame if needed
         if output_frame_id not in [None, self._camera_info.header.frame_id]:
             output_transformation = self._tf_buffer.lookup_transform(
-                output_frame_id,
-                self._camera_info.header.frame_id,
-                time)
+                target_frame=output_frame_id,
+                source_frame=self._camera_info.header.frame_id,
+                time=time)
             np_points = utils.transform_points(
                 np_points, output_transformation.transform)
 

ipm_library/ipm_library/utils.py

@@ -14,10 +14,10 @@
 
 from typing import Optional, Tuple
 
-from builtin_interfaces.msg import Time
 from geometry_msgs.msg import Transform
 import numpy as np
 from rclpy.duration import Duration
+from rclpy.time import Time
 from sensor_msgs.msg import CameraInfo
 from shape_msgs.msg import Plane
 from tf2_geometry_msgs import PointStamped
@@ -55,15 +55,16 @@ def transform_plane_to_frame(
     :param plane: The planes base point and normal vector as numpy arrays
     :param input_frame: Current frame of the plane
     :param output_frame: The desired frame of the plane
-    :param time: Timestamp which is used to query the tf buffer and get the tranform at this moment
-    :param buffer: The refrence to the used tf buffer
-    :param timeout: An optinal timeout after which an exception is raised
+    :param time: Timestamp which is used to query the tf buffer
+        and get the transform at this moment
+    :param buffer: The reference to the used tf buffer
+    :param timeout: Timeout after which an exception is raised, optional
     :returns: A Tuple containing the planes base point and normal vector in the
-         new frame at the provided timestamp
+        new frame at the provided timestamp
     """
     # Set optional timeout
     if timeout is None:
-        timeout = Duration(seconds=0.5)
+        timeout = Duration(seconds=0, nanoseconds=500_000_000)  # 0.5s
 
     # Create two points to transform the base point and the normal vector
     # The second point is generated by adding the normal to the base point