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


element
Author(s): Patrick Goebel
autogenerated on Mon Jan 6 2014 11:16:54