Ros1 manager bt update (#121)

* Light dimmer (#105) * Light dimmer * Update * delete debug * add brake * fix * formated * battery_animation 3 stage * fix * add david suggestion * Update panther_lights/src/animation/battery_animation.py Co-authored-by: Dawid Kmak <[email protected]> --------- Co-authored-by: aayli <[email protected]> Co-authored-by: Dawid Kmak <[email protected]> * add battery health info (#102) * add health info * Update README.md * add logs * add voltage mean count * update republisher node * fix typo * Update panther_battery/src/adc_node.py Co-authored-by: Krzysztof Wojciechowski <[email protected]> * Update adc_node.py * Update roboteq_republisher_node.py * fix typo --------- Co-authored-by: Dawid Kmak <[email protected]> Co-authored-by: Krzysztof Wojciechowski <[email protected]> * Update changelog * 1.0.1 * fix bug (#112) * fix bug * Pawel sugestions --------- Co-authored-by: aayli <[email protected]> * Update changelog * 1.0.2 * Ros1 motor enable srv (#113) * add motors enable srv * add reseting can callback times * disable motors if script reset fails * working solution * fix * IO initial state pth1.05 * remove unneccessary method * fix type * update README * Update panther_power_control/README.md Co-authored-by: Paweł Kowalski <[email protected]> * review changes --------- Co-authored-by: Paweł Kowalski <[email protected]> * Update changelog * 1.0.3 * Thread lock (#110) * thread lock * undo fcn naming * few more lock * revert nodes * Paweł suggestions * change * e_stop --------- Co-authored-by: aayli <[email protected]> * Update changelog * 1.0.4 * APA102 cpp implementation (#108) * Use CPP for APA102 * Add color correction * Utilise gpiod * Remove :: * Fix lights after tests * Fix default pin number * Add set brightness service * Invert LED_SBC_SEL pin * Change SPI mode * Change clock polarity * Reduce number of ROS lgger instances * Celan up code, update to libgpiod 2.0 * Update panther_lights/include/panther_lights/apa102.hpp Co-authored-by: Dawid Kmak <[email protected]> * Update panther_lights/src/lights_driver_node.cpp Co-authored-by: Dawid Kmak <[email protected]> * Update panther_lights/src/lights_driver_node.cpp Co-authored-by: Dawid Kmak <[email protected]> * Update panther_lights/include/panther_lights/apa102.hpp Co-authored-by: Dawid Kmak <[email protected]> * Review changes * Update panther_lights/src/apa102.cpp Co-authored-by: Dawid Kmak <[email protected]> * Review cleanup * Add build dependencies * FIx launchfile * Update panther_lights/CMakeLists.txt Co-authored-by: Dawid Kmak <[email protected]> * Update panther_lights/CMakeLists.txt Co-authored-by: Dawid Kmak <[email protected]> * Update panther_lights/src/apa102.cpp Co-authored-by: Dawid Kmak <[email protected]> * Update panther_lights/src/driver_node.cpp Co-authored-by: Dawid Kmak <[email protected]> * Update panther_lights/src/driver_node.cpp Co-authored-by: Dawid Kmak <[email protected]> * Review changes * Fix inlude order * Fix references and APA102 constructor * Add build dependencies for libgpiod v2 * Bring back libgpiod v1 --------- Co-authored-by: Dawid Kmak <[email protected]> * Update changelog * 1.0.5 * Rviz lights plugin (#111) * basic plugin usage - POC * add rear animation texture * fix rviz config * fix .rviz file * Update changelog * 1.0.6 * check can err (#119) * Update changelog * 1.0.7 * update bt project and launch files * remove scheduler_node * fix queuing * remove old variable --------- Co-authored-by: rafal-gorecki <[email protected]> Co-authored-by: aayli <[email protected]> Co-authored-by: Paweł Kowalski <[email protected]> Co-authored-by: Krzysztof Wojciechowski <[email protected]> Co-authored-by: action-bot <[email protected]>
husarion · Jun 12, 2023 · 3a19fdb · 3a19fdb
1 parent 1edf08d
commit 3a19fdb
Show file tree

Hide file tree

Showing 51 changed files with 1,292 additions and 771 deletions.
diff --git a/panther/CHANGELOG.rst b/panther/CHANGELOG.rst
@@ -0,0 +1,24 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package panther
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.7 (2023-05-23)
+------------------
+
+1.0.6 (2023-05-18)
+------------------
+
+1.0.5 (2023-05-10)
+------------------
+
+1.0.4 (2023-04-26)
+------------------
+
+1.0.3 (2023-04-24)
+------------------
+
+1.0.2 (2023-04-24)
+------------------
+
+1.0.1 (2023-04-21)
+------------------
diff --git a/panther/package.xml b/panther/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>panther</name>
-  <version>1.0.0</version>
+  <version>1.0.7</version>
 
   <description>Meta package that contains all packages of Panther</description>
   <license>Apache License 2.0</license>

diff --git a/panther_battery/CHANGELOG.rst b/panther_battery/CHANGELOG.rst
@@ -0,0 +1,24 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package panther_battery
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.7 (2023-05-23)
+------------------
+
+1.0.6 (2023-05-18)
+------------------
+
+1.0.5 (2023-05-10)
+------------------
+
+1.0.4 (2023-04-26)
+------------------
+
+1.0.3 (2023-04-24)
+------------------
+
+1.0.2 (2023-04-24)
+------------------
+
+1.0.1 (2023-04-21)
+------------------
diff --git a/panther_battery/README.md b/panther_battery/README.md
@@ -29,3 +29,7 @@ Node publishing Panther battery state read from motor controllers. Used in Panth
 #### Subscribes
 
 - `/panther/driver/motor_controllers_state` [*panther_msgs/DriverState*]: current motor controllers' state and error flags.
+
+#### Parameters 
+
+- `~high_bat_temp` [*float*, default: **55.0**]: The temperature of the battery at which the battery health state is incorrect.
diff --git a/panther_battery/package.xml b/panther_battery/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>panther_battery</name>
-  <version>1.0.0</version>
+  <version>1.0.7</version>
 
   <description>Nodes monitoring the battery state of Husarion Panhter robot</description>
   <license>Apache License 2.0</license>

diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py
@@ -1,6 +1,11 @@
 #!/usr/bin/python3
 
+from threading import Lock
+
+from collections import defaultdict
 import math
+from threading import Lock
+from typing import Optional, Union
 
 import rospy
 
@@ -11,14 +16,22 @@
 
 class ADCNode:
     BAT02_DETECT_THRESH = 3.03
+    V_BAT_FATAL_MIN = 27.0
+    V_BAT_FATAL_MAX = 43.0
 
     def __init__(self, name: str) -> None:
         rospy.init_node(name, anonymous=False)
 
-        self._V_driv_front = None
-        self._V_driv_rear = None
-        self._I_driv_front = None
-        self._I_driv_rear = None
+        self._high_bat_temp = rospy.get_param('~high_bat_temp', 55.0)
+
+        self._driver_battery_last_info_time: Optional[float] = None
+        self._I_driv: Optional[float] = None
+        self._V_driv: Optional[float] = None
+
+        self._volt_mean_length = 10
+        self._V_bat_hist = defaultdict(lambda: [37.0] * self._volt_mean_length)
+        self._V_bat_mean = defaultdict(lambda: 37.0)
+        self._V_driv_mean: Optional[float] = None
 
         self._A = 298.15
         self._B = 3977.0
@@ -29,6 +42,8 @@ def __init__(self, name: str) -> None:
         self._battery_count = self._check_battery_count()
         self._battery_charging = True  # const for now, later this will be evaluated
 
+        self._lock = Lock()
+
         # -------------------------------
         #   Subscribers
         # -------------------------------
@@ -59,13 +74,18 @@ def __init__(self, name: str) -> None:
         rospy.loginfo(f'[{rospy.get_name()}] Node started')
 
     def _motor_controllers_state_cb(self, driver_state: DriverState) -> None:
-        self._V_driv_front = driver_state.front.voltage
-        self._V_driv_rear = driver_state.front.current
-        self._I_driv_front = driver_state.rear.voltage
-        self._I_driv_rear = driver_state.rear.current
+        with self._lock:
+            self._driver_battery_last_info_time = rospy.get_time()
+
+            driver_voltage = (driver_state.front.voltage + driver_state.rear.voltage) / 2.0
+            self._V_driv = driver_voltage
+            self._V_driv_mean = self._count_volt_mean('V_driv', driver_voltage)
+
+            self._I_driv = driver_state.front.current + driver_state.rear.current
 
     def _io_state_cb(self, io_state: IOState) -> None:
-        self._charger_connected = io_state.charger_connected
+        with self._lock:
+            self._charger_connected = io_state.charger_connected
 
     def _battery_timer_cb(self, *args) -> None:
         try:
@@ -175,19 +195,57 @@ def _publish_battery_msg(
         battery_msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LIPO
         battery_msg.present = True
 
+        V_bat_mean = self._count_volt_mean(bat_pub, V_bat)
+
         # check battery status
-        if self._charger_connected:
-            if battery_msg.percentage >= 1.0:
-                battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL
-            elif self._battery_charging:
-                battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
+        with self._lock:
+            if self._charger_connected:
+                if battery_msg.percentage >= 1.0:
+                    battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL
+                elif self._battery_charging:
+                    battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
+                else:
+                    battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
             else:
-                battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
-        else:
-            battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
+                battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
+
+        # check battery health
+        with self._lock:
+            error_msg = None
+            if V_bat_mean < self.V_BAT_FATAL_MIN:
+                battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_DEAD
+                error_msg = 'Battery voltage is critically low!'
+            elif V_bat_mean > self.V_BAT_FATAL_MAX:
+                battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERVOLTAGE
+                error_msg = 'Battery overvoltage!'
+            elif temp_bat >= self._high_bat_temp:
+                battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERHEAT
+                error_msg = 'Battery is overheating!'
+            elif self._driver_battery_last_info_time is None:
+                battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN
+            elif (
+                rospy.get_time() - self._driver_battery_last_info_time < 0.1
+                and abs(V_bat_mean - self._V_driv_mean) > self.V_BAT_FATAL_MAX * 0.1
+            ):
+                battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNSPEC_FAILURE
+            else:
+                battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD
+
+        if error_msg is not None:
+            rospy.logerr_throttle_identical(10.0, f'[{rospy.get_name()}] {error_msg}')
 
         bat_pub.publish(battery_msg)
 
+    def _count_volt_mean(self, label: Union[rospy.Publisher, str], new_val: float) -> float:
+        # Updates the average by adding the newest and removing the oldest component of mean value,
+        # in order to avoid recalculating the entire sum every time.
+        self._V_bat_mean[label] += (new_val - self._V_bat_hist[label][0]) / self._volt_mean_length
+
+        self._V_bat_hist[label].pop(0)
+        self._V_bat_hist[label].append(new_val)
+
+        return self._V_bat_mean[label]
+
     @staticmethod
     def _read_file(path: str) -> int:
         with open(path, 'r') as file:

diff --git a/panther_battery/src/roboteq_republisher_node.py b/panther_battery/src/roboteq_republisher_node.py
@@ -1,6 +1,7 @@
 #!/usr/bin/python3
 
 from threading import Lock
+from typing import Optional
 
 import rospy
 
@@ -10,14 +11,23 @@
 
 
 class RoboteqRepublisherNode:
+    V_BAT_FATAL_MIN = 27.0
+    V_BAT_FATAL_MAX = 43.0
+
     def __init__(self, name: str) -> None:
         rospy.init_node(name, anonymous=False)
 
-        self._battery_voltage = None
-        self._battery_current = None
+        self._lock = Lock()
+
+        self._battery_voltage: Optional[float] = None
+        self._battery_current: Optional[float] = None
+        self._battery_voltage_mean = 37.0
+
+        self._volt_mean_length = 10
+        self._battery_voltage_hist = [37.0] * self._volt_mean_length
+
         self._battery_timeout = 1.0
         self._last_battery_info_time = rospy.get_time()
-        self._lock = Lock()
 
         # -------------------------------
         #   Subscribers
@@ -44,9 +54,16 @@ def __init__(self, name: str) -> None:
 
     def _motor_controllers_state_cb(self, msg: DriverState) -> None:
         with self._lock:
+            # don't update battery data if can communication error occurred
+            if msg.front.fault_flag.can_net_err or msg.rear.fault_flag.can_net_err:
+                return
+
+            new_voltage = (msg.front.voltage + msg.rear.voltage) / 2.0
+
             self._last_battery_info_time = rospy.get_time()
-            self._battery_voltage = (msg.front.voltage + msg.rear.voltage) / 2.0
+            self._battery_voltage = new_voltage
             self._battery_current = msg.front.current + msg.rear.current
+            self._update_volt_mean(new_voltage)
 
     def _battery_pub_timer_cb(self, *args) -> None:
         with self._lock:
@@ -55,25 +72,48 @@ def _battery_pub_timer_cb(self, *args) -> None:
             battery_msg.capacity = 20.0
             battery_msg.design_capacity = 20.0
             battery_msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LIPO
+
             if (
                 self._battery_voltage == None
                 or self._battery_current == None
                 or rospy.get_time() - self._last_battery_info_time > self._battery_timeout
             ):
                 battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_UNKNOWN
             else:
-                battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
                 battery_msg.voltage = self._battery_voltage
                 battery_msg.temperature = float('nan')
                 battery_msg.current = self._battery_current
                 battery_msg.percentage = (battery_msg.voltage - 32.0) / 10.0
                 battery_msg.charge = battery_msg.percentage * battery_msg.design_capacity
                 battery_msg.present = True
-                # TODO:
-                # battery_msg.power_supply_health
+
+                # TODO: check battery status
+                battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
+
+                # check battery health
+                error_msg = None
+                if self._battery_voltage < self.V_BAT_FATAL_MIN:
+                    battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_DEAD
+                    error_msg = 'Battery voltage is critically low!'
+                elif self._battery_voltage > self.V_BAT_FATAL_MAX:
+                    battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERVOLTAGE
+                    error_msg = 'Battery overvoltage!'
+                else:
+                    battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD
+
+                if error_msg is not None:
+                    rospy.logerr_throttle_identical(10.0, f'[{rospy.get_name()}] {error_msg}')
 
             self._battery_pub.publish(battery_msg)
 
+    def _update_volt_mean(self, new_val: float) -> float:
+        # Updates the average by adding the newest and removing the oldest component of mean value,
+        # in order to avoid recalculating the entire sum every time.
+        self._battery_voltage_mean += (new_val - self._battery_voltage_hist[0]) / self._volt_mean_length
+
+        self._battery_voltage_hist.pop(0)
+        self._battery_voltage_hist.append(new_val)
+
 
 def main():
     roboteq_republisher_node = RoboteqRepublisherNode('roboteq_republisher_node')

diff --git a/panther_bringup/CHANGELOG.rst b/panther_bringup/CHANGELOG.rst
@@ -0,0 +1,24 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package panther_bringup
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.7 (2023-05-23)
+------------------
+
+1.0.6 (2023-05-18)
+------------------
+
+1.0.5 (2023-05-10)
+------------------
+
+1.0.4 (2023-04-26)
+------------------
+
+1.0.3 (2023-04-24)
+------------------
+
+1.0.2 (2023-04-24)
+------------------
+
+1.0.1 (2023-04-21)
+------------------
diff --git a/panther_bringup/launch/bringup.launch b/panther_bringup/launch/bringup.launch
@@ -6,7 +6,8 @@
   <arg name="panther_common_config_file" default="$(find panther_bringup)/config/panther_common.yaml" />
   <arg name="test_animations" default="false" />
   <arg name="user_animations_file" default="" />
-  <arg name="launch_lights_scheduler" default="true" />
+  <arg name="bt_project_file" default="$(find panther_manager)/config/Panther12BT.btproj" if="$(eval panther_version >= 1.2)" />
+  <arg name="bt_project_file" default="$(find panther_manager)/config/Panther106BT.btproj" unless="$(eval panther_version >= 1.2)" />
   <arg name="shutdown_hosts_config_file" default="$(find panther_bringup)/config/shutdown_hosts.yaml" />
   <arg name="disable_manager" default="false" doc="used for testing purposes" />
 
@@ -20,7 +21,7 @@
 
   <arg name="robot_description" if="$(arg publish_robot_state)"
     default="$(find xacro)/xacro $(find panther_description)/urdf/panther.urdf.xacro
-              wheel_config_path:=$(find panther_description)/config/$(arg wheel_type).yaml
+              wheel_config_path:=$(arg wheel_config_file)
               imu_pos_x:=$(optenv PANTHER_IMU_LOCALIZATION_X 0.168)
               imu_pos_y:=$(optenv PANTHER_IMU_LOCALIZATION_Y 0.028)
               imu_pos_z:=$(optenv PANTHER_IMU_LOCALIZATION_Z 0.083)
@@ -42,7 +43,6 @@
     </include>
 
     <include file="$(find panther_lights)/launch/lights.launch">
-      <arg name="launch_scheduler" value="$(arg launch_lights_scheduler)" />
       <arg name="test" value="$(arg test_animations)" />
       <arg name="user_animations_file" value="$(arg user_animations_file)" />
     </include>
@@ -57,6 +57,7 @@
 
     <include file="$(find panther_manager)/launch/manager_bt.launch" unless="$(arg disable_manager)">
       <arg name="panther_version" value="$(arg panther_version)" />
+      <arg name="bt_project_file" value="$(arg bt_project_file)" />
       <arg name="shutdown_hosts_config_file" value="$(arg shutdown_hosts_config_file)" />
     </include>
 

diff --git a/panther_bringup/package.xml b/panther_bringup/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>panther_bringup</name>
-  <version>1.0.0</version>
+  <version>1.0.7</version>
 
   <description>Default launch files and configuration used to start Husarion Panther robot</description>
   <license>Apache License 2.0</license>