time_volt.py

Go to the documentation of this file.

#!/usr/bin/env python
#
# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#   http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.


import pickle
import csv
import pylab
import sys
import getopt

from math import *
from scipy import *
from numpy import *
import numpy as np

import rospy
import rosparam
import yaml
import savitzky

def main(argv):
    rospy.init_node('csv_proc')

    volt_values=[]
    time_values=[]

    sg = savitzky.savitzky_golay(window_size=901, order=3)

####################
# Parameters for the Python script
####################

    filename = rosparam.get_param("/csv_proc/file_name")
    robot_name = rosparam.get_param("/csv_proc/robot_name")
    mode = rosparam.get_param("/csv_proc/mode")
    yaml_file = open("voltage_filter.yaml", "w")
    yl = {}

    if(mode=="update"):
        abcd = rosparam.get_param("/csv_proc/abcd")

    try:
        opts, args = getopt.getopt(argv,"hf:r:",["ifile=", "irobot="])
    except getopt.GetoptError:
        print 'time_volt.py -f <filename> -r <robotname>'
        sys.exit(2)
    for opt, arg in opts:
        if opt == '-h':
            print 'time_volt.py -f <filename> -r <robotname>'
            sys.exit()
        elif opt in ("-f", "--ifile"):
            filename = arg
        elif opt in ("-r", "--irobot"):
            robot_name = arg


####################
# Opening the csv file and getting the values for time and voltage
####################

    with open(filename, 'rb') as csvfile:

        csvreader = csv.reader(csvfile, delimiter=' ', quotechar='|')
        for row in csvreader:
            row = row[0].split(',')
            volt_v = (float)(row[1]) * 1000.0
            if(volt_v < 48000 and volt_v > 44000):
                time_values.append((float)(row[0]))
                volt_values.append(volt_v)

    time_values[:] = [x - time_values[0] for x in time_values]
    time_values = time_values[::-1]

####################
# Plotting graphics for the Voltage vs Time
####################

    pylab.figure(1)

    pylab.plot(volt_values, time_values)
    pylab.ylabel("Time elapsed(seconds)")
    pylab.xlabel("Voltage(mV)")
    pylab.title("Time x Volt,file="+ filename.replace('.csv',''))
    pylab.grid(True)

    secArray = np.asarray(time_values)
    voltArray = np.asarray(volt_values)

    # Polyfit for the voltage values
    z1 = np.polyfit(voltArray, secArray,1)
    z2 = np.polyfit(voltArray, secArray,2)
    z3 = np.polyfit(voltArray, secArray,3)

    # Linear space for better visualization
    xp = np.linspace(49000, 43000, 100)

    # Generating the polynomial function from the fit
    p1 = np.poly1d(z1)
    p2 = np.poly1d(z2)
    p3 = np.poly1d(z3)

    pylab.plot(xp, p1(xp), 'r-', xp, p2(xp), 'g-', xp, p3(xp), 'm-')

    pylab.text(46000, 11900, 'p1=' + p1.__str__(), bbox=dict(facecolor='red', alpha=0.5))
    pylab.text(46000, 11600, 'p2=' + p2.__str__(), bbox=dict(facecolor='green', alpha=0.5))
    pylab.text(46000, 11200, 'p3=' + p3.__str__(), bbox=dict(facecolor='magenta', alpha=0.5))

    pylab.savefig(filename.replace('.csv',''), format="pdf")
    #pylab.show()

####################
# Residuals Analysis
####################

    pylab.figure(2)

    pylab.ylabel("Residuals(s)")
    pylab.xlabel("Voltage(mV)")
    pylab.title("Residuals x Voltage,file="+ filename.replace('.csv',''))

    #Evaluating the polynomial through all the volt values
    z1_val = np.polyval(z1, volt_values)
    z2_val = np.polyval(z2, volt_values)
    z3_val = np.polyval(z3, volt_values)

   # Getting the residuals from the fit functions compared to the real values
    z1_res = time_values - z1_val
    z2_res = time_values - z2_val
    z3_res = time_values - z3_val

    pylab.plot(time_values, z1_res, time_values, z2_res, time_values, z3_res)

    pylab.grid()

    pylab.legend(('Residuals 1st order', 'Residuals 2nd order', 'Residuals 3rd order'))

    pylab.savefig(filename.replace('.csv','')+'_res', format="pdf")

###################
# Savitzky Golay Filter Applied to the Battery Voltage
###################

    values_filt = sg.filter(voltArray)

    pylab.figure(3)

    pylab.subplot(211)

    pylab.plot(voltArray, time_values)

    pylab.grid(True)
    pylab.title('Comparison between real and filtered data')
    pylab.ylabel('Real Values(mV)')

    pylab.subplot(212)

    pylab.plot(values_filt, time_values)

    pylab.grid(True)
    pylab.ylabel('Filtered Values(mV)')
    pylab.xlabel('Time(s)')

#####

###################
# Filtered fits
###################
    pylab.figure(4)

    pylab.plot(values_filt, time_values)
    pylab.ylabel("Time elapsed(seconds)")
    pylab.xlabel("Voltage(mV)")
    pylab.title("Time x Volt,file="+ filename.replace('.csv',''))
    pylab.grid(True)

    secArray = np.asarray(time_values)

    # Polyfit for the voltage values
    z1 = np.polyfit(values_filt, secArray,1)
    z2 = np.polyfit(values_filt, secArray,2)
    z3 = np.polyfit(values_filt, secArray,3)

    if (mode=="initial"):
        z3_l = []

        for el in z3:
            z3_l.append((float)(el))

        abcd = z3_l
        yl["abcd"] = z3_l
        yl["theta"] = 0.
        yl["off_y"] = 0.

    # Linear space for better visualization
    xp = np.linspace(49000, 43000, 100)

    # Generating the polynomial function from the fit
    p1 = np.poly1d(z1)
    p2 = np.poly1d(z2)
    p3 = np.poly1d(z3)

    pylab.plot(xp, p1(xp), 'r-', xp, p2(xp), 'g-', xp, p3(xp), 'm-')

    pylab.text(46000, 11900, 'p1=' + p1.__str__(), bbox=dict(facecolor='red', alpha=0.5))
    pylab.text(46000, 11600, 'p2=' + p2.__str__(), bbox=dict(facecolor='green', alpha=0.5))
    pylab.text(46000, 11200, 'p3=' + p3.__str__(), bbox=dict(facecolor='magenta', alpha=0.5))

####################
# Residuals Analysis for the filtered Signal
####################

    pylab.figure(5)

    pylab.ylabel("Residuals(s)")
    pylab.xlabel("Voltage(mV)")
    pylab.title("Residuals x Voltage,file="+ filename.replace('.csv',''))

    #Evaluating the polynomial through all the time values
    z1_val = np.polyval(z1, values_filt)
    z2_val = np.polyval(z2, values_filt)
    z3_val = np.polyval(z3, values_filt)

   # Getting the residuals from the fit functions compared to the real values
    z1_res = time_values - z1_val
    z2_res = time_values - z2_val
    z3_res = time_values - z3_val

    pylab.plot(time_values, z1_res, time_values, z2_res, time_values, z3_res)

    pylab.grid()

    pylab.legend(('Residuals 1st order', 'Residuals 2nd order', 'Residuals 3rd order'))

####################
# Polynomial Evaluation for the filtered signal and the function from the non-moving case
####################

    if(mode == "update"):
        pylab.figure(6)


        poly_vals = np.polyval(abcd, values_filt)


        pylab.plot(values_filt, poly_vals, values_filt, time_values)

        pylab.legend(('Polynomial', 'Real'))

        pylab.grid()
        #####
        pylab.figure(7)

        pylab.ylabel("Time available(seconds)")
        pylab.xlabel("Voltage(mV)")
        pylab.title("Time x Volt,file="+ filename.replace('.csv',''))
        pylab.grid(True)

        poly_vals = np.polyval(abcd, values_filt)

        ss = lambda y1, y2: ((y1-y2)**2).sum()

        #theta = -0.07
        #new_x = values_filt*cos(theta) - poly_vals*sin(theta)
        #new_y = values_filt*sin(theta) + poly_vals*cos(theta)

        theta = -0.2
        theta_values = []
        cost_values = []
        off_values = []

        while theta < 0.2:
            theta +=0.01
            new_x = values_filt*cos(theta) - poly_vals*sin(theta)
            new_y = values_filt*sin(theta) + poly_vals*cos(theta)


            off_y = -6000

            cost_values_i =[]
            off_y_values_i=[]

            while off_y < 6000:
                off_y +=200
                new_y_temp = new_y
                new_y_temp = new_y_temp + off_y

                ss1=ss(time_values,new_y_temp)
                print ss1, off_y

                cost_values_i.append(ss1)
                off_y_values_i.append(off_y)

            #ss1=ss(time_values,new_y)
            #print ss1, theta
            #cost_values.append(ss1)
            theta_values.append(theta)
            cost_min = min(cost_values_i)
            cost_min_index = cost_values_i.index(cost_min)
            cost_values.append(cost_values_i[cost_min_index])
            off_values.append(off_y_values_i[cost_min_index])

        cost_min = min(cost_values)
        cost_min_index = cost_values.index(cost_min)

        theta = theta_values[cost_min_index]
        off_y = off_values[cost_min_index]

        new_x = values_filt*cos(theta) - poly_vals*sin(theta)
        new_y = values_filt*sin(theta) + poly_vals*cos(theta)

        new_y = new_y + off_y

        yl["abcd"] = abcd
        yl["theta"] = theta
        yl["off_y"] = off_y
        yl["maximum_time"] = (float)(new_y[0])

        pylab.plot(poly_vals, values_filt, time_values, values_filt, new_y, values_filt)

        pylab.legend(('Poly not moving', 'Real', 'Shifted Fit'))

    yaml.safe_dump(yl, yaml_file,default_flow_style=False)

    yaml_file.close()

    pylab.show()



if __name__=="__main__":
    main(sys.argv[1:])