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 from math import *
00019 import numpy as np
00020
00021 import rospy
00022 import savitzky
00023 from std_msgs.msg import Float64
00024 from cob_msgs.msg import EmergencyStopState, PowerState
00025
00026 class volts_filter():
00027
00028 def __init__(self):
00029 self.volts = 0.
00030 self.wsize = 61
00031 self.filter_order = 3
00032 self.theta = rospy.get_param("~theta")
00033 self.off_y = rospy.get_param("~off_y")
00034 self.abcd = rospy.get_param("~abcd")
00035 self.maximum_time = rospy.get_param("~maximum_time")
00036 self.sg = savitzky.savitzky_golay(window_size=self.wsize, order=self.filter_order)
00037 size = 2*self.wsize+1
00038 self.volt_filt = 48000*np.ones(size)
00039
00040 rospy.Subscriber("/power_board/voltage", Float64, self.callback)
00041
00042 self.pub_power = rospy.Publisher('/power_state', PowerState, queue_size=1)
00043 self.msg_power = PowerState()
00044
00045 def callback(self, data):
00046
00047 self.volts = data.data
00048 self.volts = self.volts*1000
00049
00050 if(self.volts <= 44000):
00051 self.volts = 44000
00052 time_r = 0.
00053 elif(self.volts >= 48000):
00054 self.volts = 48000
00055
00056 self.process_voltage()
00057
00058 def process_voltage(self):
00059
00060 self.volt_filt = np.insert(self.volt_filt, 0, self.volts)
00061 self.volt_filt = np.delete(self.volt_filt, -1)
00062
00063 vfilt = self.sg.filter(self.volt_filt)
00064
00065 old_settings = np.seterr(all='raise')
00066
00067 self.t_est = np.polyval(self.abcd, vfilt[self.wsize])
00068
00069 self.t_est = vfilt[self.wsize]*sin(self.theta) + self.t_est*cos(self.theta)
00070 self.t_est = self.t_est + self.off_y
00071
00072 if(self.t_est <0):
00073 self.t_est = 0
00074
00075 self.msg_power.header.stamp = rospy.Time.now()
00076 self.msg_power.time_remaining.secs = self.t_est
00077 self.msg_power.prediction_method = '3rd_order_polynom'
00078 self.msg_power.relative_capacity = (self.t_est/self.maximum_time) * 100
00079 self.msg_power.AC_present = 0
00080
00081 self.pub_power.publish(self.msg_power)
00082
00083 if __name__ == '__main__':
00084 rospy.init_node('volt_filt')
00085 vf = volts_filter()
00086
00087 while not rospy.is_shutdown():
00088 rospy.sleep(1.0)