From b50d9345030b58f3995fe6c0eabb4482d76b92fe Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Wed, 10 Apr 2024 09:59:54 +0200 Subject: [PATCH] refactor(sensors_monitor): ros2 port (#8) --- diagnostic_common_diagnostics/CMakeLists.txt | 1 + diagnostic_common_diagnostics/README.md | 15 +- .../diagnostic_common_diagnostics/ruff.toml | 2 + .../sensors_monitor.py | 252 ++++++++++++++++++ diagnostic_common_diagnostics/package.xml | 1 + 5 files changed, 270 insertions(+), 1 deletion(-) create mode 100644 diagnostic_common_diagnostics/diagnostic_common_diagnostics/ruff.toml create mode 100755 diagnostic_common_diagnostics/diagnostic_common_diagnostics/sensors_monitor.py diff --git a/diagnostic_common_diagnostics/CMakeLists.txt b/diagnostic_common_diagnostics/CMakeLists.txt index 42ad3e4e..6a337a4c 100644 --- a/diagnostic_common_diagnostics/CMakeLists.txt +++ b/diagnostic_common_diagnostics/CMakeLists.txt @@ -11,6 +11,7 @@ install(PROGRAMS ${PROJECT_NAME}/cpu_monitor.py ${PROJECT_NAME}/ntp_monitor.py ${PROJECT_NAME}/ram_monitor.py + ${PROJECT_NAME}/sensors_monitor.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/diagnostic_common_diagnostics/README.md b/diagnostic_common_diagnostics/README.md index e0d75ab1..007c943d 100644 --- a/diagnostic_common_diagnostics/README.md +++ b/diagnostic_common_diagnostics/README.md @@ -79,7 +79,20 @@ warning percentage threshold. Length of RAM readings queue. ## sensors_monitor.py -**To be ported** +The `sensors_monitor` module allows users to monitor the temperature, volt and fan speeds of their system in real-time. +It uses the [`LM Sensors` package](https://packages.debian.org/sid/utils/lm-sensors) to get the data. + +* Name of the node is "sensors_monitor_" + hostname. + +### Published Topics +#### /diagnostics +diagnostic_msgs/DiagnosticArray +The diagnostics information. + +### Parameters +#### ignore_fans +(default: false) +Whether to ignore the fan speed. ## tf_monitor.py **To be ported** diff --git a/diagnostic_common_diagnostics/diagnostic_common_diagnostics/ruff.toml b/diagnostic_common_diagnostics/diagnostic_common_diagnostics/ruff.toml new file mode 100644 index 00000000..749ef043 --- /dev/null +++ b/diagnostic_common_diagnostics/diagnostic_common_diagnostics/ruff.toml @@ -0,0 +1,2 @@ +[format] +quote-style = "single" diff --git a/diagnostic_common_diagnostics/diagnostic_common_diagnostics/sensors_monitor.py b/diagnostic_common_diagnostics/diagnostic_common_diagnostics/sensors_monitor.py new file mode 100755 index 00000000..1e4c63fe --- /dev/null +++ b/diagnostic_common_diagnostics/diagnostic_common_diagnostics/sensors_monitor.py @@ -0,0 +1,252 @@ +#!/usr/bin/env python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from __future__ import division, with_statement + +from io import StringIO +import math +import re +import socket +import subprocess + +from diagnostic_msgs.msg import DiagnosticStatus + +import diagnostic_updater as DIAG + +import rclpy +from rclpy.node import Node + + +class Sensor(object): + + def __init__(self): + self.critical = None + self.min = None + self.max = None + self.input = None + self.name = None + self.type = None + self.high = None + self.alarm = None + + def __repr__(self): + return 'Sensor object (name: {}, type: {})'.format(self.name, self.type) + + def getCrit(self): + return self.critical + + def getMin(self): + return self.min + + def getMax(self): + return self.max + + def getInput(self): + return self.input + + def getName(self): + return self.name + + def getType(self): + return self.type + + def getHigh(self): + return self.high + + def getAlarm(self): + return self.alarm + + def __str__(self): + lines = [] + lines.append(str(self.name)) + lines.append('\t' + 'Type: ' + str(self.type)) + if self.input: + lines.append('\t' + 'Input: ' + str(self.input)) + if self.min: + lines.append('\t' + 'Min: ' + str(self.min)) + if self.max: + lines.append('\t' + 'Max: ' + str(self.max)) + if self.high: + lines.append('\t' + 'High: ' + str(self.high)) + if self.critical: + lines.append('\t' + 'Crit: ' + str(self.critical)) + lines.append('\t' + 'Alarm: ' + str(self.alarm)) + return '\n'.join(lines) + + +def parse_sensor_line(line): + sensor = Sensor() + line = line.lstrip() + [name, reading] = line.split(':') + + try: + [sensor.name, sensor.type] = name.rsplit(' ', 1) + except ValueError: + return None + + if sensor.name == 'Core': + sensor.name = name + sensor.type = 'Temperature' + elif sensor.name.find('Physical id') != -1: + sensor.name = name + sensor.type = 'Temperature' + + try: + [reading, params] = reading.lstrip().split('(') + except ValueError: + return None + + sensor.alarm = False + if line.find('ALARM') != -1: + sensor.alarm = True + + if reading.find('°C') == -1: + sensor.input = float(reading.split()[0]) + else: + sensor.input = float(reading.split('°C')[0]) + + params = params.split(',') + for param in params: + m = re.search('[0-9]+.[0-9]*', param) + if param.find('min') != -1: + sensor.min = float(m.group(0)) + elif param.find('max') != -1: + sensor.max = float(m.group(0)) + elif param.find('high') != -1: + sensor.high = float(m.group(0)) + elif param.find('crit') != -1: + sensor.critical = float(m.group(0)) + + return sensor + + +def _rads_to_rpm(rads): + return rads / (2 * math.pi) * 60 + + +def _rpm_to_rads(rpm): + return rpm * (2 * math.pi) / 60 + + +def parse_sensors_output(node: Node, output): + out = StringIO(output if isinstance(output, str) else output.decode('utf-8')) + + sensorList = [] + for line in out.readlines(): + # Check for a colon + if ':' in line and 'Adapter' not in line: + s = None + try: + s = parse_sensor_line(line) + except Exception as exc: + node.get_logger().warn( + 'Unable to parse line "%s", due to %s', line, exc + ) + if s is not None: + sensorList.append(s) + return sensorList + + +def get_sensors(): + p = subprocess.Popen( + 'sensors', stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True + ) + (o, e) = p.communicate() + if not p.returncode == 0: + return '' + if not o: + return '' + return o + + +class SensorsMonitor(object): + + def __init__(self, node: Node, hostname): + self.node = node + self.hostname = hostname + self.ignore_fans = node.declare_parameter('ignore_fans', False).value + node.get_logger().info('Ignore fanspeed warnings: %s' % self.ignore_fans) + + self.updater = DIAG.Updater(node) + self.updater.setHardwareID('none') + self.updater.add('%s Sensor Status' % self.hostname, self.monitor) + + def monitor(self, stat): + try: + stat.summary(DiagnosticStatus.OK, 'OK') + for sensor in parse_sensors_output(self.node, get_sensors()): + if sensor.getType() == 'Temperature': + if sensor.getInput() > sensor.getCrit(): + stat.mergeSummary( + DiagnosticStatus.ERROR, 'Critical Temperature' + ) + elif sensor.getInput() > sensor.getHigh(): + stat.mergeSummary(DiagnosticStatus.WARN, 'High Temperature') + stat.add( + ' '.join([sensor.getName(), sensor.getType()]), + str(sensor.getInput()), + ) + elif sensor.getType() == 'Voltage': + if sensor.getInput() < sensor.getMin(): + stat.mergeSummary(DiagnosticStatus.ERROR, 'Low Voltage') + elif sensor.getInput() > sensor.getMax(): + stat.mergeSummary(DiagnosticStatus.ERROR, 'High Voltage') + stat.add( + ' '.join([sensor.getName(), sensor.getType()]), + str(sensor.getInput()), + ) + elif sensor.getType() == 'Speed': + if not self.ignore_fans: + if sensor.getInput() < sensor.getMin(): + stat.mergeSummary(DiagnosticStatus.ERROR, 'No Fan Speed') + stat.add( + ' '.join([sensor.getName(), sensor.getType()]), + str(sensor.getInput()), + ) + except Exception: + import traceback + + self.node.get_logger().error('Unable to process lm-sensors data') + self.node.get_logger().error(traceback.format_exc()) + return stat + + +if __name__ == '__main__': + rclpy.init() + hostname = socket.gethostname() + hostname_clean = hostname.translate(hostname.maketrans('-', '_')) + node = rclpy.create_node('sensors_monitor_%s' % hostname_clean) + + monitor = SensorsMonitor(node, hostname) + rclpy.spin(node) diff --git a/diagnostic_common_diagnostics/package.xml b/diagnostic_common_diagnostics/package.xml index 78d83793..e4585efc 100644 --- a/diagnostic_common_diagnostics/package.xml +++ b/diagnostic_common_diagnostics/package.xml @@ -19,6 +19,7 @@ ament_cmake_python diagnostic_updater + lm-sensors python3-ntplib python3-psutil rclpy