From a822d60ab3b53061a79193dd02111e7a5a290131 Mon Sep 17 00:00:00 2001 From: Phil Underwood Date: Tue, 9 Sep 2025 22:52:49 +0100 Subject: [PATCH 1/5] fix bugs in SetCameraInfo functions --- .../camera_info_manager/camera_info_manager.py | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/camera_info_manager_py/camera_info_manager/camera_info_manager.py b/camera_info_manager_py/camera_info_manager/camera_info_manager.py index 1360e289..6ab70f31 100644 --- a/camera_info_manager_py/camera_info_manager/camera_info_manager.py +++ b/camera_info_manager_py/camera_info_manager/camera_info_manager.py @@ -53,6 +53,17 @@ from sensor_msgs.msg import CameraInfo from sensor_msgs.srv import SetCameraInfo import yaml +import array +import numpy as np + +# tell yaml to serialise array.arrays and np.arrays as simple lists +# (these hold the matrices and distortion coefficients in CameraInfo) + +def ndarray_representer(dumper: yaml.Dumper, array: np.ndarray) -> yaml.Node: + return dumper.represent_list(array.tolist()) + +yaml.SafeDumper.add_representer(np.ndarray, ndarray_representer) +yaml.SafeDumper.add_representer(array.array, yaml.representer.Representer.represent_list) default_camera_info_url = 'file://${ROS_HOME}/camera_info/${NAME}.yaml' # parseURL() type codes: @@ -326,7 +337,7 @@ def loadCameraInfo(self): """ self._loadCalibration(self.url, self.cname) - def setCameraInfo(self, req): + def setCameraInfo(self, req, rsp): """ Set camera info request callback. @@ -339,7 +350,6 @@ def setCameraInfo(self, req): """ self.node.get_logger().debug('SetCameraInfo received for ' + self.cname) self.camera_info = req.camera_info - rsp = SetCameraInfo.Response() rsp.success = saveCalibration(req.camera_info, self.url, self.cname) if not rsp.success: rsp.status_message = 'Error storing camera calibration.' From 07640d8ed67956aab1f6841905f1411786195c7d Mon Sep 17 00:00:00 2001 From: Phil Underwood Date: Sat, 13 Sep 2025 16:18:20 +0100 Subject: [PATCH 2/5] Update camera_info_manager.py Fix pyflake warnings --- .../camera_info_manager/camera_info_manager.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/camera_info_manager_py/camera_info_manager/camera_info_manager.py b/camera_info_manager_py/camera_info_manager/camera_info_manager.py index 6ab70f31..e91b1704 100644 --- a/camera_info_manager_py/camera_info_manager/camera_info_manager.py +++ b/camera_info_manager_py/camera_info_manager/camera_info_manager.py @@ -45,6 +45,7 @@ import locale import os from pathlib import Path +import array from ament_index_python import get_package_share_directory from ament_index_python import PackageNotFoundError @@ -53,15 +54,16 @@ from sensor_msgs.msg import CameraInfo from sensor_msgs.srv import SetCameraInfo import yaml -import array import numpy as np -# tell yaml to serialise array.arrays and np.arrays as simple lists -# (these hold the matrices and distortion coefficients in CameraInfo) def ndarray_representer(dumper: yaml.Dumper, array: np.ndarray) -> yaml.Node: + """ + This makes yaml output a numpy array as though it were a simple list + """ return dumper.represent_list(array.tolist()) + yaml.SafeDumper.add_representer(np.ndarray, ndarray_representer) yaml.SafeDumper.add_representer(array.array, yaml.representer.Representer.represent_list) From 06a99a6c1b8d2c96ee791952f9562564196484b6 Mon Sep 17 00:00:00 2001 From: Phil Underwood Date: Tue, 16 Sep 2025 22:37:17 +0100 Subject: [PATCH 3/5] Make tests pass --- .../camera_info_manager/camera_info_manager.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/camera_info_manager_py/camera_info_manager/camera_info_manager.py b/camera_info_manager_py/camera_info_manager/camera_info_manager.py index e91b1704..37e62a62 100644 --- a/camera_info_manager_py/camera_info_manager/camera_info_manager.py +++ b/camera_info_manager_py/camera_info_manager/camera_info_manager.py @@ -57,14 +57,12 @@ import numpy as np -def ndarray_representer(dumper: yaml.Dumper, array: np.ndarray) -> yaml.Node: - """ - This makes yaml output a numpy array as though it were a simple list - """ +def _ndarray_representer(dumper: yaml.Dumper, array: np.ndarray) -> yaml.Node: + """Make yaml output a numpy array as though it were a simple list.""" return dumper.represent_list(array.tolist()) -yaml.SafeDumper.add_representer(np.ndarray, ndarray_representer) +yaml.SafeDumper.add_representer(np.ndarray, _ndarray_representer) yaml.SafeDumper.add_representer(array.array, yaml.representer.Representer.represent_list) default_camera_info_url = 'file://${ROS_HOME}/camera_info/${NAME}.yaml' From a66ec5c710e3611d51527ea526f3c2fc146f9e10 Mon Sep 17 00:00:00 2001 From: Phil Underwood Date: Wed, 17 Sep 2025 11:55:10 +0100 Subject: [PATCH 4/5] Update camera_info_manager.py Make pyflake happy?? --- .../camera_info_manager/camera_info_manager.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/camera_info_manager_py/camera_info_manager/camera_info_manager.py b/camera_info_manager_py/camera_info_manager/camera_info_manager.py index 37e62a62..83af8162 100644 --- a/camera_info_manager_py/camera_info_manager/camera_info_manager.py +++ b/camera_info_manager_py/camera_info_manager/camera_info_manager.py @@ -41,12 +41,13 @@ """ +import array import errno import locale import os from pathlib import Path -import array +import numpy as np from ament_index_python import get_package_share_directory from ament_index_python import PackageNotFoundError import rclpy @@ -54,7 +55,6 @@ from sensor_msgs.msg import CameraInfo from sensor_msgs.srv import SetCameraInfo import yaml -import numpy as np def _ndarray_representer(dumper: yaml.Dumper, array: np.ndarray) -> yaml.Node: From f147e947738aecfb1fa66002e80903b637d21fa2 Mon Sep 17 00:00:00 2001 From: Phil Underwood Date: Wed, 17 Sep 2025 12:20:29 +0100 Subject: [PATCH 5/5] Update camera_info_manager.py one more time... --- .../camera_info_manager/camera_info_manager.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/camera_info_manager_py/camera_info_manager/camera_info_manager.py b/camera_info_manager_py/camera_info_manager/camera_info_manager.py index 83af8162..f4080373 100644 --- a/camera_info_manager_py/camera_info_manager/camera_info_manager.py +++ b/camera_info_manager_py/camera_info_manager/camera_info_manager.py @@ -47,9 +47,9 @@ import os from pathlib import Path -import numpy as np from ament_index_python import get_package_share_directory from ament_index_python import PackageNotFoundError +import numpy as np import rclpy from rclpy.node import Node from sensor_msgs.msg import CameraInfo