Rosanswers logo

Hello, so I'm trying to do a node where I publish a Twist message on cmd_vel to control a differential robot using the keyboard. I'm doing a PID control to correct some orientation problems on the robot, that's why I need to read a message from /odometry/filtered topic. The problem I've ran into is that at the callback function here called yaw_callback(), the variable yaw doesn't seems to be updating everytime a new message is read as I expected, I tried rospy.loginfo(data) to check if I'm receiving and all seems to be ok, but when doing print(yaw) it just prints once until I press another key. I've also tried making the publisher as the callback function of my subscriber but I had the same problem :(

#!/usr/bin/env python

from __future__ import print_function
import roslib; roslib.load_manifest('teleop_twist_keyboard')
import rospy

from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion as q2e
from tf.transformations import quaternion_from_euler as e2q

import sys, select, termios, tty

moveBindings = {

def getKey():
    select.select([sys.stdin], [], [], 0)
    key = sys.stdin.read(1)
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

def vels(speed,turn):
    return "currently:\tspeed %s\tturn %s " % (speed,turn)
speed = rospy.get_param("~speed", 0.2)
turn = rospy.get_param("~turn", 0.5)
Kd = 0
Kp = 1
x = 0
y = 0
z = 0
th = 0
yaw = 0
flag = False
flag2 = False
ref = 0
err = 0
err_der = 0
errant = 0
quat = e2q(0,0,0)
# When I receive a new odom message
def yaw_callback(data): 
    global yaw,quat,euler
#    rospy.loginfo(data)
    quat[0] = data.pose.pose.orientation.x
    quat[1] = data.pose.pose.orientation.y
    quat[2] = data.pose.pose.orientation.z
    quat[3] = data.pose.pose.orientation.w
    euler = q2e(quat)
    yaw = euler[2]

if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin)
    sub = rospy.Subscriber('odometry/filtered',Odometry, yaw_callback)
    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)

    while not rospy.is_shutdown() and flag == False:  

        if key in moveBindings.keys():
            x = moveBindings[key][0]
            y = moveBindings[key][1]
            z = moveBindings[key][2]
            th = moveBindings[key][3]
            if (key == 'w' or key == 's') and flag2 == False: 
                ref = yaw
                corr = 0
                flag2 = True
            if key == 'a' or key == 'd':
                flag2 = False
                err = 0
                err_der = 0
                errant = 0
                corr = 1

            x = 0
            y = 0
            z = 0
            th = 0
            if (key == '\x03'):
                flag = True

        if flag2 == True:
            err = ref - yaw
            err_der = (err-errant)/0.1 
            errant = err
            corr = Kd*err_der + Kp*err
        twist = Twist()
        twist.linear.x = x*speed; twist.linear.y = 0; twist.linear.z = 0;
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn*corr
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

Originally posted by CuatroPalos on ROS Answers with karma: 16 on 2020-06-10

Post score: 0

Original comments

Comment by Teo Cardoso on 2020-06-10:
I tried your code here and seems to be updating the Yaw variable. What are exactly your goal and your problem with this code?

Comment by CuatroPalos on 2020-06-11:
The problem was that that getKey() function blocked the loop and didn't continued until any key was pressed. I solved it by creating another function that times out if no key is pressed.


1 Answer 1


Rosanswers logo

I solved my problem by using a function that times out if no key is pressed. It was really hard to find how to make this, so here it is if someone needs it.

def getch(timeout=0.1):
    """Returns single character of raw input or timesout"""
    class TimeoutException (Exception):

    def signalHandler (signum, frame):
        raise TimeoutException ()

    def _getch():
        global key
            fd = sys.stdin.fileno()
            old_settings = termios.tcgetattr(fd)
                select.select([sys.stdin], [], [], 0)
                key = sys.stdin.read(1)
                termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
                return key
        except TimeoutException:
            print("Timed out")
            return key
    signal.signal(signal.SIGALRM, signalHandler)
    signal.setitimer(signal.ITIMER_REAL, timeout)
        result = _getch()
    return result

Originally posted by CuatroPalos with karma: 16 on 2020-06-11

This answer was ACCEPTED on the original site

Post score: 0


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.