error_imu.py

Go to the documentation of this file.

#!/usr/bin/env python
# -*- coding: utf-8 -*-

#  -------------------------------------------
#  node: error_icp_mp.py
#  author: Marti Morta (mmorta@iri.upc.edu)
#  date: 20 Nov 2013
#  -------------------------------------------
#   Calculates icp and mid point statistics
#   Subscribers
#     scan: LaserScan

import roslib
roslib.load_manifest('iri_segway_rmp400_odom')
import os
import sys
import rospy
import math
import numpy
import time
from sensor_msgs.msg import Imu

lst_imu = []

def callback_imu(data):
  global lst_imu
  lst_imu.append(-data.angular_velocity.z)


def get_vector_statistics(name,v):
  return str("  "+name+": "+str("%.5f"%numpy.mean(v,dtype=numpy.float64))+" "+str("%.5f"%numpy.std(v,dtype=numpy.float64))+" "+\
  str("%.5f"%numpy.std(v, dtype=numpy.float64)**2)+" "+str("%.5f"%numpy.amin(v))+" "+str("%.5f"%numpy.amax(v))+" "+str(len(v)))

def sayonara():
  global lst_d_icp,lst_d_mp,nom
  f_name    = "imu_stats_"+nom+"_"+str(int(time.time()))+".txt"
  output    = open(f_name, 'w')
  print_imu = get_vector_statistics("IMU",lst_imu)
  output.write("Experiment: "+nom+"\n")
  output.write("Date:       "+str(time.asctime(time.localtime(time.time())))+"\n") 
  output.write("Results: \n") 
  output.write("        Mean   StdDev  Cov     min     max     samples\n")
  output.write(print_imu) 
  output.write("\n")
  output.write(" -----------------------------------------------------------------------------------------------\n")
  output.write("\n")
  print ""
  print "Experiment: "+nom
  print "Date:       "+str(time.asctime(time.localtime(time.time())))
  print "Results:"
  print "        Mean   StdDev  Cov     min     max     samples"
  print  print_imu
  print " -----------------------------------------------------------------------------------------------"
  print ""
  output.close()
  print "  Sayonara"


if __name__ == '__main__':
  rospy.init_node('error_imu')
  print " > IMU Error estimator <"
  r = rospy.Rate(100)
  rospy.Subscriber('/teo/sensors/imu', Imu, callback_imu)
  rospy.on_shutdown(sayonara)
  print "  Fetching data Yo"
  global nom
  nom="test"
  if len(sys.argv)>1:
    nom=sys.argv[1]
  rospy.spin()