raw.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 # ROS stuff
00004 import roslib; roslib.load_manifest('clearpath_base')
00005 import rospy
00006 
00007 from base import Clearpath
00008 
00009 
00010 class Raw(Clearpath):
00011     def __init__(self):
00012         Clearpath.__init__(self)
00013         rospy.loginfo("Using open-loop direct control")
00014 
00015         self.linear_scale = rospy.get_param('~linear_scale', 1.0)
00016         self.angular_scale = rospy.get_param('~angular_scale', 2)
00017         rospy.loginfo("Using %f m/s as 100%% output (linear scale)", self.linear_scale)
00018         rospy.loginfo("Using %f rad/s as +/- 100%% (angular scale)", self.angular_scale)
00019 
00020     def cmd_vel(self, linear_velocity, angular_velocity):
00021         linear = linear_velocity * (100.0 / self.linear_scale)
00022         diff = angular_velocity * (100.0 / self.angular_scale)
00023         left_percent = max(min(linear - diff, 100.0), -100.0)
00024         right_percent = max(min(linear + diff, 100.0), -100.0)
00025         self.horizon.set_differential_output(left_percent, right_percent)
00026 
00027 
00028 Raw().run()


clearpath_base
Author(s): Mike Purvis
autogenerated on Sun Oct 5 2014 22:52:08