Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 import rospy
00019 import numpy
00020 from std_msgs.msg import Float64
00021 from cob_msgs.msg import PowerState
00022
00023 class PowerStateAggregator():
00024
00025 def __init__(self):
00026
00027 self.current_buffer_size = rospy.get_param('~current_buffer_size', 10)
00028 self.pub_power_state = rospy.Publisher('power_state', PowerState, queue_size=1)
00029 self.voltage = None
00030 self.current = None
00031 self.last_currents = []
00032 self.last_update = rospy.Time(0)
00033 self.charging = False
00034 self.remaining_capacity = None
00035 self.full_charge_capacity = None
00036 self.temperature = None
00037 rospy.Subscriber("voltage", Float64, self.voltage_cb)
00038 rospy.Subscriber("current", Float64, self.current_cb)
00039 rospy.Subscriber("remaining_capacity", Float64, self.remaining_capacity_cb)
00040 rospy.Subscriber("full_charge_capacity", Float64, self.full_charge_capacity_cb)
00041 rospy.Subscriber("temperature", Float64, self.temperature_cb)
00042
00043 def voltage_cb(self, msg):
00044 self.last_update = rospy.Time.now()
00045 self.voltage = msg.data
00046
00047 def current_cb(self, msg):
00048 self.last_update = rospy.Time.now()
00049 self.current = msg.data
00050
00051
00052 if len(self.last_currents) >= self.current_buffer_size:
00053 self.last_currents.pop(0)
00054 self.last_currents.append(msg.data)
00055
00056 if msg.data > -1:
00057 self.charging = True
00058 else:
00059 self.charging = False
00060
00061 def remaining_capacity_cb(self, msg):
00062 self.last_update = rospy.Time.now()
00063 self.remaining_capacity = msg.data
00064
00065 def full_charge_capacity_cb(self, msg):
00066 self.last_update = rospy.Time.now()
00067 self.full_charge_capacity = msg.data
00068
00069 def temperature_cb(self, msg):
00070 self.last_update = rospy.Time.now()
00071 self.temperature = msg.data
00072
00073 def calculate_power_consumption(self):
00074 if not self.charging and self.voltage != None and self.current != None:
00075 return round(self.voltage * abs(self.current), 3)
00076 else:
00077 return 0.0
00078
00079 def calculate_relative_remaining_capacity(self):
00080 try:
00081 return round(100.0*(self.remaining_capacity/self.full_charge_capacity), 3)
00082 except ZeroDivisionError as e:
00083 rospy.logerr("ZeroDivisionError: full_charge_capacity is 0.0: %s" % (e))
00084 except:
00085 rospy.logwarn("something went wrong, cannot calculate relative remaining capacity. full_charge_capacity=%s, remaining_capacity=%s" % (self.full_charge_capacity, self.remaining_capacity))
00086 return 0.0
00087
00088 def calculate_time_remaining(self):
00089 time_remaining_max = 10.0
00090 time_remaining = time_remaining_max
00091 if len(self.last_currents) > 0:
00092 current = numpy.mean(self.last_currents)
00093
00094 if self.full_charge_capacity != None and self.remaining_capacity != None:
00095 try:
00096 if self.charging:
00097 time_remaining = round((self.full_charge_capacity - self.remaining_capacity) / abs(current), 3)
00098 else:
00099 time_remaining = round(self.remaining_capacity / abs(current), 3)
00100 except ZeroDivisionError as e:
00101 rospy.logerr("ZeroDivisionError: current is 0.0: %s" % (e))
00102 except:
00103 rospy.logwarn("something went wrong, cannot calculate time_remaining. full_charge_capacity=%s, remaining_capacity=%s, current=%s" % (self.full_charge_capacity, self.remaining_capacity, current))
00104 else:
00105 pass
00106 else:
00107 pass
00108 return min(time_remaining, time_remaining_max)
00109
00110 def publish(self):
00111 if self.voltage != None and self.current != None and self.remaining_capacity != None and self.full_charge_capacity != None and self.temperature != None and (rospy.Time.now() - self.last_update) < rospy.Duration(1):
00112 ps = PowerState()
00113 ps.header.stamp = self.last_update
00114 ps.voltage = self.voltage
00115 ps.current = self.current
00116 ps.power_consumption = self.calculate_power_consumption()
00117 ps.remaining_capacity = self.remaining_capacity
00118 ps.relative_remaining_capacity = self.calculate_relative_remaining_capacity()
00119 ps.charging = self.charging
00120 ps.time_remaining = self.calculate_time_remaining()
00121 ps.temperature = self.temperature
00122 self.pub_power_state.publish(ps)
00123 return ps
00124
00125 if __name__ == "__main__":
00126 rospy.init_node("power_state_aggregator")
00127 PSA = PowerStateAggregator()
00128 rospy.loginfo("power state aggregator running")
00129 rate = rospy.Rate(10)
00130 while not rospy.is_shutdown():
00131 PSA.publish()
00132 try:
00133 rate.sleep()
00134 except rospy.exceptions.ROSInterruptException as e:
00135 pass
00136