ubuntu ROS python键盘控制小车移动

问题描述

想要实现像小乌龟范例那样，键盘控制机器人移动。

解决方法

这种控制事件用python实现即可，不需要用c++。用到的是curses库。这样做的好处是，不用下载另外的库，如pygame等。

直接上代码：

#!/usr/bin/env python

import sys
import rospy
import curses
from geometry_msgs.msg import Twist




def main(stdscr):
    while True:
        keycode = stdscr.getch()
        if keycode == curses.KEY_UP:
            sys.stdout.write(' ahead ')
            twist.linear.x=2
            pub.publish(twist)
            twist.linear.x = 0
        elif keycode == curses.KEY_DOWN:
            sys.stdout.write(' back ')
            twist.linear.x=-2
            pub.publish(twist)
            twist.linear.x = 0
        elif keycode == curses.KEY_LEFT:
            sys.stdout.write(' left ')
            twist.linear.y=2
            pub.publish(twist)
            twist.linear.y = 0
        elif keycode == curses.KEY_RIGHT:
            sys.stdout.write(' right ')
            twist.linear.y= -2
            pub.publish(twist)
            twist.linear.y = 0
        elif keycode == ord('w'):
            sys.stdout.write(' up ')
            twist.linear.z= 2
            pub.publish(twist)
            twist.linear.z = 0
        elif keycode == ord('s'):
            sys.stdout.write(' down ')
            twist.linear.z= -2
            pub.publish(twist)
            twist.linear.z = 0
        elif keycode == ord('a'):
            sys.stdout.write(' turn_L ')
            twist.angular.z= 2
            pub.publish(twist)
            twist.angular.z= 0
        elif keycode == ord('d'):
            sys.stdout.write(' turn_R ')
            twist.angular.z= -2
            pub.publish(twist)
            twist.angular.z= 0
        elif keycode == ord('q'):
            break
        sys.stdout.flush()


if __name__ == '__main__':
    rospy.init_node('send_vel')
    pub = rospy.Publisher('/robot/cmd_vel', Twist, queue_size=1000)#改成自己的topic
    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
    curses.wrapper(main)

虽然代码看上去很长，但实际上很简单，而且很有规律。这样就实现了方向键控制前进后退等移动，wasd建，控制上升下降等，看具体的设计。按q退出。控制时，是在命令行里控制。