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
00034 roslib.load_manifest(PKG)
00035 import time
00036 import random
00037 import rospy
00038 from std_msgs.msg import Float64
00039
00040 ROTATION_JOINT = 'fl_caster_rotation_joint'
00041 TOPIC_PREFIX = 'caster_fl'
00042 SPEED = 0.1
00043 STEER_VEL = 6.0
00044
00045 class CasterCmd:
00046 def __init__(self):
00047 self.steer = STEER_VEL
00048 self.drive = SPEED
00049
00050 self._count = 0
00051
00052
00053 def update(self):
00054 if self._count == 0:
00055
00056 self.steer = -0.8 * STEER_VEL
00057 self.drive = 0
00058 elif self._count == 1:
00059 self.steer = STEER_VEL
00060 self.drive = 0
00061 else:
00062 self.steer = 0
00063 if self._count % 2 == 0:
00064 self.drive = SPEED
00065 else:
00066 self.drive = -1 * SPEED
00067
00068 self._count += 1
00069 if self._count >= 25:
00070 self._count = 0
00071
00072
00073 def main():
00074 rospy.init_node('caster_cmder')
00075 cmder = CasterCmd()
00076 pub_steer = rospy.Publisher("%s/steer" % TOPIC_PREFIX, Float64)
00077 pub_drive = rospy.Publisher("%s/drive" % TOPIC_PREFIX, Float64)
00078 pub_steer.publish(Float64(0.0))
00079 pub_drive.publish(Float64(0.0))
00080
00081 my_rate = rospy.Rate(float(rospy.get_param('cycle_rate', 1.0)))
00082 while not rospy.is_shutdown():
00083 pub_steer.publish(Float64(cmder.steer))
00084 pub_drive.publish(Float64(cmder.drive))
00085
00086 cmder.update()
00087 my_rate.sleep()
00088
00089
00090 if __name__ == '__main__':
00091 main()