raptor_teleop

#!/usr/bin/env python

# Copyright (c) 2011, 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 rospy
from geometry_msgs.msg import Twist
import traceback
import threading

import rospy
import diagnostic_updater
import diagnostic_msgs

import sys, select, os
if os.name == 'nt':
  import msvcrt
else:
  import tty, termios

MAX_LIN_VEL = 2
MAX_ANG_VEL = 2


LIN_VEL_STEP_SIZE = 0.2
ANG_VEL_STEP_SIZE = 0.2

msg = """
     CONTROL DEL RAPTOR
---------------------------
Controles:

                  arriba

     <<izquierda    ok    derecha>>

                  abajo

    (ok-enter: para frenar por completo)
   (e: finalizar conexion con el RAPTOR)
      (r: iniciar marcha y adelante)
"""

e = """
Communications Failed
"""
import sys,tty,termios
class _Getch:       
    def __call__(self):
            fd = sys.stdin.fileno()
            old_settings = termios.tcgetattr(fd)
            try:
            	tty.setraw(sys.stdin.fileno())
    	        ch = sys.stdin.read(1)
            finally:
                termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
            return ch

def get():
    inkey = _Getch()
    while(1):
            k=inkey()
            if k!='':break
    #print 'you pressed', ord(k)
    return k

def vels(target_linear_vel, target_angular_vel):
    return "currently:\tlinear vel %s\t angular vel %s " % (target_linear_vel,target_angular_vel)

def makeSimpleProfile(output, input, slop):
    if input > output:
        output = min( input, output + slop )
    elif input < output:
        output = max( input, output - slop )
    else:
        output = input

    return output

def constrain(input, low, high):
    if input < low:
      input = low
    elif input > high:
      input = high
    else:
      input = input

    return input

def checkLinearLimitVelocity(vel):

      vel = constrain(vel, -MAX_LIN_VEL, MAX_LIN_VEL)


      return vel

def checkAngularLimitVelocity(vel):

      vel = constrain(vel, -MAX_ANG_VEL, MAX_ANG_VEL)

      return vel

if __name__=="__main__":
    if os.name != 'nt':
        settings = termios.tcgetattr(sys.stdin)

    rospy.init_node('diff_drive')
    pub = rospy.Publisher('/roboclaw/speed_command', Twist, queue_size=10)


    status = 0
    target_linear_vel   = 0.0
    target_angular_vel  = 0.0
    control_linear_vel  = 0.0
    control_angular_vel = 0.0

    try:
        print(msg)
        while(1):
            key = ord(get())
            if key == 65 :
            	if target_linear_vel < 0 and target_linear_vel >= -0.8:
            		target_linear_vel=0
            		status = status + 1
            		print(vels(target_linear_vel,target_angular_vel)) 
            	elif target_linear_vel >= 0 and target_linear_vel < 0.8:
            		target_linear_vel = 0.8
            		status = status + 1
            		print(vels(target_linear_vel,target_angular_vel)) 
            	else:
            		target_linear_vel = checkLinearLimitVelocity(target_linear_vel + LIN_VEL_STEP_SIZE)
            		status = status + 1
            		print(vels(target_linear_vel,target_angular_vel))
            elif key == 66 :
            	if target_linear_vel > 0 and target_linear_vel <= 0.8:
            		target_linear_vel = 0
            		status = status + 1
            		print(vels(target_linear_vel,target_angular_vel)) 
            	elif target_linear_vel <= 0 and target_linear_vel > -0.8:
            		target_linear_vel = -0.8
            		status = status + 1
            		print(vels(target_linear_vel,target_angular_vel)) 
            	else:
            		target_linear_vel = checkLinearLimitVelocity(target_linear_vel - LIN_VEL_STEP_SIZE)
            		status = status + 1
            		print(vels(target_linear_vel,target_angular_vel))
            elif key == 68 :
            	if target_angular_vel < 0 and target_angular_vel >= -1.0:
            		target_angular_vel = 0
            		status = status + 1
            		print(vels(target_linear_vel,target_angular_vel)) 
            	elif target_angular_vel >= 0 and target_angular_vel < 1.0:
            		target_angular_vel = 1.0
            		status = status + 1
            		print(vels(target_linear_vel,target_angular_vel)) 
            	else:
            		target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
            		status = status + 1
            		print(vels(target_linear_vel,target_angular_vel))
            elif key == 67 :
            	if target_angular_vel > 0 and target_angular_vel <= 1.0:
            		target_angular_vel = 0
            		status = status + 1
            		print(vels(target_linear_vel,target_angular_vel)) 
            	elif target_angular_vel <= 0 and target_angular_vel > -1.0:
            		target_angular_vel = -1.0
            		status = status + 1
            		print(vels(target_linear_vel,target_angular_vel)) 
            	else:
            		target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
            		status = status + 1
            		print(vels(target_linear_vel,target_angular_vel))
            elif key == 13 :
            	target_linear_vel   = 0.0
            	control_linear_vel  = 0.0
            	target_angular_vel  = 0.0
            	control_angular_vel = 0.0
            	print(vels(target_linear_vel, target_angular_vel))
            else:
            	if (key == 101):
            		break
            	if status == 20 :
            		print(msg)
            		status = 0

            twist = Twist()

            control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, (LIN_VEL_STEP_SIZE/2.0))
            twist.linear.x = control_linear_vel; twist.linear.y = 0.0; twist.linear.z = 0.0

            control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
            twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel

            pub.publish(twist)

    except:
        print(e)

    finally:
        twist = Twist()
        twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
        twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
        pub.publish(twist)

    if os.name != 'nt':
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)