In this section we show how to use the computer keyboard to control the robot chassis, and the movement of the robot arm in the ROS has teleop_twist_keyboard package to control the movement of the robot chassis, but the lack of control of the robot, we can on this basis Modify the part that controls the arm control.

please see to get more detail information for teleop_twist_keyboard

1.Keyboard control diagram:

2.Control principle

2.1Chassis motion control

As described in the previous section, the ROS machine chassis is controlled by receiving Twist messages, so we only need to convert the control commands of the corresponding keyboard to Twist messages to achieve motion control of the chassis

2.2Motion control of the manipulator

In provides a servo servo control service

# A service to position a PWM

servo rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)

Control the arm, in fact, is to control the arm of the various joints of the steering gear, so we only need to control the steering wheel command, call serve can achieve the control of the robot

3. code

the following is code ,the file name is

  • Chassis motion control: retains the original teleop_twist_keyboard control logic
  • Robot control: Increases or decreases 5 ° control angle each time
#!/usr/bin/env python
import roslib
import rospy

from geometry_msgs.msg import Twist
from ros_arduino_msgs.srv import *
from math import pi as PI, degrees, radians
import sys, select, termios, tty

msg = """
Reading from the keyboard  and Publishing to Twist, Servo or sensor!
Moving around:
   u    i    o
   j    k    l

, : up (+z)
. : down (-z)

Left arm servo control:
+   1   2   3   4   5   6
-   q   w   e   r   t   y  
Right arm servo control:
+   a   s   d   f   g   h
-   z   x   c   v   b   n 

p : init the servo

CTRL-C to quit

moveBindings = {



def getradians(angle):
    return PI*angle/180

def getKey():
    tty.setraw(sys.stdin.fileno())[sys.stdin], [], [], 0)
    key =
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

def servoWrite(servoNum, value):
        print servoNum
            print value
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e

def vels(speed,turn):
    return "currently:\tspeed %s\tturn %s " % (speed,turn)

if __name__=="__main__":
        settings = termios.tcgetattr(sys.stdin)

    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)

    speed = rospy.get_param("~speed", 0.25)
    turn = rospy.get_param("~turn", 1.0)
    x = 0
    y = 0
    z = 0
    th = 0
    status = 0  

        print msg
        print vels(speed,turn)
        servoWrite(0, radians(armServoValues[0]))
        servoWrite(1, radians(armServoValues[1]))
        servoWrite(2, radians(armServoValues[2]))
        servoWrite(3, radians(armServoValues[3]))
        servoWrite(4, radians(armServoValues[4]))
        servoWrite(5, radians(armServoValues[5]))
        servoWrite(6, radians(armServoValues[6]))
            key = getKey()
            print key
            if key in moveBindings.keys():
                x = moveBindings[key][0]
                y = moveBindings[key][1]
                z = moveBindings[key][2]
                th = moveBindings[key][3]
                twist = Twist()
                twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;
                twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
            elif key in armServos.keys():  
                    if armServoValues[armServos[key][0]]<=0:
                    if armServoValues[armServos[key][0]]>=180:
                print armServoValues[armServos[key][0]]
                servoWrite(armServos[key][0], radians(armServoValues[armServos[key][0]]))
                x = 0
                y = 0
                z = 0
                th = 0
                if (key == '\x03'):

    except BaseException,e:
        print e

        twist = Twist()
        twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0

            termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

4.Start keyboard control

rosrun teleop_twist_keyboard

Leave a Reply

Scroll to top
%d bloggers like this: