From 5b9a17840ed02bb73696c87b5b898c9080281d72 Mon Sep 17 00:00:00 2001 From: Mohamed Fazil Date: Thu, 23 Feb 2023 14:11:10 -0800 Subject: [PATCH] First pass timeseries analysis based battery state detection --- hello_helpers/src/hello_helpers/hello_misc.py | 34 +++++++++++++++++++ stretch_core/nodes/stretch_driver | 28 +++++++++------ 2 files changed, 52 insertions(+), 10 deletions(-) diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index 50f79c8..c0a0777 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -272,3 +272,37 @@ def bound_ros_command(bounds, ros_pos, fail_out_of_range_goal, clip_ros_toleranc return bounds[1] return ros_pos + + +def detect_sensor_shift(data, shift_period, d_shift_thresh=0.15): + """ + Detects the timeseries trend of a given sensor values with timestamps if the change in trend + happens within the given shift period. + + Args + data: list of tuple of timestamp and sensor value + shift_period: Estimated shift period(s) + d_shift_thresh: Shift threshold of sensor value + returns + 0 No shift detected + 1 Upward shift + -1 downward shift + """ + try: + total_ts = data[-1][0] - data[0][0] + tsp = total_ts / len(data) + window_size = int(shift_period / tsp) + d = np.array([v[1] for v in data]) + for i in range(len(data)): + try: + curr_window = d[i:i + window_size] + if abs(curr_window[0] - curr_window[-1]) > d_shift_thresh: + if curr_window[0] < curr_window[-1]: + return 1 + else: + return -1 + except Exception as e: + pass + except ZeroDivisionError as e: + pass + return 0 \ No newline at end of file diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 14bdc26..85c5346 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -1,5 +1,7 @@ #! /usr/bin/env python3 +import time +import click import yaml import copy import numpy as np @@ -30,6 +32,8 @@ from std_msgs.msg import Bool, Header, String from joint_trajectory_server import JointTrajectoryAction from stretch_diagnostics import StretchDiagnostics +from hello_helpers.hello_misc import detect_sensor_shift + class StretchDriverNode: @@ -44,7 +48,7 @@ class StretchDriverNode: self.robot_mode_rwlock = RWLock() self.robot_mode = None - self.voltage_history = [] + self.voltage_history_ts = [] self.charging_state_history = [BatteryState.POWER_SUPPLY_STATUS_UNKNOWN] * 10 self.charging_state = BatteryState.POWER_SUPPLY_STATUS_UNKNOWN @@ -127,17 +131,21 @@ class StretchDriverNode: pimu_hardware_id = self.robot.pimu.board_info['hardware_id'] invalid_reading = float('NaN') v = float(robot_status['pimu']['voltage']) - self.voltage_history.append(v) - if len(self.voltage_history) > 100: - self.voltage_history.pop(0) + self.voltage_history_ts.append((time.time(), v)) + if len(self.voltage_history_ts) > 200: + self.voltage_history_ts.pop(0) self.charging_state_history.pop(0) - if v > np.mean(self.voltage_history) + 3 * np.std(self.voltage_history): - self.charging_state_history.append(BatteryState.POWER_SUPPLY_STATUS_CHARGING) - elif v < np.mean(self.voltage_history) - 3 * np.std(self.voltage_history): - self.charging_state_history.append(BatteryState.POWER_SUPPLY_STATUS_DISCHARGING) - else: - self.charging_state_history.append(BatteryState.POWER_SUPPLY_STATUS_UNKNOWN) + + charging_trend = detect_sensor_shift(self.voltage_history_ts, shift_period=6) + if charging_trend==1: + self.charging_state_history.append(BatteryState.POWER_SUPPLY_STATUS_CHARGING) + elif charging_trend==-1: + self.charging_state_history.append(BatteryState.POWER_SUPPLY_STATUS_DISCHARGING) + else: + self.charging_state_history.append(BatteryState.POWER_SUPPLY_STATUS_UNKNOWN) + filtered_charging_state = max(set(self.charging_state_history), key=self.charging_state_history.count) + if filtered_charging_state != BatteryState.POWER_SUPPLY_STATUS_UNKNOWN: if pimu_hardware_id == 0: self.charging_state = filtered_charging_state