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 cos, sin
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)
            print("new_x: {}, new_y: {}".format(new_x, new_y))


            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)
        print("new_x: {}, new_y: {}".format(new_x, new_y))

        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:])