base_controller.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     ROS Base Controller Class for the cmRobot Element 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('element')
00023 import rospy
00024 
00025 from math import sin, cos, pi
00026 from geometry_msgs.msg import Quaternion, Twist, Pose
00027 from nav_msgs.msg import Odometry
00028 from tf.broadcaster import TransformBroadcaster
00029  
00030 """ Class to send Twist commands and receive Odometry data """
00031 class BaseController:
00032     def __init__(self, controller):
00033         self.controller = controller
00034         self.rate = float(rospy.get_param("~base_controller_rate", 10))
00035         self.timeout = rospy.get_param('~base_controller_timeout', 1.0)
00036                  
00037         #self.pid_params = rospy.get_param("~pid_params", {})
00038 
00039         self.pid_params = dict()
00040         self.pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") 
00041         self.pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
00042         self.pid_params['encoder_type'] = rospy.get_param("~encoder_type", 1) 
00043         self.pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") 
00044         self.pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
00045         self.pid_params['motors_reversed'] = rospy.get_param("~motors_reversed", False)
00046         
00047         self.pid_params['init_pid'] = rospy.get_param("~init_pid", False)  
00048         self.pid_params['VPID_P'] = rospy.get_param("~VPID_P", "")
00049         self.pid_params['VPID_I'] = rospy.get_param("~VPID_I", "")
00050         self.pid_params['VPID_D']  = rospy.get_param("~VPID_D", "")
00051         self.pid_params['VPID_L'] = rospy.get_param("~VPID_L", "")
00052         self.pid_params['DPID_P'] = rospy.get_param("~DPID_P", "")
00053         self.pid_params['DPID_I'] = rospy.get_param("~DPID_I", "")
00054         self.pid_params['DPID_D'] = rospy.get_param("~DPID_D", "")
00055         self.pid_params['DPID_A'] = rospy.get_param("~DPID_A", "")
00056         self.pid_params['DPID_B'] = rospy.get_param("~DPID_B", "")
00057         
00058         self.controller.setup_base_controller(self.pid_params)
00059         
00060         self.ticks_per_meter = float(self.controller.ticks_per_meter)
00061         self.wheel_track = self.controller.wheel_track
00062         self.gear_reduction = self.controller.gear_reduction
00063         self.encoder_resolution = self.controller.encoder_resolution
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.last_cmd_vel = now
00080 
00081         # subscriptions
00082         rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
00083         
00084         # Clear any old odometry info
00085         self.controller.clear_encoder([1, 2])
00086         
00087         # Set up the odometry broadcaster
00088         self.odomPub = rospy.Publisher('odom', Odometry)
00089         self.odomBroadcaster = TransformBroadcaster()
00090         
00091         rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.ticks_per_meter) + " ticks per meter")
00092         rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz")
00093 
00094     def poll(self):
00095         now = rospy.Time.now()
00096         if now > self.t_next:
00097             # Read the encoders
00098             try:
00099                 right_enc, left_enc = self.controller.get_encoder_count([1, 2])
00100             except:
00101                 self.bad_encoder_count += 1
00102                 rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
00103                 return
00104                 
00105                 #rospy.logdebug("Encoder counts: " + str(self.enc_left) + ", " + str(self.enc_right))
00106             
00107             dt = now - self.then
00108             self.then = now
00109             dt = dt.to_sec()
00110             
00111             # calculate odometry
00112             if self.enc_left == None:
00113                 dleft = 0
00114                 dright = 0
00115             else:
00116                 dleft = (left_enc - self.enc_left) / self.ticks_per_meter
00117                 dright = (right_enc - self.enc_right) / self.ticks_per_meter
00118             
00119             self.enc_left = left_enc
00120             self.enc_right = right_enc
00121             
00122             dxy_ave = (dleft + dright) / 2.0
00123             dth = (dright - dleft) / self.wheel_track
00124             vxy = dxy_ave / dt
00125             vth = dth / dt
00126     
00127             if (dxy_ave != 0):
00128                 dx = cos(dth) * dxy_ave
00129                 dy = -sin(dth) * dxy_ave
00130                 self.x += (cos(self.th) * dx - sin(self.th) * dy)
00131                 self.y += (sin(self.th) * dx + cos(self.th) * dy)
00132     
00133             if (dth != 0):
00134                 self.th += dth 
00135     
00136             quaternion = Quaternion()
00137             quaternion.x = 0.0 
00138             quaternion.y = 0.0
00139             quaternion.z = sin(self.th / 2.0)
00140             quaternion.w = cos(self.th / 2.0)
00141     
00142             # Create the odometry transform frame broadcaster.
00143             self.odomBroadcaster.sendTransform(
00144                 (self.x, self.y, 0), 
00145                 (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
00146                 rospy.Time.now(),
00147                 "base_link",
00148                 "odom"
00149                 )
00150     
00151             odom = Odometry()
00152             odom.header.frame_id = "odom"
00153             odom.child_frame_id = "base_link"
00154             odom.header.stamp = now
00155             odom.pose.pose.position.x = self.x
00156             odom.pose.pose.position.y = self.y
00157             odom.pose.pose.position.z = 0
00158             odom.pose.pose.orientation = quaternion
00159             odom.twist.twist.linear.x = vxy
00160             odom.twist.twist.linear.y = 0
00161             odom.twist.twist.angular.z = vth
00162     
00163             self.odomPub.publish(odom)
00164             
00165             if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
00166                 self.v_left = 0
00167                 self.v_right = 0
00168             
00169             # Set motor speeds in meters per second.
00170             self.controller.mogo_m_per_s([1, 2], [self.v_right, self.v_left])         
00171             
00172     def cmdVelCallback(self, req):
00173         # Handle velocity-based movement requests
00174         self.last_cmd_vel = rospy.Time.now()
00175         
00176         x = req.linear.x         # m/s
00177         th = req.angular.z       # rad/s
00178 
00179         if x == 0:
00180             # Turn in place
00181             right = th * self.wheel_track  * self.gear_reduction / 2.0
00182             left = -right
00183         elif th == 0:
00184             # Pure forward/backward motion
00185             left = right = x
00186         else:
00187             # Rotation about a point in space
00188             left = x - th * self.wheel_track  * self.gear_reduction / 2.0
00189             right = x + th * self.wheel_track  * self.gear_reduction / 2.0
00190             
00191         self.v_left = left
00192         self.v_right = right
00193         
00194     def stop(self):
00195         print "Shutting down base controller"
00196 
00197         
00198 
00199     
00200 
00201     


element
Author(s): Patrick Goebel
autogenerated on Sun Oct 5 2014 23:44:54