head_pedals.py

Go to the documentation of this file.

#! /usr/bin/env python
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
# 
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
# 
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of the Willow Garage, Inc. nor the names of its
#       contributors may be used to endorse or promote products derived from
#       this software without specific prior written permission.
# 
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.


import roslib
roslib.load_manifest('teleop_microscribe')
import rospy
from joy.msg import *
from pr2_controllers_msgs.msg import *
from trajectory_msgs.msg import *
from pr2_cockpit_msgs.srv import *

rospy.init_node('head_pedals')

MIN_PAN = -2.7
MAX_PAN = 2.7
MIN_TILT = -0.4
MAX_TILT = 1.4
    
class PedalHead:

    def __init__(self, pedal_topic):
        self.pan = 0.0
        self.tilt = 0.0
        self.enabled = True

        self.last_pedal_time = None

        self.pub_head = rospy.Publisher('head_traj_controller/command', JointTrajectory)
        rospy.Service('~enable', Enable, self.enable)
        rospy.Subscriber(pedal_topic, Joy, self.pedal_cb)

    def enable(self, req):
        self.enabled = req.enable
        return EnableResponse(True, "")

    def pedal_cb(self, msg):
        now = rospy.get_rostime()
        if not self.last_pedal_time or (now - self.last_pedal_time) > rospy.Duration(5.0) :
            self.last_pedal_time = now
            return
        dt = now - self.last_pedal_time
        self.last_pedal_time = now
        
        PAN = 2
        DOWN = 1
        UP = 0

        # Transform the tilt axes to be in [0,1]
        up_command = -(msg.axes[UP] - 1.0) / 2.
        down_command = -(msg.axes[DOWN] - 1.0) / 2.

        pan_vel = -msg.axes[PAN]
        tilt_vel = (down_command - up_command)
        horizon = 0.2

        if self.enabled:
            self.pan += pan_vel * dt.to_sec()
            self.tilt += tilt_vel * dt.to_sec()

            self.pan = max(MIN_PAN, min(self.pan, MAX_PAN))
            self.tilt = max(MIN_TILT, min(self.tilt, MAX_TILT))

            traj = JointTrajectory()
            traj.header.stamp = rospy.get_rostime() + rospy.Duration(0.01)
            traj.joint_names = ['head_pan_joint', 'head_tilt_joint']
            traj.points = [
                JointTrajectoryPoint(positions = [self.pan + pan_vel * horizon,
                                                  self.tilt + tilt_vel * horizon],
                                     velocities = [pan_vel, tilt_vel],
                                     time_from_start = rospy.Duration(horizon))]
            self.pub_head.publish(traj)

def main():
    h = PedalHead('l_rudder_pedals')
    rospy.spin()

    
if __name__ == '__main__':
    main()