validation.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib
roslib.load_manifest('iri_segway_rmp400_odom')
import os
import sys
import rospy
import tf
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import math
from tf.transformations import euler_from_quaternion


#  -------------------------------------------
#  node: validation.py NOTE maybe its a wrong name 
#  author: Marti Morta (mmorta@iri.upc.edu)
#  date: 17 Oct 2013
#  -------------------------------------------
#
#   This node validates the segway odometry doing an straight line 
#   or a square of length L m.
#   
#   In order to perform a correct estimation of the systematic error:
#   Get data from N CW and then N CCW experiments
#   calculate the error as max(r_cw,r_cw), being r=sqrt(X²+Y²)
#   and X=1/N*sum(Ex) and Y=1/N*sum(Ey) where Ex=(x_final - x_initial)
#   
#   From the book: Where am I, chapter 5.
#   
#   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
#   Subscribers
#     odom: Odometry
#     
#   >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
#   Publishers
#     cmd_vel: Twist
#     
#   ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#   Parameters:
#     Length: Length of the line or the side
# 
#  

SB_='\033[1m' #style bold
CY_='\033[33m' #color yellow
CB_='\033[34m' #color blue
CLG_='\033[37m' #color light grey
CR_='\033[31m' #Red  
_EC='\033[0m' #end color 

# Set Publishers
p_v = rospy.Publisher('cmd_vel', Twist)


# Parameters
test_type = "line"
test_steer = "cw"
length = 3

tw_straight = Twist()
tw_straight.linear.x  = 0.25 # m/s
tw_straight.linear.y  = 0.0
tw_straight.linear.z  = 0.0
tw_straight.angular.x = 0.0
tw_straight.angular.y = 0.0
tw_straight.angular.z = 0.0

tw_turn_left = Twist()
tw_turn_left.linear.x  = 0.0
tw_turn_left.linear.y  = 0.0
tw_turn_left.linear.z  = 0.0
tw_turn_left.angular.x = 0.0
tw_turn_left.angular.y = 0.0
tw_turn_left.angular.z = 0.2 # rad/s

tw_turn_right = Twist()
tw_turn_right.linear.x  = 0.0
tw_turn_right.linear.y  = 0.0
tw_turn_right.linear.z  = 0.0
tw_turn_right.angular.x = 0.0
tw_turn_right.angular.y = 0.0
tw_turn_right.angular.z =-tw_turn_left.angular.z # rad/s

tw_zero = Twist()
tw_zero.linear.x  = 0.0
tw_zero.linear.y  = 0.0
tw_zero.linear.z  = 0.0
tw_zero.angular.x = 0.0
tw_zero.angular.y = 0.0
tw_zero.angular.z = 0.0

# Global variables
turns_done = int()
girant = bool()
tw = Twist()
ref_rotation=[]
old_x = float()
old_y = float()
distance = float()

# Odom Callback function
#   pre: Odometry message
#   post: 1) cmd_vel straight
#         2) cmd_vel CW turn
#         3) cmd_vel 0
def callback_odom(data):

  global girant, turns_done, tw, ref_rotation, old_x, old_y, distance


  q = [data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w]
  rotation = euler_from_quaternion(q)

  if(old_x == 999999):
    old_x = data.pose.pose.position.x
    old_y = data.pose.pose.position.y
    rospy.loginfo("Starting Program \n  pose x: %f y: %f \n  rotation: z: %f",data.pose.pose.position.x,data.pose.pose.position.y,math.degrees(rotation[2]))

  distance += math.sqrt((data.pose.pose.position.x-old_x)**2+(data.pose.pose.position.y-old_y)**2)
  old_x = data.pose.pose.position.x
  old_y = data.pose.pose.position.y
  
  if(test_type=="line"):
    if(distance >= length):
      p_v.publish(tw_zero)
      rospy.loginfo("Program Finished \n  distance: %f \n  pose x: %f y: %f \n  rotation: z: %f \n  turns: %d",distance,data.pose.pose.position.x,data.pose.pose.position.y,math.degrees(rotation[2]),turns_done)
      rospy.signal_shutdown("PF")

  elif(test_type=="square"):
    if(not girant and distance >= length*(turns_done+1) and turns_done < 4):
      rospy.loginfo("Translation > length. Turning %s\n  distance: %f \n  pose x: %f y: %f \n  rotation: z: %f \n  turns: %d",test_steer,distance,data.pose.pose.position.x,data.pose.pose.position.y,math.degrees(rotation[2]),turns_done)
      if(test_steer=="cw"):
        tw = tw_turn_right
      else:
        tw = tw_turn_left
      ref_rotation = rotation;
      girant = True

    elif(girant and math.fabs(rotation[2]-ref_rotation[2]) >= math.pi/2 and turns_done < 4):
      rospy.loginfo("%s turn done. Going straight\n  distance: %f \n  pose x: %f y: %f \n  rotation: z: %f \n  turns: %d",test_steer,distance,data.pose.pose.position.x,data.pose.pose.position.y,math.degrees(rotation[2]),turns_done)
      tw = tw_straight
      turns_done+=1
      girant = False

    if(turns_done>=4):
      tw = tw_zero
      print CY_
      rospy.loginfo("Program Finished")
      rospy.loginfo("%s turn done. Going straight\n  distance: %f \n  pose x: %f y: %f \n  rotation: z: %f \n  turns: %d",test_steer,distance,data.pose.pose.position.x,data.pose.pose.position.y,math.degrees(rotation[2]),turns_done)
      print _EC
      rospy.signal_shutdown("PF")

  p_v.publish(tw)
  

# Main function
#   pre: -
#   post: -
def main(args):

  if len(sys.argv) < 3:
    print " "
    print CR_+"Error: Please use at least 2 arguments as following:"+_EC
    print " "
    print CY_+"$ get_covariance.py"+SB_+" TYPE   LENGTH    (STEER)"+_EC
    print "                |-line      |- in [m]  |-cw  "
    print "                |-square               |-ccw "
    print " "

  else:
    global test_type,length,test_steer
    test_type = str(sys.argv[1])
    if(test_type!="line" and test_type!="square"):
      print CR_+"Error: Please type 'line' or 'square'"+_EC
      sys.exit(1)
    length = float(sys.argv[2])
    if(length<=0 or length >=10):
      print CR_+"Error: Please type a length between 0 and 10"+_EC
      sys.exit(1)
    if(len(sys.argv)>3):
      test_steer = str(sys.argv[3])
      if(test_steer!="cw" and test_steer!="ccw"):
        print CR_+"Error: Please type 'cw' or 'ccw'"+_EC
        sys.exit(1)
    
    rospy.init_node('validation')
    
    print ""
    print CY_+"-------------------------------------------------------------------------------------------------"+_EC
    print SB_+"                         Odometry Validation Node"+_EC
    print CY_+"-------------------------------------------------------------------------------------------------"+_EC
    print ""
    
    r = rospy.Rate(50) # 10hz
    
    global girant,turns_done,tw,ref_rotation, old_x, old_y, distance
    girant = False
    turns_done=0
    tw = tw_straight
    old_x = old_y = 999999
    distance = 0.0
    
    print CB_
    rospy.loginfo("Test: %s",   test_type)
    rospy.loginfo("Length: %f", length)
    rospy.loginfo("Turn: %s",   test_steer)
    rospy.loginfo("vT: %f m/s",   tw_straight.linear.x)
    rospy.loginfo("vR: %f rad/s", tw_turn_left.angular.z)
    print ""+_EC
    
    raw_input('Press INTRO as soon as you are ready')
    print ""
    rospy.Subscriber('odom', Odometry, callback_odom)
    p_v.publish(tw_straight)
    
    rospy.spin()


if __name__ == '__main__':
    main(sys.argv)