Publisher-Subscriber node not invoking callback function in python

asked 2020-06-10 01:22:16 -0600

CuatroPalos
16 ●1 ●2 ●1

updated 2020-06-10 01:24:56 -0600

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 = {
        'w':(1,0,0,1),
        'a':(0,0,0,1),
        'd':(0,0,0,-1),
        's':(-1,0,0,1),
        }

def getKey():
    tty.setraw(sys.stdin.fileno())
    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
corr=0
key='k'
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]
    print(yaw)

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

    while not rospy.is_shutdown() and flag == False:  
        print(ref,yaw)

        key=getKey()
        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

        else:
            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
        pub.publish(twist)
        rate.sleep()  
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

edit retag flag offensive close merge delete

Comments

I tried your code here and seems to be updating the Yaw variable. What are exactly your goal and your problem with this code?

Teo Cardoso ( 2020-06-10 11:24:09 -0600 )edit

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.

CuatroPalos ( 2020-06-11 22:59:53 -0600 )edit

add a comment

0

answered 2020-06-11 23:05:43 -0600

CuatroPalos
16 ●1 ●2 ●1

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):
        pass

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


    def _getch():
        global key
        try:
            fd = sys.stdin.fileno()
            old_settings = termios.tcgetattr(fd)
            try:
                tty.setraw(sys.stdin.fileno())
                select.select([sys.stdin], [], [], 0)
                key = sys.stdin.read(1)
            finally:
                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)
    try:
        result = _getch()
    finally:
       signal.alarm(0)
    return result