contact_state_error.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 from jsk_footstep_controller.msg import GroundContactState
00003 import rospy
00004 import numpy
00005 from math import pi
00006 global data_pitch
00007 global data_roll
00008 global data_z
00009 data_pitch = []
00010 data_roll = []
00011 data_z = []
00012 def callback(msg):
00013     # if msg.contact_state == GroundContactState.CONTACT_BOTH_GROUND:
00014     #     print "ok"
00015     # else:
00016     #     print "no"
00017     data_pitch.append(msg.error_pitch_angle)
00018     data_roll.append(msg.error_roll_angle)
00019     data_z.append(msg.error_z)
00020     print "samples: %d" % len(data_pitch)
00021     print " pitch:"
00022     print "   average: %f rad (%f deg)" % (numpy.average(data_pitch), numpy.average(data_pitch) / pi * 180)
00023     print "   stddev: %f" % numpy.std(data_pitch)
00024     print " roll:"
00025     print "   average: %f rad (%f deg)" % (numpy.average(data_roll), numpy.average(data_roll) / pi * 180)
00026     print "   stddev: %f" % numpy.std(data_roll)
00027     print " z:"
00028     print "   average: %f m" % (numpy.average(data_z))
00029     print "   stddev: %f" % numpy.std(data_z)
00030     
00031 
00032 def main():
00033     s = rospy.Subscriber("/footcoords/contact_state", GroundContactState, callback)
00034     rospy.spin()
00035 
00036 if __name__ == "__main__":
00037   rospy.init_node("contact_state_error")
00038   main()


jsk_footstep_controller
Author(s):
autogenerated on Wed Sep 16 2015 04:38:12