Zoom Camera Info manager properly ported to ROS 2

camera_info_manager_py/camera_info_manager/camera_info_manager.py

@@ -221,7 +221,7 @@ def __str__(self):
 
         :returns: Return string representation of :class:CameraInfoManager.
         """
-        return '[' + self.cname + ']' + str(self.utm)
+        return '[' + self.cname + ']' + str(self.url)
 
     def getCameraInfo(self):
         """
@@ -326,11 +326,12 @@ def loadCameraInfo(self):
         """
         self._loadCalibration(self.url, self.cname)
 
-    def setCameraInfo(self, req):
+    def setCameraInfo(self, req, rsp):
         """
         Set camera info request callback.
 
         :param req: SetCameraInfo request message.
+        :param rsp: SetCameraInfo response message to populate.
         :returns: SetCameraInfo response message, success is True if
                   message handled.
 
@@ -339,7 +340,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.'
@@ -643,16 +643,21 @@ def saveCalibrationFile(ci, filename, cname):
     :param cname: Camera name.
     :returns: True if able to save the data.
     """
-    # make calibration dictionary from CameraInfo fields and camera name
+    # CameraInfo numeric fields are array.array or numpy arrays on the rclpy side,
+    # whose elements (numpy.float64) PyYAML's SafeDumper cannot represent; coerce
+    # to plain Python floats before dumping.
+    def _to_floats(seq):
+        return [float(x) for x in seq]
+
     calib = {
-        'image_width': ci.width,
-        'image_height': ci.height,
+        'image_width': int(ci.width),
+        'image_height': int(ci.height),
         'camera_name': cname,
         'distortion_model': ci.distortion_model,
-        'distortion_coefficients': {'data': ci.d, 'rows': 1, 'cols': len(ci.d)},
-        'camera_matrix': {'data': ci.k, 'rows': 3, 'cols': 3},
-        'rectification_matrix': {'data': ci.r, 'rows': 3, 'cols': 3},
-        'projection_matrix': {'data': ci.p, 'rows': 3, 'cols': 4},
+        'distortion_coefficients': {'data': _to_floats(ci.d), 'rows': 1, 'cols': len(ci.d)},
+        'camera_matrix': {'data': _to_floats(ci.k), 'rows': 3, 'cols': 3},
+        'rectification_matrix': {'data': _to_floats(ci.r), 'rows': 3, 'cols': 3},
+        'projection_matrix': {'data': _to_floats(ci.p), 'rows': 3, 'cols': 4},
     }
 
     # make sure the directory exists and the file is writable

camera_info_manager_py/camera_info_manager/zoom_camera_info_manager.py

@@ -65,10 +65,11 @@ class ZoomCameraInfoManager(CameraInfoManager):
     calibrate a zoom camera.
     """
 
-    def __init__(self, min_zoom, max_zoom, cname='camera', url='', namespace=''):
+    def __init__(self, node, min_zoom, max_zoom, cname='camera', url='', namespace=''):
         """
         Construct the manager.
 
+        :param node: rclpy Node used to create the ``set_camera_info`` service and logger.
         :param int min_zoom: The minimum zoom level.
                              The zoom values should linearly affect the focal distance.
         :param int max_zoom: The maximum zoom level.
@@ -80,7 +81,7 @@ def __init__(self, min_zoom, max_zoom, cname='camera', url='', namespace=''):
                                  If a namespace is specified, the '/' separator required between it
                                  and ``set_camera_info`` will be supplied automatically.
         """
-        CameraInfoManager.__init__(self, cname, url, namespace)
+        CameraInfoManager.__init__(self, node, cname, url, namespace)
 
         self._min_zoom = min_zoom
         self._max_zoom = max_zoom
@@ -110,7 +111,7 @@ def __enter__(self):
         return self
 
     def __exit__(self, exc_type, exc_val, exc_tb):
-        self.svc.shutdown()
+        self.node.destroy_service(self.svc)
 
 
 class ApproximateZoomCameraInfoManager(ZoomCameraInfoManager):
@@ -140,6 +141,7 @@ class ApproximateZoomCameraInfoManager(ZoomCameraInfoManager):
 
     def __init__(
         self,
+        node,
         min_fov,
         max_fov,
         initial_image_width,
@@ -153,6 +155,7 @@ def __init__(
         """
         Construct the manager.
 
+        :param node: rclpy Node used to create the ``set_camera_info`` service and logger.
         :param float min_fov: The minimum horizontal field of view. Corresponds to
             maximum zoom level.
         :param float max_fov: The maximum horizontal field of view. Corresponds to
@@ -170,7 +173,7 @@ def __init__(
             If a namespace is specified, the '/' separator required between it and
             ``set_camera_info`` will be supplied automatically.
         """
-        ZoomCameraInfoManager.__init__(self, min_zoom, max_zoom, cname, url, namespace)
+        ZoomCameraInfoManager.__init__(self, node, min_zoom, max_zoom, cname, url, namespace)
 
         self._min_fov = min_fov
         self._max_fov = max_fov
@@ -267,11 +270,13 @@ class InterpolatingZoomCameraInfoManager(ZoomCameraInfoManager):
     """
 
     def __init__(
-        self, calibration_url_template, zoom_levels, cname='camera', url='', namespace=''
+        self, node, calibration_url_template, zoom_levels,
+        cname='camera', url='', namespace=''
     ):
         """
         Construct the manager.
 
+        :param node: rclpy Node used to create the ``set_camera_info`` service and logger.
         :param string calibration_url_template: Template of the URL that contains the
             calibration files. The template string should contain a single '%d'
             substitution, which will be substituted with zoom level.
@@ -286,7 +291,7 @@ def __init__(
             ``set_camera_info`` will be supplied automatically.
         """
         ZoomCameraInfoManager.__init__(
-            self, min(zoom_levels), max(zoom_levels), cname, url, namespace
+            self, node, min(zoom_levels), max(zoom_levels), cname, url, namespace
         )
 
         self._calibration_url_template = calibration_url_template
@@ -295,7 +300,10 @@ def __init__(
         self._camera_infos = None
 
     def _update_camera_info(self):
-        if not self.isCalibrated() or self._camera_infos is None:
+        # Use the per-zoom calibrations as the source of truth: the base
+        # camera_info loaded by the parent may be empty (no top-level URL is
+        # required for InterpolatingZoom).
+        if not self._camera_infos:
             return
 
         if self._zoom in list(self._camera_infos.keys()):