jsk_pr2_startup: battery_debug

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import commands
00005 import os.path
00006 from pr2_msgs.msg import BatteryServer
00007 
00008 class BatteryLog:
00009     def __init__(self):
00010         rospy.Subscriber("/battery/server", BatteryServer, self.battery_server_cb)
00011         self.store_path = "/var/ros/battery_debug_log/battery_debug.log"
00012         self.sys_uptime = int(float(commands.getoutput("cat /proc/uptime |awk '{print $1}'")))
00013         self.date_in_sec = int(commands.getoutput("date +\"%s\""))
00014         self.sys_started_date = self.date_in_sec - self.sys_uptime
00015         self.log = ["\n","\n","\n","\n"]
00016         self.appending = True
00017         if not os.path.isfile(self.store_path):
00018             with open(self.store_path, "w") as f:
00019                 f.write("system started date(s)|date|ID|Rel.(%)|Abs.(%)|Voltage(mV)|Temperture(C)|\n\n\n\n\n")
00020                 f.close()
00021         if (self.sys_started_date - self.read_sys_started_date() <= 60 * 1): # 1mins, consider to be same boot
00022             self.appending =False
00023 
00024 
00025     def battery_server_cb(self, msg):
00026         bats = msg.battery
00027         charge_list = [bat.batReg[14] for bat in bats]
00028         min_abs_charge = reduce(min, charge_list)
00029         sub_id = charge_list.index(min_abs_charge)
00030         bat_id = (msg.id + sub_id / 10.0)
00031         rel_charge = bats[sub_id].batReg[13]
00032         voltage = bats[sub_id].batReg[9]
00033         temp = bats[sub_id].batReg[8] * 0.1 - 273.15
00034         
00035         self.log[msg.id] = "{0} {1} {2} {3} {4} {5} {6}\n".format(self.sys_started_date, commands.getoutput("date"), bat_id, rel_charge, min_abs_charge,  voltage, temp)
00036         rospy.loginfo(self.log[msg.id])
00037         if self.appending:
00038             self.append_data()
00039         else:
00040             self.replace_data()
00041 
00042     def read_sys_started_date(self):
00043         with open(self.store_path, 'r') as f:
00044             lst = f.readlines()
00045             try:
00046                 return int(float(str.split(lst[-1])[0]))
00047             except Exception as e:
00048                 rospy.logwarn("faild to read system started date from file: %s" % e)
00049                 return self.sys_started_date
00050 
00051     def replace_data(self):
00052         f = open(self.store_path, 'r')
00053         lines = f.readlines()
00054         f.close()
00055         f= open(self.store_path, 'w')
00056         f.writelines([l for l in lines[:-4]])
00057         for s in self.log:
00058             f.write(s)
00059         f.close()
00060 
00061     def append_data(self):
00062         self.appending = False
00063         with open(self.store_path, 'a') as f:
00064             f.write("\nsystem started date(s)|date|ID|Rel.(%)|Abs.(%)|Voltage(mV)|Temperture(C)|\n")
00065             for s in self.log:
00066                 f.write(s)
00067 
00068 
00069 
00070 if __name__ == "__main__":
00071     rospy.init_node('battery_log')
00072     obj = BatteryLog()
00073     rospy.spin()
00074