base_controller.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     Base Controller Class for the Robotics Connection Serializer(TM) microcontroller
00005     Borrowed heavily from Mike Feguson's ArbotiX base_controller.py code.
00006     
00007     Created for The Pi Robot Project: http://www.pirobot.org
00008     Copyright (c) 2010 Patrick Goebel.  All rights reserved.
00009 
00010     This program is free software; you can redistribute it and/or modify
00011     it under the terms of the GNU General Public License as published by
00012     the Free Software Foundation; either version 2 of the License, or
00013     (at your option) any later version.
00014     
00015     This program is distributed in the hope that it will be useful,
00016     but WITHOUT ANY WARRANTY; without even the implied warranty of
00017     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018     GNU General Public License for more details at:
00019     
00020     http://www.gnu.org/licenses
00021 """
00022 import roslib; roslib.load_manifest('serializer')
00023 import rospy
00024 
00025 from threading import Thread, Event
00026 from math import sin, cos, pi
00027 from geometry_msgs.msg import Quaternion
00028 from geometry_msgs.msg import Twist
00029 from geometry_msgs.msg import Pose
00030 from nav_msgs.msg import Odometry
00031 from tf.broadcaster import TransformBroadcaster
00032 
00033 class base_controller(Thread):
00034     """ Controller to handle movement & odometry feedback for a differential drive mobile base. """
00035     def __init__(self, Serializer, name):
00036         Thread.__init__ (self)
00037         self.finished = Event()
00038         
00039         # Handle for the Serializer
00040         self.mySerializer = Serializer
00041 
00042         # Parameters
00043         self.rate = float(rospy.get_param("~base_controller_rate", 10))
00044         self.ticks_per_meter = float(self.mySerializer.ticks_per_meter)
00045         self.wheel_track = self.mySerializer.wheel_track
00046         self.gear_reduction = self.mySerializer.gear_reduction
00047         self.encoder_resolution = self.mySerializer.encoder_resolution
00048         
00049         now = rospy.Time.now()
00050         
00051         self.t_delta = rospy.Duration(1.0/self.rate)
00052         self.t_next = now + self.t_delta
00053         self.then = now # time for determining dx/dy
00054 
00055         # internal data        
00056         self.enc_left = 0            # encoder readings
00057         self.enc_right = 0
00058         self.x = 0.                  # position in xy plane
00059         self.y = 0.
00060         self.th = 0.                 # rotation in radians
00061         self.encoder_error = False
00062 
00063         # subscriptions
00064         rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
00065         
00066         # Clear any old odometry info
00067         self.mySerializer.clear_encoder([1, 2])
00068         
00069         # Set up the odometry broadcaster
00070         self.odomPub = rospy.Publisher('odom', Odometry)
00071         self.odomBroadcaster = TransformBroadcaster()
00072         
00073         rospy.loginfo("Started Base Controller '"+ name +"' for a base of " + str(self.wheel_track) + "m wide with " + str(self.ticks_per_meter) + " ticks per meter")
00074 
00075     def run(self):
00076         rosRate = rospy.Rate(self.rate)
00077         rospy.loginfo("Publishing Odometry data at: " + str(self.rate) + " Hz")
00078         
00079         old_left = old_right = 0
00080         bad_encoder_count = 0
00081         
00082         while not rospy.is_shutdown() and not self.finished.isSet():
00083             now = rospy.Time.now()         
00084             if now > self.t_next:
00085                 dt = now - self.then
00086                 self.then = now
00087                 dt = dt.to_sec()
00088                 
00089                 # read encoders
00090                 try:
00091                     left, right = self.mySerializer.get_encoder_count([1, 2])
00092                 except:
00093                     self.encoder_error = True
00094                     rospy.loginfo("Could not read encoders: " + str(bad_encoder_count))
00095                     bad_encoder_count += 1
00096                     rospy.loginfo("Encoder counts at exception: " + str(left) + ", " + str(right)) 
00097                     continue
00098                 
00099                 if self.encoder_error:
00100                     self.enc_left = left
00101                     self.enc_right = right
00102                     self.encoder_error = False
00103                     continue
00104                 
00105                 # calculate odometry
00106                 dleft = (left - self.enc_left) / self.ticks_per_meter
00107                 dright = (right - self.enc_right) / self.ticks_per_meter
00108                 
00109                 self.enc_left = left
00110                 self.enc_right = right
00111                 
00112                 dxy_ave = (dleft + dright) / 2.0
00113                 dth = (dright - dleft) / self.wheel_track
00114                 vxy = dxy_ave / dt
00115                 vth = dth / dt
00116     
00117                 if (dxy_ave != 0):
00118                     dx = cos(dth) * dxy_ave
00119                     dy = -sin(dth) * dxy_ave
00120                     self.x += (cos(self.th) * dx - sin(self.th) * dy)
00121                     self.y += (sin(self.th) * dx + cos(self.th) * dy)
00122     
00123                 if (dth != 0):
00124                     self.th += dth 
00125     
00126                 quaternion = Quaternion()
00127                 quaternion.x = 0.0 
00128                 quaternion.y = 0.0
00129                 quaternion.z = sin(self.th / 2.0)
00130                 quaternion.w = cos(self.th / 2.0)
00131     
00132                 # Create the odometry transform frame broadcaster.
00133                 self.odomBroadcaster.sendTransform(
00134                     (self.x, self.y, 0), 
00135                     (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
00136                     rospy.Time.now(),
00137                     "base_link",
00138                     "odom"
00139                     )
00140     
00141                 odom = Odometry()
00142                 odom.header.frame_id = "odom"
00143                 odom.child_frame_id = "base_link"
00144                 odom.header.stamp = now
00145                 odom.pose.pose.position.x = self.x
00146                 odom.pose.pose.position.y = self.y
00147                 odom.pose.pose.position.z = 0
00148                 odom.pose.pose.orientation = quaternion
00149                 odom.twist.twist.linear.x = vxy
00150                 odom.twist.twist.linear.y = 0
00151                 odom.twist.twist.angular.z = vth
00152     
00153                 self.odomPub.publish(odom)
00154                 
00155                 self.t_next = now + self.t_delta
00156                 
00157             rosRate.sleep()
00158 
00159 
00160     def cmdVelCallback(self, req):
00161         """ Handle velocity-based movement requests. """
00162         x = req.linear.x         # m/s
00163         th = req.angular.z       # rad/s
00164 
00165         if x == 0:
00166             # Turn in place
00167             right = th * self.wheel_track  * self.gear_reduction / 2.0
00168             left = -right
00169         elif th == 0:   
00170             # Pure forward/backward motion
00171             left = right = x
00172         else:
00173             # Rotation about a point in space
00174             left = x - th * self.wheel_track  * self.gear_reduction / 2.0
00175             right = x + th * self.wheel_track  * self.gear_reduction / 2.0
00176 
00177         # Set motor speeds in meters per second.
00178         self.mySerializer.mogo_m_per_s([1, 2], [left, right])
00179         
00180     def stop(self):
00181         print "Shutting down base controller"
00182         self.finished.set()
00183         self.join()
00184         
00185 
00186     
00187 
00188     


serializer
Author(s): Patrick Goebel
autogenerated on Mon Oct 6 2014 12:12:01