Revision history [back]

This my solutions at moment. Most important part is this:

        future = self.cli.call_async(self.req)

        while not future.done():
            time.sleep(0.5)
            print("Waiting...")

Full code:

class CheckZividCalibration(Node):
    """
    Class that checks the Zivid calibration
    """

    def __init__(self):
        super().__init__("check_zivid_calibration")

        # this group allows for callbacks to
        self.group = ReentrantCallbackGroup()

        # Create client for getting frames from the Zivid
        self.cli = self.create_client(zci.Capture3d, 'zivid_camera/capture', callback_group=self.group)
        # Service for checking the calibration
        self.srv = self.create_service(gci.CheckZividCalibrationRequest, 'check_zivid_calibration', self.check_zivid_calibration, callback_group=self.group)

        # wait for service
        self.req = zci.Capture3d.Request()
        while not self.cli.wait_for_service(1.0):
            self.get_logger().info("zivid service not available, waiting again...")

    def check_zivid_calibration(self, request: gci.CheckZividCalibrationRequest.Request, response: gci.CheckZividCalibrationRequest.Response):
        """
        Checks if the camera is still calibrated properly
        :return:
        """

        self.get_logger().info("Getting frame...")

        future = self.cli.call_async(self.req)

        while not future.done():
            time.sleep(0.5)
            print("Waiting...")

        # print(future.result())

        result = future.result()
        point_cloud = pointcloud2_to_array(result.cloud)
        point_cloud = split_rgb_field(point_cloud)
        corners = get_checkerboard_coords_from_zivid_frame(point_cloud)

        self.get_logger().info("got the frame!")
        response.success = True
        response.message = "Camera still calibrated properly"
        return response


def main(args=None):
    rclpy.init(args=args)

    executor = MultiThreadedExecutor(num_threads=8)

    check_zivid_calibration = CheckZividCalibration()

    executor.add_node(check_zivid_calibration)

    executor.spin()

    executor.shutdown()

    check_zivid_calibration.destroy_node()

    rclpy.shutdown()