From 658239fee921f052692dc971a2ccb22c2ad69737 Mon Sep 17 00:00:00 2001 From: Rich Henthorn Date: Thu, 16 Mar 2023 13:41:37 -0700 Subject: [PATCH 01/21] Created buoy-type logging for buoy_sim --- sim_pblog/.gitignore | 129 ++++++++++ sim_pblog/LICENSE | 201 +++++++++++++++ sim_pblog/README.md | 7 + sim_pblog/config/sim_pblog.yaml | 4 + sim_pblog/launch/sim_pblog.launch.py | 41 +++ sim_pblog/package.xml | 21 ++ sim_pblog/resource/sim_pblog | 0 sim_pblog/setup.cfg | 4 + sim_pblog/setup.py | 32 +++ sim_pblog/sim_pblog/__init__.py | 0 sim_pblog/sim_pblog/sim_pblog.py | 361 +++++++++++++++++++++++++++ sim_pblog/test/test_copyright.py | 23 ++ sim_pblog/test/test_flake8.py | 25 ++ sim_pblog/test/test_pep257.py | 23 ++ 14 files changed, 871 insertions(+) create mode 100644 sim_pblog/.gitignore create mode 100644 sim_pblog/LICENSE create mode 100644 sim_pblog/README.md create mode 100644 sim_pblog/config/sim_pblog.yaml create mode 100644 sim_pblog/launch/sim_pblog.launch.py create mode 100644 sim_pblog/package.xml create mode 100644 sim_pblog/resource/sim_pblog create mode 100644 sim_pblog/setup.cfg create mode 100644 sim_pblog/setup.py create mode 100644 sim_pblog/sim_pblog/__init__.py create mode 100755 sim_pblog/sim_pblog/sim_pblog.py create mode 100644 sim_pblog/test/test_copyright.py create mode 100644 sim_pblog/test/test_flake8.py create mode 100644 sim_pblog/test/test_pep257.py diff --git a/sim_pblog/.gitignore b/sim_pblog/.gitignore new file mode 100644 index 0000000..b6e4761 --- /dev/null +++ b/sim_pblog/.gitignore @@ -0,0 +1,129 @@ +# 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/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..0580a98 --- /dev/null +++ b/sim_pblog/config/sim_pblog.yaml @@ -0,0 +1,4 @@ +/sim_pblog: + ros__parameters: + foo: 1.0 + diff --git a/sim_pblog/launch/sim_pblog.launch.py b/sim_pblog/launch/sim_pblog.launch.py new file mode 100644 index 0000000..1912384 --- /dev/null +++ b/sim_pblog/launch/sim_pblog.launch.py @@ -0,0 +1,41 @@ +# 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_ros.actions import Node + + +package_name = 'sim_pblog' + +def generate_launch_description(): + ld = LaunchDescription() + 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', + parameters=[config] + ) + + ld.add_action(node) + + return ld diff --git a/sim_pblog/package.xml b/sim_pblog/package.xml new file mode 100644 index 0000000..c98eb0e --- /dev/null +++ b/sim_pblog/package.xml @@ -0,0 +1,21 @@ + + + + sim_pblog + 0.0.0 + MBARI Power Buoy Simulated Logger + Richard Henthorn + Apache 2.0 + + buoy_interfaces + rclpy + buoy_api_py + 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..c42c6d6 --- /dev/null +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -0,0 +1,361 @@ +#!/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 os +import gzip +import math +from datetime import datetime +from tf_transformations import euler_from_quaternion + +from buoy_api import Interface +from buoy_interfaces.msg import PCRecord +from buoy_interfaces.msg import BCRecord +from buoy_interfaces.msg import XBRecord +from buoy_interfaces.msg import SCRecord +from buoy_interfaces.msg import TFRecord +import rclpy + +WECLOGHOME = 'WEC_LOG_DIR' +ALTLOGHOME = '/tmp' # assume we always have write permission to /tmp +NEWFILEINTERVAL = 2 #60*60 # limit the 'size' of each log file to one hour + +# Controller IDs used as the first value in each record +BatteryConID = 0 +SpringConID = 1 +PowerConID = 2 +TrefoilConID = 4 +CrossbowID = 5 + +# 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): + super().__init__('sim_pblog') + #self.set_params() + self.logdir = None + self.logfile = None + self.logfilename = None + self.logtime = None + self.pc_header = "" + self.bc_header = "" + self.xb_header = "" + self.sc_header = "" + self.tf_header = "" + self.logfile_setup() + + def __del__(self): + self.close_zip_logfile() + + + # 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): + # Create new log folder at the start of this instance of the logger + if (self.logdir == None): + self.logdir = self.logdir_setup() + + # close existing log file and zip it shut + if (self.logfile != None): + self.close_zip_logfile() + + + # Open new file in logdir using the date & time (2023.03.23T13.09.54.csv) + self.logfilename = self.logdir + '/' + \ + datetime.now().strftime("%Y.%m.%dT%I.%M.%S") + '.csv' + self.logfile = open(self.logfilename, mode="w", encoding='utf-8') + + # Point a link called 'latest' to the new directory + # Renaming a temporary link works as 'ln -sf' + templink = self.logdir + '/' + '__bogus__' + os.symlink(self.logfilename, templink) + latest = self.logdir + '/' + 'latest' + os.rename(templink, latest) + + self.get_logger().info(f'New log file: {self.logfilename}') + self.write_header() + + + # Write CSV header line by writing each controller section + def write_header(self): + # Preamble/Header section column names first + self.logfile.write("Source ID, Timestamp (epoch seconds), ") + + # Write each header section + 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 record line by writing each controller section + def write_record(self, source_id, data): + # Calculate float value epoch seconds and write the record preamble/header + nanos = (data.header.stamp.sec * 1e9) + data.header.stamp.nanosec + timestamp = (1.0*nanos) / 1e9 # convert from nanoseconds to seconds + self.logfile.write(f'{source_id}, {timestamp}, ') + + # Pass the data to writers, let them decide what to write + self.write_pc(data) + self.write_bc(data) + self.write_xb(data) + self.write_sc(data) + self.write_tf(data) + self.write_eol() + + # Create a fresh logfile when interval time has passed + if (self.logtime == None): + self.logtime = timestamp + elif (timestamp > self.logtime + NEWFILEINTERVAL): + self.logfile_setup() + self.logtime = timestamp + + + def write_eol(self): + self.logfile.write('\n') + + + def close_zip_logfile(self): + if (self.logfile != None): + self.logfile.close() + with open(self.logfilename, 'rb') as logf: + with gzip.open(f'{self.logfilename}.gz', 'wb') as gzfile: + self.get_logger().info(f'{self.logfilename} -> {self.logfilename}.gz') + gzfile.writelines(logf) + os.remove(self.logfilename) + + + # 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): + # Use WEC_LOG_DIR env variable for base of log tree + wec_log_dir = os.getenv(WECLOGHOME) + if (None == wec_log_dir): + self.get_logger().info(f'WEC_LOG_DIR env variable not set, using {ALTLOGHOME} as log home') + wec_log_dir = ALTLOGHOME + elif (False == os.access(wec_log_dir, os.W_OK)): + self.get_logger().info(f'No write access to {wec_log_dir}, using {ALTLOGHOME} as log home') + wec_log_dir = ALTLOGHOME + + + # Use today's date to create directory name, e.g., 2023-03-23.002 + now = datetime.today().strftime('%Y-%m-%d') + count = 0 + dirname = wec_log_dir + '/' + now + ".{nnn:03}".format(nnn=count) + while (os.path.exists(dirname)): + count = count +1 + dirname = wec_log_dir + '/' + now + ".{nnn:03}".format(nnn=count) + + if (False == os.path.exists(dirname)): + os.makedirs(dirname) + + # Point a link called 'latest' to the new directory + # Renaming a temporary link works as 'ln -sf' + templink = wec_log_dir + '/' + '__bogus__' + os.symlink(dirname, templink) + latest = wec_log_dir + '/' + 'latest' + 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}, {data.voltage}, {data.wcurrent}, ' + + f'{data.bcurrent}, {data.status}, {data.loaddc}, {data.target_v}, ' + + f'{data.target_a}, {data.diff_press}, {data.sd_rpm}, {data.scale}, ' + + f'{data.retract}, {data.torque}, {data.bias_current}, ' + + f'{data.charge_curr_limit}, {data.draw_curr_limit}, ') + 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}, {data.ips}, ' + + f'{data.vbalance}, {data.vstopcharge}, {data.gfault}, ' + + f'{data.hydrogen}, {data.status}, ') + else: + self.logfile.write("," * self.bc_header.count(',')) + + + ############ Crossbow AHRS 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)}, {math.degrees(pitch)}, {math.degrees(yaw)}, ' + + f'{imu.angular_velocity.x}, {imu.angular_velocity.y}, {imu.angular_velocity.z}, ' + + f'{imu.linear_acceleration.x}, {imu.linear_acceleration.y}, {imu.linear_acceleration.z}, ' + + f'{ned.z}, {ned.y}, {ned.z}, ' + + f'{gps.latitude}, {gps.longitude}, {gps.altitude}, ' + + f'{tmp.temperature}, ') + 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): + self.logfile.write(f'{data.load_cell}, {data.range_finder}, ' + + f'{data.upper_psi}, {data.lower_psi}, {data.status}, {data.epoch}, ' + + f'{data.salinity}, {data.temperature}, ') + 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}, ' + + f'{data.battery_voltage}, {data.pressure}, ' + + f'{imu.orientation.x}, {imu.orientation.y}, {imu.orientation.z}, {imu.orientation.w}, ' + + f'{mag.magnetic_field.x}, {mag.magnetic_field.y}, {mag.magnetic_field.z}, {data.status}, ' + + f'{imu.linear_acceleration.x}, {imu.linear_acceleration.y}, {imu.linear_acceleration.z}, ' + + f'{data.vpe_status}, ' + + f'{imu.linear_acceleration.x}, {imu.linear_acceleration.y}, {imu.linear_acceleration.z}, ' + + f'{data.comms_timeouts}, {data.motor_status}, {data.motor_current}, {data.encoder}, ') + else: + self.logfile.write("," * self.tf_header.count(',')) + + + + + # Delete any unused callback + def ahrs_callback(self, data): + '''Callback for '/ahrs_data' topic from XBowAHRS''' + self.write_record(CrossbowID, data) + + def battery_callback(self, data): + '''Callback for '/battery_data' topic from Battery Controller''' + self.write_record(BatteryConID, data) + + def spring_callback(self, data): + '''Callback for '/spring_data' topic from Spring Controller''' + self.write_record(SpringConID, data) + + def power_callback(self, data): + '''Callback for '/power_data' topic from Power Controller''' + self.write_record(PowerConID, data) + #self.csv_log.writerow([data.seq_num, data.voltage, data.sd_rpm]) + + def trefoil_callback(self, data): + '''Callback for '/trefoil_data' topic from Trefoil Controller''' + self.write_record(TrefoilConID, data) + + def powerbuoy_callback(self, data): + '''Callback for '/powerbuoy_data' topic -- Aggregated data from all topics''' + #self.get_logger().info('Received Powerbuoy data') + + def set_params(self): + '''Use ROS2 declare_parameter and get_parameter to set policy params''' + pass + + +def main(): + rclpy.init() + pblog = WECLogger() + 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' From aaa329b262a218fa1a8056533ff8659e76948b31 Mon Sep 17 00:00:00 2001 From: Rich Henthorn Date: Thu, 16 Mar 2023 14:24:41 -0700 Subject: [PATCH 02/21] Clean up formatting to match buoy logging --- sim_pblog/sim_pblog/sim_pblog.py | 50 ++++++++++++++++---------------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index c42c6d6..b3cd3bb 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -120,7 +120,7 @@ def write_record(self, source_id, data): # Calculate float value epoch seconds and write the record preamble/header nanos = (data.header.stamp.sec * 1e9) + data.header.stamp.nanosec timestamp = (1.0*nanos) / 1e9 # convert from nanoseconds to seconds - self.logfile.write(f'{source_id}, {timestamp}, ') + self.logfile.write(f'{source_id}, {timestamp:.3f}, ') # Pass the data to writers, let them decide what to write self.write_pc(data) @@ -204,11 +204,11 @@ def write_pc_header(self): # 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}, {data.voltage}, {data.wcurrent}, ' + - f'{data.bcurrent}, {data.status}, {data.loaddc}, {data.target_v}, ' + - f'{data.target_a}, {data.diff_press}, {data.sd_rpm}, {data.scale}, ' + - f'{data.retract}, {data.torque}, {data.bias_current}, ' + - f'{data.charge_curr_limit}, {data.draw_curr_limit}, ') + self.logfile.write(f'{data.rpm:.1f}, {data.voltage:.1f}, {data.wcurrent:.2f}, ' + + f'{data.bcurrent:.2f}, {data.status}, {data.loaddc:.2f}, {data.target_v:.1f}, ' + + f'{data.target_a:.2f}, {data.diff_press:.3f}, {data.sd_rpm:.1f}, {data.scale:.2f}, ' + + f'{data.retract:.2f}, {data.torque:.2f}, {data.bias_current:.2f}, ' + + f'{data.charge_curr_limit:.2f}, {data.draw_curr_limit:.2f}, ') else: self.logfile.write("," * self.pc_header.count(',')) @@ -226,9 +226,9 @@ def write_bc_header(self): # 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}, {data.ips}, ' + - f'{data.vbalance}, {data.vstopcharge}, {data.gfault}, ' + - f'{data.hydrogen}, {data.status}, ') + self.logfile.write(f'{data.voltage:.1f}, {data.ips:.2f}, ' + + f'{data.vbalance:.2f}, {data.vstopcharge:.2f}, {data.gfault:.2f}, ' + + f'{data.hydrogen:.2f}, {data.status}, ') else: self.logfile.write("," * self.bc_header.count(',')) @@ -257,12 +257,12 @@ def write_xb(self, data): imu.orientation.z, imu.orientation.w]) self.logfile.write( - f'{math.degrees(roll)}, {math.degrees(pitch)}, {math.degrees(yaw)}, ' + - f'{imu.angular_velocity.x}, {imu.angular_velocity.y}, {imu.angular_velocity.z}, ' + - f'{imu.linear_acceleration.x}, {imu.linear_acceleration.y}, {imu.linear_acceleration.z}, ' + - f'{ned.z}, {ned.y}, {ned.z}, ' + - f'{gps.latitude}, {gps.longitude}, {gps.altitude}, ' + - f'{tmp.temperature}, ') + f'{math.degrees(roll):.3f}, {math.degrees(pitch):.3f}, {math.degrees(yaw):.3f}, ' + + f'{imu.angular_velocity.x:.3f}, {imu.angular_velocity.y:.3f}, {imu.angular_velocity.z:.3f}, ' + + f'{imu.linear_acceleration.x:.3f}, {imu.linear_acceleration.y:.3f}, {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(',')) @@ -273,16 +273,16 @@ def write_xb(self, data): 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, \ +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): - self.logfile.write(f'{data.load_cell}, {data.range_finder}, ' + - f'{data.upper_psi}, {data.lower_psi}, {data.status}, {data.epoch}, ' + - f'{data.salinity}, {data.temperature}, ') + self.logfile.write(f'{data.load_cell}, {data.range_finder:.2f}, ' + + f'{data.upper_psi:.2f}, {data.lower_psi:.2f}, {data.status}, {data.epoch}, ' + + f'{data.salinity:.6f}, {data.temperature:.3f}, ') else: self.logfile.write("," * self.sc_header.count(',')) @@ -305,13 +305,13 @@ 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}, ' + - f'{data.battery_voltage}, {data.pressure}, ' + - f'{imu.orientation.x}, {imu.orientation.y}, {imu.orientation.z}, {imu.orientation.w}, ' + - f'{mag.magnetic_field.x}, {mag.magnetic_field.y}, {mag.magnetic_field.z}, {data.status}, ' + - f'{imu.linear_acceleration.x}, {imu.linear_acceleration.y}, {imu.linear_acceleration.z}, ' + + 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}, {imu.orientation.z:.3f}, {imu.orientation.w:.3f}, ' + + f'{mag.magnetic_field.x:.3f}, {mag.magnetic_field.y:.3f}, {mag.magnetic_field.z:.3f}, {data.status}, ' + + f'{imu.linear_acceleration.x:.3f}, {imu.linear_acceleration.y:.3f}, {imu.linear_acceleration.z:.3f}, ' + f'{data.vpe_status}, ' + - f'{imu.linear_acceleration.x}, {imu.linear_acceleration.y}, {imu.linear_acceleration.z}, ' + + f'{imu.linear_acceleration.x:.3f}, {imu.linear_acceleration.y:.3f}, {imu.linear_acceleration.z:.3f}, ' + f'{data.comms_timeouts}, {data.motor_status}, {data.motor_current}, {data.encoder}, ') else: self.logfile.write("," * self.tf_header.count(',')) From 4410cc616df07acfcbb92ed94494e4dde9b0fc02 Mon Sep 17 00:00:00 2001 From: Rich Henthorn Date: Fri, 17 Mar 2023 10:34:33 -0700 Subject: [PATCH 03/21] Fix flake8 and pep8 erros --- sim_pblog/sim_pblog/sim_pblog.py | 216 +++++++++++++++---------------- 1 file changed, 103 insertions(+), 113 deletions(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index b3cd3bb..27c1484 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -14,30 +14,32 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os import gzip -import math +import os + from datetime import datetime +import math + from tf_transformations import euler_from_quaternion from buoy_api import Interface -from buoy_interfaces.msg import PCRecord from buoy_interfaces.msg import BCRecord -from buoy_interfaces.msg import XBRecord +from buoy_interfaces.msg import PCRecord from buoy_interfaces.msg import SCRecord +from buoy_interfaces.msg import XBRecord from buoy_interfaces.msg import TFRecord import rclpy WECLOGHOME = 'WEC_LOG_DIR' -ALTLOGHOME = '/tmp' # assume we always have write permission to /tmp -NEWFILEINTERVAL = 2 #60*60 # limit the 'size' of each log file to one hour +ALTLOGHOME = '/tmp' # assume we always have write permission to /tmp +NEWFILEINTERVAL = 60 * 60 # limit the 'size' of each log file to one hour # Controller IDs used as the first value in each record BatteryConID = 0 -SpringConID = 1 -PowerConID = 2 +SpringConID = 1 +PowerConID = 2 TrefoilConID = 4 -CrossbowID = 5 +CrossbowID = 5 # WECLogger - duplicate legacy pblog behavior substituting ROS2 topics for CANBus # Description: @@ -51,44 +53,43 @@ # # Notes : (1) There is always a trailing comma in all header and # record sections -# + + class WECLogger(Interface): def __init__(self): super().__init__('sim_pblog') - #self.set_params() - self.logdir = None - self.logfile = None + self.logdir = None + self.logfile = None self.logfilename = None - self.logtime = None - self.pc_header = "" - self.bc_header = "" - self.xb_header = "" - self.sc_header = "" - self.tf_header = "" + self.logtime = None + self.pc_header = '' + self.bc_header = '' + self.xb_header = '' + self.sc_header = '' + self.tf_header = '' self.logfile_setup() def __del__(self): self.close_zip_logfile() - # 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): # Create new log folder at the start of this instance of the logger - if (self.logdir == None): + if (self.logdir is None): self.logdir = self.logdir_setup() # close existing log file and zip it shut - if (self.logfile != None): + if (self.logfile is not None): self.close_zip_logfile() - # Open new file in logdir using the date & time (2023.03.23T13.09.54.csv) - self.logfilename = self.logdir + '/' + \ - datetime.now().strftime("%Y.%m.%dT%I.%M.%S") + '.csv' - self.logfile = open(self.logfilename, mode="w", encoding='utf-8') + self.logfilename = self.logdir \ + + '/' + datetime.now().strftime('%Y.%m.%dT%I.%M.%S') + '.csv' + self.logfile = open(self.logfilename, mode='w', encoding='utf-8') # Point a link called 'latest' to the new directory # Renaming a temporary link works as 'ln -sf' @@ -100,13 +101,12 @@ def logfile_setup(self): self.get_logger().info(f'New log file: {self.logfilename}') self.write_header() - # Write CSV header line by writing each controller section def write_header(self): # Preamble/Header section column names first - self.logfile.write("Source ID, Timestamp (epoch seconds), ") + self.logfile.write('Source ID, Timestamp (epoch seconds),') - # Write each header section + # Write each header section - order matters self.write_pc_header() self.write_bc_header() self.write_xb_header() @@ -114,7 +114,6 @@ def write_header(self): self.write_tf_header() self.write_eol() - # Write a complete record line by writing each controller section def write_record(self, source_id, data): # Calculate float value epoch seconds and write the record preamble/header @@ -122,7 +121,7 @@ def write_record(self, source_id, data): timestamp = (1.0*nanos) / 1e9 # convert from nanoseconds to seconds self.logfile.write(f'{source_id}, {timestamp:.3f}, ') - # Pass the data to writers, let them decide what to write + # Pass data and decisoin of what to write to writers - order matters self.write_pc(data) self.write_bc(data) self.write_xb(data) @@ -131,19 +130,18 @@ def write_record(self, source_id, data): self.write_eol() # Create a fresh logfile when interval time has passed - if (self.logtime == None): + if (self.logtime is None): self.logtime = timestamp elif (timestamp > self.logtime + NEWFILEINTERVAL): self.logfile_setup() self.logtime = timestamp - def write_eol(self): self.logfile.write('\n') - + # Close the current log file and zip it def close_zip_logfile(self): - if (self.logfile != None): + if (self.logfile is not None): self.logfile.close() with open(self.logfilename, 'rb') as logf: with gzip.open(f'{self.logfilename}.gz', 'wb') as gzfile: @@ -151,30 +149,29 @@ def close_zip_logfile(self): gzfile.writelines(logf) os.remove(self.logfilename) - # 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): # Use WEC_LOG_DIR env variable for base of log tree wec_log_dir = os.getenv(WECLOGHOME) - if (None == wec_log_dir): + if (None is wec_log_dir): self.get_logger().info(f'WEC_LOG_DIR env variable not set, using {ALTLOGHOME} as log home') wec_log_dir = ALTLOGHOME - elif (False == os.access(wec_log_dir, os.W_OK)): + elif (False is os.access(wec_log_dir, os.W_OK)): self.get_logger().info(f'No write access to {wec_log_dir}, using {ALTLOGHOME} as log home') wec_log_dir = ALTLOGHOME - # Use today's date to create directory name, e.g., 2023-03-23.002 now = datetime.today().strftime('%Y-%m-%d') count = 0 - dirname = wec_log_dir + '/' + now + ".{nnn:03}".format(nnn=count) + dirname = wec_log_dir + '/' + now + '.{nnn:03}'.format(nnn=count) while (os.path.exists(dirname)): - count = count +1 - dirname = wec_log_dir + '/' + now + ".{nnn:03}".format(nnn=count) - - if (False == os.path.exists(dirname)): + count = count + 1 + dirname = wec_log_dir + '/' + now + '.{nnn:03}'.format(nnn=count) + + if (False is os.path.exists(dirname)): os.makedirs(dirname) # Point a link called 'latest' to the new directory @@ -186,39 +183,38 @@ def logdir_setup(self): self.get_logger().info(f'New log directory: {dirname}') return dirname - - - ############ Power Controller write functions ############ + # 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.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}, {data.target_v:.1f}, ' + - f'{data.target_a:.2f}, {data.diff_press:.3f}, {data.sd_rpm:.1f}, {data.scale:.2f}, ' + - f'{data.retract:.2f}, {data.torque:.2f}, {data.bias_current:.2f}, ' + - f'{data.charge_curr_limit:.2f}, {data.draw_curr_limit:.2f}, ') + self.logfile.write(f'{data.rpm:.1f}, {data.voltage:.1f}, {data.wcurrent:.2f}, ' + + f'{data.bcurrent:.2f}, {data.status}, {data.loaddc:.2f}, {data.target_v:.1f}, ' + + f'{data.target_a:.2f}, {data.diff_press:.3f}, {data.sd_rpm:.1f}, {data.scale:.2f}, ' + + f'{data.retract:.2f}, {data.torque:.2f}, {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 ############# + # 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.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) @@ -226,20 +222,19 @@ def write_bc_header(self): # 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}, {data.gfault:.2f}, ' + - f'{data.hydrogen:.2f}, {data.status}, ') + self.logfile.write(f'{data.voltage:.1f}, {data.ips:.2f}, ' + + f'{data.vbalance:.2f}, {data.vstopcharge:.2f}, {data.gfault:.2f}, ' + + f'{data.hydrogen:.2f}, {data.status}, ') else: self.logfile.write("," * self.bc_header.count(',')) - - ############ Crossbow AHRS write functions ############# + # 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.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) @@ -257,96 +252,91 @@ def write_xb(self, data): 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}, {imu.angular_velocity.z:.3f}, ' + - f'{imu.linear_acceleration.x:.3f}, {imu.linear_acceleration.y:.3f}, {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}, ') + f'{math.degrees(roll):.3f}, {math.degrees(pitch):.3f}, {math.degrees(yaw):.3f}, ' + + f'{imu.angular_velocity.x:.3f}, {imu.angular_velocity.y:.3f}, {imu.angular_velocity.z:.3f}, ' + + f'{imu.linear_acceleration.x:.3f}, {imu.linear_acceleration.y:.3f}, {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 ############# + # 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.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): - self.logfile.write(f'{data.load_cell}, {data.range_finder:.2f}, ' + - f'{data.upper_psi:.2f}, {data.lower_psi:.2f}, {data.status}, {data.epoch}, ' + - f'{data.salinity:.6f}, {data.temperature:.3f}, ') + self.logfile.write(f'{data.load_cell}, {data.range_finder:.2f}, ' + + f'{data.upper_psi:.2f}, {data.lower_psi:.2f}, {data.status}, {data.epoch}, ' + + f'{data.salinity:.6f}, {data.temperature:.3f}, ') else: self.logfile.write("," * self.sc_header.count(',')) - - ############ Trefoil Controller write functions ############# + # 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.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}, {imu.orientation.z:.3f}, {imu.orientation.w:.3f}, ' + - f'{mag.magnetic_field.x:.3f}, {mag.magnetic_field.y:.3f}, {mag.magnetic_field.z:.3f}, {data.status}, ' + - f'{imu.linear_acceleration.x:.3f}, {imu.linear_acceleration.y:.3f}, {imu.linear_acceleration.z:.3f}, ' + - f'{data.vpe_status}, ' + - f'{imu.linear_acceleration.x:.3f}, {imu.linear_acceleration.y:.3f}, {imu.linear_acceleration.z:.3f}, ' + - f'{data.comms_timeouts}, {data.motor_status}, {data.motor_current}, {data.encoder}, ') + 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}, {imu.orientation.z:.3f}, {imu.orientation.w:.3f}, ' + + f'{mag.magnetic_field.x:.3f}, {mag.magnetic_field.y:.3f}, {mag.magnetic_field.z:.3f}, {data.status}, ' + + f'{imu.linear_acceleration.x:.3f}, {imu.linear_acceleration.y:.3f}, {imu.linear_acceleration.z:.3f}, ' + + f'{data.vpe_status}, ' + + f'{imu.linear_acceleration.x:.3f}, {imu.linear_acceleration.y:.3f}, {imu.linear_acceleration.z:.3f}, ' + + f'{data.comms_timeouts}, {data.motor_status}, {data.motor_current}, {data.encoder}, ') else: self.logfile.write("," * self.tf_header.count(',')) - - - # Delete any unused callback def ahrs_callback(self, data): - '''Callback for '/ahrs_data' topic from XBowAHRS''' + """Callback for '/ahrs_data' topic from XBowAHRS""" self.write_record(CrossbowID, data) def battery_callback(self, data): - '''Callback for '/battery_data' topic from Battery Controller''' + """Callback for '/battery_data' topic from Battery Controller""" self.write_record(BatteryConID, data) def spring_callback(self, data): - '''Callback for '/spring_data' topic from Spring Controller''' + """Callback for '/spring_data' topic from Spring Controller""" self.write_record(SpringConID, data) def power_callback(self, data): - '''Callback for '/power_data' topic from Power Controller''' + """Callback for '/power_data' topic from Power Controller""" self.write_record(PowerConID, data) - #self.csv_log.writerow([data.seq_num, data.voltage, data.sd_rpm]) def trefoil_callback(self, data): - '''Callback for '/trefoil_data' topic from Trefoil Controller''' + """Callback for '/trefoil_data' topic from Trefoil Controller""" self.write_record(TrefoilConID, data) def powerbuoy_callback(self, data): - '''Callback for '/powerbuoy_data' topic -- Aggregated data from all topics''' - #self.get_logger().info('Received Powerbuoy data') + """Callback for '/powerbuoy_data' topic -- Aggregated data from all topics""" def set_params(self): - '''Use ROS2 declare_parameter and get_parameter to set policy params''' + """Use ROS2 declare_parameter and get_parameter to set policy params""" pass From 5f17a56e61870fbe1293197f95190765c99f90ce Mon Sep 17 00:00:00 2001 From: Rich Henthorn Date: Fri, 17 Mar 2023 11:19:47 -0700 Subject: [PATCH 04/21] Fix flake8 and pep8 errors 2 --- sim_pblog/sim_pblog/sim_pblog.py | 71 +++++++++++++++++--------------- 1 file changed, 38 insertions(+), 33 deletions(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index 27c1484..f03614d 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -1,6 +1,6 @@ #!/usr/bin/python3 -# Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute +# 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. @@ -14,22 +14,21 @@ # See the License for the specific language governing permissions and # limitations under the License. +from datetime import datetime import gzip import os - -from datetime import datetime import math -from tf_transformations import euler_from_quaternion - 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 XBRecord from buoy_interfaces.msg import TFRecord +from buoy_interfaces.msg import XBRecord import rclpy +from tf_transformations import euler_from_quaternion + WECLOGHOME = 'WEC_LOG_DIR' ALTLOGHOME = '/tmp' # assume we always have write permission to /tmp NEWFILEINTERVAL = 60 * 60 # limit the 'size' of each log file to one hour @@ -157,10 +156,10 @@ def logdir_setup(self): # Use WEC_LOG_DIR env variable for base of log tree wec_log_dir = os.getenv(WECLOGHOME) if (None is wec_log_dir): - self.get_logger().info(f'WEC_LOG_DIR env variable not set, using {ALTLOGHOME} as log home') + self.get_logger().info(f'WEC_LOG_DIR env variable not set, using {ALTLOGHOME}') wec_log_dir = ALTLOGHOME elif (False is os.access(wec_log_dir, os.W_OK)): - self.get_logger().info(f'No write access to {wec_log_dir}, using {ALTLOGHOME} as log home') + self.get_logger().info(f'No write access to {wec_log_dir}, using {ALTLOGHOME}') wec_log_dir = ALTLOGHOME # Use today's date to create directory name, e.g., 2023-03-23.002 @@ -202,12 +201,14 @@ def write_pc_header(self): 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}, {data.target_v:.1f}, ' - + f'{data.target_a:.2f}, {data.diff_press:.3f}, {data.sd_rpm:.1f}, {data.scale:.2f}, ' - + f'{data.retract:.2f}, {data.torque:.2f}, {data.bias_current:.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(',')) + self.logfile.write(',' * self.pc_header.count(',')) # Battery Controller write functions # BC header section @@ -223,10 +224,10 @@ def write_bc_header(self): 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}, {data.gfault:.2f}, ' - + f'{data.hydrogen:.2f}, {data.status}, ') + + f'{data.vbalance:.2f}, {data.vstopcharge:.2f}, ' + + f'{data.gfault:.2f}, {data.hydrogen:.2f}, {data.status}, ') else: - self.logfile.write("," * self.bc_header.count(',')) + self.logfile.write(',' * self.pc_header.count(',')) # Crossbow AHRS Controller write functions # XB header section @@ -253,13 +254,15 @@ def write_xb(self, data): 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}, {imu.angular_velocity.z:.3f}, ' - + f'{imu.linear_acceleration.x:.3f}, {imu.linear_acceleration.y:.3f}, {imu.linear_acceleration.z:.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(',')) + self.logfile.write(',' * self.pc_header.count(',')) # Spring Controller write functions # SC header section @@ -275,10 +278,11 @@ def write_sc_header(self): def write_sc(self, data): if (type(data) is SCRecord): self.logfile.write(f'{data.load_cell}, {data.range_finder:.2f}, ' - + f'{data.upper_psi:.2f}, {data.lower_psi:.2f}, {data.status}, {data.epoch}, ' + + 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(',')) + self.logfile.write(',' * self.pc_header.count(',')) # Trefoil Controller write functions # TF header section @@ -302,41 +306,42 @@ def write_tf(self, data): 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}, {imu.orientation.z:.3f}, {imu.orientation.w:.3f}, ' - + f'{mag.magnetic_field.x:.3f}, {mag.magnetic_field.y:.3f}, {mag.magnetic_field.z:.3f}, {data.status}, ' - + f'{imu.linear_acceleration.x:.3f}, {imu.linear_acceleration.y:.3f}, {imu.linear_acceleration.z:.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}, {imu.linear_acceleration.y:.3f}, {imu.linear_acceleration.z:.3f}, ' - + f'{data.comms_timeouts}, {data.motor_status}, {data.motor_current}, {data.encoder}, ') + + 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(',')) + self.logfile.write(',' * self.tf_header.count(',')) # Delete any unused callback def ahrs_callback(self, data): - """Callback for '/ahrs_data' topic from XBowAHRS""" self.write_record(CrossbowID, data) def battery_callback(self, data): - """Callback for '/battery_data' topic from Battery Controller""" self.write_record(BatteryConID, data) def spring_callback(self, data): - """Callback for '/spring_data' topic from Spring Controller""" self.write_record(SpringConID, data) def power_callback(self, data): - """Callback for '/power_data' topic from Power Controller""" self.write_record(PowerConID, data) def trefoil_callback(self, data): - """Callback for '/trefoil_data' topic from Trefoil Controller""" self.write_record(TrefoilConID, data) def powerbuoy_callback(self, data): - """Callback for '/powerbuoy_data' topic -- Aggregated data from all topics""" + pass def set_params(self): - """Use ROS2 declare_parameter and get_parameter to set policy params""" pass From 48edd2f04420272cf6a6bb72e8b9dc0f2c5cc2ff Mon Sep 17 00:00:00 2001 From: Rich Henthorn Date: Fri, 17 Mar 2023 11:41:15 -0700 Subject: [PATCH 05/21] Fix flake8 and pep8 errors 3 --- sim_pblog/sim_pblog/sim_pblog.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index f03614d..74477c7 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -16,8 +16,8 @@ from datetime import datetime import gzip -import os import math +import os from buoy_api import Interface from buoy_interfaces.msg import BCRecord From 3eb209cdea15d772807179c2ffc36bd30b759ee4 Mon Sep 17 00:00:00 2001 From: Rich Henthorn Date: Fri, 17 Mar 2023 12:20:19 -0700 Subject: [PATCH 06/21] Formatting erros in launch script - empty line needed --- sim_pblog/launch/sim_pblog.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/sim_pblog/launch/sim_pblog.launch.py b/sim_pblog/launch/sim_pblog.launch.py index 1912384..2260d7b 100644 --- a/sim_pblog/launch/sim_pblog.launch.py +++ b/sim_pblog/launch/sim_pblog.launch.py @@ -21,6 +21,7 @@ package_name = 'sim_pblog' + def generate_launch_description(): ld = LaunchDescription() config = os.path.join( From d24f24700e50578c593f56109c78a279dd142efa Mon Sep 17 00:00:00 2001 From: Rich Henthorn Date: Wed, 22 Mar 2023 14:28:32 -0700 Subject: [PATCH 07/21] Add epoch seconds offset to simulation time, log that value as the timestamp --- sim_pblog/sim_pblog/sim_pblog.py | 56 ++++++++++++++++++++------------ 1 file changed, 35 insertions(+), 21 deletions(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index 74477c7..8be45f3 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from datetime import datetime +from datetime import datetime, timedelta import gzip import math import os @@ -61,12 +61,14 @@ def __init__(self): self.logdir = None self.logfile = None self.logfilename = None - self.logtime = None + self.logfiletime = datetime.fromtimestamp(0) self.pc_header = '' self.bc_header = '' self.xb_header = '' self.sc_header = '' self.tf_header = '' + self.start_time = datetime.now() + self.logger_time = self.start_time self.logfile_setup() def __del__(self): @@ -85,10 +87,11 @@ def logfile_setup(self): if (self.logfile is not None): self.close_zip_logfile() - # Open new file in logdir using the date & time (2023.03.23T13.09.54.csv) + # Open new file in logdir using the logger_time (2023.03.23T13.09.54.csv) self.logfilename = self.logdir \ - + '/' + datetime.now().strftime('%Y.%m.%dT%I.%M.%S') + '.csv' + + '/' + self.logger_time.strftime('%Y.%m.%dT%I.%M.%S') + '.csv' self.logfile = open(self.logfilename, mode='w', encoding='utf-8') + self.write_header() # Point a link called 'latest' to the new directory # Renaming a temporary link works as 'ln -sf' @@ -98,14 +101,13 @@ def logfile_setup(self): os.rename(templink, latest) self.get_logger().info(f'New log file: {self.logfilename}') - self.write_header() - # Write CSV header line by writing each controller section + # 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 + # Write each header section - order matters! self.write_pc_header() self.write_bc_header() self.write_xb_header() @@ -113,14 +115,20 @@ def write_header(self): self.write_tf_header() self.write_eol() - # Write a complete record line by writing each controller section + # Write a complete data record by writing each controller data section def write_record(self, source_id, data): - # Calculate float value epoch seconds and write the record preamble/header - nanos = (data.header.stamp.sec * 1e9) + data.header.stamp.nanosec - timestamp = (1.0*nanos) / 1e9 # convert from nanoseconds to seconds - self.logfile.write(f'{source_id}, {timestamp:.3f}, ') + self.update_logger_time(data) + + # Create a fresh logfile when interval time has passed since the + # current logfile was created + if (self.logger_time > (self.logfiletime + timedelta(seconds=NEWFILEINTERVAL))): + 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 decisoin of what to write to writers - order matters + # Pass data and delegate writing to section writers - order matters! self.write_pc(data) self.write_bc(data) self.write_xb(data) @@ -128,12 +136,18 @@ def write_record(self, source_id, data): self.write_tf(data) self.write_eol() - # Create a fresh logfile when interval time has passed - if (self.logtime is None): - self.logtime = timestamp - elif (timestamp > self.logtime + NEWFILEINTERVAL): - self.logfile_setup() - self.logtime = timestamp + # 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 + micros = (1.0 * data.header.stamp.nanosec) / 1000. # convert to micros + delta = timedelta(seconds=data.header.stamp.sec, microseconds=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') @@ -162,8 +176,8 @@ def logdir_setup(self): self.get_logger().info(f'No write access to {wec_log_dir}, using {ALTLOGHOME}') wec_log_dir = ALTLOGHOME - # Use today's date to create directory name, e.g., 2023-03-23.002 - now = datetime.today().strftime('%Y-%m-%d') + # Use logger_time date to create directory name, e.g., 2023-03-23.002 + now = self.logger_time.strftime('%Y-%m-%d') count = 0 dirname = wec_log_dir + '/' + now + '.{nnn:03}'.format(nnn=count) while (os.path.exists(dirname)): From eee754402b1ecbec920b74281a593bfa9471bf3b Mon Sep 17 00:00:00 2001 From: Rich Henthorn Date: Thu, 23 Mar 2023 12:02:36 -0700 Subject: [PATCH 08/21] Corrected header line --- sim_pblog/sim_pblog/sim_pblog.py | 47 ++++++++++++++------------------ 1 file changed, 21 insertions(+), 26 deletions(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index 8be45f3..089b9ab 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -201,12 +201,11 @@ def logdir_setup(self): # 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.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 @@ -227,10 +226,9 @@ def write_pc(self, data): # 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.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 @@ -246,11 +244,10 @@ def write_bc(self, data): # 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.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 @@ -282,10 +279,9 @@ def write_xb(self, data): # 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.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 @@ -302,13 +298,12 @@ def write_sc(self, data): # 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.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 From b6e8558f1d98dcb694e188b9d187ba3fe7cf80a8 Mon Sep 17 00:00:00 2001 From: Rich Henthorn Date: Mon, 27 Mar 2023 10:20:39 -0700 Subject: [PATCH 09/21] Move log home and file interval to class attributes --- sim_pblog/sim_pblog/sim_pblog.py | 28 +++++++++------------------- 1 file changed, 9 insertions(+), 19 deletions(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index 089b9ab..ad5de08 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -29,10 +29,6 @@ from tf_transformations import euler_from_quaternion -WECLOGHOME = 'WEC_LOG_DIR' -ALTLOGHOME = '/tmp' # assume we always have write permission to /tmp -NEWFILEINTERVAL = 60 * 60 # limit the 'size' of each log file to one hour - # Controller IDs used as the first value in each record BatteryConID = 0 SpringConID = 1 @@ -58,6 +54,7 @@ class WECLogger(Interface): def __init__(self): super().__init__('sim_pblog') + self.loghome = '/tmp' # TODO: needs to come from cfg or param self.logdir = None self.logfile = None self.logfilename = None @@ -69,6 +66,7 @@ def __init__(self): self.tf_header = '' self.start_time = datetime.now() self.logger_time = self.start_time + self.logfileinterval = 60 * 60 # in seconds TODO: needs to come from cfg or param self.logfile_setup() def __del__(self): @@ -119,9 +117,8 @@ def write_header(self): def write_record(self, source_id, data): self.update_logger_time(data) - # Create a fresh logfile when interval time has passed since the - # current logfile was created - if (self.logger_time > (self.logfiletime + timedelta(seconds=NEWFILEINTERVAL))): + # Create a fresh logfile when interval time has passed + if (self.logger_time > (self.logfiletime + timedelta(seconds=self.logfileinterval))): self.logfiletime = self.logger_time self.logfile_setup() @@ -167,31 +164,24 @@ def close_zip_logfile(self): # Example: "2023-03-31.005 would be created on the 6th run on March 31st def logdir_setup(self): - # Use WEC_LOG_DIR env variable for base of log tree - wec_log_dir = os.getenv(WECLOGHOME) - if (None is wec_log_dir): - self.get_logger().info(f'WEC_LOG_DIR env variable not set, using {ALTLOGHOME}') - wec_log_dir = ALTLOGHOME - elif (False is os.access(wec_log_dir, os.W_OK)): - self.get_logger().info(f'No write access to {wec_log_dir}, using {ALTLOGHOME}') - wec_log_dir = ALTLOGHOME + self.get_logger().info(f'Using {self.loghome} as logging home') # Use logger_time date to create directory name, e.g., 2023-03-23.002 now = self.logger_time.strftime('%Y-%m-%d') count = 0 - dirname = wec_log_dir + '/' + now + '.{nnn:03}'.format(nnn=count) + dirname = self.loghome + '/' + now + '.{nnn:03}'.format(nnn=count) while (os.path.exists(dirname)): count = count + 1 - dirname = wec_log_dir + '/' + now + '.{nnn:03}'.format(nnn=count) + dirname = self.loghome + '/' + now + '.{nnn:03}'.format(nnn=count) if (False is os.path.exists(dirname)): os.makedirs(dirname) # Point a link called 'latest' to the new directory # Renaming a temporary link works as 'ln -sf' - templink = wec_log_dir + '/' + '__bogus__' + templink = self.loghome + '/' + '__templn__' os.symlink(dirname, templink) - latest = wec_log_dir + '/' + 'latest' + latest = self.loghome + '/' + 'latest' os.rename(templink, latest) self.get_logger().info(f'New log directory: {dirname}') From a6ad9cbfc993e23cb20d7eae69203dc4001a5a51 Mon Sep 17 00:00:00 2001 From: andermi Date: Wed, 3 May 2023 15:43:53 -0700 Subject: [PATCH 10/21] Add pblog args (#47) * add launch args for log paths; add ros2 param for interval; fix symlinks; add rosdeps Signed-off-by: Michael Anderson * Update package.xml fix rosdep * fix atexit gzip Signed-off-by: Michael Anderson * linter Signed-off-by: Michael Anderson * update default pblog root; stop gzipping at exit; fix gzipping at start Signed-off-by: Michael Anderson * remove unused import Signed-off-by: Michael Anderson * Correct faulty CrossbowID value from 5 to 3 --------- Signed-off-by: Michael Anderson Co-authored-by: Rich Henthorn --- .gitignore | 136 +++++++++++++++++++++++++-- sim_pblog/.gitignore | 129 ------------------------- sim_pblog/config/sim_pblog.yaml | 2 +- sim_pblog/launch/sim_pblog.launch.py | 24 ++++- sim_pblog/package.xml | 3 + sim_pblog/sim_pblog/sim_pblog.py | 111 +++++++++++++--------- 6 files changed, 220 insertions(+), 185 deletions(-) delete mode 100644 sim_pblog/.gitignore 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/sim_pblog/.gitignore b/sim_pblog/.gitignore deleted file mode 100644 index b6e4761..0000000 --- a/sim_pblog/.gitignore +++ /dev/null @@ -1,129 +0,0 @@ -# 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/sim_pblog/config/sim_pblog.yaml b/sim_pblog/config/sim_pblog.yaml index 0580a98..0cdf7d9 100644 --- a/sim_pblog/config/sim_pblog.yaml +++ b/sim_pblog/config/sim_pblog.yaml @@ -1,4 +1,4 @@ /sim_pblog: ros__parameters: - foo: 1.0 + logfileinterval_mins: 60 diff --git a/sim_pblog/launch/sim_pblog.launch.py b/sim_pblog/launch/sim_pblog.launch.py index 2260d7b..30ada7e 100644 --- a/sim_pblog/launch/sim_pblog.launch.py +++ b/sim_pblog/launch/sim_pblog.launch.py @@ -15,7 +15,11 @@ 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 @@ -23,20 +27,36 @@ def generate_launch_description(): - ld = LaunchDescription() + 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 index c98eb0e..e43a8db 100644 --- a/sim_pblog/package.xml +++ b/sim_pblog/package.xml @@ -10,6 +10,9 @@ buoy_interfaces rclpy buoy_api_py + tf_transformations + python3-transforms3d + ament_copyright ament_flake8 ament_pep257 diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index ad5de08..7d790ee 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -20,21 +20,24 @@ 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 tf_transformations import euler_from_quaternion + # Controller IDs used as the first value in each record BatteryConID = 0 SpringConID = 1 PowerConID = 2 +CrossbowID = 3 TrefoilConID = 4 -CrossbowID = 5 # WECLogger - duplicate legacy pblog behavior substituting ROS2 topics for CANBus # Description: @@ -52,50 +55,48 @@ class WECLogger(Interface): - def __init__(self): - super().__init__('sim_pblog') - self.loghome = '/tmp' # TODO: needs to come from cfg or param - self.logdir = None + def __init__(self, loghome, logdir=None): + super().__init__('sim_pblog', check_for_services=False) + 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 = '' - self.start_time = datetime.now() - self.logger_time = self.start_time - self.logfileinterval = 60 * 60 # in seconds TODO: needs to come from cfg or param - self.logfile_setup() - - def __del__(self): - self.close_zip_logfile() # 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): - # Create new log folder at the start of this instance of the logger - if (self.logdir is None): - self.logdir = self.logdir_setup() - # close existing log file and zip it shut if (self.logfile is not None): self.close_zip_logfile() # Open new file in logdir using the logger_time (2023.03.23T13.09.54.csv) - self.logfilename = self.logdir \ - + '/' + self.logger_time.strftime('%Y.%m.%dT%I.%M.%S') + '.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') self.write_header() # Point a link called 'latest' to the new directory # Renaming a temporary link works as 'ln -sf' - templink = self.logdir + '/' + '__bogus__' - os.symlink(self.logfilename, templink) - latest = self.logdir + '/' + 'latest' + 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}') @@ -118,7 +119,7 @@ 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))): + if (self.logger_time > (self.logfiletime + timedelta(seconds=self.logfileinterval_sec))): self.logfiletime = self.logger_time self.logfile_setup() @@ -151,37 +152,50 @@ def write_eol(self): # Close the current log file and zip it def close_zip_logfile(self): - if (self.logfile is not None): + 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: - self.get_logger().info(f'{self.logfilename} -> {self.logfilename}.gz') gzfile.writelines(logf) - os.remove(self.logfilename) + 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): + def logdir_setup(self, basename=None): self.get_logger().info(f'Using {self.loghome} as logging home') - # Use logger_time date to create directory name, e.g., 2023-03-23.002 - now = self.logger_time.strftime('%Y-%m-%d') - count = 0 - dirname = self.loghome + '/' + now + '.{nnn:03}'.format(nnn=count) - while (os.path.exists(dirname)): - count = count + 1 - dirname = self.loghome + '/' + now + '.{nnn:03}'.format(nnn=count) - - if (False is os.path.exists(dirname)): + 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' to the new directory + # Point a link called 'latest_csv' to the new directory # Renaming a temporary link works as 'ln -sf' - templink = self.loghome + '/' + '__templn__' - os.symlink(dirname, templink) - latest = self.loghome + '/' + 'latest' + templink = os.path.join(self.loghome, '__templn__') + os.symlink(basename, templink) + latest = os.path.join(self.loghome, 'latest_csv') os.rename(templink, latest) self.get_logger().info(f'New log directory: {dirname}') @@ -321,7 +335,6 @@ def write_tf(self, data): else: self.logfile.write(',' * self.tf_header.count(',')) - # Delete any unused callback def ahrs_callback(self, data): self.write_record(CrossbowID, data) @@ -337,16 +350,22 @@ def power_callback(self, data): def trefoil_callback(self, data): self.write_record(TrefoilConID, data) - def powerbuoy_callback(self, data): - pass - def set_params(self): - pass + 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(): - rclpy.init() - pblog = WECLogger() + 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() From 6732d210d7cb2b427fe222b01a9d18d2e7142a95 Mon Sep 17 00:00:00 2001 From: Rich Henthorn Date: Thu, 4 May 2023 10:20:20 -0700 Subject: [PATCH 11/21] Corrected error: using pc_header to calculate the 'blank' cells in a record when xb_header, bc_header, and sc_header should have been used --- sim_pblog/sim_pblog/sim_pblog.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index 7d790ee..a3c713a 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -243,7 +243,7 @@ def write_bc(self, data): + f'{data.vbalance:.2f}, {data.vstopcharge:.2f}, ' + f'{data.gfault:.2f}, {data.hydrogen:.2f}, {data.status}, ') else: - self.logfile.write(',' * self.pc_header.count(',')) + self.logfile.write(',' * self.bc_header.count(',')) # Crossbow AHRS Controller write functions # XB header section @@ -277,7 +277,7 @@ def write_xb(self, data): + f'{gps.latitude:.5f}, {gps.longitude:.5f}, {gps.altitude:.3f}, ' + f'{tmp.temperature:.3f}, ') else: - self.logfile.write(',' * self.pc_header.count(',')) + self.logfile.write(',' * self.xb_header.count(',')) # Spring Controller write functions # SC header section @@ -296,7 +296,7 @@ def write_sc(self, data): + f'{data.status}, {data.epoch}, ' + f'{data.salinity:.6f}, {data.temperature:.3f}, ') else: - self.logfile.write(',' * self.pc_header.count(',')) + self.logfile.write(',' * self.sc_header.count(',')) # Trefoil Controller write functions # TF header section From accde041632a1a91d8d04a0ad1d0716a53e67962 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Thu, 11 May 2023 11:25:15 -0700 Subject: [PATCH 12/21] tweak msg time a little Signed-off-by: Michael Anderson --- sim_pblog/sim_pblog/sim_pblog.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index a3c713a..9312532 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -28,6 +28,7 @@ from buoy_interfaces.msg import XBRecord import rclpy +from rclpy.time import Time from tf_transformations import euler_from_quaternion @@ -138,8 +139,9 @@ def write_record(self, source_id, data): def update_logger_time(self, data): # Timestamps in the data messages contain the time since the # start of the simulation beginning at zero - micros = (1.0 * data.header.stamp.nanosec) / 1000. # convert to micros - delta = timedelta(seconds=data.header.stamp.sec, microseconds=micros) + 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 From 73a5b2769c126b36cbce42edd0390f369058d6c2 Mon Sep 17 00:00:00 2001 From: andermi Date: Mon, 15 May 2023 14:23:11 -0700 Subject: [PATCH 13/21] Fix pbcmd terminate (#50) * add async proxy to await future for pack rate calls Signed-off-by: Michael Anderson * typo Signed-off-by: Michael Anderson --------- Signed-off-by: Michael Anderson --- buoy_api_py/buoy_api/interface.py | 28 ++++++++++++++++++++++++---- pbcmd/pbcmd/pbcmd.py | 4 ++-- 2 files changed, 26 insertions(+), 6 deletions(-) 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): From 1877a19b2112eaf1e98885f008531ad14db989ec Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Mon, 15 May 2023 20:43:50 -0700 Subject: [PATCH 14/21] line buffered csv Signed-off-by: Michael Anderson --- sim_pblog/sim_pblog/sim_pblog.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index 9312532..813bfb8 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -90,7 +90,7 @@ def logfile_setup(self): # 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') + 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 From e3061da4bbbcb6bd14d42346cb10f434ae9140cb Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Mon, 15 May 2023 21:04:42 -0700 Subject: [PATCH 15/21] linter Signed-off-by: Michael Anderson --- sim_pblog/sim_pblog/sim_pblog.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index 813bfb8..e44c742 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -90,7 +90,10 @@ def logfile_setup(self): # 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.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 From 5118cbf93f43c35601b367bc5fcee07da6908263 Mon Sep 17 00:00:00 2001 From: andermi Date: Tue, 16 May 2023 12:54:51 -0700 Subject: [PATCH 16/21] Update sim_pblog/sim_pblog/sim_pblog.py --- sim_pblog/sim_pblog/sim_pblog.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index e44c742..1a2e4c9 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -200,7 +200,7 @@ def logdir_setup(self, basename=None): # 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') + latest = os.path.join(self.loghome, 'latest_csv_dir') os.rename(templink, latest) self.get_logger().info(f'New log directory: {dirname}') From 53f45d5797e44dba56aaae9c3c3b187cf71d380d Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 16 May 2023 14:15:25 -0700 Subject: [PATCH 17/21] fix data loss with log rolling Signed-off-by: Michael Anderson --- sim_pblog/sim_pblog/sim_pblog.py | 36 ++++++++++++++++++++++++++------ 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index e44c742..794165a 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -14,9 +14,11 @@ # 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 get_context, cpu_count import os from buoy_api import Interface @@ -33,6 +35,25 @@ from tf_transformations import euler_from_quaternion +# Close the current log file and zip it +def close_zip_logfile(logfile, logfilename): + 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) + self.get_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 @@ -58,6 +79,7 @@ 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 @@ -78,6 +100,8 @@ def __init__(self, loghome, logdir=None): 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 @@ -85,7 +109,7 @@ def __init__(self, loghome, logdir=None): def logfile_setup(self): # close existing log file and zip it shut if (self.logfile is not None): - self.close_zip_logfile() + self.zip_pool.apply_async(close_zip_logfile, (self.logfile, self.logfilename,)) # 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' @@ -167,11 +191,11 @@ def close_zip_logfile(self): # 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) + # 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 From 35e3ad24b223ad3f673c66f4fad894bb0b0289d8 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 16 May 2023 14:52:20 -0700 Subject: [PATCH 18/21] bugfix Signed-off-by: Michael Anderson --- sim_pblog/sim_pblog/sim_pblog.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index 120cd84..e0b2f20 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -36,13 +36,13 @@ # Close the current log file and zip it -def close_zip_logfile(logfile, logfilename): +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) - self.get_logger().info(f'{logfilename} -> {logfilename}.gz') + logger.info(f'{logfilename} -> {logfilename}.gz') os.remove(logfilename) # Point a link called 'latest' to the new directory @@ -109,7 +109,7 @@ def __init__(self, loghome, logdir=None): 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.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' From 0b57d3916e81963d5f2184a7026d4ddbaf70111c Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 16 May 2023 14:59:05 -0700 Subject: [PATCH 19/21] linter Signed-off-by: Michael Anderson --- sim_pblog/sim_pblog/sim_pblog.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index e0b2f20..ac2125d 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -109,7 +109,10 @@ def __init__(self, loghome, logdir=None): 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(),)) + 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' From c48f52c328c4422d24c9c742481f6c8488ff6271 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 16 May 2023 15:21:42 -0700 Subject: [PATCH 20/21] linter Signed-off-by: Michael Anderson --- sim_pblog/sim_pblog/sim_pblog.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index ac2125d..68f317a 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -18,7 +18,7 @@ from datetime import datetime, timedelta import gzip import math -from multiprocessing import get_context, cpu_count +from multiprocessing import cpu_count, get_context import os from buoy_api import Interface From a20591c2660013ab7f430734b0951fc1311b4794 Mon Sep 17 00:00:00 2001 From: Rich Henthorn Date: Mon, 22 May 2023 16:43:47 -0700 Subject: [PATCH 21/21] Convert SC values from newtons and meters to lbs and inches --- sim_pblog/sim_pblog/sim_pblog.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/sim_pblog/sim_pblog/sim_pblog.py b/sim_pblog/sim_pblog/sim_pblog.py index 68f317a..338c0c0 100755 --- a/sim_pblog/sim_pblog/sim_pblog.py +++ b/sim_pblog/sim_pblog/sim_pblog.py @@ -61,6 +61,10 @@ def close_zip_logfile(logfile, logfilename, logger): 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, @@ -323,7 +327,9 @@ def write_sc_header(self): # SC record section def write_sc(self, data): if (type(data) is SCRecord): - self.logfile.write(f'{data.load_cell}, {data.range_finder:.2f}, ' + 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}, ')