diff_controller.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004   diff_controller.py - controller for a differential drive
00005   Copyright (c) 2010-2011 Vanadium Labs LLC.  All right reserved.
00006 
00007   Redistribution and use in source and binary forms, with or without
00008   modification, are permitted provided that the following conditions are met:
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 Vanadium Labs LLC nor the names of its 
00015         contributors may be used to endorse or promote products derived 
00016         from this software without specific prior written permission.
00017   
00018   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00019   ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020   WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021   DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
00022   INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00023   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00024   OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00025   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00026   OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00027   ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 """
00029 
00030 import rospy
00031 
00032 from math import sin,cos,pi
00033 
00034 from geometry_msgs.msg import Quaternion
00035 from geometry_msgs.msg import Twist
00036 from nav_msgs.msg import Odometry
00037 from diagnostic_msgs.msg import *
00038 from tf.broadcaster import TransformBroadcaster
00039 
00040 from ax12 import *
00041 from controllers import *
00042 from struct import unpack
00043 
00044 class DiffController(Controller):
00045     """ Controller to handle movement & odometry feedback for a differential 
00046             drive mobile base. """
00047     def __init__(self, device, name):
00048         Controller.__init__(self, device, name)
00049         self.pause = True
00050         self.last_cmd = rospy.Time.now()
00051 
00052         # parameters: rates and geometry
00053         self.rate = rospy.get_param('~controllers/'+name+'/rate',10.0)
00054         self.timeout = rospy.get_param('~controllers/'+name+'/timeout',1.0)
00055         self.t_delta = rospy.Duration(1.0/self.rate)
00056         self.t_next = rospy.Time.now() + self.t_delta
00057         self.ticks_meter = float(rospy.get_param('~controllers/'+name+'/ticks_meter'))
00058         self.base_width = float(rospy.get_param('~controllers/'+name+'/base_width'))
00059 
00060         self.base_frame_id = rospy.get_param('~controllers/'+name+'/base_frame_id', 'base_link')
00061         self.odom_frame_id = rospy.get_param('~controllers/'+name+'/odom_frame_id', 'odom')
00062 
00063         # parameters: PID
00064         self.Kp = rospy.get_param('~controllers/'+name+'/Kp', 5)
00065         self.Kd = rospy.get_param('~controllers/'+name+'/Kd', 1)
00066         self.Ki = rospy.get_param('~controllers/'+name+'/Ki', 0)
00067         self.Ko = rospy.get_param('~controllers/'+name+'/Ko', 50)
00068 
00069         # parameters: acceleration
00070         self.accel_limit = rospy.get_param('~controllers/'+name+'/accel_limit', 0.1)
00071         self.max_accel = int(self.accel_limit*self.ticks_meter/self.rate)
00072 
00073         # output for joint states publisher
00074         self.joint_names = ["base_l_wheel_joint","base_r_wheel_joint"]
00075         self.joint_positions = [0,0]
00076         self.joint_velocities = [0,0]
00077 
00078         # internal data            
00079         self.v_left = 0                 # current setpoint velocity
00080         self.v_right = 0
00081         self.v_des_left = 0             # cmd_vel setpoint
00082         self.v_des_right = 0
00083         self.enc_left = None            # encoder readings
00084         self.enc_right = None
00085         self.x = 0                      # position in xy plane
00086         self.y = 0
00087         self.th = 0
00088         self.dx = 0                     # speeds in x/rotation
00089         self.dr = 0
00090         self.then = rospy.Time.now()    # time for determining dx/dy
00091 
00092         # subscriptions
00093         rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
00094         self.odomPub = rospy.Publisher("odom",Odometry)
00095         self.odomBroadcaster = TransformBroadcaster()
00096                 
00097         rospy.loginfo("Started DiffController ("+name+"). Geometry: " + str(self.base_width) + "m wide, " + str(self.ticks_meter) + " ticks/m.")
00098 
00099     def startup(self):
00100         if not self.fake:
00101             self.setup(self.Kp,self.Kd,self.Ki,self.Ko) 
00102     
00103     def update(self):
00104         now = rospy.Time.now()
00105         if now > self.t_next:
00106             elapsed = now - self.then
00107             self.then = now
00108             elapsed = elapsed.to_sec()
00109 
00110             if self.fake:
00111                 x = cos(self.th)*self.dx*elapsed
00112                 y = -sin(self.th)*self.dx*elapsed
00113                 self.x += cos(self.th)*self.dx*elapsed
00114                 self.y += sin(self.th)*self.dx*elapsed
00115                 self.th += self.dr*elapsed
00116             else:
00117                 # read encoders
00118                 try:
00119                     left, right = self.status()
00120                 except Exception as e:
00121                     rospy.logerr("Could not update encoders: " + str(e))
00122                     return
00123                 rospy.logdebug("Encoders: " + str(left) +","+ str(right))
00124 
00125                 # calculate odometry
00126                 if self.enc_left == None:
00127                     d_left = 0
00128                     d_right = 0
00129                 else:
00130                     d_left = (left - self.enc_left)/self.ticks_meter
00131                     d_right = (right - self.enc_right)/self.ticks_meter
00132                 self.enc_left = left
00133                 self.enc_right = right
00134 
00135                 d = (d_left+d_right)/2
00136                 th = (d_right-d_left)/self.base_width
00137                 self.dx = d / elapsed
00138                 self.dr = th / elapsed
00139 
00140                 if (d != 0):
00141                     x = cos(th)*d
00142                     y = -sin(th)*d
00143                     self.x = self.x + (cos(self.th)*x - sin(self.th)*y)
00144                     self.y = self.y + (sin(self.th)*x + cos(self.th)*y)
00145                 if (th != 0):
00146                     self.th = self.th + th
00147 
00148             # publish or perish
00149             quaternion = Quaternion()
00150             quaternion.x = 0.0 
00151             quaternion.y = 0.0
00152             quaternion.z = sin(self.th/2)
00153             quaternion.w = cos(self.th/2)
00154             self.odomBroadcaster.sendTransform(
00155                 (self.x, self.y, 0), 
00156                 (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
00157                 rospy.Time.now(),
00158                 self.base_frame_id,
00159                 self.odom_frame_id
00160                 )
00161 
00162             odom = Odometry()
00163             odom.header.stamp = now
00164             odom.header.frame_id = self.odom_frame_id
00165             odom.pose.pose.position.x = self.x
00166             odom.pose.pose.position.y = self.y
00167             odom.pose.pose.position.z = 0
00168             odom.pose.pose.orientation = quaternion
00169             odom.child_frame_id = self.base_frame_id
00170             odom.twist.twist.linear.x = self.dx
00171             odom.twist.twist.linear.y = 0
00172             odom.twist.twist.angular.z = self.dr
00173             self.odomPub.publish(odom)
00174 
00175             if now > (self.last_cmd + rospy.Duration(self.timeout)):
00176                 self.v_des_left = 0
00177                 self.v_des_right = 0
00178 
00179             # update motors
00180             if not self.fake:
00181                 if self.v_left < self.v_des_left:
00182                     self.v_left += self.max_accel
00183                     if self.v_left > self.v_des_left:
00184                         self.v_left = self.v_des_left
00185                 else:
00186                     self.v_left -= self.max_accel
00187                     if self.v_left < self.v_des_left:
00188                         self.v_left = self.v_des_left
00189                 
00190                 if self.v_right < self.v_des_right:
00191                     self.v_right += self.max_accel
00192                     if self.v_right > self.v_des_right:
00193                         self.v_right = self.v_des_right
00194                 else:
00195                     self.v_right -= self.max_accel
00196                     if self.v_right < self.v_des_right:
00197                         self.v_right = self.v_des_right
00198                 self.write(self.v_left, self.v_right)
00199 
00200             self.t_next = now + self.t_delta
00201  
00202     def shutdown(self):
00203         if not self.fake:
00204             self.write(0,0)
00205 
00206     def cmdVelCb(self,req):
00207         """ Handle movement requests. """
00208         self.last_cmd = rospy.Time.now()
00209         if self.fake:
00210             self.dx = req.linear.x        # m/s
00211             self.dr = req.angular.z       # rad/s
00212         else:
00213             # set motor speeds in ticks per 1/30s
00214             self.v_des_left = int( ((req.linear.x - (req.angular.z * self.base_width/2.0)) * self.ticks_meter) / 30.0)
00215             self.v_des_right = int( ((req.linear.x + (req.angular.z * self.base_width/2.0)) * self.ticks_meter) / 30.0)
00216 
00217     def getDiagnostics(self):
00218         """ Get a diagnostics status. """
00219         msg = DiagnosticStatus()
00220         msg.name = self.name
00221         msg.level = DiagnosticStatus.OK
00222         msg.message = "OK"
00223         if not self.fake:
00224             msg.values.append(KeyValue("Left", str(self.enc_left)))
00225             msg.values.append(KeyValue("Right", str(self.enc_right)))
00226         msg.values.append(KeyValue("dX", str(self.dx)))
00227         msg.values.append(KeyValue("dR", str(self.dr)))
00228         return msg
00229 
00230     ###
00231     ### Controller Specification: 
00232     ###
00233     ###  setup: Kp, Kd, Ki, Ko (all unsigned char)
00234     ###
00235     ###  write: left_speed, right_speed (2-byte signed, ticks per frame)
00236     ###
00237     ###  status: left_enc, right_enc (4-byte signed)
00238     ### 
00239     
00240     def setup(self, kp, kd, ki, ko):
00241         success = self.device.execute(253, AX_CONTROL_SETUP, [10, kp, kd, ki, ko])
00242 
00243     def write(self, left, right):
00244         """ Send a closed-loop speed. Base PID loop runs at 30Hz, these values
00245                 are therefore in ticks per 1/30 second. """
00246         left = left&0xffff
00247         right = right&0xffff
00248         success = self.device.execute(253, AX_CONTROL_WRITE, [10, left%256, left>>8, right%256, right>>8])
00249 
00250     def status(self):
00251         """ read 32-bit (signed) encoder values. """
00252         values = self.device.execute(253, AX_CONTROL_STAT, [10])
00253         left_values = "".join([chr(k) for k in values[0:4] ])        
00254         right_values = "".join([chr(k) for k in values[4:] ])
00255         try:
00256             left = unpack('=l',left_values)[0]
00257             right = unpack('=l',right_values)[0]
00258             return [left, right]
00259         except:
00260             return None
00261 


arbotix_python
Author(s): Michael Ferguson
autogenerated on Sun Oct 5 2014 22:16:05