base_controller.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     A base controller class for the Arduino microcontroller
00005     
00006     Borrowed heavily from Mike Feguson's ArbotiX base_controller.py code.
00007     
00008     Created for the Pi Robot Project: http://www.pirobot.org
00009     Copyright (c) 2010 Patrick Goebel.  All rights reserved.
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 2 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 at:
00020     
00021     http://www.gnu.org/licenses
00022 """
00023 import roslib; roslib.load_manifest('ros_arduino_python')
00024 import rospy
00025 import os
00026 
00027 from math import sin, cos, pi
00028 from geometry_msgs.msg import Quaternion, Twist, Pose
00029 from nav_msgs.msg import Odometry
00030 from tf.broadcaster import TransformBroadcaster
00031  
00032 """ Class to receive Twist commands and publish Odometry data """
00033 class BaseController:
00034     def __init__(self, arduino, base_frame):
00035         self.arduino = arduino
00036         self.base_frame = base_frame
00037         self.rate = float(rospy.get_param("~base_controller_rate", 10))
00038         self.timeout = rospy.get_param("~base_controller_timeout", 1.0)
00039         self.stopped = False
00040                  
00041         pid_params = dict()
00042         pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") 
00043         pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
00044         pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") 
00045         pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
00046         pid_params['Kp'] = rospy.get_param("~Kp", 20)
00047         pid_params['Kd'] = rospy.get_param("~Kd", 12)
00048         pid_params['Ki'] = rospy.get_param("~Ki", 0)
00049         pid_params['Ko'] = rospy.get_param("~Ko", 50)
00050         
00051         self.accel_limit = rospy.get_param('~accel_limit', 0.1)
00052         self.motors_reversed = rospy.get_param("~motors_reversed", False)
00053         
00054         # Set up PID parameters and check for missing values
00055         self.setup_pid(pid_params)
00056             
00057         # How many encoder ticks are there per meter?
00058         self.ticks_per_meter = self.encoder_resolution * self.gear_reduction  / (self.wheel_diameter * pi)
00059         
00060         # What is the maximum acceleration we will tolerate when changing wheel speeds?
00061         self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
00062                 
00063         # Track how often we get a bad encoder count (if any)
00064         self.bad_encoder_count = 0
00065                         
00066         now = rospy.Time.now()    
00067         self.then = now # time for determining dx/dy
00068         self.t_delta = rospy.Duration(1.0 / self.rate)
00069         self.t_next = now + self.t_delta
00070 
00071         # internal data        
00072         self.enc_left = None            # encoder readings
00073         self.enc_right = None
00074         self.x = 0                      # position in xy plane
00075         self.y = 0
00076         self.th = 0                     # rotation in radians
00077         self.v_left = 0
00078         self.v_right = 0
00079         self.v_des_left = 0             # cmd_vel setpoint
00080         self.v_des_right = 0
00081         self.last_cmd_vel = now
00082 
00083         # subscriptions
00084         rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
00085         
00086         # Clear any old odometry info
00087         self.arduino.reset_encoders()
00088         
00089         # Set up the odometry broadcaster
00090         self.odomPub = rospy.Publisher('odom', Odometry)
00091         self.odomBroadcaster = TransformBroadcaster()
00092         
00093         rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
00094         rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")
00095         
00096     def setup_pid(self, pid_params):
00097         # Check to see if any PID parameters are missing
00098         missing_params = False
00099         for param in pid_params:
00100             if pid_params[param] == "":
00101                 print("*** PID Parameter " + param + " is missing. ***")
00102                 missing_params = True
00103         
00104         if missing_params:
00105             os._exit(1)
00106                 
00107         self.wheel_diameter = pid_params['wheel_diameter']
00108         self.wheel_track = pid_params['wheel_track']
00109         self.encoder_resolution = pid_params['encoder_resolution']
00110         self.gear_reduction = pid_params['gear_reduction']
00111         
00112         self.Kp = pid_params['Kp']
00113         self.Kd = pid_params['Kd']
00114         self.Ki = pid_params['Ki']
00115         self.Ko = pid_params['Ko']
00116         
00117         self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko)
00118 
00119     def poll(self):
00120         now = rospy.Time.now()
00121         if now > self.t_next:
00122             # Read the encoders
00123             try:
00124                 left_enc, right_enc = self.arduino.get_encoder_counts()
00125             except:
00126                 self.bad_encoder_count += 1
00127                 rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
00128                 return
00129                             
00130             dt = now - self.then
00131             self.then = now
00132             dt = dt.to_sec()
00133             
00134             # calculate odometry
00135             if self.enc_left == None:
00136                 dright = 0
00137                 dleft = 0
00138             else:
00139                 dright = (right_enc - self.enc_right) / self.ticks_per_meter
00140                 dleft = (left_enc - self.enc_left) / self.ticks_per_meter
00141 
00142             self.enc_right = right_enc
00143             self.enc_left = left_enc
00144             
00145             dxy_ave = (dright + dleft) / 2.0
00146             dth = (dright - dleft) / self.wheel_track
00147             vxy = dxy_ave / dt
00148             vth = dth / dt
00149                 
00150             if (dxy_ave != 0):
00151                 dx = cos(dth) * dxy_ave
00152                 dy = -sin(dth) * dxy_ave
00153                 self.x += (cos(self.th) * dx - sin(self.th) * dy)
00154                 self.y += (sin(self.th) * dx + cos(self.th) * dy)
00155     
00156             if (dth != 0):
00157                 self.th += dth 
00158     
00159             quaternion = Quaternion()
00160             quaternion.x = 0.0 
00161             quaternion.y = 0.0
00162             quaternion.z = sin(self.th / 2.0)
00163             quaternion.w = cos(self.th / 2.0)
00164     
00165             # Create the odometry transform frame broadcaster.
00166             self.odomBroadcaster.sendTransform(
00167                 (self.x, self.y, 0), 
00168                 (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
00169                 rospy.Time.now(),
00170                 self.base_frame,
00171                 "odom"
00172                 )
00173     
00174             odom = Odometry()
00175             odom.header.frame_id = "odom"
00176             odom.child_frame_id = self.base_frame
00177             odom.header.stamp = now
00178             odom.pose.pose.position.x = self.x
00179             odom.pose.pose.position.y = self.y
00180             odom.pose.pose.position.z = 0
00181             odom.pose.pose.orientation = quaternion
00182             odom.twist.twist.linear.x = vxy
00183             odom.twist.twist.linear.y = 0
00184             odom.twist.twist.angular.z = vth
00185 
00186             self.odomPub.publish(odom)
00187             
00188             if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
00189                 self.v_des_left = 0
00190                 self.v_des_right = 0
00191                 
00192             if self.v_left < self.v_des_left:
00193                 self.v_left += self.max_accel
00194                 if self.v_left > self.v_des_left:
00195                     self.v_left = self.v_des_left
00196             else:
00197                 self.v_left -= self.max_accel
00198                 if self.v_left < self.v_des_left:
00199                     self.v_left = self.v_des_left
00200             
00201             if self.v_right < self.v_des_right:
00202                 self.v_right += self.max_accel
00203                 if self.v_right > self.v_des_right:
00204                     self.v_right = self.v_des_right
00205             else:
00206                 self.v_right -= self.max_accel
00207                 if self.v_right < self.v_des_right:
00208                     self.v_right = self.v_des_right
00209             
00210             # Set motor speeds in encoder ticks per PID loop
00211             if not self.stopped:
00212                 self.arduino.drive(self.v_left, self.v_right)
00213                 
00214             self.t_next = now + self.t_delta
00215             
00216     def stop(self):
00217         self.stopped = True
00218         self.arduino.drive(0, 0)
00219             
00220     def cmdVelCallback(self, req):
00221         # Handle velocity-based movement requests
00222         self.last_cmd_vel = rospy.Time.now()
00223         
00224         x = req.linear.x         # m/s
00225         th = req.angular.z       # rad/s
00226 
00227         if x == 0:
00228             # Turn in place
00229             right = th * self.wheel_track  * self.gear_reduction / 2.0
00230             left = -right
00231         elif th == 0:
00232             # Pure forward/backward motion
00233             left = right = x
00234         else:
00235             # Rotation about a point in space
00236             left = x - th * self.wheel_track  * self.gear_reduction / 2.0
00237             right = x + th * self.wheel_track  * self.gear_reduction / 2.0
00238             
00239         self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE)
00240         self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE)
00241         
00242 
00243         
00244 
00245     
00246 
00247     


ros_arduino_python
Author(s): Patrick Goebel
autogenerated on Thu Jun 6 2019 21:43:59