diff --git a/common/tier4_calibration_views/tier4_calibration_views/image_view_ui.py b/common/tier4_calibration_views/tier4_calibration_views/image_view_ui.py index 9d2c2d5b..710a9aa2 100644 --- a/common/tier4_calibration_views/tier4_calibration_views/image_view_ui.py +++ b/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 diff --git a/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/interactive_calibrator.py b/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/interactive_calibrator.py index 51e5d972..ee572013 100644 --- a/sensor/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/interactive_calibrator.py +++ b/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([]) diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py index cfc2f054..9d8ea6e5 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py +++ b/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], diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py index e1b7a7b9..f9c65abd 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py +++ b/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", diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index 9bd5a7bf..baca8b04 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/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 diff --git a/sensor/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py b/sensor/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py index b3f4f0dc..dc0e9be2 100755 --- a/sensor/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py +++ b/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, ) diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_base.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_base.py index 3a06a771..db719fdf 100644 --- a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_base.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_base.py @@ -48,7 +48,9 @@ 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)) @@ -56,24 +58,43 @@ def __init__(self, ros_interface: RosInterface): 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) diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py index 8696e030..5d509cae 100644 --- a/sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py +++ b/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): diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py index 2a298736..94c2022d 100644 --- a/sensor/sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/sensor_calibration_manager.py @@ -42,6 +42,7 @@ from launch.actions.set_launch_configuration import SetLaunchConfiguration from launch.frontend import Parser from launch.launch_description import LaunchDescription +import psutil import rclpy from sensor_calibration_manager.calibration_manager_model import CalibratorManagerModel from sensor_calibration_manager.calibrator_base import CalibratorState @@ -68,6 +69,9 @@ def __init__(self): self.ros_interface: RosInterface = None self.tfs_dict: Dict[str, Dict[str, None]] = defaultdict(lambda: defaultdict(None)) + # ROS launch process + self.process = None + # Threading variables self.lock = threading.RLock() @@ -197,8 +201,8 @@ def launch_calibrators( def on_calibrator_state_changed(self, state: CalibratorState): text_dict = { - CalibratorState.WAITING_TFS: "Waiting for the required TFs to be available", CalibratorState.WAITING_SERVICES: "Waiting for calibration services", + CalibratorState.WAITING_TFS: "Waiting for the required TFs to be available", CalibratorState.READY: "Ready to calibrate", CalibratorState.CALIBRATING: "Calibrating...", CalibratorState.FINISHED: "Calibration finished", @@ -292,6 +296,17 @@ def tf_graph_callback2(self, tfs_dict): self.tfs_dict, required_frames=self.calibrator.required_frames ) + def terminate_calibrators(self): + if self.process: + process = psutil.Process(self.process.pid) + for proc in process.children(recursive=True): + proc.kill() + + process.kill() + logging.info("Launch process terminated") + else: + logging.info("No launch process to terminate") + def main(args=None): os.environ["QT_QPA_PLATFORM_PLUGIN_PATH"] = "" @@ -299,13 +314,14 @@ def main(args=None): rclpy.init(args=args) - try: - signal.signal(signal.SIGINT, sigint_handler) - calibration_manager = SensorCalibrationManager() # noqa: F841 + signal.signal(signal.SIGINT, sigint_handler) + calibration_manager = SensorCalibrationManager() # noqa: F841 + try: sys.exit(app.exec_()) except (KeyboardInterrupt, SystemExit): logging.info("Received sigint. Quitting...") + calibration_manager.terminate_calibrators() rclpy.shutdown() diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/types.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/types.py index 6f612736..1a59ca63 100644 --- a/sensor/sensor_calibration_manager/sensor_calibration_manager/types.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/types.py @@ -19,8 +19,8 @@ class CalibratorState(Enum): - WAITING_TFS = 1 - WAITING_SERVICES = 2 + WAITING_SERVICES = 1 + WAITING_TFS = 2 READY = 3 CALIBRATING = 4 FINISHED = 5 diff --git a/sensor/sensor_calibration_manager/sensor_calibration_manager/views/tf_view.py b/sensor/sensor_calibration_manager/sensor_calibration_manager/views/tf_view.py index 34854afa..cbef67c0 100644 --- a/sensor/sensor_calibration_manager/sensor_calibration_manager/views/tf_view.py +++ b/sensor/sensor_calibration_manager/sensor_calibration_manager/views/tf_view.py @@ -184,7 +184,7 @@ def setTfs( else "black" ) graph_list.append( - f'"{parent}" -> "{child}"[color={color} label=" x={x:.4f}\\ny={y:.4f}\\nz={z:.4f}\\nyaw={yaw:.4f}\\npitch={pitch:.4f}\\nroll={roll:.4f}\\n"];\n' + f'"{parent}" -> "{child}"[color={color} label=" x={x:.4f}\\ny={y:.4f}\\nz={z:.4f}\\nyaw={yaw:.4f}\\npitch={pitch:.4f}\\nroll={roll:.4f}\\n"];\n' # noqa E231 ) if changed_frames_dict is not None: