caster_cmder.py
Go to the documentation of this file.00001 #! /usr/bin/env python 00002 # 00003 # Copyright (c) 2009, Willow Garage, Inc. 00004 # All rights reserved. 00005 # 00006 # Redistribution and use in source and binary forms, with or without 00007 # modification, are permitted provided that the following conditions are met: 00008 # 00009 # * Redistributions of source code must retain the above copyright 00010 # notice, this list of conditions and the following disclaimer. 00011 # * Redistributions in binary form must reproduce the above copyright 00012 # notice, this list of conditions and the following disclaimer in the 00013 # documentation and/or other materials provided with the distribution. 00014 # * Neither the name of the Willow Garage, Inc. nor the names of its 00015 # contributors may be used to endorse or promote products derived from 00016 # this software without specific prior written permission. 00017 # 00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00028 # POSSIBILITY OF SUCH DAMAGE. 00029 00030 ##\author Kevin Watts 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 #Changed from 100, 11/20 00043 STEER_VEL = 6.0 #Changed from 100, 11/20 00044 00045 class CasterCmd: 00046 def __init__(self): 00047 self.steer = STEER_VEL 00048 self.drive = SPEED 00049 00050 self._count = 0 00051 00052 ##\brief 8% duty cycle on turn, regular sequence 00053 def update(self): 00054 if self._count == 0: 00055 # Slower steering velocity to make caster process 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()