From 81eb6f69ca0753d354f4f8d63632de2bdd7deb6d Mon Sep 17 00:00:00 2001 From: "bhandare.aj" Date: Thu, 7 Aug 2025 15:05:02 -0400 Subject: [PATCH 1/3] Added package for downsamping --- src/auxiliary/auxiliary/__init__.py | 0 src/auxiliary/auxiliary/downsampler.py | 85 ++++++++++++++++++++++++++ src/auxiliary/package.xml | 18 ++++++ src/auxiliary/resource/auxiliary | 0 src/auxiliary/setup.cfg | 4 ++ src/auxiliary/setup.py | 26 ++++++++ src/auxiliary/test/test_copyright.py | 25 ++++++++ src/auxiliary/test/test_flake8.py | 25 ++++++++ src/auxiliary/test/test_pep257.py | 23 +++++++ 9 files changed, 206 insertions(+) create mode 100644 src/auxiliary/auxiliary/__init__.py create mode 100644 src/auxiliary/auxiliary/downsampler.py create mode 100644 src/auxiliary/package.xml create mode 100644 src/auxiliary/resource/auxiliary create mode 100644 src/auxiliary/setup.cfg create mode 100644 src/auxiliary/setup.py create mode 100644 src/auxiliary/test/test_copyright.py create mode 100644 src/auxiliary/test/test_flake8.py create mode 100644 src/auxiliary/test/test_pep257.py diff --git a/src/auxiliary/auxiliary/__init__.py b/src/auxiliary/auxiliary/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/auxiliary/auxiliary/downsampler.py b/src/auxiliary/auxiliary/downsampler.py new file mode 100644 index 0000000..27eb4ce --- /dev/null +++ b/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 = 0 + self.skipcount_cam1 = 0 + 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 = self.create_publisher( + Image, + self.cam0_output_topic, + 10 + ) + + self.publisher = 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.sampling_freq == 0: + self.publisher.publish(msg) + self.skipcount_cam0 = 0 + else: + self.skipcount_cam0 += 1 + + def callback_cam1(self, msg): + + if self.skipcount_cam1 % self.sampling_freq == 0: + self.publisher.publish(msg) + self.skipcount_cam1 = 0 + 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() \ No newline at end of file diff --git a/src/auxiliary/package.xml b/src/auxiliary/package.xml new file mode 100644 index 0000000..cd42100 --- /dev/null +++ b/src/auxiliary/package.xml @@ -0,0 +1,18 @@ + + + + auxiliary + 0.0.0 + TODO: Package description + ajax + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/auxiliary/resource/auxiliary b/src/auxiliary/resource/auxiliary new file mode 100644 index 0000000..e69de29 diff --git a/src/auxiliary/setup.cfg b/src/auxiliary/setup.cfg new file mode 100644 index 0000000..32870be --- /dev/null +++ b/src/auxiliary/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/auxiliary +[install] +install_scripts=$base/lib/auxiliary diff --git a/src/auxiliary/setup.py b/src/auxiliary/setup.py new file mode 100644 index 0000000..6cc8525 --- /dev/null +++ b/src/auxiliary/setup.py @@ -0,0 +1,26 @@ +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/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ajax', + maintainer_email='bhandare.aj@northeastern.edu', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'downsampler = auxiliary.downsampler:main' + ], + }, +) diff --git a/src/auxiliary/test/test_copyright.py b/src/auxiliary/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/auxiliary/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/auxiliary/test/test_flake8.py b/src/auxiliary/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/auxiliary/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/auxiliary/test/test_pep257.py b/src/auxiliary/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/auxiliary/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From b5afb51c2c5fb3c41fc5674c1be1cbabccd17149 Mon Sep 17 00:00:00 2001 From: "bhandare.aj" Date: Thu, 7 Aug 2025 16:17:43 -0400 Subject: [PATCH 2/3] Added downsampler to launch file --- launch/global_launch.py | 9 +++++++++ src/auxiliary/auxiliary/downsampler.py | 20 ++++++++++---------- src/auxiliary/resource/auxiliary | 0 src/auxiliary/setup.py | 6 ++---- src/auxiliary/test/test_copyright.py | 25 ------------------------- src/auxiliary/test/test_flake8.py | 25 ------------------------- src/auxiliary/test/test_pep257.py | 23 ----------------------- 7 files changed, 21 insertions(+), 87 deletions(-) delete mode 100644 src/auxiliary/resource/auxiliary delete mode 100644 src/auxiliary/test/test_copyright.py delete mode 100644 src/auxiliary/test/test_flake8.py delete mode 100644 src/auxiliary/test/test_pep257.py diff --git a/launch/global_launch.py b/launch/global_launch.py index 41828be..c17480d 100644 --- a/launch/global_launch.py +++ b/launch/global_launch.py @@ -6,6 +6,7 @@ DeclareLaunchArgument, SetEnvironmentVariable, # Added for cyclonedds ) +from launch_ros.actions import Node from launch.conditions import IfCondition from launch.substitutions import ( LaunchConfiguration, @@ -120,6 +121,11 @@ def generate_launch_description(): ], ) + downsampler_node = Node( + package = 'auxiliary', + executable='downsampler' + ) + # List of topics to record (cleaned, no leading space) rosbag_topics = [ "/parameter_events", @@ -174,6 +180,8 @@ def generate_launch_description(): "/ouster/imu", "/ouster/points", "/ouster/telemetry", + "/downsampled_imgs/cam0/image_raw", + "/downsampled_imgs/cam1/image_raw", ] rosbag_record = TimerAction( @@ -209,5 +217,6 @@ def generate_launch_description(): rmw_zenohd_process, delayed_launch, rosbag_record, + downsampler_node, ] ) diff --git a/src/auxiliary/auxiliary/downsampler.py b/src/auxiliary/auxiliary/downsampler.py index 27eb4ce..53633d4 100644 --- a/src/auxiliary/auxiliary/downsampler.py +++ b/src/auxiliary/auxiliary/downsampler.py @@ -24,8 +24,8 @@ def __init__(self): self.cam0_output_topic = '/downsampled_imgs/cam0/image_raw' self.cam1_output_topic = '/downsampled_imgs/cam1/image_raw' - self.skipcount_cam0 = 0 - self.skipcount_cam1 = 0 + self.skipcount_cam0 = 1 + self.skipcount_cam1 = 1 self.cam0_subscriber = self.create_subscription( Image, self.cam0_input_topic, @@ -40,13 +40,13 @@ def __init__(self): 10 ) - self.publisher = self.create_publisher( + self.publisher_cam0 = self.create_publisher( Image, self.cam0_output_topic, 10 ) - self.publisher = self.create_publisher( + self.publisher_cam1 = self.create_publisher( Image, self.cam1_output_topic, 10 @@ -59,17 +59,17 @@ def __init__(self): def callback_cam0(self, msg): - if self.skipcount_cam0 % self.sampling_freq == 0: - self.publisher.publish(msg) - self.skipcount_cam0 = 0 + 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.sampling_freq == 0: - self.publisher.publish(msg) - self.skipcount_cam1 = 0 + if self.skipcount_cam1 == self.skipcount: + self.publisher_cam1.publish(msg) + self.skipcount_cam1 = 1 else: self.skipcount_cam1 += 1 diff --git a/src/auxiliary/resource/auxiliary b/src/auxiliary/resource/auxiliary deleted file mode 100644 index e69de29..0000000 diff --git a/src/auxiliary/setup.py b/src/auxiliary/setup.py index 6cc8525..4146510 100644 --- a/src/auxiliary/setup.py +++ b/src/auxiliary/setup.py @@ -7,14 +7,12 @@ version='0.0.0', packages=find_packages(exclude=['test']), data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), ], install_requires=['setuptools'], zip_safe=True, - maintainer='ajax', - maintainer_email='bhandare.aj@northeastern.edu', + maintainer='neuroam', + maintainer_email='neuroam@northeastern.edu', description='TODO: Package description', license='TODO: License declaration', tests_require=['pytest'], diff --git a/src/auxiliary/test/test_copyright.py b/src/auxiliary/test/test_copyright.py deleted file mode 100644 index 97a3919..0000000 --- a/src/auxiliary/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/src/auxiliary/test/test_flake8.py b/src/auxiliary/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/src/auxiliary/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/src/auxiliary/test/test_pep257.py b/src/auxiliary/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/src/auxiliary/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' From f5f1007e03fed1c6d11bb55f115b0decb024feda Mon Sep 17 00:00:00 2001 From: "bhandare.aj" Date: Thu, 7 Aug 2025 17:13:38 -0400 Subject: [PATCH 3/3] moved changes from global to calib launch file --- launch/calib_launch.py | 57 +++++++++++++++++++++++++++++++++------ launch/global_launch.py | 13 ++------- src/auxiliary/package.xml | 2 +- 3 files changed, 52 insertions(+), 20 deletions(-) diff --git a/launch/calib_launch.py b/launch/calib_launch.py index 6704580..f9362b6 100644 --- a/launch/calib_launch.py +++ b/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,13 +94,19 @@ 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( @@ -85,13 +114,23 @@ def generate_launch_description(): 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, ]) diff --git a/launch/global_launch.py b/launch/global_launch.py index c17480d..03127a2 100644 --- a/launch/global_launch.py +++ b/launch/global_launch.py @@ -6,7 +6,7 @@ DeclareLaunchArgument, SetEnvironmentVariable, # Added for cyclonedds ) -from launch_ros.actions import Node + from launch.conditions import IfCondition from launch.substitutions import ( LaunchConfiguration, @@ -120,12 +120,6 @@ def generate_launch_description(): ), ], ) - - downsampler_node = Node( - package = 'auxiliary', - executable='downsampler' - ) - # List of topics to record (cleaned, no leading space) rosbag_topics = [ "/parameter_events", @@ -180,8 +174,6 @@ def generate_launch_description(): "/ouster/imu", "/ouster/points", "/ouster/telemetry", - "/downsampled_imgs/cam0/image_raw", - "/downsampled_imgs/cam1/image_raw", ] rosbag_record = TimerAction( @@ -217,6 +209,5 @@ def generate_launch_description(): rmw_zenohd_process, delayed_launch, rosbag_record, - downsampler_node, ] - ) + ) \ No newline at end of file diff --git a/src/auxiliary/package.xml b/src/auxiliary/package.xml index cd42100..dee22d4 100644 --- a/src/auxiliary/package.xml +++ b/src/auxiliary/package.xml @@ -4,7 +4,7 @@ auxiliary 0.0.0 TODO: Package description - ajax + neuroam TODO: License declaration ament_copyright