Added downsampling for camera images

launch/calib_launch.py

@@ -6,13 +6,29 @@
     DeclareLaunchArgument,
     SetEnvironmentVariable,  # Added for cyclonedds
 )
+from launch_ros.actions import Node
 from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from ament_index_python.packages import get_package_share_directory
 import os
 
+COMPUTER_HOSTNAME = os.uname()[1]
+HOSTNAME_TO_DOMAIN_ID = {
+    "payload0": 0,
+    "payload1": 1,
+    "payload2": 2,
+    "payload3": 3,
+    "payload4": 4,
+}
+if COMPUTER_HOSTNAME not in HOSTNAME_TO_DOMAIN_ID:
+    raise RuntimeError(
+        f"Unknown hostname {COMPUTER_HOSTNAME}, should be one of: {list(HOSTNAME_TO_DOMAIN_ID.keys())}"
+    )
 
+import datetime
+now = datetime.datetime.now()
+BAG_FNAME = f"{COMPUTER_HOSTNAME}_{now.strftime('%Y%m%d_%H%M')}"
 def generate_launch_description():
     # Declare launch argument for optional rosbag recording
     record_rosbag_arg = DeclareLaunchArgument(
@@ -21,8 +37,15 @@ def generate_launch_description():
         description='Set to true to record rosbag'
     )
 
+    bag_name_arg = DeclareLaunchArgument(
+        "bag_name",
+        default_value=BAG_FNAME,
+        description="Name for the rosbag file (without extension)",
+    )
     record_rosbag = LaunchConfiguration('record_rosbag')
 
+    bag_name = LaunchConfiguration("bag_name")
+
     # Start rmw_zenohd daemon
     rmw_zenohd_process = ExecuteProcess(
         cmd=['ros2', 'run', 'rmw_zenoh_cpp', 'rmw_zenohd'],
@@ -71,27 +94,43 @@ def generate_launch_description():
             ),
         ]
     )
-
+
+    downsampler_node = Node(
+        package = 'auxiliary',
+        executable='downsampler'
+    )
     # List of topics to record (cleaned, no leading space)
     rosbag_topics = [
     '/vectornav/imu',
     '/cam_sync/cam0/image_raw', 
     '/cam_sync/cam1/image_raw',
     '/ouster/points',
+    "/downsampled_imgs/cam0/image_raw",
+    "/downsampled_imgs/cam1/image_raw",
     ]
 
     rosbag_record = TimerAction(
         period=20.0,
         actions=[
             ExecuteProcess(
                 cmd=[
-                    'ros2', 'bag', 'record', '-s', 'mcap',
-                    '-o', '/home/roam1/data/calib_bag', '--max-cache-size', '2147483648', '--storage-preset-profile', 'fastwrite',
-                ] + rosbag_topics,
-                output='screen'
+                    "ros2",
+                    "bag",
+                    "record",
+                    "-s",
+                    "mcap",
+                    "-o",
+                    PathJoinSubstitution(["/home/neuroam/data/", bag_name]),
+                    "--max-cache-size",
+                    "6442450944",
+                    "--storage-preset-profile",
+                    "fastwrite",
+                ]
+                + rosbag_topics,
+                output="screen",
             )
         ],
-        condition=IfCondition(record_rosbag)
+        condition=IfCondition(record_rosbag),
     )
 
     def get_domain_id():
@@ -126,8 +165,10 @@ def get_domain_id():
         zenoh_env,
         ros_domain_id,
         record_rosbag_arg,
+        bag_name_arg,
         rmw_zenohd_process,
         delayed_launch,
-        rosbag_record
+        rosbag_record,
+        downsampler_node,
     ])
 

launch/global_launch.py

@@ -6,6 +6,7 @@
     DeclareLaunchArgument,
     SetEnvironmentVariable,  # Added for cyclonedds
 )
+
 from launch.conditions import IfCondition
 from launch.substitutions import (
     LaunchConfiguration,
@@ -119,7 +120,6 @@ def generate_launch_description():
             ),
         ],
     )
-
     # List of topics to record (cleaned, no leading space)
     rosbag_topics = [
         "/parameter_events",
@@ -210,4 +210,4 @@ def generate_launch_description():
             delayed_launch,
             rosbag_record,
         ]
-    )
+    )

src/auxiliary/auxiliary/downsampler.py

@@ -0,0 +1,85 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image  # Change this to match your message type
+import time
+
+CAMERA_FREQ = 20
+
+class DownsamplerNode(Node):
+    def __init__(self):
+        super().__init__('downsampler_node')
+
+        self.declare_parameter('sampling_freq', 5.0)  # Publish at max 1 Hz
+        self.sampling_freq = self.get_parameter('sampling_freq').get_parameter_value().double_value
+
+        # ADD CHECK FOR PERFECT FACTOR ONLY DOWNSAMPLES ??
+        if self.sampling_freq > CAMERA_FREQ:
+            self.get_logger().error(f"Downsampling rate at {self.sampling_freq}, is set  higher than original rate")
+        else:
+            self.skipcount = CAMERA_FREQ / self.sampling_freq
+
+        self.cam0_input_topic = '/cam_sync/cam0/image_raw'
+        self.cam1_input_topic = '/cam_sync/cam1/image_raw'
+
+        self.cam0_output_topic = '/downsampled_imgs/cam0/image_raw'
+        self.cam1_output_topic = '/downsampled_imgs/cam1/image_raw'
+
+        self.skipcount_cam0 = 1
+        self.skipcount_cam1 = 1
+        self.cam0_subscriber = self.create_subscription(
+            Image,
+            self.cam0_input_topic,
+            self.callback_cam0,
+            10
+        )
+
+        self.cam1_subscriber = self.create_subscription(
+            Image,
+            self.cam1_input_topic,
+            self.callback_cam1,
+            10
+        )
+
+        self.publisher_cam0 = self.create_publisher(
+            Image,
+            self.cam0_output_topic,
+            10
+        )
+
+        self.publisher_cam1 = self.create_publisher(
+            Image,
+            self.cam1_output_topic,
+            10
+        )
+
+        self.get_logger().info(f"Subscribed to: {self.cam0_input_topic}")
+        self.get_logger().info(f"Subscribed to: {self.cam1_input_topic}")
+        self.get_logger().info(f"Publishing downsampled messages to: {self.cam0_output_topic} at {self.sampling_freq} Hz")
+        self.get_logger().info(f"Publishing downsampled messages to: {self.cam1_output_topic} at {self.sampling_freq} Hz")
+
+    def callback_cam0(self, msg):
+
+        if self.skipcount_cam0 == self.skipcount:
+            self.publisher_cam0.publish(msg)
+            self.skipcount_cam0 = 1
+        else:
+            self.skipcount_cam0 += 1
+
+    def callback_cam1(self, msg):
+
+        if self.skipcount_cam1 == self.skipcount:
+            self.publisher_cam1.publish(msg)
+            self.skipcount_cam1 = 1
+        else:
+            self.skipcount_cam1 += 1
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = DownsamplerNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/auxiliary/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>auxiliary</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">neuroam</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/auxiliary/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/auxiliary
+[install]
+install_scripts=$base/lib/auxiliary

src/auxiliary/setup.py

@@ -0,0 +1,24 @@
+from setuptools import find_packages, setup
+
+package_name = 'auxiliary'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='neuroam',
+    maintainer_email='[email protected]',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'downsampler = auxiliary.downsampler:main'
+        ],
+    },
+)