laptop_battery.py 文件源码

def update(self):
        with self._mutex:
            diag = DiagnosticArray()
            diag.header.stamp = rospy.get_rostime()

            info_update_ok  = rospy.get_time() - self._last_info_update  < 5.0 / self._batt_info_rate
            state_update_ok = rospy.get_time() - self._last_state_update < 5.0 / self._batt_state_rate

            if info_update_ok:
                self._msg.design_capacity = self._batt_design_capacity
                self._msg.capacity        = self._batt_last_full_capacity
            else:
                self._msg.design_capacity = 0.0
                self._msg.capacity        = 0.0

            if info_update_ok and state_update_ok and self._msg.capacity != 0:
                self._msg.percentage = int(self._msg.charge / self._msg.capacity * 100.0)

            diag_stat = _laptop_charge_to_diag(self._msg)
            if not info_update_ok or not state_update_ok:
                diag_stat.level   = DiagnosticStatus.ERROR
                diag_stat.message = 'Laptop battery data stale'

            diag.status.append(diag_stat)

            self._diag_pub.publish(diag)
            self._power_pub.publish(self._msg)