get_sensor_statistics.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib
roslib.load_manifest('teo_apps')
import os
import sys
import numpy as np
import rospy
import rosbag
import tf
import pprint
import math
import subprocess, yaml
from sensor_msgs.msg import Imu
from iri_segway_rmp_msgs.msg import SegwayRMP400Status
from geometry_msgs.msg import Quaternion
from tf.transformations import euler_from_quaternion

# Terminal colors for Ubuntu/Linux
SB_='\033[1m' #style bold
CY_='\033[33m' #color yellow
CLG_='\033[37m' #color light grey
_EC='\033[0m' #end color

# Get the bag
if len(sys.argv) < 2:
  print " "
  print "Error: Please use 1 argument to know the bag"
  print " "
  print " get_covariance.py BAG_FILE PRINT_BAG_INFO_BOOL"
else:
  elbag = str(sys.argv[1])

  # Initialize vectors to store the data
  imu_r=[]; imu_p=[]; imu_y=[]; imu_vr=[]; imu_vp=[]; imu_vy=[]
  seg0_r=[]; seg0_p=[]; seg0_y=[]; seg0_vr=[]; seg0_vp=[]; seg0_vy=[];
  seg1_r=[]; seg1_p=[]; seg1_y=[]; seg1_vr=[]; seg1_vp=[]; seg1_vy=[];

  # Open the bag
  bag = rosbag.Bag(elbag)
  info_dict = yaml.load(subprocess.Popen(['rosbag', 'info', '--yaml', elbag], stdout=subprocess.PIPE).communicate()[0])

  print ""
  print CY_+"-------------------------------------------------------------------------------------------------"+_EC
  print SB_+"                         Sensor Statistics"+_EC
  print CY_+"-------------------------------------------------------------------------------------------------"+_EC
  
  if len(sys.argv) > 2 and str(sys.argv[2])=='True':
    print "\n"+CLG_
    pprint.pprint(info_dict)
    print _EC
    print "\n"+CY_+"-------------------------------------------------------------------------------------------------"+_EC+"\n"

  # Traverse bag and save the information
  for topic, msg, t in bag.read_messages(topics=['/teo/segway/status', '/teo/sensors/imu']):
    if topic=='/teo/segway/status':
      seg0_r.append(msg.rmp200[0].roll_angle)
      seg0_p.append(msg.rmp200[0].pitch_angle)
      seg0_y.append(msg.rmp200[0].yaw_displacement)
      seg0_vr.append(msg.rmp200[0].roll_rate)
      seg0_vp.append(msg.rmp200[0].pitch_rate)
      seg0_vy.append(msg.rmp200[0].yaw_rate)
      seg1_r.append(msg.rmp200[1].roll_angle)
      seg1_p.append(msg.rmp200[1].pitch_angle)
      seg1_y.append(msg.rmp200[1].yaw_displacement)
      seg1_vr.append(msg.rmp200[1].roll_rate)
      seg1_vp.append(msg.rmp200[1].pitch_rate)
      seg1_vy.append(msg.rmp200[1].yaw_rate)
    elif topic =='/teo/sensors/imu':
      q = [msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]
      roll_imu, pitch_imu, yaw_imu = euler_from_quaternion(q)
      if roll_imu <0:
        roll_imu = math.pi+roll_imu # trick
      else:
        roll_imu = -math.pi+roll_imu
      imu_r.append(roll_imu)
      imu_p.append(pitch_imu)
      imu_y.append(yaw_imu)
      imu_vr.append(msg.angular_velocity.x)
      imu_vp.append(msg.angular_velocity.y)
      imu_vy.append(msg.angular_velocity.z)

  # Close the bag
  bag.close()

  def print_vector_statistics(name,v):
    print "  "+name+":\t","%.5f"%np.mean(v, dtype=np.float64)  ,"\t",\
                          "%.5f"%np.std(v, dtype=np.float64)   ,"\t",\
                          "%.5f"%np.std(v, dtype=np.float64)**2,"\t",\
                          "%.5f"%np.amin(v),"\t",\
                          "%.5f"%np.amax(v),"\t",\
                          len(v)

  # Beauty Print the results
  print ""
  print "Bag: ",elbag.split('/')[-1]
  print ""
  print "       Mean\t        Std dev\t        Cov\t        min\t        max\t        samples"
  print SB_+"IMU:"
  print_vector_statistics("r",imu_r)
  print_vector_statistics("p",imu_p)
  print_vector_statistics("y",imu_y)
  print_vector_statistics("vr",imu_vr)
  print_vector_statistics("vp",imu_vp)
  print_vector_statistics("vy",imu_vy)
  print "Segway (Base 0):"
  print_vector_statistics("r",seg0_r)
  print_vector_statistics("p",seg0_p)
  print_vector_statistics("y",seg0_y)
  print_vector_statistics("vr",seg0_vr)
  print_vector_statistics("vp",seg0_vp)
  print_vector_statistics("vy",seg0_vy)
  print "Segway (Base 1):"
  print_vector_statistics("r",seg1_r)
  print_vector_statistics("p",seg1_p)
  print_vector_statistics("y",seg1_y)
  print_vector_statistics("vr",seg1_vr)
  print_vector_statistics("vp",seg1_vp)
  print_vector_statistics("vy",seg1_vy)

  print "\n"+_EC+CY_+"-------------------------------------------------------------------------------------------------"+_EC