First pass timeseries analysis based battery state detection

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

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