fix(sensor_calibration_manager): fixed sensor calibration manager related issues

sensor/sensor_calibration_manager/sensor_calibration_manager/calibrator_base.py

@@ -48,32 +48,54 @@ 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)
+        pass
 
     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 +124,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")

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 @@
         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 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,20 +296,32 @@
                 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("Lanch process terminated")
+        else:
+            logging.info("No launch process to terminate")
+
 
 def main(args=None):
     os.environ["QT_QPA_PLATFORM_PLUGIN_PATH"] = ""
     app = QApplication(sys.argv)
 
     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()
 
 

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