fix(sensor_calibration_manager): fixed sensor calibration manager related issues

common/tier4_calibration_views/tier4_calibration_views/image_view_ui.py

@@ -380,11 +380,15 @@ def sensor_data_callback(self):
             self.image_view.update()
             self.graphics_view.update()
 
-            self.setWindowTitle(f"Image view (camera-lidar delay={1000*self.delay_tmp:.2f} ms)")
+            self.setWindowTitle(
+                f"Image view (camera-lidar delay={1000*self.delay_tmp:.2f} ms)"  # noqa E231
+            )
 
     def sensor_data_delay_callback(self, delay):
         # This method is executed in the UI thread
-        self.setWindowTitle(f"Image view (camera-lidar delay={1000*self.delay_tmp:.2f} ms)")
+        self.setWindowTitle(
+            f"Image view (camera-lidar delay={1000*self.delay_tmp:.2f} ms)"  # noqa E231
+        )
 
     def transform_callback(self):
         # This method is executed in the UI thread

sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/interactive_calibrator.py

@@ -294,8 +294,8 @@ def screenshot_callback():
             index = 0
 
             while True:
-                file_name_raw = f"screenshot_{index:04d}_raw.png"
-                file_name_roi = f"screenshot_{index:04d}_roi.png"
+                file_name_raw = f"screenshot_{index:04d}_raw.png"  # noqa E231
+                file_name_roi = f"screenshot_{index:04d}_roi.png"  # noqa E231
 
                 if not os.path.exists(file_name_raw):
                     pixmap_raw.save(file_name_raw, "PNG", -1)
@@ -464,7 +464,7 @@ def update_calibration_status(self):
                 image_calibration_points,
                 transform_matrix=self.calibrated_transform,
             )
-            calibrated_error_string = f"{calibrated_error:.2f}"
+            calibrated_error_string = f"{calibrated_error:.2f}"  # noqa E231
             self.calibrated_error = calibrated_error
         else:
             calibrated_error_string = ""
@@ -476,7 +476,7 @@ def update_calibration_status(self):
                 image_calibration_points,
                 transform_matrix=self.source_transform,
             )
-            source_error_string = f"{source_error:.2f}"
+            source_error_string = f"{source_error:.2f}"  # noqa E231
         else:
             source_error_string = ""
             source_inliers = np.array([])

sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py

@@ -471,7 +471,7 @@ def _pre_rejection_filter_impl(
             num_inliers = inlier_mask.sum()
 
             logging.info(
-                f"Iteration {it}: inliers: {num_inliers} | mean rms: {rms_errors.mean():.2f} | min rms: {rms_errors.min():.2f} | max rms: {rms_errors.max():.2f}"
+                f"Iteration {it}: inliers: {num_inliers} | mean rms: {rms_errors.mean():.2f} | min rms: {rms_errors.min():.2f} | max rms: {rms_errors.max():.2f}"  # noqa E231
             )
 
             if num_inliers > max_inliers or (
@@ -482,7 +482,7 @@ def _pre_rejection_filter_impl(
                 max_inliers = num_inliers
 
         logging.info(
-            f"Pre rejection inliers = {max_inliers}/{len(detections)} | threshold = {pre_rejection_max_rms_error:.2f}"
+            f"Pre rejection inliers = {max_inliers}/{len(detections)} | threshold = {pre_rejection_max_rms_error:.2f}"  # noqa E231
         )
 
         return model, [detections[i] for i in best_inlier_mask.nonzero()[0]]
@@ -555,7 +555,7 @@ def _entropy_maximization_subsampling_impl(
                     max_delta_entropy = delta_entropy
                     max_delta_entropy_idx = idx
 
-            logging.info(f"iteration={it}: delta entropy={max_delta_entropy:.3f}")
+            logging.info(f"iteration={it}: delta entropy={max_delta_entropy:.3f}")  # noqa E231
             accepted_array[max_delta_entropy_idx] = True
             add_detection(
                 detections[max_delta_entropy_idx],

sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py

@@ -140,7 +140,7 @@ def plot_detection_set(i: int, name: str, detections: List[BoardDetection]):
         height = detections[0].get_image_height()
         width = detections[0].get_image_width()
 
-        axes[0, i].set_title(f"{name} pixel heatmap\n(entropy={pixel_entropy:.2f})")
+        axes[0, i].set_title(f"{name} pixel heatmap\n(entropy={pixel_entropy:.2f})")  # noqa E231
         pixel_heatmap_ax = axes[0, i].imshow(
             pixel_heatmap,
             cmap="jet",
@@ -150,7 +150,9 @@ def plot_detection_set(i: int, name: str, detections: List[BoardDetection]):
         )
         plt.colorbar(pixel_heatmap_ax, ax=axes[0, i])
 
-        axes[1, i].set_title(f"{name} rotation heatmap\n(entropy{rotation_entropy:.2f})")
+        axes[1, i].set_title(
+            f"{name} rotation heatmap\n(entropy{rotation_entropy:.2f})"  # noqa E231
+        )
         rotation_heatmap_ax = axes[1, i].imshow(
             rotation_heatmap,
             cmap="jet",

sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py

@@ -689,7 +689,7 @@ def process_calibration_results(
         self.calibrated_camera_model = calibrated_model
 
         self.calibration_status_label.setText("Calibration status: idle")
-        self.calibration_time_label.setText(f"Calibration time: {dt:.2f}s")
+        self.calibration_time_label.setText(f"Calibration time: {dt:.2f}s")  # noqa E231
         self.calibration_training_samples_label.setText(
             f"Training samples: {num_training_detections}"
         )
@@ -700,9 +700,11 @@ def process_calibration_results(
             f"\tPost rejection inliers: {num_training_post_rejection_inliers}"
         )
 
-        self.calibration_training_rms_label.setText(f"\trms error (all): {training_rms_error:.3f}")
+        self.calibration_training_rms_label.setText(
+            f"\trms error (all): {training_rms_error:.3f}"  # noqa E231
+        )
         self.calibration_training_inlier_rms_label.setText(
-            f"\trms error (inliers): {training_inlier_rms_error:.3f}"
+            f"\trms error (inliers): {training_inlier_rms_error:.3f}"  # noqa E231
         )
 
         self.calibration_evaluation_samples_label.setText(
@@ -713,10 +715,10 @@ def process_calibration_results(
         )
 
         self.calibration_evaluation_rms_label.setText(
-            f"\trms error (all): {evaluation_rms_error:.3f}"
+            f"\trms error (all): {evaluation_rms_error:.3f}"  # noqa E231
         )
         self.calibration_evaluation_inlier_rms_label.setText(
-            f"\trms error (inliers): {evaluation_inlier_rms_error:.3f}"
+            f"\trms error (inliers): {evaluation_inlier_rms_error:.3f}"  # noqa E231
         )
 
         # self.calibrator_type_combobox.setEnabled(True) TODO implement this later
@@ -741,7 +743,7 @@ def process_evaluation_results(
         self.undistortion_alpha_spinbox.setEnabled(True)
 
         self.calibration_status_label.setText("Calibration status: idle")
-        self.calibration_time_label.setText(f"Calibration time: {dt:.2f}s")
+        self.calibration_time_label.setText(f"Calibration time: {dt:.2f}s")  # noqa E231
         self.calibration_training_samples_label.setText(
             f"Training samples: {num_training_detections}"
         )
@@ -750,9 +752,11 @@ def process_evaluation_results(
             f"\tPost rejection inliers: {num_training_post_rejection_inliers}"
         )
 
-        self.calibration_training_rms_label.setText(f"\trms error (all): {training_rms_error:.3f}")
+        self.calibration_training_rms_label.setText(
+            f"\trms error (all): {training_rms_error:.3f}"  # noqa E231
+        )
         self.calibration_training_inlier_rms_label.setText(
-            f"\trms error (inliers): {training_inlier_rms_error:.3f}"
+            f"\trms error (inliers): {training_inlier_rms_error:.3f}"  # noqa E231
         )
 
         self.calibration_evaluation_samples_label.setText(
@@ -763,10 +767,10 @@ def process_evaluation_results(
         )
 
         self.calibration_evaluation_rms_label.setText(
-            f"\trms error (all): {evaluation_rms_error:.3f}"
+            f"\trms error (all): {evaluation_rms_error:.3f}"  # noqa E231
         )
         self.calibration_evaluation_inlier_rms_label.setText(
-            f"\trms error (inliers): {evaluation_inlier_rms_error:.3f}"
+            f"\trms error (inliers): {evaluation_inlier_rms_error:.3f}"  # noqa E231
         )
 
         # self.calibrator_type_combobox.setEnabled(True) TODO implement this later
@@ -807,10 +811,10 @@ def on_save_clicked(self):
             os.mkdir(evaluation_folder)
 
         for index, image in enumerate(self.data_collector.get_training_images()):
-            cv2.imwrite(os.path.join(training_folder, f"{index:04d}.jpg"), image)
+            cv2.imwrite(os.path.join(training_folder, f"{index:04d}.jpg"), image)  # noqa E231
 
         for index, image in enumerate(self.data_collector.get_evaluation_images()):
-            cv2.imwrite(os.path.join(evaluation_folder, f"{index:04d}.jpg"), image)
+            cv2.imwrite(os.path.join(evaluation_folder, f"{index:04d}.jpg"), image)  # noqa E231
 
     def process_detection_results(self, img: np.array, detection: BoardDetection):
         """Process the results from an object detection."""
@@ -907,35 +911,37 @@ def process_detection_results(self, img: np.array, detection: BoardDetection):
 
             self.raw_detection_label.setText("Detected: True")
             self.raw_linear_error_rms_label.setText(
-                f"Linear error rms: {detection.get_linear_error_rms():.2f} px"
+                f"Linear error rms: {detection.get_linear_error_rms():.2f} px"  # noqa E231
+            )
+            self.rough_tilt_label.setText(
+                f"Rough tilt: {detection.get_tilt():.2f} degrees"  # noqa E231
             )
-            self.rough_tilt_label.setText(f"Rough tilt: {detection.get_tilt():.2f} degrees")
             self.rough_angles_label.setText(
-                f"Rough angles: x={rough_angles[0]:.2f} y={rough_angles[1]:.2f} degrees"
+                f"Rough angles: x={rough_angles[0]:.2f} y={rough_angles[1]:.2f} degrees"  # noqa E231
             )
             self.rough_position_label.setText(
-                f"Rough position: x={pose_translation[0]:.2f} y={pose_translation[1]:.2f} z={pose_translation[2]:.2f}"
+                f"Rough position: x={pose_translation[0]:.2f} y={pose_translation[1]:.2f} z={pose_translation[2]:.2f}"  # noqa E231
             )
-            self.skew_label.setText(f"Skew: {detection.get_normalized_skew():.2f}")
+            self.skew_label.setText(f"Skew: {detection.get_normalized_skew():.2f}")  # noqa E231
             self.relative_area_label.setText(
-                f"Relative area: {100.0*detection.get_normalized_size():.2f}"
+                f"Relative area: {100.0*detection.get_normalized_size():.2f}"  # noqa E231
             )
 
             self.single_shot_reprojection_error_max_label.setText(
-                f"Reprojection error (max): {reprojection_error_max:.3f} px ({100.0 * reprojection_error_max_relative:.2f}%)"
+                f"Reprojection error (max): {reprojection_error_max:.3f} px ({100.0 * reprojection_error_max_relative:.2f}%)"  # noqa E231
             )
             self.single_shot_reprojection_error_avg_label.setText(
-                f"Reprojection error (avg): {reprojection_error_mean:.3f} px ({100.0 * reprojection_error_mean_relative:.2f}%)"
+                f"Reprojection error (avg): {reprojection_error_mean:.3f} px ({100.0 * reprojection_error_mean_relative:.2f}%)"  # noqa E231
             )
             self.single_shot_reprojection_error_rms_label.setText(
-                f"Reprojection error (rms): {reprojection_error_rms:.3f} px ({100.0 * reprojection_error_rms_relative:.2f}%)"
+                f"Reprojection error (rms): {reprojection_error_rms:.3f} px ({100.0 * reprojection_error_rms_relative:.2f}%)"  # noqa E231
             )
 
             self.training_occupancy_rate_label.setText(
-                f"Training occupancy: {100.0*self.data_collector.get_training_occupancy_rate():.2f}"
+                f"Training occupancy: {100.0*self.data_collector.get_training_occupancy_rate():.2f}"  # noqa E231
             )
             self.evaluation_occupancy_rate_label.setText(
-                f"Evaluation occupancy: {100.0*self.data_collector.get_evaluation_occupancy_rate():.2f}"
+                f"Evaluation occupancy: {100.0*self.data_collector.get_evaluation_occupancy_rate():.2f}"  # noqa E231
             )
 
         # Draw training / evaluation points

sensor/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py

@@ -151,12 +151,12 @@ def draw_avg_subplots(self):
         if len(self.num_of_reflectors_list) > 0:
             # draw annotations for the last point
             self.subplot2.annotate(
-                f"{self.calibration_distance_error_list[-1]:.2f}",
+                f"{self.calibration_distance_error_list[-1]:.2f}",  # noqa E231
                 xy=(self.num_of_reflectors_list[-1], self.calibration_distance_error_list[-1]),
                 color=self.color_distance,
             )
             self.subplot3.annotate(
-                f"{self.calibration_yaw_error_list[-1]:.2f}",
+                f"{self.calibration_yaw_error_list[-1]:.2f}",  # noqa E231
                 xy=(self.num_of_reflectors_list[-1], self.calibration_yaw_error_list[-1]),
                 color=self.color_yaw,
             )
@@ -197,12 +197,12 @@ def draw_crossval_subplots(self):
         # annotate the last value
         if len(self.crossval_sample_list) > 0:
             self.subplot0.annotate(
-                f"{self.crossval_distance_error_list[-1]:.2f}",
+                f"{self.crossval_distance_error_list[-1]:.2f}",  # noqa E231
                 xy=(self.crossval_sample_list[-1], self.crossval_distance_error_list[-1]),
                 color=self.color_distance,
             )
             self.subplot1.annotate(
-                f"{self.crossval_yaw_error_list[-1]:.2f}",
+                f"{self.crossval_yaw_error_list[-1]:.2f}",  # noqa E231
                 xy=(self.crossval_sample_list[-1], self.crossval_yaw_error_list[-1]),
                 color=self.color_yaw,
             )

sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_base.py

@@ -48,32 +48,53 @@ def __init__(self, ros_interface: RosInterface):
         self.ros_interface = ros_interface
         self.calibrators: List[CalibratorServiceWrapper] = []
         self.expected_calibration_frames: List[FramePair] = []
-        self.state = CalibratorState.WAITING_TFS
+        self.state = CalibratorState.WAITING_SERVICES
+        self.services_available = False
+        self.tfs_ready = False
 
         self.calibration_result_tfs = defaultdict(lambda: defaultdict(Transform))
         self.calibration_result_transforms = defaultdict(lambda: defaultdict(Transform))
 
         self.check_tf_timer = QTimer()
         self.check_tf_timer.timeout.connect(self.on_check_tf_timer)
         self.check_tf_timer.start(500)
+        self.state_changed_signal.emit(self.state)
         logging.debug("CalibratorBase: constructor end")
 
-    def init():
-        logging.debug("CalibratorBase: Calibrator init?")
+    def update_status(self):
+
+        prev_state = self.state
+
+        if self.state in [CalibratorState.WAITING_SERVICES, CalibratorState.READY]:
+            if self.services_available and not self.tfs_ready:
+                self.state = CalibratorState.WAITING_TFS
+            elif self.services_available and self.tfs_ready:
+                self.state = CalibratorState.READY
+            elif not self.services_available and self.state == CalibratorState.READY:
+                self.state = CalibratorState.WAITING_SERVICES
+
+        if self.state == CalibratorState.WAITING_TFS and self.tfs_ready:
+            self.state = (
+                CalibratorState.READY
+                if self.services_available
+                else CalibratorState.WAITING_SERVICES
+            )
+
+        if prev_state != self.state:
+            self.state_changed_signal.emit(self.state)
 
     def on_check_tf_timer(self):
         logging.debug("CalibratorBase: on_check_tf_timer")
-        assert self.state == CalibratorState.WAITING_TFS
-        tfs_ready = all(
+        assert self.state in [CalibratorState.WAITING_TFS, CalibratorState.WAITING_SERVICES]
+        self.tfs_ready = all(
             self.ros_interface.can_transform(self.required_frames[0], frame)
             for frame in self.required_frames[1:]
         )
 
-        if tfs_ready:
-            self.state = CalibratorState.WAITING_SERVICES
-            self.state_changed_signal.emit(self.state)
+        if self.tfs_ready:
             self.check_tf_timer.stop()
             logging.debug("CalibratorBase: on_check_tf_timer stop")
+            self.update_status()
         else:
             for frame in self.required_frames[1:]:
                 if not self.ros_interface.can_transform(self.required_frames[0], frame):
@@ -102,15 +123,10 @@ def add_calibrator(self, service_name: str, expected_calibration_frames: List[Fr
         self.calibrators.append(calibration_wrapper)
 
     def on_service_status_changed(self):
-        if self.state in [CalibratorState.WAITING_SERVICES, CalibratorState.READY]:
-            services_available = all([calibrator.is_available() for calibrator in self.calibrators])
-
-            if services_available and self.state == CalibratorState.WAITING_SERVICES:
-                self.state = CalibratorState.READY
-                self.state_changed_signal.emit(self.state)
-            elif not services_available and self.state == CalibratorState.READY:
-                self.state = CalibratorState.WAITING_SERVICES
-                self.state_changed_signal.emit(self.state)
+        self.services_available = all(
+            [calibrator.is_available() for calibrator in self.calibrators]
+        )
+        self.update_status()
 
     def on_calibration_result(self):
         logging.debug("CalibratorBase: on_calibration_result")
@@ -161,7 +177,7 @@ def post_process_internal(self):
         logging.debug("Before post_process")
         for parent, children_and_transforms in self.calibration_result_tfs.items():
             for child, transform in children_and_transforms.items():
-                logging.debug(f"{parent}->{child}:\n{transform}")
+                logging.debug(f"{parent}->{child}:\n{transform}")  # noqa E231
 
         calibration_transforms = {
             parent: {
@@ -182,7 +198,7 @@ def post_process_internal(self):
         logging.debug("After post_process")
         for parent, children_and_transforms in self.processed_calibration_result_tfs.items():
             for child, transform in children_and_transforms.items():
-                logging.debug(f"{parent}->{child}:\n{transform}")
+                logging.debug(f"{parent}->{child}:\n{transform}")  # noqa E231
 
     def post_process(self, calibration_transforms) -> Dict[str, Dict[str, np.array]]:
         return calibration_transforms
@@ -218,8 +234,8 @@ def save_results(self, path, use_rpy=True):
                 output_dict[parent][child] = d
 
         def float_representer(dumper, value):
-            text = "{0:.6f}".format(value)
-            return dumper.represent_scalar("tag:yaml.org,2002:float", text)
+            text = "{0:.6f}".format(value)  # noqa E231
+            return dumper.represent_scalar("tag:yaml.org,2002:float", text)  # noqa E231
 
         yaml.add_representer(float, float_representer)
 

sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py

@@ -105,7 +105,7 @@ def on_timer(self):
         tf = time.time()
 
         for i in range(len(self.elapsed_times)):
-            self.elapsed_times[i] = f"{tf - self.start_times[i]:.2f}"
+            self.elapsed_times[i] = f"{tf - self.start_times[i]:.2f}"  # noqa E231
 
         self.data_changed.emit()
 
@@ -138,7 +138,7 @@ def on_result(self, calibration_results: List[CalibrationResult]):
             self.scores[i] = calibration_result.score
             self.status_messages[i] = calibration_result.message.data
             self.finished_list[i] = True
-            self.elapsed_times[i] = f"{tf - self.start_times[i]:.2f}"
+            self.elapsed_times[i] = f"{tf - self.start_times[i]:.2f}"  # noqa E231
             received_calibration_ids.append(i)
 
         for i in range(len(self.parents)):
@@ -161,7 +161,7 @@ def finished(self):
         return all(self.finished_list)
 
     def result_ros_callback(self, result: ExtrinsicCalibrator.Response):
-        logging.debug(f"{threading.get_ident() }: result_ros_callback")
+        logging.debug(f"{threading.get_ident()}: result_ros_callback")
         self.result_signal.emit(result.results)
 
     def status_ros_callback(self, status: bool):