From d7637f9ae21d7b698262b413da4ca391e284eb28 Mon Sep 17 00:00:00 2001 From: Alex Gisi Date: Tue, 12 Nov 2024 14:44:31 -0600 Subject: [PATCH] Initial implementation --- .gitignore | 162 ++++++++++++++++++++++++++++ config.yaml | 6 ++ package.xml | 18 ++++ readme.md | 7 ++ rosbag_to_csv/__init__.py | 0 rosbag_to_csv/data_recorder_node.py | 136 +++++++++++++++++++++++ setup.cfg | 4 + setup.py | 27 +++++ 8 files changed, 360 insertions(+) create mode 100644 .gitignore create mode 100644 config.yaml create mode 100644 package.xml create mode 100644 readme.md create mode 100644 rosbag_to_csv/__init__.py create mode 100644 rosbag_to_csv/data_recorder_node.py create mode 100644 setup.cfg create mode 100644 setup.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..efa407c --- /dev/null +++ b/.gitignore @@ -0,0 +1,162 @@ +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# poetry +# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control +#poetry.lock + +# pdm +# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. +#pdm.lock +# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it +# in version control. +# https://pdm.fming.dev/latest/usage/project/#working-with-version-control +.pdm.toml +.pdm-python +.pdm-build/ + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# PyCharm +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +#.idea/ \ No newline at end of file diff --git a/config.yaml b/config.yaml new file mode 100644 index 0000000..b96b9f4 --- /dev/null +++ b/config.yaml @@ -0,0 +1,6 @@ +subscriptions: + - topic_name: /w200_0083/platform/motor/right/status/velocity + message_type: std_msgs/msg/Float64 + fields: + - name: data + field_path: data diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..99ae898 --- /dev/null +++ b/package.xml @@ -0,0 +1,18 @@ + + + + rosbag_to_csv + 0.0.0 + TODO: Package description + alex + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/readme.md b/readme.md new file mode 100644 index 0000000..705b50d --- /dev/null +++ b/readme.md @@ -0,0 +1,7 @@ +# rosbag-to-csv +Stream selected topics from a rosbag into a csv. Just set the config.yaml appropriately. + +## Use +``` + ros2 run rosbag_to_csv data_recorder --ros-args -p config_file:=/path/to/rosbag_to_csv_ws/src/rosbag_to_csv/config.yaml -p interval:=0.1 -p output_csv:=/path/to/output.csv +``` diff --git a/rosbag_to_csv/__init__.py b/rosbag_to_csv/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/rosbag_to_csv/data_recorder_node.py b/rosbag_to_csv/data_recorder_node.py new file mode 100644 index 0000000..37acf31 --- /dev/null +++ b/rosbag_to_csv/data_recorder_node.py @@ -0,0 +1,136 @@ +import rclpy +from rclpy.node import Node +import yaml +import csv +import threading +from rosidl_runtime_py.utilities import get_message +from rclpy.qos import QoSProfile +from ament_index_python import get_package_share_directory + +class DataRecorderNode(Node): + def __init__(self): + super().__init__('data_recorder') + + # Declare and get parameters + self.declare_parameter('config_file', 'config.yaml') + self.declare_parameter('interval', 0.05) + self.declare_parameter('output_csv', 'output.csv') + + config_file = self.get_parameter('config_file').value + interval = self.get_parameter('interval').value + self.output_csv = self.get_parameter('output_csv').value + + # Load configuration + with open(config_file, 'r') as f: + config = yaml.safe_load(f) + + self.recorded_subscriptions = {} + self.latest_messages = {} + self.fields = [] + self.data_lock = threading.Lock() + self.data = [] + + # Set up subscribers based on the config file + for sub in config['subscriptions']: + topic_name = sub['topic_name'] + message_type_str = sub['message_type'] + fields = sub['fields'] + + # Dynamically import the message type + msg_module = get_message(message_type_str) + if msg_module is None: + self.get_logger().error(f"Could not import message type {message_type_str}") + continue + + # Create a subscription for each topic + subscription = self.create_subscription( + msg_module, + topic_name, + self.create_callback(topic_name), + QoSProfile(depth=10) + ) + self.recorded_subscriptions[topic_name] = { + 'subscription': subscription, + 'message_type': msg_module, + 'fields': fields + } + self.latest_messages[topic_name] = None + + # Collect field information + for field in fields: + field_name = field['name'] + field_path = field['field_path'] + self.fields.append({ + 'name': field_name, + 'topic_name': topic_name, + 'field_path': field_path + }) + + # Set up a timer for recording data at fixed intervals + self.timer = self.create_timer(interval, self.record_data) + + def create_callback(self, topic_name): + def callback(msg): + self.message_callback(msg, topic_name) + return callback + + def message_callback(self, msg, topic_name): + with self.data_lock: + self.latest_messages[topic_name] = msg + + def get_field_value(self, msg, field_path): + attrs = field_path.split('.') + value = msg + try: + for attr in attrs: + value = getattr(value, attr) + return value + except AttributeError: + self.get_logger().error(f"Could not get field '{field_path}' from message on topic '{msg}'") + return None + + def record_data(self): + with self.data_lock: + row = {} + for field in self.fields: + topic_name = field['topic_name'] + field_name = field['name'] + field_path = field['field_path'] + msg = self.latest_messages.get(topic_name) + if msg is not None: + value = self.get_field_value(msg, field_path) + if value is not None: + row[field_name] = value + else: + row[field_name] = '' + else: + row[field_name] = '' + self.data.append(row) + + def write_csv(self): + if not self.data: + self.get_logger().info("No data to write to CSV.") + return + + field_names = [field['name'] for field in self.fields] + with open(self.output_csv, 'w', newline='') as csvfile: + writer = csv.DictWriter(csvfile, fieldnames=field_names) + writer.writeheader() + for row in self.data: + writer.writerow(row) + self.get_logger().info(f"Data written to {self.output_csv}") + +def main(args=None): + rclpy.init(args=args) + node = DataRecorderNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.write_csv() + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..5baff0e --- /dev/null +++ b/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rosbag_to_csv +[install] +install_scripts=$base/lib/rosbag_to_csv diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..c737790 --- /dev/null +++ b/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +package_name = 'rosbag_to_csv' + +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']), + ('share/' + package_name, ['config.yaml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='alex', + maintainer_email='apgisi11@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'data_recorder = rosbag_to_csv.data_recorder_node:main' + ], + }, +)