Proposing some minor changes

ipm_interfaces/package.xml

@@ -12,11 +12,9 @@
 
   <exec_depend>rosidl_default_runtime</exec_depend>
 
-  <depend>geometry_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>shape_msgs</depend>
   <depend>vision_msgs</depend>
-  <depend>std_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

ipm_interfaces/srv/MapPoint.srv

@@ -1,8 +1,8 @@
 shape_msgs/Plane plane
 vision_msgs/Point2D point
 builtin_interfaces/Time time
-std_msgs/String plane_frame_id
-std_msgs/String output_frame_id
+string plane_frame_id
+string output_frame_id
 ---
 # Result code defintions
 uint8 RESULT_SUCCESS=0

ipm_interfaces/srv/MapPointCloud2.srv

@@ -1,7 +1,7 @@
 shape_msgs/Plane plane
 sensor_msgs/PointCloud2 points
-std_msgs/String plane_frame_id
-std_msgs/String output_frame_id
+string plane_frame_id
+string output_frame_id
 ---
 # Result code defintions
 uint8 RESULT_SUCCESS=0

ipm_library/ipm_library/ipm.py

@@ -12,7 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from typing import Optional
+from typing import Optional, Tuple
 
 from builtin_interfaces.msg import Time
 from ipm_library import utils
@@ -23,6 +23,7 @@
 import numpy as np
 from sensor_msgs.msg import CameraInfo
 from shape_msgs.msg import Plane
+from std_msgs.msg import Header
 from tf2_geometry_msgs import PointStamped
 import tf2_ros
 from vision_msgs.msg import Point2D
@@ -76,8 +77,8 @@ def map_point(
             self,
             plane: Plane,
             point: Point2D,
-            time: Time,
-            plane_frame_id: str,
+            time: Time = Time(),
+            plane_frame_id: Optional[str] = None,
             output_frame_id: Optional[str] = None) -> PointStamped:
         """
         Map `Point2DStamped` to 3D `Point` assuming point lies on given plane.
@@ -87,21 +88,25 @@ def map_point(
 
         :param plane: Plane in which the mapping should happen
         :param point: Point that should be mapped
-        :param time: Time at which the point (or the image where it is from) is was captured
-        :param plane_frame_id: TF2 frame referenced for the plane
-        :param output_frame_id: TF2 frame in which the output should be provided
+        :param time: Time that point (or the image where it is from) was captured. If not
+            provided, current time will be used for transforms.
+        :param plane_frame_id: TF2 frame that the plane is defined in. If not provided, it is
+            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
         :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
         """
         # Create numpy array from point and call map_points()
-        np_point = self.map_points(
+        header, np_points = self.map_points(
             plane,
             np.array([[point.x, point.y]]),
             time,
             plane_frame_id,
-            output_frame_id=None)[0]
+            output_frame_id)
+        np_point = np_points[0]
 
         # Check if we have any nan values, aka if we have a valid intersection
         if np.isnan(np_point).any():
@@ -112,32 +117,29 @@ def map_point(
         intersection_stamped.point.x = np_point[0]
         intersection_stamped.point.y = np_point[1]
         intersection_stamped.point.z = np_point[2]
-        intersection_stamped.header.stamp = time
-        intersection_stamped.header.frame_id = self._camera_info.header.frame_id
-
-        # Transform output point if output frame if needed
-        if output_frame_id not in [None, self._camera_info.header.frame_id]:
-            intersection_stamped = self._tf_buffer.transform(
-                intersection_stamped, output_frame_id)
+        intersection_stamped.header = header
 
         return intersection_stamped
 
     def map_points(
             self,
             plane_msg: Plane,
             points: np.ndarray,
-            time: Time,
-            plane_frame_id: str,
-            output_frame_id: Optional[str] = None) -> np.ndarray:
+            time: Time = Time(),
+            plane_frame_id: Optional[str] = None,
+            output_frame_id: Optional[str] = None) -> Tuple[Header, np.ndarray]:
         """
         Map image points onto a given plane using the latest CameraInfo intrinsics.
 
         :param plane_msg: Plane in which the mapping should happen
         :param points: Points that should be mapped in the form of
             a nx2 numpy array where n is the number of points
-        :param time: Time at which the point (or the image where it is from) is was captured
-        :param plane_frame_id: TF2 frame referenced for the plane
-        :param output_frame_id: TF2 frame in which the output should be provided
+        :param time: Time that points (or the image where it is from) was captured. If not
+            provided, current time will be used for transforms.
+        :param plane_frame_id: TF2 frame that the plane is defined in. If not provided, it is
+            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 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 InvalidPlaneException if the plane is invalid
@@ -148,6 +150,14 @@ def map_points(
         if not np.any(plane_msg.coef[:3]):
             raise InvalidPlaneException
 
+        # If no plane_frame_id is provided, use _camera_info's frame_id
+        if plane_frame_id is None:
+            plane_frame_id = self._camera_info.header.frame_id
+
+        # If no output_frame_id is provided, use _camera_info's frame_id
+        if output_frame_id is None:
+            output_frame_id = self._camera_info.header.frame_id
+
         # Convert plane from general form to point normal form
         plane = utils.plane_general_to_point_normal(plane_msg)
 
@@ -156,7 +166,7 @@ def map_points(
             plane=plane,
             input_frame=plane_frame_id,
             output_frame=self._camera_info.header.frame_id,
-            stamp=time,
+            time=time,
             buffer=self._tf_buffer)
 
         # Convert points to float if they aren't allready
@@ -179,4 +189,7 @@ def map_points(
             np_points = utils.transform_points(
                 np_points, output_transformation.transform)
 
-        return np_points
+        # Create header
+        header = Header(frame_id=output_frame_id, stamp=time)
+
+        return (header, np_points)

ipm_library/ipm_library/utils.py

@@ -14,6 +14,7 @@
 
 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
@@ -45,7 +46,7 @@ def transform_plane_to_frame(
         plane: Tuple[np.ndarray, np.ndarray],
         input_frame: str,
         output_frame: str,
-        stamp,
+        time: Time,
         buffer: tf2_ros.Buffer,
         timeout: Optional[Duration] = None) -> Tuple[np.ndarray, np.ndarray]:
     """
@@ -54,30 +55,29 @@ 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 stamp: Timestamp which is used to query
-        the tf buffer and get the tranform at this moment
+    :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
     :returns: A Tuple containing the planes base point and normal vector in the
          new frame at the provided timestamp
     """
-    # Set optinal timeout
+    # Set optional timeout
     if timeout is None:
         timeout = Duration(seconds=0.5)
 
     # 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
     field_normal = PointStamped()
     field_normal.header.frame_id = input_frame
-    field_normal.header.stamp = stamp
+    field_normal.header.stamp = time
     field_normal.point.x = plane[0][0] + plane[1][0]
     field_normal.point.y = plane[0][1] + plane[1][1]
     field_normal.point.z = plane[0][2] + plane[1][2]
     field_normal = buffer.transform(
         field_normal, output_frame, timeout=timeout)
     field_point = PointStamped()
     field_point.header.frame_id = input_frame
-    field_point.header.stamp = stamp
+    field_point.header.stamp = time
     field_point.point.x = plane[0][0]
     field_point.point.y = plane[0][1]
     field_point.point.z = plane[0][2]

ipm_library/package.xml

@@ -10,12 +10,8 @@
   <depend>geometry_msgs</depend>
   <depend>ipm_interfaces</depend>
   <depend>python3-numpy</depend>
-  <depend>sensor_msgs</depend>
-  <depend>shape_msgs</depend>
-  <depend>std_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
-  <depend>vision_msgs</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

ipm_library/test/test_ipm.py

@@ -75,14 +75,7 @@ def test_ipm_map_point_no_transform():
     point_original = np.array([[point_original_x], [point_original_y]])
     point_original_msg = Point2D(x=point_original_x, y=point_original_y)
     # Map points
-    point_mapped_msg = ipm.map_point(
-        plane,
-        point_original_msg,
-        Time(),
-        camera_info.header.frame_id)
-    # Check header
-    assert point_mapped_msg.header == camera_info.header, 'Point header does not match'
-
+    point_mapped_msg = ipm.map_point(plane, point_original_msg)
     # Perform projection back into 2D image using projection matrix K to ensure that
     # it's the same as the original point
     point_mapped_vec = np.array([[point_mapped_msg.point.x],
@@ -119,11 +112,7 @@ def test_ipm_map_points_no_transform():
         [0, 0, 0]
     ])
     # Map points
-    points_mapped = ipm.map_points(
-        plane,
-        points,
-        Time(),
-        camera_info.header.frame_id)
+    _, points_mapped = ipm.map_points(plane, points)
     # Make goal points array, x and y are not exactly 0 because of the camera calibration as
     # well as an uneven amount of pixels
     goal_point_array = np.array([
@@ -152,11 +141,7 @@ def test_ipm_map_point_no_transform_no_intersection():
     # Test if a NoIntersectionError is raised
     with pytest.raises(NoIntersectionError):
         # Map points
-        ipm.map_point(
-            plane,
-            point,
-            Time(),
-            camera_info.header.frame_id)
+        ipm.map_point(plane, point)
 
 
 def test_ipm_map_points_no_transform_no_intersection():
@@ -175,11 +160,7 @@ def test_ipm_map_points_no_transform_no_intersection():
         [0, 0, 0]
     ])
     # Map points
-    points_mapped = ipm.map_points(
-        plane,
-        points,
-        Time(),
-        camera_info.header.frame_id)
+    _, points_mapped = ipm.map_points(plane, points)
     # Make goal points array, x and y are not exactly 0 because of the camera calibration as
     # well as an uneven amount of pixels
     goal_point_array = np.array([
@@ -214,7 +195,6 @@ def test_ipm_map_point():
     point_mapped = ipm.map_point(
         plane,
         point,
-        Time(),
         plane_frame_id='base_footprint',
         output_frame_id='base_footprint')
     # Check header
@@ -258,10 +238,9 @@ def test_ipm_map_points():
         [0, 0, 0]
     ])
     # Map points
-    points_mapped = ipm.map_points(
+    _, points_mapped = ipm.map_points(
         plane,
         points=points,
-        time=Time(),
         plane_frame_id='base_footprint',
         output_frame_id='base_footprint')
     # Make goal points array, x and y are not exactly 0 because of the camera calibration as
@@ -279,18 +258,30 @@ def test_map_point_invalid_plane_exception():
     """Check InvalidPlaneException is raised if a plane is invalid, i.e. a=b=c=0."""
     ipm = IPM(tf2.Buffer(), CameraInfo())
     with pytest.raises(InvalidPlaneException):
-        ipm.map_point(Plane(), Point2D(), Time(), '')
+        ipm.map_point(Plane(), Point2D())
 
 
 def test_map_points_invalid_plane_exception():
     """Check InvalidPlaneException is raised if a plane is invalid, i.e. a=b=c=0."""
     ipm = IPM(tf2.Buffer(), CameraInfo())
     with pytest.raises(InvalidPlaneException):
-        ipm.map_points(Plane(), np.array([]), Time(), '')
+        ipm.map_points(Plane(), np.array([]))
 
 
 def test_camera_info_not_set():
     """Check CameraInfoNotSetException is raised if camera info is not set."""
     ipm = IPM(tf2.Buffer())
     with pytest.raises(CameraInfoNotSetException):
-        ipm.map_point(Plane(), Point2D(), Time(), '')
+        ipm.map_point(Plane(), Point2D())
+
+
+def test_map_point_current_time_used_when_time_parameter_is_not_provided():
+    ipm = IPM(tf2.Buffer(), camera_info)
+    point = ipm.map_point(Plane(coef=[0.0, 0.0, 1.0, -1.0]), Point2D())
+    assert point.header.stamp == Time()
+
+
+def test_map_point_camera_frame_used_when_output_frame_id_parameter_is_not_provided():
+    ipm = IPM(tf2.Buffer(), camera_info)
+    point = ipm.map_point(Plane(coef=[0.0, 0.0, 1.0, -1.0]), Point2D())
+    assert point.header.frame_id == camera_info.header.frame_id