fix: restart node instead of writer.close()

rbs_utils/rbs_utils/scripts/recording_demo_via_rosbag.py

@@ -3,7 +3,8 @@
   recording_demo_lifecycle_node_via_RosBag
   ROS 2 program for recording a demo via RosBag
 
-  @shalenikol release 0.2
+  @shalenikol release 0.3
+  !!! In "Humble" writer.close() not worked !!! Waiting "Jazzy"
 """
 import os
 import shutil
@@ -24,7 +25,7 @@ from sensor_msgs.msg import Image, JointState
 NODE_NAME_DEFAULT = "lc_record_demo"
 PARAM_SKILL_CFG   = "lc_record_demo_cfg"
 
-OUTPUT_PATH_DEFAULT = "my_bag"
+OUTPUT_PATH_DEFAULT = "rbs_bag"
 WRITER_ID = "sqlite3"
 TOPIC_TYPES = ["sensor_msgs/msg/JointState", "sensor_msgs/msg/Image"]
 TOPIC_CLASSES = [JointState, Image]
@@ -33,7 +34,6 @@ class RecordingDemo(Node):
     """ lifecycle node """
 
     def __init__(self, **kwargs):
-        """Construct the node."""
         self.mode: int = 0 # 0 - нет записи, 1 - есть запись
         self.topics = []
         self._sub = []
@@ -51,20 +51,6 @@ class RecordingDemo(Node):
             if s["name"] == "output_path":
                 self.output_path = s["value"]
         
-        # check for output folder existence
-        if os.path.isdir(self.output_path):
-            x = 1
-            while os.path.isdir(self.output_path + str(x)):
-                x += 1
-            shutil.move(self.output_path, self.output_path + str(x))
-
-        self.writer = rosbag2_py.SequentialWriter()
-        # storage_options = rosbag2_py._storage.StorageOptions(
-        #     uri=self.output_path,
-        #     storage_id=WRITER_ID)
-        # converter_options = rosbag2_py._storage.ConverterOptions("", "")
-        # self.writer.open(storage_options, converter_options)
-
     def get_list_topics(self) -> list:
         topic_names_and_types = get_topic_names_and_types(node=self, include_hidden_topics=False)
 
@@ -76,16 +62,18 @@ class RecordingDemo(Node):
 
         return topic_list
 
-    def on_configure(self, state: State) -> TransitionCallbackReturn:
-        """
-        Configure the node, after a configuring transition is requested.
+    def check_output_folder(self, path: str) -> None:
+        # check for output folder existence
+        if os.path.isdir(path):
+            x = 1
+            while os.path.isdir(path + str(x)):
+                x += 1
+            shutil.move(path, path + str(x))
+
+    def on_configure(self, state: State) -> TransitionCallbackReturn:
+        self.check_output_folder(self.output_path)
+        self.writer = rosbag2_py.SequentialWriter()
 
-        :return: The state machine either invokes a transition to the "inactive" state or stays
-            in "unconfigured" depending on the return value.
-            TransitionCallbackReturn.SUCCESS transitions to "inactive".
-            TransitionCallbackReturn.FAILURE transitions to "unconfigured".
-            TransitionCallbackReturn.ERROR or any uncaught exceptions to "errorprocessing"
-        """
         storage_options = rosbag2_py._storage.StorageOptions(uri=self.output_path, storage_id=WRITER_ID)
         converter_options = rosbag2_py._storage.ConverterOptions("", "")
         self.writer.open(storage_options, converter_options)
@@ -103,7 +91,7 @@ class RecordingDemo(Node):
         return TransitionCallbackReturn.SUCCESS
 
     def on_activate(self, state: State) -> TransitionCallbackReturn:
-        self.get_logger().info('on_activate() is called.')
+        self.get_logger().info('on_activate() is called')
         # Create the subscribers.
         for id, topic in enumerate(self.topics):
             sub = self.create_subscription(topic["class"], topic["name"],
@@ -115,7 +103,7 @@ class RecordingDemo(Node):
         return super().on_activate(state)
 
     def on_deactivate(self, state: State) -> TransitionCallbackReturn:
-        self.get_logger().info('on_deactivate() is called.')
+        self.get_logger().info("on_deactivate() is called")
         # Destroy the subscribers.
         for sub in self._sub:
             self.destroy_subscription(sub)
@@ -125,27 +113,22 @@ class RecordingDemo(Node):
         return super().on_deactivate(state)
 
     def on_cleanup(self, state: State) -> TransitionCallbackReturn:
-        """
-        Cleanup the node.
-        """
         for sub in self._sub:
             self.destroy_subscription(sub)
         self._sub.clear()
 
-        self.writer.close()
+        # self.writer.close() # / del self.writer
 
-        self.get_logger().info('on_cleanup() is called.')
+        self.get_logger().info("on_cleanup() is called")
+        self.timer = self.create_timer(0.01, self.timer_callback)
         return TransitionCallbackReturn.SUCCESS
 
     def on_shutdown(self, state: State) -> TransitionCallbackReturn:
-        """
-        Shutdown the node.
-        """
         for sub in self._sub:
             self.destroy_subscription(sub)
         self._sub.clear()
 
-        self.get_logger().info('on_shutdown() is called.')
+        self.get_logger().info("on_shutdown() is called")
         return TransitionCallbackReturn.SUCCESS
     
     def topic_callback(self, topic_name, msg):
@@ -154,6 +137,14 @@ class RecordingDemo(Node):
             serialize_message(msg),
             self.get_clock().now().nanoseconds)
 
+    def timer_callback(self):
+        self.get_logger().info("Restarting node..")
+        self.destroy_node()
+        rclpy.shutdown()
+        rclpy.init()
+        lc_node = RecordingDemo()
+        rclpy.spin(lc_node)
+
 def main():
     rclpy.init()