error_icp_mp.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 copy import deepcopy
from geometry_msgs.msg import Twist,Pose
from sensor_msgs.msg import LaserScan
from iri_laser_icp.srv import GetRelativePose


last_scan = LaserScan()
lst_d_icp = []
lst_d_mp  = []


def callback_laser(data):
  global last_scan,lst_d_icp,lst_d_mp
  current_scan = data;
  prev_L = Pose()
  if last_scan.header.seq > 0:
    [icp,mp] = icp_client(last_scan,current_scan,prev_L)
    lst_d_icp.append(icp)
    lst_d_mp.append(mp)
  last_scan = deepcopy(current_scan)


def icp_client(ref,sens,d):
  rospy.wait_for_service('iri_laser_icp/get_relative_pose',10)
  try:
      get_rpose = rospy.ServiceProxy('iri_laser_icp/get_relative_pose', GetRelativePose)
      rel_pose = get_rpose(ref,sens,d).pose_rel.pose.pose
      d_icp = math.sqrt(rel_pose.position.x**2+rel_pose.position.y**2)
      d_mp  = ref.ranges[int(len(ref.ranges)/2)] - sens.ranges[int(len(sens.ranges)/2)]
      return [d_icp,d_mp]
  except rospy.ServiceException, e:
      print CR_+"Service call failed: %s"%e+_EC


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    = "icp_mp_stats_"+nom+"_"+str(int(time.time()))+".txt"
  output    = open(f_name, 'w')
  print_icp = get_vector_statistics("ICP",lst_d_icp)
  print_mp  = get_vector_statistics(" MP",lst_d_mp)
  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_icp)  
  output.write("\n")
  output.write(print_mp)  
  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_icp  
  print  print_mp 
  print " -----------------------------------------------------------------------------------------------"
  print ""
  output.close()
  print "  Sayonara"


if __name__ == '__main__':
  rospy.init_node('error_icp_mp')
  print " > ICP & MP Error estimator <"
  r = rospy.Rate(20)
  rospy.Subscriber('/teo/sensors/front_laser', LaserScan, callback_laser)
  rospy.on_shutdown(sayonara)
  print "  Fetching data Yo"
  global nom
  nom="test"
  if len(sys.argv)>1:
    nom=sys.argv[1]
  rospy.spin()