check can err

husarion · May 22, 2023 · 3f404b7 · 3f404b7
1 parent 07315ae
commit 3f404b7
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/panther_battery/src/roboteq_republisher_node.py b/panther_battery/src/roboteq_republisher_node.py
@@ -54,6 +54,10 @@ 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()