teleop_twist_keyboard.py not working as expected

[FrankyForkFingerz]

Hello,

if i run in an ros environment „rosrun teleop_twist_keyboard teleop_twist_keyboard.py“ to control the motors directions everthing is working fine except the directions to turn and drive curves.

In my opinion there are mixed with each other. For example if i press „l“ the base is moving counterclockwise and if i press „j“ the base is moving clockwise. Same for „o“ „u“ … What do you think?

[juvinski]

Hi,

have you found a solution? Mine is the same.

[FrankyForkFingerz]

Hi juvinski,

this problem has been a while but if i remember correct i was modifing the code of the "teleop_twist_keyboard.py" file until it was fitting.

I can try to look inside the code again but probably i have no time until the weekend to check that.

But i think the trick was to change in the "move bindings" the signs for "j" and "l" until it was fitting.

But i was not really happy with that solution, its just a workaround and not solving the rootcause in my opinion.

If you find a better solution please let me know.

Best regards

!/usr/bin/env python

from future import print_function

import threading

import roslib; roslib.load_manifest('teleop_twist_keyboard') import rospy

from geometry_msgs.msg import Twist

import sys, select, termios, tty

msg = """ Reading from the keyboard and Publishing to Twist!

Moving around: u i o j k l m , . For Holonomic mode (strafing), hold down the shift key:

U I O J K L M < > t : up (+z) b : down (-z) anything else : stop q/z : increase/decrease max speeds by 10% w/x : increase/decrease only linear speed by 10% e/c : increase/decrease only angular speed by 10% CTRL-C to quit """

moveBindings = { 'i':(1,0,0,0), 'o':(1,0,0,-1), 'j':(0,0,0,1), 'l':(0,0,0,-1), 'u':(1,0,0,1), ',':(-1,0,0,0), '.':(-1,0,0,1), 'm':(-1,0,0,-1), 'O':(1,-1,0,0), 'I':(1,0,0,0), 'J':(0,1,0,0), 'L':(0,-1,0,0), 'U':(1,1,0,0), '<':(-1,0,0,0), '>':(-1,-1,0,0), 'M':(-1,1,0,0), 't':(0,0,1,0), 'b':(0,0,-1,0), }

speedBindings={ 'q':(1.1,1.1), 'z':(.9,.9), 'w':(1.1,1), 'x':(.9,1), 'e':(1,1.1), 'c':(1,.9), }

class PublishThread(threading.Thread): def init(self, rate): super(PublishThread, self).init() self.publisher = rospy.Publisher('cmd_vel', Twist, queue_size = 1) self.x = 0.0 self.y = 0.0 self.z = 0.0 self.th = 0.0 self.speed = 0.0 self.turn = 0.0 self.condition = threading.Condition() self.done = False

    # Set timeout to None if rate is 0 (causes new_message to wait forever
    # for new data to publish)
    if rate != 0.0:
        self.timeout = 1.0 / rate
    else:
        self.timeout = None

    self.start()

def wait_for_subscribers(self):
    i = 0
    while not rospy.is_shutdown() and self.publisher.get_num_connections() == 0:
        if i == 4:
            print("Waiting for subscriber to connect to {}".format(self.publisher.name))
        rospy.sleep(0.5)
        i += 1
        i = i % 5
    if rospy.is_shutdown():
        raise Exception("Got shutdown request before subscribers connected")

def update(self, x, y, z, th, speed, turn):
    self.condition.acquire()
    self.x = x
    self.y = y
    self.z = z
    self.th = th
    self.speed = speed
    self.turn = turn
    # Notify publish thread that we have a new message.
    self.condition.notify()
    self.condition.release()

def stop(self):
    self.done = True
    self.update(0, 0, 0, 0, 0, 0)
    self.join()

def run(self):
    twist = Twist()
    while not self.done:
        self.condition.acquire()
        # Wait for a new message or timeout.
        self.condition.wait(self.timeout)

        # Copy state into twist message.
        twist.linear.x = self.x * self.speed
        twist.linear.y = self.y * self.speed
        twist.linear.z = self.z * self.speed
        twist.angular.x = 0
        twist.angular.y = 0
        twist.angular.z = self.th * self.turn

        self.condition.release()

        # Publish.
        self.publisher.publish(twist)

    # Publish stop message when thread exits.
    twist.linear.x = 0
    twist.linear.y = 0
    twist.linear.z = 0
    twist.angular.x = 0
    twist.angular.y = 0
    twist.angular.z = 0
    self.publisher.publish(twist)

def getKey(keytimeout): tty.setraw(sys.stdin.fileno()) rlist, , _ = select.select([sys.stdin], [], [], key_timeout) if rlist: key = sys.stdin.read(1) else: key = '' termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) return key

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

if name=="main": settings = termios.tcgetattr(sys.stdin)

rospy.init_node('teleop_twist_keyboard')

speed = rospy.get_param("~speed", 0.5)
turn = rospy.get_param("~turn", 1.0)
repeat = rospy.get_param("~repeat_rate", 0.0)
key_timeout = rospy.get_param("~key_timeout", 0.0)
if key_timeout == 0.0:
    key_timeout = None

pub_thread = PublishThread(repeat)

x = 0
y = 0
z = 0
th = 0
status = 0

try:
    pub_thread.wait_for_subscribers()
    pub_thread.update(x, y, z, th, speed, turn)

    print(msg)
    print(vels(speed,turn))
    while(1):
        key = getKey(key_timeout)
        if key in moveBindings.keys():
            x = moveBindings[key][0]
            y = moveBindings[key][1]
            z = moveBindings[key][2]
            th = moveBindings[key][3]
        elif key in speedBindings.keys():
            speed = speed * speedBindings[key][0]
            turn = turn * speedBindings[key][1]

            print(vels(speed,turn))
            if (status == 14):
                print(msg)
            status = (status + 1) % 15
        else:
            # Skip updating cmd_vel if key timeout and robot already
            # stopped.
            if key == '' and x == 0 and y == 0 and z == 0 and th == 0:
                continue
            x = 0
            y = 0
            z = 0
            th = 0
            if (key == '\x03'):
                break

        pub_thread.update(x, y, z, th, speed, turn)

except Exception as e:
    print(e)

finally:
    pub_thread.stop()

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