twist_to_motors.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 """
00003    twist_to_motors - converts a twist message to motor commands.  Needed for navigation stack
00004    
00005    
00006     Copyright (C) 2012 Jon Stephan. 
00007      
00008     This program is free software: you can redistribute it and/or modify
00009     it under the terms of the GNU General Public License as published by
00010     the Free Software Foundation, either version 3 of the License, or
00011     (at your option) any later version.
00012 
00013     This program is distributed in the hope that it will be useful,
00014     but WITHOUT ANY WARRANTY; without even the implied warranty of
00015     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016     GNU General Public License for more details.
00017 
00018     You should have received a copy of the GNU General Public License
00019     along with this program.  If not, see <http://www.gnu.org/licenses/>.
00020 """
00021 
00022 import rospy
00023 import roslib
00024 from std_msgs.msg import Float32
00025 from geometry_msgs.msg import Twist 
00026 
00027 #############################################################
00028 #############################################################
00029 class TwistToMotors():
00030 #############################################################
00031 #############################################################
00032 
00033     #############################################################
00034     def __init__(self):
00035     #############################################################
00036         rospy.init_node("twist_to_motors")
00037         nodename = rospy.get_name()
00038         rospy.loginfo("%s started" % nodename)
00039     
00040         self.w = rospy.get_param("~base_width", 0.2)
00041     
00042         self.pub_lmotor = rospy.Publisher('lwheel_vtarget', Float32)
00043         self.pub_rmotor = rospy.Publisher('rwheel_vtarget', Float32)
00044         rospy.Subscriber('twist', Twist, self.twistCallback)
00045     
00046     
00047         self.rate = rospy.get_param("~rate", 50)
00048         self.timeout_ticks = rospy.get_param("~timeout_ticks", 2)
00049         self.left = 0
00050         self.right = 0
00051         
00052     #############################################################
00053     def spin(self):
00054     #############################################################
00055     
00056         r = rospy.Rate(self.rate)
00057         idle = rospy.Rate(10)
00058         then = rospy.Time.now()
00059         self.ticks_since_target = self.timeout_ticks
00060     
00061         ###### main loop  ######
00062         while not rospy.is_shutdown():
00063         
00064             while not rospy.is_shutdown() and self.ticks_since_target < self.timeout_ticks:
00065                 self.spinOnce()
00066                 r.sleep()
00067             idle.sleep()
00068                 
00069     #############################################################
00070     def spinOnce(self):
00071     #############################################################
00072     
00073         # dx = (l + r) / 2
00074         # dr = (r - l) / w
00075             
00076         self.right = 1.0 * self.dx + self.dr * self.w / 2 
00077         self.left = 1.0 * self.dx - self.dr * self.w / 2
00078         # rospy.loginfo("publishing: (%d, %d)", left, right) 
00079                 
00080         self.pub_lmotor.publish(self.left)
00081         self.pub_rmotor.publish(self.right)
00082             
00083         self.ticks_since_target += 1
00084 
00085     #############################################################
00086     def twistCallback(self,msg):
00087     #############################################################
00088         # rospy.loginfo("-D- twistCallback: %s" % str(msg))
00089         self.ticks_since_target = 0
00090         self.dx = msg.linear.x
00091         self.dr = msg.angular.z
00092         self.dy = msg.linear.y
00093     
00094 #############################################################
00095 #############################################################
00096 if __name__ == '__main__':
00097     """ main """
00098     twistToMotors = TwistToMotors()
00099     twistToMotors.spin()


differential_drive
Author(s): Jon Stephan
autogenerated on Sun Jan 5 2014 11:04:42