From 4929dc6eaeb1dc2c4a03328e4c45e10c841217b6 Mon Sep 17 00:00:00 2001 From: Jan Gutsche <7gutsche@informatik.uni-hamburg.de> Date: Mon, 6 Mar 2023 18:21:54 +0100 Subject: [PATCH] Fix type checks and typos --- ipm_image_node/ipm_image_node/ipm.py | 25 ++++++++++++++++--------- ipm_image_node/test/test_ipm.py | 19 +++++++++++-------- ipm_library/ipm_library/ipm.py | 28 ++++++++++++++-------------- ipm_library/ipm_library/utils.py | 13 +++++++------ 4 files changed, 48 insertions(+), 37 deletions(-) diff --git a/ipm_image_node/ipm_image_node/ipm.py b/ipm_image_node/ipm_image_node/ipm.py index 0916fe6..a8b43d2 100644 --- a/ipm_image_node/ipm_image_node/ipm.py +++ b/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 diff --git a/ipm_image_node/test/test_ipm.py b/ipm_image_node/test/test_ipm.py index 2946f37..bd523c2 100644 --- a/ipm_image_node/test/test_ipm.py +++ b/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' diff --git a/ipm_library/ipm_library/ipm.py b/ipm_library/ipm_library/ipm.py index e3dfb29..96d3cf1 100644 --- a/ipm_library/ipm_library/ipm.py +++ b/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,13 +42,13 @@ 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. @@ -57,7 +56,7 @@ def set_camera_info(self, camera_info: CameraInfo) -> None: """ 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) diff --git a/ipm_library/ipm_library/utils.py b/ipm_library/ipm_library/utils.py index bd92fb0..42e1040 100644 --- a/ipm_library/ipm_library/utils.py +++ b/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