keyboard teleop but publish a same message at a fixed rate?

asked 2020-05-25 06:09:38 -0500

drtritm gravatar image

updated 2020-05-25 06:10:05 -0500

Hi everyone, I tried to modify the source of package keyboard_teleop_twist to be able to send a same twist message at a fixed rate, because I want to keep the same twist message instead of keeping pushing down the button. I tried to used rospy.Rate but it did not work. Here is my code:

import rospy from geometry_msgs.msg import Twist import roslib; roslib.load_manifest('teleop_twist_keyboard') import rospy import tty import sys import termios import select 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), } class teleop():

def __init__(self):

    rospy.init_node('teleop', anonymous=False)

    rospy.loginfo(" Press CtrL+c to stop ")

    rospy.on_shutdown(self.shutdown)

    self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi',
    Twist, queue_size=10)
    orig_settings = termios.tcgetattr(sys.stdin)

    tty.setcbreak(sys.stdin)

    rate = rospy.Rate(30);


    while not rospy.is_shutdown():
        key =sys.stdin.read(1)[0]
        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; twist.linear.y = y; twist.linear.z = z
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th
        self.cmd_vel.publish(twist)
        rate.sleep()

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, orig_settings)  
def shutdown(self):

    rospy.loginfo("Stopping")
    self.cmd_vel.publish(Twist())

    rospy.sleep(1)

if __name__ == '__main__': try: teleop() except: rospy.loginfo("end")

edit retag flag offensive close merge delete

Comments

PS: it just publishes the message if I push the button

drtritm gravatar image drtritm  ( 2020-05-25 06:18:07 -0500 )edit