Develop

.env

@@ -1,6 +1,6 @@
 # This top-level .env file under AirStack/ defines variables that are propagated through docker-compose.yaml
 PROJECT_NAME="airstack"
-PROJECT_VERSION="0.12.0"
+PROJECT_VERSION="0.13.0"
 # can replace with your docker hub username
 PROJECT_DOCKER_REGISTRY="airlab-storage.andrew.cmu.edu:5001/shared"
 DEFAULT_ISAAC_SCENE="omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack/fire_academy.scene.usd"

common/inputrc

@@ -0,0 +1,67 @@
+# /etc/inputrc - global inputrc for libreadline
+# See readline(3readline) and `info rluserman' for more information.
+
+# Be 8 bit clean.
+set input-meta on
+set output-meta on
+
+# To allow the use of 8bit-characters like the german umlauts, uncomment
+# the line below. However this makes the meta key not work as a meta key,
+# which is annoying to those which don't need to type in 8-bit characters.
+
+# set convert-meta off
+
+# try to enable the application keypad when it is called.  Some systems
+# need this to enable the arrow keys.
+# set enable-keypad on
+
+# see /usr/share/doc/bash/inputrc.arrows for other codes of arrow keys
+
+# do not bell on tab-completion
+# set bell-style none
+# set bell-style visible
+
+# some defaults / modifications for the emacs mode
+$if mode=emacs
+
+# allow the use of the Home/End keys
+"\e[1~": beginning-of-line
+"\e[4~": end-of-line
+
+# allow the use of the Delete/Insert keys
+"\e[3~": delete-char
+"\e[2~": quoted-insert
+
+# mappings for "page up" and "page down" to step to the beginning/end
+# of the history
+# "\e[5~": beginning-of-history
+# "\e[6~": end-of-history
+
+# alternate mappings for "page up" and "page down" to search the history
+"\e[5~": history-search-backward
+"\e[6~": history-search-forward
+
+# mappings for Ctrl-left-arrow and Ctrl-right-arrow for word moving
+"\e[1;5C": forward-word
+"\e[1;5D": backward-word
+"\e[5C": forward-word
+"\e[5D": backward-word
+"\e\e[C": forward-word
+"\e\e[D": backward-word
+
+$if term=rxvt
+"\e[7~": beginning-of-line
+"\e[8~": end-of-line
+"\eOc": forward-word
+"\eOd": backward-word
+$endif
+
+# for non RH/Debian xterm, can't hurt for RH/Debian xterm
+# "\eOH": beginning-of-line
+# "\eOF": end-of-line
+
+# for freebsd console
+# "\e[H": beginning-of-line
+# "\e[F": end-of-line
+
+$endif

common/ros_packages/bag_recorder_pid/.gitignore

@@ -0,0 +1,6 @@
+__pycache__/
+health_monitor/health_monitor/__pycache__
+health_monitor/__pycache__/*
+*.pyc
+.vscode/
+logging/

common/ros_packages/bag_recorder_pid/bag_record_pid/bag_record_node.py

@@ -0,0 +1,203 @@
+
+import copy
+from rclpy.node import Node
+from pathlib import Path
+from std_msgs.msg import Bool
+import yaml
+import subprocess
+import signal
+import time
+import os
+import rclpy
+from rclpy.qos import QoSProfile, QoSReliabilityPolicy
+import datetime
+
+class BagRecorderNode(Node):
+    def __init__(self):
+        super().__init__("bag_record_node")
+
+        self.node_name = self.get_name() # Get the full node name, including namespace if any
+
+        self.declare_parameter(
+            "cfg_path", str(Path(__file__).parents[3] / "config/cfg.yaml")
+        )
+
+        self.declare_parameter(
+            "output_dir", str("/logging/")
+        )
+
+        self.declare_parameter(
+            "mcap_qos_dir", ""
+        )
+
+        self.cfg_path     = self.get_parameter("cfg_path").get_parameter_value().string_value
+        self.output_dir   = self.get_parameter("output_dir").get_parameter_value().string_value
+        self.mcap_qos_dir = self.get_parameter("mcap_qos_dir").get_parameter_value().string_value
+
+        self.active = False
+        self.cfg = yaml.safe_load(open(self.cfg_path))
+
+        # TODO: check if the output directory exists.
+        # Exit if it does not exist.
+        os.chdir(self.output_dir)
+
+        self.command_prefix = ["ros2", "bag", "record", "-s", "mcap"]
+        self.commands = dict()
+        self.add_topics()
+
+        self.process = dict()
+
+        # Create QoS profile for reliable communication
+        reliable_qos = QoSProfile(
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            depth=10
+        )
+
+        # Use reliable QoS for the status publisher
+        self.status_pub = self.create_publisher(
+            Bool, 
+            f"{self.node_name}/bag_recording_status", 
+            reliable_qos
+        )
+
+        self.toggle_status = self.create_subscription(
+            Bool, 
+            f"{self.node_name}/set_recording_status", 
+            self.set_status_callback, 
+            10
+        )
+
+        self.timer = self.create_timer(0.5, self.pub_status_callback)
+
+    def add_topics(self):
+        '''The configuration file looks like
+
+            sections:
+                gps_lidar_spot_depth_status:
+                    mcap_qos: mcap_qos.yaml
+                    args: 
+                    - -b
+                    - 4000000000 # ~4GB
+                    - --max-cache-size 
+                    - 1073741824 # 1GB
+                    topics:
+                    - /tf
+                    - gq7/ekf/llh_position
+
+            The -o or --output argument should not be specified here.
+            The "mcap_qos" field here will be interpreted as the filename of the MCAP QoS profile.
+
+            self.commands[section_name] = {
+                'prefix': [],
+                'suffix': [],
+            }
+        '''
+        namespace = self.get_namespace()
+
+        for section_name, section_config in self.cfg['sections'].items():
+            self.commands[section_name] = dict()
+
+            # Command lists.
+            self.commands[section_name]['prefix'] = []
+            self.commands[section_name]['suffix'] = []
+
+            # Populate the initial command line.
+            self.commands[section_name]['prefix'].extend(self.command_prefix)
+
+            # Add the args to the command line.
+            str_args = [ str(c) for c in section_config['args'] ]
+            self.commands[section_name]['prefix'].extend(str_args)
+
+            # Set the mcap qos profile.
+            if section_config['mcap_qos'] != "":
+                mcap_qos_path = os.path.join(self.mcap_qos_dir, str(section_config['mcap_qos']))
+                self.commands[section_name]['prefix'].append('--storage-config-file')
+                self.commands[section_name]['prefix'].append(mcap_qos_path)
+
+            self.get_logger().warn(
+                f'CMD for section {section_name}: '
+                f'{" ".join(self.commands[section_name]["prefix"])}' )
+
+            # Add the topics to the command at the end.
+            self.get_logger().warn(f"Recording section {section_name} topics:")
+            if 'exclude' in section_config.keys():
+                if 'topics' in section_config.keys():
+                    self.get_logger().error('Cannot mix exclude with topics.')
+                    exit()
+
+                self.commands[section_name]['suffix'].append('--all')
+                for topic in section_config['exclude']:
+                    self.commands[section_name]['suffix'].append('--exclude')
+                    self.commands[section_name]['suffix'].append(topic)
+                self.get_logger().info(str(self.commands[section_name]))
+            else:
+                for topic in section_config['topics']:
+                    if topic.startswith('/'):
+                        full_topic_name = topic
+                    else:
+                        full_topic_name = f"{namespace}/{topic}"
+
+                    self.commands[section_name]['suffix'].append(full_topic_name)
+                    self.get_logger().warn(f"{full_topic_name}")
+
+    def pub_status_callback(self):
+        msg = Bool()
+        msg.data = self.active
+        self.status_pub.publish(msg)
+
+    def set_status_callback(self, msg):
+        if msg.data:
+            self.run()
+        else:
+            self.interrupt()
+
+    def run(self):
+        if not self.active:
+            self.active = True
+
+            time_suffix = f"{datetime.datetime.now().strftime('%Y%m%d_%H%M%S')}"
+
+            for section_name, command_dict in self.commands.items():
+                cmd = copy.deepcopy(command_dict['prefix'])
+
+                # Set the output filename.
+                output_filename = f"{section_name}_{time_suffix}"
+                cmd.append('-o')
+                cmd.append(output_filename)
+
+                # Appending an empty string will cause the ros2 bag record to consider the space as a topic
+                # and introduce an error.
+                if len(command_dict['suffix']) > 0:
+                    cmd.extend(command_dict['suffix'])
+
+                self.get_logger().warn(f"Running command: {' '.join(cmd)}")
+                #self.get_logger().warn(f"Running command: {cmd}")
+
+                self.process[section_name] = dict()
+                self.process[section_name]['process'] = subprocess.Popen(cmd)
+                self.process[section_name]['pid'] = self.process[section_name]['process'].pid
+                self.process[section_name]['output_filename'] = output_filename
+                self.get_logger().warn(f"Started Recording Section {section_name} with PID {self.process[section_name]['pid']} to {output_filename}")
+
+    def interrupt(self):
+        if self.active:
+            for section_name, process in self.process.items():
+                process['process'].send_signal(signal.SIGINT)
+                process['process'].wait()
+                self.get_logger().info(f"Ending Recording of Section {section_name} with PID {process['pid']}")
+                self.get_logger().warn(f"Output filename: {process['output_filename']}")
+            self.active = False
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = BagRecorderNode()
+    rclpy.spin(node)
+    node.interrupt()
+
+    node.destroy_node()
+    rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()

common/ros_packages/bag_recorder_pid/config/cfg.yaml

@@ -0,0 +1,2 @@
+topics:
+  ["/a", "/b"]

common/ros_packages/bag_recorder_pid/config/example-exclude.yaml

@@ -0,0 +1,47 @@
+# Any line that does not start with a / will be automatically prefixed using the 
+# namespace of the bag_record_pid node.
+# mcap_qos is the filename of the MCAP QoS profile. The actual directory will be prefixed by the
+# bag_record_pid node via a user specified argument. All MCAP QoS files must be in the same directory.
+# The -o or --output argument should not be specified here.
+# The -s mcap option will be added automatically.
+sections:
+  spot_rgb:
+    mcap_qos: mcap_qos.yaml
+    args: 
+      - -b
+      - 4000000000 # ~4GB
+      - --max-cache-size 
+      - 1073741824 # 1GB
+    exclude:
+      - /tf
+      - /tf_static
+
+# Extra or skipped topics:
+# - spot/depth_registered/back/points
+# - spot/depth_registered/frontleft/points
+# - spot/depth_registered/frontright/points
+# - spot/depth_registered/left/points
+# - spot/depth_registered/right/point
+# - spot/depth_registered/back/camera_info
+# - spot/depth_registered/back/image
+# - spot/depth_registered/frontleft/camera_info
+# - spot/depth_registered/frontleft/image
+# - spot/depth_registered/frontright/camera_info
+# - spot/depth_registered/frontright/image
+# - spot/depth_registered/hand/points
+# - spot/depth_registered/hand/camera_info
+# - spot/depth_registered/hand/image
+# - spot/depth_registered/left/camera_info
+# - spot/depth_registered/left/image
+# - spot/depth_registered/right/camera_info
+# - spot/depth_registered/right/image
+# ouster/scan
+# spot/camera/back/compressed
+# spot/camera/frontleft/compressed
+# spot/camera/frontright/compressed
+# spot/camera/hand/compressed
+# spot/camera/left/compressed
+# spot/camera/right/compressed
+# spot/depth_registered/image_rect/compressed
+# spot/depth_registered/image_rect/compressedDepth
+# spot/depth_registered/image_rect/theora