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
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059 import roslib; roslib.load_manifest('cob_relayboard')
00060 import rospy
00061 import time
00062 import csv
00063 from cob_relayboard.msg import EmergencyStopState
00064 from std_msgs.msg import Float64
00065 import savitzky
00066 import numpy as np
00067 from math import *
00068 from pr2_msgs.msg import PowerState
00069
00070 class volts_filter():
00071
00072 def __init__(self):
00073 self.volts = 0.
00074 self.wsize = 61
00075 self.filter_order = 3
00076 self.theta = rospy.get_param("~theta")
00077 self.off_y = rospy.get_param("~off_y")
00078 self.abcd = rospy.get_param("~abcd")
00079 self.maximum_time = rospy.get_param("~maximum_time")
00080 self.sg = savitzky.savitzky_golay(window_size=self.wsize, order=self.filter_order)
00081 size = 2*self.wsize+1
00082 self.volt_filt = 48000*np.ones(size)
00083
00084 rospy.Subscriber("/power_board/voltage", Float64, self.callback)
00085
00086 self.pub_power = rospy.Publisher('/power_state', PowerState)
00087 self.msg_power = PowerState()
00088
00089 def callback(self, data):
00090
00091 self.volts = data.data
00092 self.volts = self.volts*1000
00093
00094 if(self.volts <= 44000):
00095 self.volts = 44000
00096 time_r = 0.
00097 elif(self.volts >= 48000):
00098 self.volts = 48000
00099
00100 self.process_voltage()
00101
00102 def process_voltage(self):
00103
00104 self.volt_filt = np.insert(self.volt_filt, 0, self.volts)
00105 self.volt_filt = np.delete(self.volt_filt, -1)
00106
00107 vfilt = self.sg.filter(self.volt_filt)
00108
00109 old_settings = np.seterr(all='raise')
00110
00111 self.t_est = np.polyval(self.abcd, vfilt[self.wsize])
00112
00113 self.t_est = vfilt[self.wsize]*sin(self.theta) + self.t_est*cos(self.theta)
00114 self.t_est = self.t_est + self.off_y
00115
00116 if(self.t_est <0):
00117 self.t_est = 0
00118
00119 self.msg_power.header.stamp = rospy.Time.now()
00120 self.msg_power.time_remaining.secs = self.t_est
00121 self.msg_power.prediction_method = '3rd_order_polynom'
00122 self.msg_power.relative_capacity = (self.t_est/self.maximum_time) * 100
00123 self.msg_power.AC_present = 0
00124
00125 self.pub_power.publish(self.msg_power)
00126
00127 if __name__ == '__main__':
00128 rospy.init_node('volt_filt')
00129 vf = volts_filter()
00130
00131 while not rospy.is_shutdown():
00132
00133
00134 rospy.sleep(1.0)
00135
00136
00137