camera_calibration for ros2

camera_calibration/src/camera_calibration/calibrator.py → camera_calibration/camera_calibration/calibrator.py

@@ -39,6 +39,7 @@
 from io import BytesIO
 import cv2
 import cv_bridge
+print(cv_bridge.__file__)
 import image_geometry
 import math
 import numpy.linalg
@@ -49,7 +50,6 @@
 import time
 from distutils.version import LooseVersion
 
-
 # Supported calibration patterns
 class Patterns:
     Chessboard, Circles, ACircles = list(range(3))
@@ -435,10 +435,10 @@ def lrmsg(self, d, k, r, p):
             msg.distortion_model = "rational_polynomial"
         else:
             msg.distortion_model = "plumb_bob"
-        msg.D = numpy.ravel(d).copy().tolist()
-        msg.K = numpy.ravel(k).copy().tolist()
-        msg.R = numpy.ravel(r).copy().tolist()
-        msg.P = numpy.ravel(p).copy().tolist()
+        msg.d = numpy.ravel(d).copy().tolist()
+        msg.k = numpy.ravel(k).copy().tolist()
+        msg.r = numpy.ravel(r).copy().tolist()
+        msg.p = numpy.ravel(p).copy().tolist()
         return msg
 
     def lrreport(self, d, k, r, p):
@@ -668,10 +668,10 @@ def from_message(self, msg, alpha = 0.0):
         """ Initialize the camera calibration from a CameraInfo message """
 
         self.size = (msg.width, msg.height)
-        self.intrinsics = numpy.array(msg.K, dtype=numpy.float64, copy=True).reshape((3, 3))
-        self.distortion = numpy.array(msg.D, dtype=numpy.float64, copy=True).reshape((len(msg.D), 1))
-        self.R = numpy.array(msg.R, dtype=numpy.float64, copy=True).reshape((3, 3))
-        self.P = numpy.array(msg.P, dtype=numpy.float64, copy=True).reshape((3, 4))
+        self.intrinsics = numpy.array(msg.k, dtype=numpy.float64, copy=True).reshape((3, 3))
+        self.distortion = numpy.array(msg.d, dtype=numpy.float64, copy=True).reshape((len(msg.d), 1))
+        self.R = numpy.array(msg.r, dtype=numpy.float64, copy=True).reshape((3, 3))
+        self.P = numpy.array(msg.p, dtype=numpy.float64, copy=True).reshape((3, 4))
 
         self.set_alpha(0.0)
 
@@ -798,8 +798,9 @@ def do_tarfile_save(self, tf):
         """ Write images and calibration solution to a tarfile object """
 
         def taradd(name, buf):
-            if isinstance(buf, basestring):
-                s = StringIO(buf)
+            if isinstance(buf, str):
+                buf = bytes(buf, encoding="utf-8")
+                s = BytesIO(buf)
             else:
                 s = BytesIO(buf)
             ti = tarfile.TarInfo(name)
@@ -1121,8 +1122,9 @@ def do_tarfile_save(self, tf):
                [("right-%04d.png" % i, im) for i,(_, _, im) in enumerate(self.db)])
 
         def taradd(name, buf):
-            if isinstance(buf, basestring):
-                s = StringIO(buf)
+            if isinstance(buf, str):
+                buf = bytes(buf, encoding="utf-8")
+                s = BytesIO(buf)
             else:
                 s = BytesIO(buf)
             ti = tarfile.TarInfo(name)
@@ -1147,4 +1149,4 @@ def do_tarfile_calibration(self, filename):
 
         ##\todo Check that the filenames match and stuff
 
-        self.cal(limages, rimages)
+        self.cal(limages, rimages)

camera_calibration/src/camera_calibration/camera_calibrator.py → camera_calibration/camera_calibration/camera_calibrator.py

@@ -36,7 +36,8 @@
 import message_filters
 import numpy
 import os
-import rospy
+import rclpy
+from rclpy.node import Node
 import sensor_msgs.msg
 import sensor_msgs.srv
 import threading
@@ -72,7 +73,7 @@ def run(self):
             cv2.imshow("display", im)
             k = cv2.waitKey(6) & 0xFF
             if k in [27, ord('q')]:
-                rospy.signal_shutdown('Quit')
+                rclpy.shutdown()
             elif k == ord('s'):
                 self.opencv_calibration_node.screendump(im)
 
@@ -90,42 +91,40 @@ def run(self):
             self.function(self.queue[0])
 
 
-class CalibrationNode:
-    def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0,
-                 pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0):
-        if service_check:
+class CalibrationNode(Node):
+    def __init__(self, boards, service_check = False, synchronizer = message_filters.TimeSynchronizer, flags = 0,
+                 pattern=Patterns.Chessboard, camera_name='camera',camera='camera',left_camera='left_camera',right_camera='right_camera', image='image', checkerboard_flags=0):
+        # TODO will enable this function as soon as set_camera_info enabled
+        # if service_check:
             # assume any non-default service names have been set.  Wait for the service to become ready
-            for svcname in ["camera", "left_camera", "right_camera"]:
-                remapped = rospy.remap_name(svcname)
-                if remapped != svcname:
-                    fullservicename = "%s/set_camera_info" % remapped
-                    print("Waiting for service", fullservicename, "...")
-                    try:
-                        rospy.wait_for_service(fullservicename, 5)
-                        print("OK")
-                    except rospy.ROSException:
-                        print("Service not found")
-                        rospy.signal_shutdown('Quit')
-
+            # for svcname in ["camera", "left_camera", "right_camera"]:
+            #     remapped = svcname
+            #     if remapped != svcname:
+            #         fullservicename = "%s/set_camera_info" % remapped
+            #         print("Waiting for service", fullservicename, "...")
+            #         try:
+            #             rclpy.wait_for_service(fullservicename, 5)
+            #             print("OK")
+            #         except:
+            #             print("Service not found")
+            #             rclpy.shutdown()
+        super().__init__(self._node_name)
         self._boards = boards
         self._calib_flags = flags
         self._checkerboard_flags = checkerboard_flags
         self._pattern = pattern
         self._camera_name = camera_name
-        lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image)
-        rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image)
+        lsub = message_filters.Subscriber(self, sensor_msgs.msg.Image, left_camera)
+        rsub = message_filters.Subscriber(self, sensor_msgs.msg.Image, right_camera)
         ts = synchronizer([lsub, rsub], 4)
         ts.registerCallback(self.queue_stereo)
 
-        msub = message_filters.Subscriber('image', sensor_msgs.msg.Image)
+        msub = message_filters.Subscriber(self,  sensor_msgs.msg.Image, image)
         msub.registerCallback(self.queue_monocular)
 
-        self.set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"),
-                                                          sensor_msgs.srv.SetCameraInfo)
-        self.set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"),
-                                                               sensor_msgs.srv.SetCameraInfo)
-        self.set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"),
-                                                                sensor_msgs.srv.SetCameraInfo)
+        self.set_camera_info_cli = self.create_client(sensor_msgs.srv.SetCameraInfo, "%s/set_camera_info" % camera)
+        self.set_left_camera_cli = self.create_client(sensor_msgs.srv.SetCameraInfo, "%s/set_camera_info" % left_camera)
+        self.set_right_camera_cli = self.create_client(sensor_msgs.srv.SetCameraInfo, "%s/set_camera_info" % right_camera)
 
         self.q_mono = deque([], 1)
         self.q_stereo = deque([], 1)
@@ -180,33 +179,36 @@ def handle_stereo(self, msg):
 
 
     def check_set_camera_info(self, response):
-        if response.success:
+        if response.result().success:
             return True
 
         for i in range(10):
             print("!" * 80)
         print()
-        print("Attempt to set camera info failed: " + response.status_message)
+        print("Attempt to set camera info failed: " + response.result().status_message)
         print()
         for i in range(10):
             print("!" * 80)
         print()
-        rospy.logerr('Unable to set camera info for calibration. Failure message: %s' % response.status_message)
+        self.get_logger().error('Unable to set camera info for calibration. Failure message: %s' % response.result().status_message)
         return False
 
     def do_upload(self):
         self.c.report()
         print(self.c.ost())
         info = self.c.as_message()
-
+        req = sensor_msgs.srv.SetCameraInfo.Request()
         rv = True
         if self.c.is_mono:
-            response = self.set_camera_info_service(info)
+            req.camera_info = info
+            response = self.set_camera_info_cli.call_async(req)
             rv = self.check_set_camera_info(response)
         else:
-            response = self.set_left_camera_info_service(info[0])
+            req.camera_info = info[0]
+            response = self.set_left_camera_info_service(req)
             rv = rv and self.check_set_camera_info(response)
-            response = self.set_right_camera_info_service(info[1])
+            req.camera_info = info[1]
+            response = self.set_right_camera_info_service(req)
             rv = rv and self.check_set_camera_info(response)
         return rv
 
@@ -218,14 +220,12 @@ class OpenCVCalibrationNode(CalibrationNode):
     FONT_THICKNESS = 2
 
     def __init__(self, *args, **kwargs):
-
-        CalibrationNode.__init__(self, *args, **kwargs)
-
+        self._node_name = 'camera_calibration'
         self.queue_display = deque([], 1)
         self.display_thread = DisplayThread(self.queue_display, self)
         self.display_thread.setDaemon(True)
         self.display_thread.start()
-
+        CalibrationNode.__init__(self, *args, **kwargs)
     @classmethod
     def putText(cls, img, text, org, color = (0,0,0)):
         cv2.putText(img, text, org, cls.FONT_FACE, cls.FONT_SCALE, color, thickness = cls.FONT_THICKNESS)
@@ -243,9 +243,11 @@ def on_mouse(self, event, x, y, flags, param):
                 if 280 <= y < 380:
                     self.c.do_save()
                 elif 380 <= y < 480:
-                    # Only shut down if we set camera info correctly, #3993
-                    if self.do_upload():
-                        rospy.signal_shutdown('Quit')
+                    # TODO will enabel do_upload function as soon as set_camera_info service is ready for ros2
+                    print("set_camera_info isn't ready for ros2, do nothing!")
+                    # Only shut down if we set camera info correctly, #399f we set camera info correctly, #3993
+                    # if self.do_upload():
+                    #     rclpy.shutdown()
 
     def on_scale(self, scalevalue):
         if self.c.calibrated:
@@ -349,4 +351,4 @@ def redraw_stereo(self, drawable):
                 self.putText(display, "dim", (2 * width, self.y(2)))
                 self.putText(display, "%.3f" % drawable.dim, (2 * width, self.y(3)))
 
-        self.queue_display.append(display)
+        self.queue_display.append(display)

camera_calibration/src/camera_calibration/camera_checker.py → camera_calibration/camera_calibration/camera_checker.py

@@ -37,7 +37,8 @@
 import functools
 import message_filters
 import numpy
-import rospy
+import rclpy
+from rclpy.node import Node
 import sensor_msgs.msg
 import sensor_msgs.srv
 import threading
@@ -69,27 +70,28 @@ def __init__(self, queue, function):
         self.function = function
 
     def run(self):
-        while not rospy.is_shutdown():
-            while not rospy.is_shutdown():
+        while not rclpy.is_shutdown():
+            while not rclpy.is_shutdown():
                 m = self.queue.get()
                 if self.queue.empty():
                     break
             self.function(m)
 
-class CameraCheckerNode:
+class CameraCheckerNode(Node):
 
-    def __init__(self, chess_size, dim, approximate=0):
+    def __init__(self, chess_size, dim, monocular, stereo, approximate=0):
         self.board = ChessboardInfo()
         self.board.n_cols = chess_size[0]
         self.board.n_rows = chess_size[1]
         self.board.dim = dim
-
+        self._node_name = "camera_checker"
+        super().__init__(self._node_name)
         # make sure n_cols is not smaller than n_rows, otherwise error computation will be off
         if self.board.n_cols < self.board.n_rows:
             self.board.n_cols, self.board.n_rows = self.board.n_rows, self.board.n_cols
 
-        image_topic = rospy.resolve_name("monocular") + "/image_rect"
-        camera_topic = rospy.resolve_name("monocular") + "/camera_info"
+        image_topic = monocular + "/image_rect"
+        camera_topic = monocular + "/camera_info"
 
         tosync_mono = [
             (image_topic, sensor_msgs.msg.Image),
@@ -104,10 +106,10 @@ def __init__(self, chess_size, dim, approximate=0):
         tsm = sync([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10)
         tsm.registerCallback(self.queue_monocular)
 
-        left_topic = rospy.resolve_name("stereo") + "/left/image_rect"
-        left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info"
-        right_topic = rospy.resolve_name("stereo") + "/right/image_rect"
-        right_camera_topic = rospy.resolve_name("stereo") + "/right/camera_info"
+        left_topic = stereo + "/left/image_rect"
+        left_camera_topic = stereo + "/left/camera_info"
+        right_topic = stereo + "/right/image_rect"
+        right_camera_topic = stereo + "/right/camera_info"
 
         tosync_stereo = [
             (left_topic, sensor_msgs.msg.Image),

camera_calibration/camera_calibration/nodes/__init__.py

@@ -0,0 +1,15 @@
+"""
+ Copyright (c) 2018 Intel Corporation
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+      http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+"""