base_controller_threads.py

Go to the documentation of this file.

#!/usr/bin/env python

"""
    ROS Base Controller Class for the cmRobot Element microcontroller
    Borrowed heavily from Mike Feguson's ArbotiX base_controller.py code.
    
    Created for The Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2010 Patrick Goebel.  All rights reserved.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.
    
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:
    
    http://www.gnu.org/licenses
"""
import roslib; roslib.load_manifest('element')
import rospy

from math import sin, cos, pi
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Pose
from nav_msgs.msg import Odometry
from tf.broadcaster import TransformBroadcaster
import threading

""" Class to send Twist commands and receive Odometry data """
class BaseController:
    def __init__(self, controller, thread_queue):
        self.controller = controller
        self.rate = float(rospy.get_param("~base_controller_rate", 10))
        self.timeout = rospy.get_param('~base_controller_timeout', 1.0)
        self.interval = 1.0 / self.rate
        
        #self.pid_params = rospy.get_param("~pid_params", {})

        self.pid_params = dict()
        self.pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") 
        self.pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
        self.pid_params['encoder_type'] = rospy.get_param("~encoder_type", 1) 
        self.pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") 
        self.pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
        self.pid_params['motors_reversed'] = rospy.get_param("~motors_reversed", False)
        
        self.pid_params['init_pid'] = rospy.get_param("~init_pid", False)  
        self.pid_params['VPID_P'] = rospy.get_param("~VPID_P", "")
        self.pid_params['VPID_I'] = rospy.get_param("~VPID_I", "")
        self.pid_params['VPID_D']  = rospy.get_param("~VPID_D", "")
        self.pid_params['VPID_L'] = rospy.get_param("~VPID_L", "")
        self.pid_params['DPID_P'] = rospy.get_param("~DPID_P", "")
        self.pid_params['DPID_I'] = rospy.get_param("~DPID_I", "")
        self.pid_params['DPID_D'] = rospy.get_param("~DPID_D", "")
        self.pid_params['DPID_A'] = rospy.get_param("~DPID_A", "")
        self.pid_params['DPID_B'] = rospy.get_param("~DPID_B", "")
        
        self.controller.setup_base_controller(self.pid_params)
        
        self.ticks_per_meter = float(self.controller.ticks_per_meter)
        self.wheel_track = self.controller.wheel_track
        self.gear_reduction = self.controller.gear_reduction
        self.encoder_resolution = self.controller.encoder_resolution
        self.bad_encoder_count = 0
                        
        now = rospy.Time.now()    
        self.then = now # time for determining dx/dy

        # internal data        
        self.enc_left = None            # encoder readings
        self.enc_right = None
        self.x = 0                      # position in xy plane
        self.y = 0
        self.th = 0                     # rotation in radians
        self.v_left = 0
        self.v_right = 0
        self.last_cmd_vel = rospy.Time.now()

        # subscriptions
        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
        
        # Clear any old odometry info
        self.controller.clear_encoder([1, 2])
        
        # Set up the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry)
        self.odomBroadcaster = TransformBroadcaster()
        
        rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.ticks_per_meter) + " ticks per meter")
        rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz")
        
    def poll_odom(self, thread_queue):
        t = threading.Timer(self.interval, self.poll_odom, [thread_queue])
        t.daemon = True
        t.start()
        thread_queue.put(self.get_odom)

    def get_odom(self):
        now = rospy.Time.now()
        
        # Read the encoders
        try:
            right_enc, left_enc = self.controller.get_encoder_count([1, 2])
        except:
            self.bad_encoder_count += 1
            rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
            return
            
            #rospy.logdebug("Encoder counts: " + str(self.enc_left) + ", " + str(self.enc_right))
        
        dt = now - self.then
        self.then = now
        dt = dt.to_sec()
        
        # calculate odometry
        if self.enc_left == None:
            dleft = 0
            dright = 0
        else:
            dleft = (left_enc - self.enc_left) / self.ticks_per_meter
            dright = (right_enc - self.enc_right) / self.ticks_per_meter
        
        self.enc_left = left_enc
        self.enc_right = right_enc
        
        dxy_ave = (dleft + dright) / 2.0
        dth = (dright - dleft) / self.wheel_track
        vxy = dxy_ave / dt
        vth = dth / dt

        if (dxy_ave != 0):
            dx = cos(dth) * dxy_ave
            dy = -sin(dth) * dxy_ave
            self.x += (cos(self.th) * dx - sin(self.th) * dy)
            self.y += (sin(self.th) * dx + cos(self.th) * dy)

        if (dth != 0):
            self.th += dth 

        quaternion = Quaternion()
        quaternion.x = 0.0 
        quaternion.y = 0.0
        quaternion.z = sin(self.th / 2.0)
        quaternion.w = cos(self.th / 2.0)

        # Create the odometry transform frame broadcaster.
        self.odomBroadcaster.sendTransform(
            (self.x, self.y, 0), 
            (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            rospy.Time.now(),
            "base_link",
            "odom"
            )

        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.child_frame_id = "base_link"
        odom.header.stamp = now
        odom.pose.pose.position.x = self.x
        odom.pose.pose.position.y = self.y
        odom.pose.pose.position.z = 0
        odom.pose.pose.orientation = quaternion
        odom.twist.twist.linear.x = vxy
        odom.twist.twist.linear.y = 0
        odom.twist.twist.angular.z = vth

        self.odomPub.publish(odom)
        
        if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
            self.v_left = 0
            self.v_right = 0
        
        # Set motor speeds in meters per second.
        print self.v_left, self.v_right
        self.controller.mogo_m_per_s([1, 2], [self.v_right, self.v_left])         
            
    def cmdVelCallback(self, req):
        # Handle velocity-based movement requests
        self.last_cmd_vel = rospy.Time.now()
        
        x = req.linear.x         # m/s
        th = req.angular.z       # rad/s

        if x == 0:
            # Turn in place
            right = th * self.wheel_track  * self.gear_reduction / 2.0
            left = -right
        elif th == 0:
            # Pure forward/backward motion
            left = right = x
        else:
            # Rotation about a point in space
            left = x - th * self.wheel_track  * self.gear_reduction / 2.0
            right = x + th * self.wheel_track  * self.gear_reduction / 2.0
            
        self.v_left = left
        self.v_right = right
        
    def stop(self):
        print "Shutting down base controller"