Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 PKG = "life_test"
00033 import roslib; roslib.load_manifest(PKG)
00034
00035 import sys
00036 import rospy
00037
00038 from time import sleep
00039 from std_msgs.msg import Float64
00040 from sensor_msgs.msg import JointState
00041
00042 from optparse import OptionParser
00043
00044 class LastMessage():
00045 def __init__(self, topic, msg_type):
00046 self.msg = None
00047 rospy.Subscriber(topic, msg_type, self.callback)
00048
00049 def last(self):
00050 return self.msg
00051
00052 def callback(self, msg):
00053 self.msg = msg
00054
00055 def main():
00056 parser = OptionParser()
00057 parser.add_option("-l", "--low", dest="low_only",
00058 help = "Keep in lower end only", action="store_true",
00059 default = False)
00060 parser.add_option("-s", "--slow", dest="slow",
00061 help = "Move only every 5 minutes",
00062 action="store_true", default=False)
00063
00064
00065 (options, args) = parser.parse_args()
00066
00067 rospy.init_node('torso_life_test')
00068
00069 pub = rospy.Publisher("torso_lift_velocity_controller/command", Float64, latch = True)
00070 last_state = LastMessage('joint_states', JointState)
00071 turn_count = 0
00072 vel = 10.0
00073 pub.publish(Float64(vel))
00074 print 'Published', vel
00075
00076 try:
00077 while not last_state.msg and not rospy.is_shutdown(): sleep(0.1)
00078 while not rospy.is_shutdown():
00079 sleep(0.01)
00080 jnt_states = last_state.last()
00081 torso_idx = -1
00082 for index, name in enumerate(jnt_states.name):
00083 if name == 'torso_lift_joint':
00084 torso_idx = index
00085 break
00086 if torso_idx < 0:
00087 rospy.logwarn("The joint %s was not found in the joints states" % 'torso_lift_joint')
00088
00089 if abs(jnt_states.velocity[torso_idx]) < 0.002:
00090 turn_count += 1
00091 else:
00092 turn_count = 0
00093
00094 if turn_count > 25:
00095 vel = -1 * vel
00096 turn_count = 0
00097 pub.publish(Float64(vel))
00098
00099 if options.slow:
00100 sleep(5)
00101
00102 except KeyboardInterrupt, e:
00103 pass
00104 except:
00105 import traceback
00106 rospy.logerr(traceback.format_exc())
00107 traceback.print_exc()
00108
00109
00110 if __name__ == '__main__':
00111 main()