From 68681d7916001a0f98806e4ed9768d325a680f75 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Wed, 31 Aug 2022 22:03:33 +0900 Subject: [PATCH 01/15] add default parameters for time and plane_frame_id Signed-off-by: Kenji Brameld --- ipm_library/ipm_library/ipm.py | 14 +++++++++----- ipm_library/ipm_library/utils.py | 12 ++++++------ ipm_library/test/test_ipm.py | 9 +++++---- ipm_service/test/test_ipm_service.py | 9 ++------- 4 files changed, 22 insertions(+), 22 deletions(-) diff --git a/ipm_library/ipm_library/ipm.py b/ipm_library/ipm_library/ipm.py index b048ca5..300eb52 100644 --- a/ipm_library/ipm_library/ipm.py +++ b/ipm_library/ipm_library/ipm.py @@ -76,8 +76,8 @@ def map_point( self, plane: Plane, point: Point2D, - time: Time, - plane_frame_id: str, + time: Optional[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. @@ -126,8 +126,8 @@ def map_points( self, plane_msg: Plane, points: np.ndarray, - time: Time, - plane_frame_id: str, + time: Optional[Time] = Time(), + plane_frame_id: Optional[str] = None, output_frame_id: Optional[str] = None) -> np.ndarray: """ Map image points onto a given plane using the latest CameraInfo intrinsics. @@ -148,6 +148,10 @@ 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 + # Convert plane from general form to point normal form plane = utils.plane_general_to_point_normal(plane_msg) @@ -156,7 +160,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 diff --git a/ipm_library/ipm_library/utils.py b/ipm_library/ipm_library/utils.py index 007995f..c7b2195 100644 --- a/ipm_library/ipm_library/utils.py +++ b/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,14 +55,13 @@ 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) @@ -69,7 +69,7 @@ def transform_plane_to_frame( # 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] @@ -77,7 +77,7 @@ def transform_plane_to_frame( 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] diff --git a/ipm_library/test/test_ipm.py b/ipm_library/test/test_ipm.py index b202f61..cbfc551 100644 --- a/ipm_library/test/test_ipm.py +++ b/ipm_library/test/test_ipm.py @@ -77,11 +77,12 @@ def test_ipm_map_point_no_transform(): # Map points point_mapped_msg = ipm.map_point( plane, - point_original_msg, - Time(), - camera_info.header.frame_id) + point_original_msg) # Check header - assert point_mapped_msg.header == camera_info.header, 'Point header does not match' + assert point_mapped_msg.header.frame_id == camera_info.header.frame_id, \ + "Mapped point's frame_id doesn't match the one from camera_info, but it should." + assert point_mapped_msg.header.stamp == Time(), \ + "Mapped point's stamp should be builtin_interfaces.msg.Time(), but it isn't." # Perform projection back into 2D image using projection matrix K to ensure that # it's the same as the original point diff --git a/ipm_service/test/test_ipm_service.py b/ipm_service/test/test_ipm_service.py index 42c52b2..1c6497c 100644 --- a/ipm_service/test/test_ipm_service.py +++ b/ipm_service/test/test_ipm_service.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from builtin_interfaces.msg import Time from ipm_interfaces.srv import MapPoint, MapPointCloud2 from ipm_library.ipm import IPM from ipm_service.ipm import IPMService @@ -164,9 +163,7 @@ def test_map_point(): ipm = IPM(Buffer(), camera_info) expected_point = ipm.map_point( plane, - point, - Time(), - plane_frame_id='camera_optical_frame') + point) assert future.result().point == expected_point rclpy.shutdown() @@ -260,9 +257,7 @@ def test_map_point_cloud(): ipm = IPM(Buffer(), camera_info) expected_points = ipm.map_points( plane, - points, - time=Time(), - plane_frame_id='camera_optical_frame') + points) np.testing.assert_allclose( read_points_numpy(future.result().points), From 25b83314a796fb3a30c127b6d1c484e7d59a7e31 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Thu, 1 Sep 2022 06:58:12 +0900 Subject: [PATCH 02/15] don't use optional for parameters that can't have None as a value Signed-off-by: Kenji Brameld --- ipm_library/ipm_library/ipm.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ipm_library/ipm_library/ipm.py b/ipm_library/ipm_library/ipm.py index 300eb52..7d88489 100644 --- a/ipm_library/ipm_library/ipm.py +++ b/ipm_library/ipm_library/ipm.py @@ -76,7 +76,7 @@ def map_point( self, plane: Plane, point: Point2D, - time: Optional[Time] = Time(), + time: Time = Time(), plane_frame_id: Optional[str] = None, output_frame_id: Optional[str] = None) -> PointStamped: """ @@ -126,7 +126,7 @@ def map_points( self, plane_msg: Plane, points: np.ndarray, - time: Optional[Time] = Time(), + time: Time = Time(), plane_frame_id: Optional[str] = None, output_frame_id: Optional[str] = None) -> np.ndarray: """ From f32c8229790d366ee84378627d92a79eedb17f5e Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Thu, 1 Sep 2022 07:09:57 +0900 Subject: [PATCH 03/15] move out tests for header into own tests Signed-off-by: Kenji Brameld --- ipm_library/test/test_ipm.py | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/ipm_library/test/test_ipm.py b/ipm_library/test/test_ipm.py index cbfc551..79ed12c 100644 --- a/ipm_library/test/test_ipm.py +++ b/ipm_library/test/test_ipm.py @@ -78,12 +78,6 @@ def test_ipm_map_point_no_transform(): point_mapped_msg = ipm.map_point( plane, point_original_msg) - # Check header - assert point_mapped_msg.header.frame_id == camera_info.header.frame_id, \ - "Mapped point's frame_id doesn't match the one from camera_info, but it should." - assert point_mapped_msg.header.stamp == Time(), \ - "Mapped point's stamp should be builtin_interfaces.msg.Time(), but it isn't." - # 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], @@ -295,3 +289,15 @@ def test_camera_info_not_set(): ipm = IPM(tf2.Buffer()) with pytest.raises(CameraInfoNotSetException): ipm.map_point(Plane(), Point2D(), Time(), '') + + +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 From bcc8a505594d3f756e485b6f0fdc76237265a86a Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Thu, 1 Sep 2022 07:34:52 +0900 Subject: [PATCH 04/15] return tuple from map_points Signed-off-by: Kenji Brameld --- ipm_library/ipm_library/ipm.py | 27 +++++++++++++++------------ ipm_library/test/test_ipm.py | 6 +++--- ipm_service/ipm_service/ipm.py | 9 ++------- ipm_service/test/test_ipm_service.py | 2 +- 4 files changed, 21 insertions(+), 23 deletions(-) diff --git a/ipm_library/ipm_library/ipm.py b/ipm_library/ipm_library/ipm.py index 7d88489..6ef19a7 100644 --- a/ipm_library/ipm_library/ipm.py +++ b/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 @@ -96,12 +97,13 @@ def map_point( :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,13 +114,7 @@ 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 @@ -128,7 +124,7 @@ def map_points( points: np.ndarray, time: Time = Time(), plane_frame_id: Optional[str] = None, - output_frame_id: Optional[str] = None) -> np.ndarray: + output_frame_id: Optional[str] = None) -> Tuple[Header, np.ndarray]: """ Map image points onto a given plane using the latest CameraInfo intrinsics. @@ -152,6 +148,10 @@ def map_points( 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) @@ -183,4 +183,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) diff --git a/ipm_library/test/test_ipm.py b/ipm_library/test/test_ipm.py index 79ed12c..89cf4ed 100644 --- a/ipm_library/test/test_ipm.py +++ b/ipm_library/test/test_ipm.py @@ -114,7 +114,7 @@ def test_ipm_map_points_no_transform(): [0, 0, 0] ]) # Map points - points_mapped = ipm.map_points( + _, points_mapped = ipm.map_points( plane, points, Time(), @@ -170,7 +170,7 @@ def test_ipm_map_points_no_transform_no_intersection(): [0, 0, 0] ]) # Map points - points_mapped = ipm.map_points( + _, points_mapped = ipm.map_points( plane, points, Time(), @@ -253,7 +253,7 @@ 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(), diff --git a/ipm_service/ipm_service/ipm.py b/ipm_service/ipm_service/ipm.py index 5a5cd8d..c1ed666 100644 --- a/ipm_service/ipm_service/ipm.py +++ b/ipm_service/ipm_service/ipm.py @@ -21,7 +21,6 @@ from rclpy.node import Node from sensor_msgs.msg import CameraInfo from sensor_msgs_py.point_cloud2 import create_cloud_xyz32, read_points_numpy -from std_msgs.msg import Header import tf2_ros as tf2 @@ -106,7 +105,7 @@ def point_cloud_mapping_callback( # Map the given point and handle different result scenarios try: - mapped_points = self.ipm.map_points( + header, mapped_points = self.ipm.map_points( request.plane, read_points_numpy(request.points), request.points.header.stamp, @@ -114,11 +113,7 @@ def point_cloud_mapping_callback( output_frame_id) # Convert them into a PointCloud2 - response.points = create_cloud_xyz32( - Header( - stamp=request.points.header.stamp, - frame_id=output_frame_id), - mapped_points) + response.points = create_cloud_xyz32(header, mapped_points) response.result = MapPointCloud2.Response.RESULT_SUCCESS except InvalidPlaneException: diff --git a/ipm_service/test/test_ipm_service.py b/ipm_service/test/test_ipm_service.py index 1c6497c..eba2436 100644 --- a/ipm_service/test/test_ipm_service.py +++ b/ipm_service/test/test_ipm_service.py @@ -255,7 +255,7 @@ def test_map_point_cloud(): assert future.result().result == MapPointCloud2.Response.RESULT_SUCCESS ipm = IPM(Buffer(), camera_info) - expected_points = ipm.map_points( + _, expected_points = ipm.map_points( plane, points) From 69d02b4b289632656cad1ae8610cdd639c8d4fa6 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Thu, 1 Sep 2022 07:34:59 +0900 Subject: [PATCH 05/15] remove unneeded dependencies Signed-off-by: Kenji Brameld --- ipm_interfaces/package.xml | 1 - ipm_library/package.xml | 4 ---- 2 files changed, 5 deletions(-) diff --git a/ipm_interfaces/package.xml b/ipm_interfaces/package.xml index 1a9e6e8..97193a2 100644 --- a/ipm_interfaces/package.xml +++ b/ipm_interfaces/package.xml @@ -12,7 +12,6 @@ rosidl_default_runtime - geometry_msgs sensor_msgs shape_msgs vision_msgs diff --git a/ipm_library/package.xml b/ipm_library/package.xml index c73a414..09d20e7 100644 --- a/ipm_library/package.xml +++ b/ipm_library/package.xml @@ -10,12 +10,8 @@ geometry_msgs ipm_interfaces python3-numpy - sensor_msgs - shape_msgs - std_msgs tf2 tf2_geometry_msgs - vision_msgs ament_copyright ament_flake8 From b288ec1d8c7bc532167afa2ef63e7435628acdf2 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Thu, 1 Sep 2022 08:04:59 +0900 Subject: [PATCH 06/15] remove unneeded arguments to map_point because there are default arguments Signed-off-by: Kenji Brameld --- ipm_library/test/test_ipm.py | 26 ++++++-------------------- 1 file changed, 6 insertions(+), 20 deletions(-) diff --git a/ipm_library/test/test_ipm.py b/ipm_library/test/test_ipm.py index 89cf4ed..a147bdc 100644 --- a/ipm_library/test/test_ipm.py +++ b/ipm_library/test/test_ipm.py @@ -114,11 +114,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([ @@ -147,11 +143,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(): @@ -170,11 +162,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([ @@ -209,7 +197,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 @@ -256,7 +243,6 @@ def test_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 @@ -274,21 +260,21 @@ 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(): From a32743d8e9fd034b3cbbcd5fa70411ef6b499679 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Thu, 1 Sep 2022 08:17:42 +0900 Subject: [PATCH 07/15] use builtin string type in service interfaces Signed-off-by: Kenji Brameld --- ipm_interfaces/package.xml | 1 - ipm_interfaces/srv/MapPoint.srv | 4 ++-- ipm_interfaces/srv/MapPointCloud2.srv | 4 ++-- ipm_service/ipm_service/ipm.py | 28 +++++++++++++++++++-------- ipm_service/test/test_ipm_service.py | 10 ++-------- 5 files changed, 26 insertions(+), 21 deletions(-) diff --git a/ipm_interfaces/package.xml b/ipm_interfaces/package.xml index 97193a2..c2c690f 100644 --- a/ipm_interfaces/package.xml +++ b/ipm_interfaces/package.xml @@ -15,7 +15,6 @@ sensor_msgs shape_msgs vision_msgs - std_msgs ament_lint_auto ament_lint_common diff --git a/ipm_interfaces/srv/MapPoint.srv b/ipm_interfaces/srv/MapPoint.srv index f1972b3..a0c4966 100644 --- a/ipm_interfaces/srv/MapPoint.srv +++ b/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 diff --git a/ipm_interfaces/srv/MapPointCloud2.srv b/ipm_interfaces/srv/MapPointCloud2.srv index 3e371f8..892e9dd 100644 --- a/ipm_interfaces/srv/MapPointCloud2.srv +++ b/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 diff --git a/ipm_service/ipm_service/ipm.py b/ipm_service/ipm_service/ipm.py index c1ed666..5feb771 100644 --- a/ipm_service/ipm_service/ipm.py +++ b/ipm_service/ipm_service/ipm.py @@ -58,11 +58,17 @@ def point_mapping_callback( response.result = MapPoint.Response.RESULT_NO_CAMERA_INFO return response - # Map optional marking from '' to None - if request.output_frame_id.data == '': + # Map optional plane_frame_id from '' to None + if request.plane_frame_id == '': + plane_frame_id = None + else: + plane_frame_id = request.plane_frame_id + + # Map optional output_frame_id from '' to None + if request.output_frame_id == '': output_frame_id = None else: - output_frame_id = request.output_frame_id.data + output_frame_id = request.output_frame_id # Maps the given point and handle different result scenarios try: @@ -70,7 +76,7 @@ def point_mapping_callback( request.plane, request.point, request.time, - request.plane_frame_id.data, + plane_frame_id, output_frame_id) response.result = MapPoint.Response.RESULT_SUCCESS except NoIntersectionError: @@ -97,11 +103,17 @@ def point_cloud_mapping_callback( response.result = MapPointCloud2.Response.RESULT_NO_CAMERA_INFO return response - # Map optional marking from '' to None - if request.output_frame_id.data == '': + # Map optional plane_frame_id from '' to None + if request.plane_frame_id == '': + plane_frame_id = None + else: + plane_frame_id = request.plane_frame_id + + # Map optional output_frame_id from '' to None + if request.output_frame_id == '': output_frame_id = self.ipm.get_camera_info().header.frame_id else: - output_frame_id = request.output_frame_id.data + output_frame_id = request.output_frame_id # Map the given point and handle different result scenarios try: @@ -109,7 +121,7 @@ def point_cloud_mapping_callback( request.plane, read_points_numpy(request.points), request.points.header.stamp, - request.plane_frame_id.data, + plane_frame_id, output_frame_id) # Convert them into a PointCloud2 diff --git a/ipm_service/test/test_ipm_service.py b/ipm_service/test/test_ipm_service.py index eba2436..905f6c5 100644 --- a/ipm_service/test/test_ipm_service.py +++ b/ipm_service/test/test_ipm_service.py @@ -148,10 +148,7 @@ def test_map_point(): plane.coef[3] = -1.0 # 1 meter distance client = test_node.create_client(MapPoint, 'map_point') - req = MapPoint.Request( - point=point, - plane=plane, - plane_frame_id=String(data='camera_optical_frame')) + req = MapPoint.Request(point=point, plane=plane) future = client.call_async(req) rclpy.spin_once(ipm_service_node, timeout_sec=0.1) @@ -242,10 +239,7 @@ def test_map_point_cloud(): plane.coef[3] = -1.0 # 1 meter distance client = test_node.create_client(MapPointCloud2, 'map_pointcloud2') - req = MapPointCloud2.Request( - points=point_cloud, - plane=plane, - plane_frame_id=String(data='camera_optical_frame')) + req = MapPointCloud2.Request(points=point_cloud, plane=plane) future = client.call_async(req) rclpy.spin_once(ipm_service_node, timeout_sec=0.1) From 9d9d717080e9bbf8773b953bf32f6766e6f4ffb7 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Thu, 1 Sep 2022 08:35:01 +0900 Subject: [PATCH 08/15] update docstring Signed-off-by: Kenji Brameld --- ipm_library/ipm_library/ipm.py | 18 ++++++++++++------ ipm_library/test/test_ipm.py | 4 +--- ipm_service/ipm_service/ipm.py | 2 +- ipm_service/test/test_ipm_service.py | 10 +++------- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/ipm_library/ipm_library/ipm.py b/ipm_library/ipm_library/ipm.py index 6ef19a7..628e611 100644 --- a/ipm_library/ipm_library/ipm.py +++ b/ipm_library/ipm_library/ipm.py @@ -88,9 +88,12 @@ 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 @@ -131,9 +134,12 @@ def map_points( :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 diff --git a/ipm_library/test/test_ipm.py b/ipm_library/test/test_ipm.py index a147bdc..d3e6d9e 100644 --- a/ipm_library/test/test_ipm.py +++ b/ipm_library/test/test_ipm.py @@ -75,9 +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) + 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], diff --git a/ipm_service/ipm_service/ipm.py b/ipm_service/ipm_service/ipm.py index 5feb771..df0e792 100644 --- a/ipm_service/ipm_service/ipm.py +++ b/ipm_service/ipm_service/ipm.py @@ -111,7 +111,7 @@ def point_cloud_mapping_callback( # Map optional output_frame_id from '' to None if request.output_frame_id == '': - output_frame_id = self.ipm.get_camera_info().header.frame_id + output_frame_id = None else: output_frame_id = request.output_frame_id diff --git a/ipm_service/test/test_ipm_service.py b/ipm_service/test/test_ipm_service.py index 905f6c5..aacde13 100644 --- a/ipm_service/test/test_ipm_service.py +++ b/ipm_service/test/test_ipm_service.py @@ -20,7 +20,7 @@ from sensor_msgs.msg import CameraInfo from sensor_msgs_py.point_cloud2 import create_cloud, PointField, read_points_numpy from shape_msgs.msg import Plane -from std_msgs.msg import Header, String +from std_msgs.msg import Header from tf2_ros import Buffer from vision_msgs.msg import Point2D @@ -158,9 +158,7 @@ def test_map_point(): assert future.result().result == MapPoint.Response.RESULT_SUCCESS ipm = IPM(Buffer(), camera_info) - expected_point = ipm.map_point( - plane, - point) + expected_point = ipm.map_point(plane, point) assert future.result().point == expected_point rclpy.shutdown() @@ -249,9 +247,7 @@ def test_map_point_cloud(): assert future.result().result == MapPointCloud2.Response.RESULT_SUCCESS ipm = IPM(Buffer(), camera_info) - _, expected_points = ipm.map_points( - plane, - points) + _, expected_points = ipm.map_points(plane, points) np.testing.assert_allclose( read_points_numpy(future.result().points), From caa905e6a99111702aaeb5c3f5779161f90c8a21 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Thu, 1 Sep 2022 20:58:20 +0900 Subject: [PATCH 09/15] add back geometry_msgs dependency Signed-off-by: Kenji Brameld --- ipm_interfaces/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/ipm_interfaces/package.xml b/ipm_interfaces/package.xml index c2c690f..937884b 100644 --- a/ipm_interfaces/package.xml +++ b/ipm_interfaces/package.xml @@ -12,6 +12,7 @@ rosidl_default_runtime + geometry_msgs sensor_msgs shape_msgs vision_msgs From 27621b14626964c4e1d3f2788bea481b5bbc503a Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Fri, 2 Sep 2022 08:10:44 +0900 Subject: [PATCH 10/15] re-add dependencies and don't depend on them being transitive dependencies Signed-off-by: Kenji Brameld --- ipm_library/package.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ipm_library/package.xml b/ipm_library/package.xml index 09d20e7..c73a414 100644 --- a/ipm_library/package.xml +++ b/ipm_library/package.xml @@ -10,8 +10,12 @@ geometry_msgs ipm_interfaces python3-numpy + sensor_msgs + shape_msgs + std_msgs tf2 tf2_geometry_msgs + vision_msgs ament_copyright ament_flake8 From 0ac3eb2599f1d47e1d818145bdb6b93bf573b534 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Fri, 2 Sep 2022 08:13:17 +0900 Subject: [PATCH 11/15] remove unused dependency from ipm_interfaces' CMakeLists.txt Signed-off-by: Kenji Brameld --- ipm_interfaces/CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ipm_interfaces/CMakeLists.txt b/ipm_interfaces/CMakeLists.txt index 007a80b..3fe1006 100644 --- a/ipm_interfaces/CMakeLists.txt +++ b/ipm_interfaces/CMakeLists.txt @@ -11,7 +11,6 @@ find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(sensor_msgs REQUIRED) find_package(shape_msgs REQUIRED) -find_package(std_msgs REQUIRED) find_package(vision_msgs REQUIRED) if(BUILD_TESTING) @@ -22,7 +21,7 @@ endif() rosidl_generate_interfaces(${PROJECT_NAME} "srv/MapPoint.srv" "srv/MapPointCloud2.srv" - DEPENDENCIES geometry_msgs sensor_msgs shape_msgs std_msgs vision_msgs + DEPENDENCIES geometry_msgs sensor_msgs shape_msgs vision_msgs ) ament_export_dependencies(rosidl_default_runtime) From f637a3ed035e2a81cac6ef0885f6fd9dc346c599 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Fri, 2 Sep 2022 08:16:55 +0900 Subject: [PATCH 12/15] add builtin_interfaces as dependency Signed-off-by: Kenji Brameld --- ipm_interfaces/CMakeLists.txt | 3 ++- ipm_interfaces/package.xml | 1 + ipm_library/package.xml | 1 + 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/ipm_interfaces/CMakeLists.txt b/ipm_interfaces/CMakeLists.txt index 3fe1006..7133d96 100644 --- a/ipm_interfaces/CMakeLists.txt +++ b/ipm_interfaces/CMakeLists.txt @@ -7,6 +7,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(sensor_msgs REQUIRED) @@ -21,7 +22,7 @@ endif() rosidl_generate_interfaces(${PROJECT_NAME} "srv/MapPoint.srv" "srv/MapPointCloud2.srv" - DEPENDENCIES geometry_msgs sensor_msgs shape_msgs vision_msgs + DEPENDENCIES builtin_interfaces geometry_msgs sensor_msgs shape_msgs vision_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/ipm_interfaces/package.xml b/ipm_interfaces/package.xml index 937884b..bf869c4 100644 --- a/ipm_interfaces/package.xml +++ b/ipm_interfaces/package.xml @@ -12,6 +12,7 @@ rosidl_default_runtime + builtin_interfaces geometry_msgs sensor_msgs shape_msgs diff --git a/ipm_library/package.xml b/ipm_library/package.xml index c73a414..d7790ec 100644 --- a/ipm_library/package.xml +++ b/ipm_library/package.xml @@ -7,6 +7,7 @@ Florian Vahl Apache License 2.0 + builtin_interfaces geometry_msgs ipm_interfaces python3-numpy From 9c14aaf6176f724cfd39bf093d2ef8d15044ddce Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Tue, 20 Sep 2022 18:18:44 +0900 Subject: [PATCH 13/15] make time parameter required Signed-off-by: Kenji Brameld --- ipm_library/ipm_library/ipm.py | 10 ++++------ ipm_library/test/test_ipm.py | 20 +++++++++++--------- ipm_service/test/test_ipm_service.py | 5 +++-- 3 files changed, 18 insertions(+), 17 deletions(-) diff --git a/ipm_library/ipm_library/ipm.py b/ipm_library/ipm_library/ipm.py index 628e611..faa2844 100644 --- a/ipm_library/ipm_library/ipm.py +++ b/ipm_library/ipm_library/ipm.py @@ -77,7 +77,7 @@ def map_point( self, plane: Plane, point: Point2D, - time: Time = Time(), + time: Time, plane_frame_id: Optional[str] = None, output_frame_id: Optional[str] = None) -> PointStamped: """ @@ -88,8 +88,7 @@ def map_point( :param plane: Plane in which the mapping should happen :param point: Point that should be mapped - :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 time: Time that point (or the image where it is from) was captured. :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, @@ -125,7 +124,7 @@ def map_points( self, plane_msg: Plane, points: np.ndarray, - time: Time = Time(), + time: Time, plane_frame_id: Optional[str] = None, output_frame_id: Optional[str] = None) -> Tuple[Header, np.ndarray]: """ @@ -134,8 +133,7 @@ def map_points( :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 that points (or the image where it is from) was captured. If not - provided, current time will be used for transforms. + :param time: Time that points (or the image where it is from) was captured. :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, diff --git a/ipm_library/test/test_ipm.py b/ipm_library/test/test_ipm.py index d3e6d9e..9e09384 100644 --- a/ipm_library/test/test_ipm.py +++ b/ipm_library/test/test_ipm.py @@ -75,7 +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) + point_mapped_msg = ipm.map_point(plane, point_original_msg, Time()) # 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], @@ -112,7 +112,7 @@ def test_ipm_map_points_no_transform(): [0, 0, 0] ]) # Map points - _, points_mapped = ipm.map_points(plane, points) + _, points_mapped = ipm.map_points(plane, points, Time()) # 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([ @@ -141,7 +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) + ipm.map_point(plane, point, Time()) def test_ipm_map_points_no_transform_no_intersection(): @@ -160,7 +160,7 @@ def test_ipm_map_points_no_transform_no_intersection(): [0, 0, 0] ]) # Map points - _, points_mapped = ipm.map_points(plane, points) + _, points_mapped = ipm.map_points(plane, points, Time()) # 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([ @@ -195,6 +195,7 @@ def test_ipm_map_point(): point_mapped = ipm.map_point( plane, point, + time=Time(), plane_frame_id='base_footprint', output_frame_id='base_footprint') # Check header @@ -241,6 +242,7 @@ def test_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 @@ -258,30 +260,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()) + ipm.map_point(Plane(), Point2D(), Time()) 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([])) + ipm.map_points(Plane(), np.array([]), Time()) 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()) + ipm.map_point(Plane(), Point2D(), Time()) 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()) + point = ipm.map_point(Plane(coef=[0.0, 0.0, 1.0, -1.0]), Point2D(), Time()) 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()) + point = ipm.map_point(Plane(coef=[0.0, 0.0, 1.0, -1.0]), Point2D(), Time()) assert point.header.frame_id == camera_info.header.frame_id diff --git a/ipm_service/test/test_ipm_service.py b/ipm_service/test/test_ipm_service.py index aacde13..6b326bb 100644 --- a/ipm_service/test/test_ipm_service.py +++ b/ipm_service/test/test_ipm_service.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +from builtin_interfaces.msg import Time from ipm_interfaces.srv import MapPoint, MapPointCloud2 from ipm_library.ipm import IPM from ipm_service.ipm import IPMService @@ -158,7 +159,7 @@ def test_map_point(): assert future.result().result == MapPoint.Response.RESULT_SUCCESS ipm = IPM(Buffer(), camera_info) - expected_point = ipm.map_point(plane, point) + expected_point = ipm.map_point(plane, point, Time()) assert future.result().point == expected_point rclpy.shutdown() @@ -247,7 +248,7 @@ def test_map_point_cloud(): assert future.result().result == MapPointCloud2.Response.RESULT_SUCCESS ipm = IPM(Buffer(), camera_info) - _, expected_points = ipm.map_points(plane, points) + _, expected_points = ipm.map_points(plane, points, Time()) np.testing.assert_allclose( read_points_numpy(future.result().points), From a15ad109bfcf91cf7cba7d8e81bf770cfdd795ce Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Tue, 20 Sep 2022 18:27:00 +0900 Subject: [PATCH 14/15] revert some formatting changes Signed-off-by: Kenji Brameld --- ipm_library/test/test_ipm.py | 22 +++++++++++++++++----- ipm_service/test/test_ipm_service.py | 18 ++++++++++++++---- 2 files changed, 31 insertions(+), 9 deletions(-) diff --git a/ipm_library/test/test_ipm.py b/ipm_library/test/test_ipm.py index 9e09384..ff0bc3f 100644 --- a/ipm_library/test/test_ipm.py +++ b/ipm_library/test/test_ipm.py @@ -75,7 +75,10 @@ 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()) + point_mapped_msg = ipm.map_point( + plane, + point_original_msg, + Time()) # 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], @@ -112,7 +115,10 @@ def test_ipm_map_points_no_transform(): [0, 0, 0] ]) # Map points - _, points_mapped = ipm.map_points(plane, points, Time()) + _, points_mapped = ipm.map_points( + plane, + points, + Time()) # 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([ @@ -141,7 +147,10 @@ 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()) + ipm.map_point( + plane, + point, + Time()) def test_ipm_map_points_no_transform_no_intersection(): @@ -160,7 +169,10 @@ def test_ipm_map_points_no_transform_no_intersection(): [0, 0, 0] ]) # Map points - _, points_mapped = ipm.map_points(plane, points, Time()) + _, points_mapped = ipm.map_points( + plane, + points, + Time()) # 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([ @@ -195,7 +207,7 @@ def test_ipm_map_point(): point_mapped = ipm.map_point( plane, point, - time=Time(), + Time(), plane_frame_id='base_footprint', output_frame_id='base_footprint') # Check header diff --git a/ipm_service/test/test_ipm_service.py b/ipm_service/test/test_ipm_service.py index 6b326bb..56bfc6a 100644 --- a/ipm_service/test/test_ipm_service.py +++ b/ipm_service/test/test_ipm_service.py @@ -149,7 +149,9 @@ def test_map_point(): plane.coef[3] = -1.0 # 1 meter distance client = test_node.create_client(MapPoint, 'map_point') - req = MapPoint.Request(point=point, plane=plane) + req = MapPoint.Request( + point=point, + plane=plane) future = client.call_async(req) rclpy.spin_once(ipm_service_node, timeout_sec=0.1) @@ -159,7 +161,10 @@ def test_map_point(): assert future.result().result == MapPoint.Response.RESULT_SUCCESS ipm = IPM(Buffer(), camera_info) - expected_point = ipm.map_point(plane, point, Time()) + expected_point = ipm.map_point( + plane, + point, + Time()) assert future.result().point == expected_point rclpy.shutdown() @@ -238,7 +243,9 @@ def test_map_point_cloud(): plane.coef[3] = -1.0 # 1 meter distance client = test_node.create_client(MapPointCloud2, 'map_pointcloud2') - req = MapPointCloud2.Request(points=point_cloud, plane=plane) + req = MapPointCloud2.Request( + points=point_cloud, + plane=plane) future = client.call_async(req) rclpy.spin_once(ipm_service_node, timeout_sec=0.1) @@ -248,7 +255,10 @@ def test_map_point_cloud(): assert future.result().result == MapPointCloud2.Response.RESULT_SUCCESS ipm = IPM(Buffer(), camera_info) - _, expected_points = ipm.map_points(plane, points, Time()) + _, expected_points = ipm.map_points( + plane, + points, + Time()) np.testing.assert_allclose( read_points_numpy(future.result().points), From c9c59341f27d75ad75c4b067c6048da7a591d3eb Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 3 Oct 2022 20:59:19 +0900 Subject: [PATCH 15/15] remove redundant test Signed-off-by: Kenji Brameld --- ipm_library/test/test_ipm.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/ipm_library/test/test_ipm.py b/ipm_library/test/test_ipm.py index ff0bc3f..032b54a 100644 --- a/ipm_library/test/test_ipm.py +++ b/ipm_library/test/test_ipm.py @@ -289,12 +289,6 @@ def test_camera_info_not_set(): ipm.map_point(Plane(), Point2D(), Time()) -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(), Time()) - 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(), Time())