diff --git a/.gitignore b/.gitignore index a97d25d..42518ff 100644 --- a/.gitignore +++ b/.gitignore @@ -2,10 +2,132 @@ __pycache__ *.swp *.save* -splinter/include/splinter -splinter/lib -splinter/splinter -splinter/assets -splinter/CREDITS.md -splinter/docs -splinter/README_SPLINTER.md +# 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/ +pip-wheel-metadata/ +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/ + +# 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 +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +.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 + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow +__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/ diff --git a/buoy_api_py/buoy_api/interface.py b/buoy_api_py/buoy_api/interface.py index 313680c..02f2dd1 100644 --- a/buoy_api_py/buoy_api/interface.py +++ b/buoy_api_py/buoy_api/interface.py @@ -233,36 +233,56 @@ def use_sim_time(self, enable=True): self.set_parameters([Parameter('use_sim_time', Parameter.Type.BOOL, enable)]) # set publish rate of PC Microcontroller telemetry - def set_pc_pack_rate_param(self, rate_hz=50.0): + def set_pc_pack_rate_param(self, rate_hz=50.0, blocking=True): + return asyncio.run(self._set_pc_pack_rate_param(rate_hz, blocking)) + + async def _set_pc_pack_rate_param(self, rate_hz=50.0, blocking=True): request = SetParameters.Request() request.parameters = [Parameter(name='publish_rate', value=float(rate_hz)).to_parameter_msg()] self.pc_pack_rate_param_future_ = self.pc_pack_rate_param_client_.call_async(request) self.pc_pack_rate_param_future_.add_done_callback(self.param_response_callback) + if blocking: + await self.pc_pack_rate_param_future_ # set publish rate of SC Microcontroller telemetry - def set_sc_pack_rate_param(self, rate_hz=50.0): + def set_sc_pack_rate_param(self, rate_hz=50.0, blocking=True): + return asyncio.run(self._set_sc_pack_rate_param(rate_hz, blocking)) + + async def _set_sc_pack_rate_param(self, rate_hz=50.0, blocking=True): request = SetParameters.Request() request.parameters = [Parameter(name='publish_rate', value=float(rate_hz)).to_parameter_msg()] self.sc_pack_rate_param_future_ = self.sc_pack_rate_param_client_.call_async(request) self.sc_pack_rate_param_future_.add_done_callback(self.param_response_callback) + if blocking: + await self.sc_pack_rate_param_future_ # set publish rate of PC Microcontroller telemetry - def set_pc_pack_rate(self, rate_hz=50): + def set_pc_pack_rate(self, rate_hz=50, blocking=True): + return asyncio.run(self._set_pc_pack_rate(rate_hz, blocking)) + + async def _set_pc_pack_rate(self, rate_hz=50, blocking=True): request = PCPackRateCommand.Request() request.rate_hz = int(rate_hz) self.pc_pack_rate_future_ = self.pc_pack_rate_client_.call_async(request) self.pc_pack_rate_future_.add_done_callback(self.default_service_response_callback) + if blocking: + await self.pc_pack_rate_future_ # set publish rate of SC Microcontroller telemetry - def set_sc_pack_rate(self, rate_hz=50): + def set_sc_pack_rate(self, rate_hz=50, blocking=True): + return asyncio.run(self._set_sc_pack_rate(rate_hz, blocking)) + + async def _set_sc_pack_rate(self, rate_hz=50, blocking=True): request = SCPackRateCommand.Request() request.rate_hz = int(rate_hz) self.sc_pack_rate_future_ = self.sc_pack_rate_client_.call_async(request) self.sc_pack_rate_future_.add_done_callback(self.default_service_response_callback) + if blocking: + await self.sc_pack_rate_future_ def send_pump_command(self, duration_mins, blocking=True): return asyncio.run(self._send_pump_command(duration_mins, blocking)) diff --git a/pbcmd/pbcmd/pbcmd.py b/pbcmd/pbcmd/pbcmd.py index 91f7743..511fbf0 100644 --- a/pbcmd/pbcmd/pbcmd.py +++ b/pbcmd/pbcmd/pbcmd.py @@ -156,7 +156,7 @@ def sc_pack_rate(parser): print('Executing sc_pack_rate to Set the CANBUS packet rate ' + f'from the Spring Controller: {args.rate_hz} Hz') _pbcmd = _PBCmd() - _pbcmd.set_sc_pack_rate_param(args.rate_hz) + _pbcmd.set_sc_pack_rate(args.rate_hz) def pc_PackRate(parser): @@ -169,7 +169,7 @@ def pc_PackRate(parser): print('Executing pc_PackRate to Set the CANBUS packet rate ' + f'from the Power Controller: {args.rate_hz} Hz') _pbcmd = _PBCmd() - _pbcmd.set_pc_pack_rate_param(args.rate_hz) + _pbcmd.set_pc_pack_rate(args.rate_hz) def pc_Scale(parser): diff --git a/sim_pblog/LICENSE b/sim_pblog/LICENSE new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/sim_pblog/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/sim_pblog/README.md b/sim_pblog/README.md new file mode 100644 index 0000000..20c5a67 --- /dev/null +++ b/sim_pblog/README.md @@ -0,0 +1,7 @@ +# sim_pblog +Python app simulates MBARI's wave energy conversion buoy Logger function using ROS2 topics. + +Please see [buoy_msgs/buoy_api_py](https://github.com/osrf/buoy_msgs/tree/main/buoy_api_py) +for controller [examples](https://github.com/osrf/buoy_msgs/tree/main/buoy_api_py/buoy_api/examples) + +## Modified template for python ROS2-enabled controller simulators diff --git a/sim_pblog/config/sim_pblog.yaml b/sim_pblog/config/sim_pblog.yaml new file mode 100644 index 0000000..0cdf7d9 --- /dev/null +++ b/sim_pblog/config/sim_pblog.yaml @@ -0,0 +1,4 @@ +/sim_pblog: + ros__parameters: + logfileinterval_mins: 60 + diff --git a/sim_pblog/launch/sim_pblog.launch.py b/sim_pblog/launch/sim_pblog.launch.py new file mode 100644 index 0000000..30ada7e --- /dev/null +++ b/sim_pblog/launch/sim_pblog.launch.py @@ -0,0 +1,62 @@ +# Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + + +package_name = 'sim_pblog' + + +def generate_launch_description(): + loghome_launch_arg = DeclareLaunchArgument( + 'loghome', default_value=['~/.pblogs'], + description='root log directory' + ) + + logdir_launch_arg = DeclareLaunchArgument( + 'logdir', default_value=[''], + description='specific log directory in loghome' + ) + + config = os.path.join( + get_package_share_directory(package_name), + 'config', + 'sim_pblog.yaml' + ) + + node = Node( + package=package_name, + name='sim_pblog', + executable='sim_pblog', + arguments=[ + '--loghome', LaunchConfiguration('loghome'), + '--logdir', LaunchConfiguration('logdir') + ], + parameters=[config] + ) + + ld = LaunchDescription() + ld.add_action(loghome_launch_arg) + ld.add_action(logdir_launch_arg) + ld.add_action(node) + + return ld diff --git a/sim_pblog/package.xml b/sim_pblog/package.xml new file mode 100644 index 0000000..e43a8db --- /dev/null +++ b/sim_pblog/package.xml @@ -0,0 +1,24 @@ + + + + sim_pblog + 0.0.0 + MBARI Power Buoy Simulated Logger + Richard Henthorn + Apache 2.0 + + buoy_interfaces + rclpy + buoy_api_py + tf_transformations + python3-transforms3d + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/sim_pblog/resource/sim_pblog b/sim_pblog/resource/sim_pblog new file mode 100644 index 0000000..e69de29 diff --git a/sim_pblog/setup.cfg b/sim_pblog/setup.cfg new file mode 100644 index 0000000..dc9c30f --- /dev/null +++ b/sim_pblog/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/sim_pblog +[install] +install_scripts=$base/lib/sim_pblog diff --git a/sim_pblog/setup.py b/sim_pblog/setup.py new file mode 100644 index 0000000..58cf783 --- /dev/null +++ b/sim_pblog/setup.py @@ -0,0 +1,32 @@ +from glob import glob +import os + +from setuptools import setup + + +package_name = 'sim_pblog' + +setup( + name=package_name, + version='0.0.0', + packages=[f'{package_name}'], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='henthorn', + maintainer_email='henthorn@mbari.org', + description='MBARI Power Buoy Simulated Logger', + license='Apache 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + f'sim_pblog = {package_name}.sim_pblog:main', + ], + }, +) diff --git a/sim_pblog/sim_pblog/__init__.py b/sim_pblog/sim_pblog/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py new file mode 100755 index 0000000..338c0c0 --- /dev/null +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -0,0 +1,412 @@ +#!/usr/bin/python3 + +# Copyright 2022 Open Source Robotics Foundation,Inc. and Monterey Bay Aquarium Research Institute +# +# 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. + +import atexit +from datetime import datetime, timedelta +import gzip +import math +from multiprocessing import cpu_count, get_context +import os + +from buoy_api import Interface + +from buoy_interfaces.msg import BCRecord +from buoy_interfaces.msg import PCRecord +from buoy_interfaces.msg import SCRecord +from buoy_interfaces.msg import TFRecord +from buoy_interfaces.msg import XBRecord + +import rclpy +from rclpy.time import Time + +from tf_transformations import euler_from_quaternion + + +# Close the current log file and zip it +def close_zip_logfile(logfile, logfilename, logger): + if logfile is not None and not logfile.closed: + logfile.close() + with open(logfilename, 'rb') as logf: + with gzip.open(f'{logfilename}.gz', 'wb') as gzfile: + gzfile.writelines(logf) + logger.info(f'{logfilename} -> {logfilename}.gz') + os.remove(logfilename) + + # Point a link called 'latest' to the new directory + # Renaming a temporary link works as 'ln -sf' + # csv_gz = os.path.basename(self.logfilename) + '.gz' + # templink = os.path.join(self.logdir, '__templn__') + # os.symlink(csv_gz, templink) + # latest = os.path.join(self.logdir, 'latest') + # os.rename(templink, latest) + + +# Controller IDs used as the first value in each record +BatteryConID = 0 +SpringConID = 1 +PowerConID = 2 +CrossbowID = 3 +TrefoilConID = 4 + +# For unit conversions +Meters2Inches = 39.37008 # in = m * Meters2Inches +Newtons2Pounds = 0.22481 # lbs = n * Newtons2Pounds + +# WECLogger - duplicate legacy pblog behavior substituting ROS2 topics for CANBus +# Description: +# Init phase: (1) Subscribe to all WEC controllers (e.g., power controller, +# spring controller, etc) +# (2) Create and open CSV output log file, with header line, ready +# for writing log records +# Run phase : (1) Controller topic callback functions receive topic messages +# (2) Callback functions write new log records with message data +# (3) Fresh log files are created and old files zipped every hour +# +# Notes : (1) There is always a trailing comma in all header and +# record sections + + +class WECLogger(Interface): + + def __init__(self, loghome, logdir=None): + super().__init__('sim_pblog', check_for_services=False) + self.zip_pool = get_context('spawn').Pool(processes=cpu_count()) + self.start_time = datetime.now() + self.logger_time = self.start_time + + self.loghome = os.path.expanduser(loghome) + # Create new log folder at the start of this instance of the logger + self.logdir = self.logdir_setup(logdir) + self.logfile = None + self.logfilename = None + self.logfiletime = datetime.fromtimestamp(0) + + self.logfileinterval_sec = 60 * 60 # in seconds + # Get params from config if available to set self.logfileinterval_sec + self.set_params() + + self.pc_header = '' + self.bc_header = '' + self.xb_header = '' + self.sc_header = '' + self.tf_header = '' + + atexit.register(self.zip_pool.terminate) + + # Create and open a new log file + # The system time is used to create a unique CSV log file name + # Example: "2023.03.31T23-59-59.csv" would be created just before midnight on March 31st + + def logfile_setup(self): + # close existing log file and zip it shut + if (self.logfile is not None): + self.zip_pool.apply_async(close_zip_logfile, + (self.logfile, + self.logfilename, + self.get_logger(),)) + + # Open new file in logdir using the logger_time (2023.03.23T13.09.54.csv) + csv = self.logger_time.strftime('%Y.%m.%dT%I.%M.%S') + '.csv' + self.logfilename = os.path.join(self.logdir, csv) + self.logfile = open(self.logfilename, + mode='w', + encoding='utf-8', + buffering=1) # line buffer + self.write_header() + + # Point a link called 'latest' to the new directory + # Renaming a temporary link works as 'ln -sf' + templink = os.path.join(self.logdir, '__templn__') + os.symlink(csv, templink) + latest = os.path.join(self.logdir, 'latest') + os.rename(templink, latest) + + self.get_logger().info(f'New log file: {self.logfilename}') + + # Write CSV header by writing each controller header section + def write_header(self): + # Preamble/Header section column names first + self.logfile.write('Source ID, Timestamp (epoch seconds),') + + # Write each header section - order matters! + self.write_pc_header() + self.write_bc_header() + self.write_xb_header() + self.write_sc_header() + self.write_tf_header() + self.write_eol() + + # Write a complete data record by writing each controller data section + def write_record(self, source_id, data): + self.update_logger_time(data) + + # Create a fresh logfile when interval time has passed + if (self.logger_time > (self.logfiletime + timedelta(seconds=self.logfileinterval_sec))): + self.logfiletime = self.logger_time + self.logfile_setup() + + # Use epoch seconds from logger_time to write the record preamble/header + self.logfile.write(f'{source_id}, {self.logger_time.timestamp():.3f}, ') + + # Pass data and delegate writing to section writers - order matters! + self.write_pc(data) + self.write_bc(data) + self.write_xb(data) + self.write_sc(data) + self.write_tf(data) + self.write_eol() + + # Increment logger_time using timestamp in data header + def update_logger_time(self, data): + # Timestamps in the data messages contain the time since the + # start of the simulation beginning at zero + msg_sec, msg_nsec = Time.from_msg(data.header.stamp).seconds_nanoseconds() + msg_micros = (1.0 * msg_nsec) / 1000. # convert to micros + delta = timedelta(seconds=msg_sec, microseconds=msg_micros) + # Calculated logger time + newtime = self.start_time + delta + # Occasionally data messages arrive out of time sequence + # Ensure logfile timestamps are always increasing + if (newtime > self.logger_time): + self.logger_time = newtime + + def write_eol(self): + self.logfile.write('\n') + + # Close the current log file and zip it + def close_zip_logfile(self): + if self.logfile is not None and not self.logfile.closed: + self.logfile.close() + with open(self.logfilename, 'rb') as logf: + with gzip.open(f'{self.logfilename}.gz', 'wb') as gzfile: + gzfile.writelines(logf) + self.get_logger().info(f'{self.logfilename} -> {self.logfilename}.gz') + os.remove(self.logfilename) + + # Point a link called 'latest' to the new directory + # Renaming a temporary link works as 'ln -sf' + # csv_gz = os.path.basename(self.logfilename) + '.gz' + # templink = os.path.join(self.logdir, '__templn__') + # os.symlink(csv_gz, templink) + # latest = os.path.join(self.logdir, 'latest') + # os.rename(templink, latest) + + # Create a new directory for log files created for this instance of logger + # The system date is used to create a unique directory name for this run + # Example: "2023-03-31.005 would be created on the 6th run on March 31st + + def logdir_setup(self, basename=None): + self.get_logger().info(f'Using {self.loghome} as logging home') + + if basename: + dirname = os.path.join(self.loghome, basename) + else: + # Use logger_time date to create directory name, e.g., 2023-03-23.002 + now = self.logger_time.strftime('%Y-%m-%d') + count = 0 + basename = now + '.{nnn:03}'.format(nnn=count) + dirname = os.path.join(self.loghome, basename) + while os.path.exists(dirname): + count = count + 1 + basename = now + '.{nnn:03}'.format(nnn=count) + dirname = os.path.join(self.loghome, basename) + + if not os.path.exists(dirname): + os.makedirs(dirname) + + # Point a link called 'latest_csv' to the new directory + # Renaming a temporary link works as 'ln -sf' + templink = os.path.join(self.loghome, '__templn__') + os.symlink(basename, templink) + latest = os.path.join(self.loghome, 'latest_csv_dir') + os.rename(templink, latest) + + self.get_logger().info(f'New log directory: {dirname}') + return dirname + + # Power Controller write functions + # PC header section + + def write_pc_header(self): + self.pc_header = """ \ +PC RPM, PC Bus Voltage (V), PC Winding Curr (A), PC Battery Curr (A), PC Status, \ +PC Load Dump Current (A), PC Target Voltage (V), PC Target Curr (A), \ +PC Diff PSI, PC RPM Std Dev, PC Scale, PC Retract, PC Aux Torque (mV), \ +PC Bias Curr (A), PC Charge Curr (A), PC Draw Curr (A), """ + self.logfile.write(self.pc_header) + + # PC record section + # Just write the commas unless data is the correct type + + def write_pc(self, data): + if (type(data) is PCRecord): + self.logfile.write(f'{data.rpm:.1f}, {data.voltage:.1f}, {data.wcurrent:.2f}, ' + + f'{data.bcurrent:.2f}, {data.status}, {data.loaddc:.2f}, ' + + f'{data.target_v:.1f}, {data.target_a:.2f}, ' + + f'{data.diff_press:.3f}, {data.sd_rpm:.1f}, {data.scale:.2f}, ' + + f'{data.retract:.2f}, {data.torque:.2f}, ' + + f'{data.bias_current:.2f}, ' + + f'{data.charge_curr_limit:.2f}, {data.draw_curr_limit:.2f}, ') + else: + self.logfile.write(',' * self.pc_header.count(',')) + + # Battery Controller write functions + # BC header section + def write_bc_header(self): + self.bc_header = """ \ +BC Voltage, BC Ips, BC V Balance, BC V Stopcharge, BC Ground Fault, \ +BC_Hydrogen, BC Status, """ + self.logfile.write(self.bc_header) + + # BC record section + # Just write the commas unless data is the correct type + def write_bc(self, data): + if (type(data) is BCRecord): + self.logfile.write(f'{data.voltage:.1f}, {data.ips:.2f}, ' + + f'{data.vbalance:.2f}, {data.vstopcharge:.2f}, ' + + f'{data.gfault:.2f}, {data.hydrogen:.2f}, {data.status}, ') + else: + self.logfile.write(',' * self.bc_header.count(',')) + + # Crossbow AHRS Controller write functions + # XB header section + def write_xb_header(self): + self.xb_header = """ \ +XB Roll XB Angle (deg), XB Pitch Angle (deg), XB Yaw Angle (deg), \ +XB X Rate, XB Y Rate, XB Z Rate, XB X Accel, XB Y Accel, XB Z Accel, \ +XB North Vel, XB East Vel, XB Down Vel, XB Lat, XB Long, XB Alt, XB Temp, """ + self.logfile.write(self.xb_header) + + # XB record section + # Just write the commas unless data is the correct type + def write_xb(self, data): + if (type(data) is XBRecord): + imu = data.imu + gps = data.gps + ned = data.ned_velocity + tmp = data.x_rate_temp + # get roll, pitch, and yaw in degrees + (roll, pitch, yaw) = euler_from_quaternion([imu.orientation.x, + imu.orientation.y, + imu.orientation.z, + imu.orientation.w]) + self.logfile.write( + f'{math.degrees(roll):.3f}, {math.degrees(pitch):.3f}, {math.degrees(yaw):.3f}, ' + + f'{imu.angular_velocity.x:.3f}, {imu.angular_velocity.y:.3f}, ' + + f'{imu.angular_velocity.z:.3f}, ' + + f'{imu.linear_acceleration.x:.3f}, {imu.linear_acceleration.y:.3f}, ' + + f'{imu.linear_acceleration.z:.3f}, ' + + f'{ned.z:.3f}, {ned.y:.3f}, {ned.z:.3f}, ' + + f'{gps.latitude:.5f}, {gps.longitude:.5f}, {gps.altitude:.3f}, ' + + f'{tmp.temperature:.3f}, ') + else: + self.logfile.write(',' * self.xb_header.count(',')) + + # Spring Controller write functions + # SC header section + # Just write the commas unless data is the correct type + def write_sc_header(self): + self.sc_header = """ \ +SC Load Cell (lbs), SC Range Finder (in), \ +SC Upper PSI, SC Lower PSI, SC Status, CTD Time, CTD Salinity, CTD Temp, """ + self.logfile.write(self.sc_header) + + # SC record section + def write_sc(self, data): + if (type(data) is SCRecord): + range_inches = data.range_finder * Meters2Inches + load_lbs = data.load_cell * Newtons2Pounds + self.logfile.write(f'{load_lbs:.2f}, {range_inches:.2f}, ' + + f'{data.upper_psi:.2f}, {data.lower_psi:.2f}, ' + + f'{data.status}, {data.epoch}, ' + + f'{data.salinity:.6f}, {data.temperature:.3f}, ') + else: + self.logfile.write(',' * self.sc_header.count(',')) + + # Trefoil Controller write functions + # TF header section + + def write_tf_header(self): + self.tf_header = """ \ +TF Power-Loss Timeouts, TF Tether Volt, TF Batt Volt, TF Pressure psi, \ +TF Qtn 1, TF Qtn 2, TF Qtn 3, TF Qtn 4, TF Mag 1 gauss, TF Mag 2, TF Mag 3, TF Status, \ +TF Ang Rate 1 rad/sec, TF Ang Rate 2, TF Ang Rate 3, TF VPE status, \ +TF Accel 1 m/sec^2, TF Accel 2, TF Accel 3, TF Comms-Loss Timeouts, \ +TF Maxon status, TF Motor curren mA, TF Encoder counts, """ + self.logfile.write(self.tf_header) + + # TF record section + # Just write the commas unless data is the correct type + + def write_tf(self, data): + if (type(data) is TFRecord): + imu = data.imu + mag = data.mag + self.logfile.write(f'{data.power_timeouts}, {data.tether_voltage:.3f}, ' + + f'{data.battery_voltage:.3f}, {data.pressure:.3f}, ' + + f'{imu.orientation.x:.3f}, {imu.orientation.y:.3f}, ' + + f'{imu.orientation.z:.3f}, {imu.orientation.w:.3f}, ' + + f'{mag.magnetic_field.x:.3f}, {mag.magnetic_field.y:.3f}, ' + + f'{mag.magnetic_field.z:.3f}, {data.status}, ' + + f'{imu.angular_velocity.x:.3f}, ' + + f'{imu.angular_velocity.y:.3f}, ' + + f'{imu.angular_velocity.z:.3f}, ' + + f'{data.vpe_status}, ' + + f'{imu.linear_acceleration.x:.3f}, ' + + f'{imu.linear_acceleration.y:.3f}, ' + + f'{imu.linear_acceleration.z:.3f}, ' + + f'{data.comms_timeouts}, {data.motor_status}, ' + + f'{data.motor_current}, {data.encoder}, ') + else: + self.logfile.write(',' * self.tf_header.count(',')) + + def ahrs_callback(self, data): + self.write_record(CrossbowID, data) + + def battery_callback(self, data): + self.write_record(BatteryConID, data) + + def spring_callback(self, data): + self.write_record(SpringConID, data) + + def power_callback(self, data): + self.write_record(PowerConID, data) + + def trefoil_callback(self, data): + self.write_record(TrefoilConID, data) + + def set_params(self): + self.declare_parameter('logfileinterval_mins', int(self.logfileinterval_sec / 60)) + self.logfileinterval_sec = \ + 60 * self.get_parameter('logfileinterval_mins').get_parameter_value().integer_value + + +def main(): + import argparse + parser = argparse.ArgumentParser() + loghome_arg = parser.add_argument('--loghome', default='~/.pblogs', help='root log directory') + logdir_arg = parser.add_argument('--logdir', help='specific log directory in loghome') + args, extras = parser.parse_known_args() + + rclpy.init(args=extras) + pblog = WECLogger(args.loghome if args.loghome else loghome_arg.default, + args.logdir if args.logdir else logdir_arg.default) + rclpy.spin(pblog) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/sim_pblog/test/test_copyright.py b/sim_pblog/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/sim_pblog/test/test_copyright.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_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/sim_pblog/test/test_flake8.py b/sim_pblog/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/sim_pblog/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/sim_pblog/test/test_pep257.py b/sim_pblog/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/sim_pblog/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'