wheel_loopback.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 """ 
00003     wheel_loopback - simulates a wheel - just for testing
00004     
00005 """
00006 
00007 #!/usr/bin/env python
00008 #   Copyright 2012 Jon Stephan
00009 #   jfstepha@gmail.com
00010 #
00011 #   This program is free software: you can redistribute it and/or modify
00012 #   it under the terms of the GNU General Public License as published by
00013 #   the Free Software Foundation, either version 3 of the License, or
00014 #   (at your option) any later version.
00015 #
00016 #   This program is distributed in the hope that it will be useful,
00017 #   but WITHOUT ANY WARRANTY; without even the implied warranty of
00018 #   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019 #   GNU General Public License for more details.
00020 #
00021 #   You should have received a copy of the GNU General Public License
00022 #   along with this program.  If not, see <http://www.gnu.org/licenses/>.
00023 
00024 import rospy
00025 import roslib
00026 from std_msgs.msg import Float32
00027 from std_msgs.msg import Int16
00028 
00029 ################################################
00030 ################################################
00031 class WheelLoopback():
00032 ################################################
00033 ################################################
00034 
00035     ###############################################
00036     def __init__(self):
00037     ###############################################
00038         rospy.init_node("wheel_loopback");
00039         self.nodename = rospy.get_name()
00040         rospy.loginfo("%s started" % self.nodename)
00041         
00042         self.rate = rospy.get_param("~rate", 200)
00043         self.timeout_secs = rospy.get_param("~timeout_secs", 0.5)
00044         self.ticks_meter = float(rospy.get_param('~ticks_meter', 50))
00045         self.velocity_scale = float(rospy.get_param('~velocity_scale', 255))
00046         self.latest_motor = 0
00047         
00048         self.wheel = 0
00049         
00050         rospy.Subscriber('motor', Int16, self.motor_callback)
00051         
00052         self.pub_wheel = rospy.Publisher('wheel', Int16)
00053         
00054     ###############################################
00055     def spin(self):
00056     ###############################################
00057         r = rospy.Rate
00058         self.secs_since_target = self.timeout_secs
00059         self.then = rospy.Time.now()
00060         self.latest_msg_time = rospy.Time.now()
00061         rospy.loginfo("-D- spinning")
00062         
00063         ###### main loop #########
00064         while not rospy.is_shutdown():
00065             while not rospy.is_shutdown() and self.secs_since_target < self.timeout_secs:
00066                 self.spinOnce()
00067                 r.sleep
00068                 self.secs_since_target = rospy.Time.now() - self.latest_msg_time
00069                 self.secs_since_target = self.secs_since_target.to_sec()
00070                 #rospy.loginfo("  inside: secs_since_target: %0.3f" % self.secs_since_target)
00071                 
00072             # it's been more than timeout_ticks since we recieved a message
00073             self.secs_since_target = rospy.Time.now() - self.latest_msg_time
00074             self.secs_since_target = self.secs_since_target.to_sec()
00075             # rospy.loginfo("  outside: secs_since_target: %0.3f" % self.secs_since_target)
00076             self.velocity = 0
00077             r.sleep
00078         
00079     ###############################################
00080     def spinOnce(self):
00081     ###############################################
00082         self.velocity = self.latest_motor / self.velocity_scale
00083         if abs(self.velocity) > 0:
00084             self.seconds_per_tick = abs( 1 / (self.velocity * self.ticks_meter))
00085             elapsed = rospy.Time.now() - self.then 
00086             elapsed = elapsed.to_sec()
00087             rospy.loginfo("spinOnce: vel=%0.3f sec/tick=%0.3f elapsed:%0.3f" % (self.velocity, self.seconds_per_tick, elapsed))
00088         
00089             if (elapsed > self.seconds_per_tick):
00090                 rospy.loginfo("incrementing wheel")
00091                 if self.velocity > 0:
00092                     self.wheel += 1
00093                 else:
00094                     self.wheel -= 1
00095                 self.pub_wheel.publish(self.wheel)
00096                 self.then = rospy.Time.now()
00097             
00098         
00099     
00100     ###############################################
00101     def motor_callback(self, msg):
00102     ###############################################
00103         # rospy.loginfo("%s recieved %d" % (self.nodename, msg.data))
00104         self.latest_motor = msg.data
00105         self.latest_msg_time = rospy.Time.now()
00106         
00107         
00108         
00109 ################################################
00110 ################################################
00111 if __name__ == '__main__':
00112     """ main """
00113     wheelLoopback = WheelLoopback()
00114     wheelLoopback.spin()


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