ROS探索总结-8.键盘控制

本文转载自古月居，原作者古月，原文链接：https://www.guyuehome.com/253。
键盘控制

如果尝试过前面的例子，有没有感觉每次让机器人移动还要在终端里输入指令，这也太麻烦了，有没有办法通过键盘来控制机器人的移动呢？答案室当然的了。我研究了其他几个机器人键盘控制的代码，还是有所收获的，最后移植到了smartcar上，实验成功。

一、创建控制包

首先，我们为键盘控制单独建立一个包：

roscreate-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp  

rosmake

如果你已经忘记了怎么建立包，请参考：http://www.ros.org/wiki/ROS/Tutorials/CreatingPackage

二、简单的消息发布

在机器人仿真中，主要控制机器人移动的就是Twist()结构，如果我们编程将这个结构通过程序发布成topic，自然就可以控制机器人了。我们先用简单的Python来尝试一下。

之前的模拟中，我们使用的都是在命令行下进行的消息发布，现在我们需要把这些命令转换成Python 代码，封装到一个单独的节点中去。针对之前的命令行，我们可以很简单的在smartcar_teleop /scripts文件夹下编写如下的控制代码：

#!/usr/bin/env python  

import roslib; roslib.load_manifest('smartcar_teleop')  

import rospy  

from geometry_msgs.msg import Twist  

from std_msgs.msg import String  

 

class Teleop:  

    def __init__(self):  

        pub = rospy.Publisher('cmd_vel', Twist)  

        rospy.init_node('smartcar_teleop')  

        rate = rospy.Rate(rospy.get_param('~hz', 1))  

        self.cmd = None  

 

        cmd = Twist()  

        cmd.linear.x = 0.2  

        cmd.linear.y = 0  

        cmd.linear.z = 0  

        cmd.angular.z = 0  

        cmd.angular.z = 0  

        cmd.angular.z = 0.5  

 

        self.cmd = cmd  

        while not rospy.is_shutdown():  

            str = "hello world %s" % rospy.get_time()  

            rospy.loginfo(str)  

            pub.publish(self.cmd)  

            rate.sleep()  

 

if __name__ == '__main__':Teleop()

python代码在ROS中是不需要编译的。先运行之前教程中用到的smartcar机器人，在rviz中进行显示，然后新建终端，输入如下命令：

   rosrun smartcar_teleop teleop.py

也可以建立一个launch文件运行：

<launch>  

  <arg name="cmd_topic" default="cmd_vel" />  

  <node pkg="smartcar_teleop" type="teleop.py" name="smartcar_teleop">  

    <remap from="cmd_vel" to="$(arg cmd_topic)" />  

  </node>  

</launch>

在rviz中看一下机器人是不是动起来了！

三、加入键盘控制

当然前边的程序不是我们要的，我们需要的键盘控制。

1、移植

因为ROS的代码具有很强的可移植性，所以用键盘控制的代码其实可以直接从其他机器人包中移植过来，在这里我主要参考的是erratic_robot，在这个机器人的代码中有一个erratic_teleop包，可以直接移植过来使用。

首先，我们将其中src文件夹下的keyboard.cpp代码文件直接拷贝到我们smartcar_teleop包的src文件夹下，然后修改CMakeLists.txt文件，将下列代码加入文件底部：

rosbuild_add_boost_directories()  

rosbuild_add_executable(smartcar_keyboard_teleop src/keyboard.cpp)  

target_link_libraries(smartcar_keyboard_teleop boost_thread)

编译完成后，运行smartcar模型。重新打开一个终端，打开键盘控制节点：
ROS探索总结-8.键盘控制

在终端中按下键盘里的“W”、“S”、“D”、“A”以及“Shift”键进行机器人的控制。效果如下图：
ROS探索总结-8.键盘控制

2、复用

因为代码是我们直接复制过来的，其中有很多与之前erratic机器人相关的变量，我们把代码稍作修改，变成自己机器人可读性较强的代码。

#include <termios.h>  

#include <signal.h>  

#include <math.h>  

#include <stdio.h>  

#include <stdlib.h>  

#include <sys/poll.h>  

 

#include <boost/thread/thread.hpp>  

#include <ros/ros.h>  

#include <geometry_msgs/Twist.h>  

 

#define KEYCODE_W 0x77  

#define KEYCODE_A 0x61  

#define KEYCODE_S 0x73  

#define KEYCODE_D 0x64  

 

#define KEYCODE_A_CAP 0x41  

#define KEYCODE_D_CAP 0x44  

#define KEYCODE_S_CAP 0x53  

#define KEYCODE_W_CAP 0x57  

 

class SmartCarKeyboardTeleopNode  

{  

    private:  

        double walk_vel_;  

        double run_vel_;  

        double yaw_rate_;  

        double yaw_rate_run_;  

 

        geometry_msgs::Twist cmdvel_;  

        ros::NodeHandle n_;  

        ros::Publisher pub_;  

 

    public:  

        SmartCarKeyboardTeleopNode()  

        {  

            pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);  

 

            ros::NodeHandle n_private("~");  

            n_private.param("walk_vel", walk_vel_, 0.5);  

            n_private.param("run_vel", run_vel_, 1.0);  

            n_private.param("yaw_rate", yaw_rate_, 1.0);  

            n_private.param("yaw_rate_run", yaw_rate_run_, 1.5);  

        }  

 

        ~SmartCarKeyboardTeleopNode() { }  

        void keyboardLoop();  

 

        void stopRobot()  

        {  

            cmdvel_.linear.x = 0.0;  

            cmdvel_.angular.z = 0.0;  

            pub_.publish(cmdvel_);  

        }  

};  

 

SmartCarKeyboardTeleopNode* tbk;  

int kfd = 0;  

struct termios cooked, raw;  

bool done;  

 

int main(int argc, char** argv)  

{  

    ros::init(argc,argv,"tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);  

    SmartCarKeyboardTeleopNode tbk;  

 

    boost::thread t = boost::thread(boost::bind(&SmartCarKeyboardTeleopNode::keyboardLoop, &tbk));  

 

    ros::spin();  

 

    t.interrupt();  

    t.join();  

    tbk.stopRobot();  

    tcsetattr(kfd, TCSANOW, &cooked);  

 

    return(0);  

}  

 

void SmartCarKeyboardTeleopNode::keyboardLoop()  

{  

    char c;  

    double max_tv = walk_vel_;  

    double max_rv = yaw_rate_;  

    bool dirty = false;  

    int speed = 0;  

    int turn = 0;  

 

    // get the console in raw mode  

    tcgetattr(kfd, &cooked);  

    memcpy(&raw, &cooked, sizeof(struct termios));  

    raw.c_lflag &=~ (ICANON | ECHO);  

    raw.c_cc[VEOL] = 1;  

    raw.c_cc[VEOF] = 2;  

    tcsetattr(kfd, TCSANOW, &raw);  

 

    puts("Reading from keyboard");  

    puts("Use WASD keys to control the robot");  

    puts("Press Shift to move faster");  

 

    struct pollfd ufd;  

    ufd.fd = kfd;  

    ufd.events = POLLIN;  

 

    for(;;)  

    {  

        boost::this_thread::interruption_point();  

 

        // get the next event from the keyboard  

        int num;  

 

        if ((num = poll(&ufd, 1, 250)) < 0)  

        {  

            perror("poll():");  

            return;  

        }  

        else if(num > 0)  

        {  

            if(read(kfd, &c, 1) < 0)  

            {  

                perror("read():");  

                return;  

            }  

        }  

        else  

        {  

            if (dirty == true)  

            {  

                stopRobot();  

                dirty = false;  

            }  

 

            continue;  

        }  

 

        switch(c)  

        {  

            case KEYCODE_W:  

                max_tv = walk_vel_;  

                speed = 1;  

                turn = 0;  

                dirty = true;  

                break;  

            case KEYCODE_S:  

                max_tv = walk_vel_;  

                speed = -1;  

                turn = 0;  

                dirty = true;  

                break;  

            case KEYCODE_A:  

                max_rv = yaw_rate_;  

                speed = 0;  

                turn = 1;  

                dirty = true;  

                break;  

            case KEYCODE_D:  

                max_rv = yaw_rate_;  

                speed = 0;  

                turn = -1;  

                dirty = true;  

                break;  

 

            case KEYCODE_W_CAP:  

                max_tv = run_vel_;  

                speed = 1;  

                turn = 0;  

                dirty = true;  

                break;  

            case KEYCODE_S_CAP:  

                max_tv = run_vel_;  

                speed = -1;  

                turn = 0;  

                dirty = true;  

                break;  

            case KEYCODE_A_CAP:  

                max_rv = yaw_rate_run_;  

                speed = 0;  

                turn = 1;  

                dirty = true;  

                break;  

            case KEYCODE_D_CAP:  

                max_rv = yaw_rate_run_;  

                speed = 0;  

                turn = -1;  

                dirty = true;  

                break;                

            default:  

                max_tv = walk_vel_;  

                max_rv = yaw_rate_;  

                speed = 0;  

                turn = 0;  

                dirty = false;  

        }  

        cmdvel_.linear.x = speed * max_tv;  

        cmdvel_.angular.z = turn * max_rv;  

        pub_.publish(cmdvel_);  

    }  

}

参考链接：

http://ros.org/wiki/turtlebot_teleop/Tutorials/Teleoperation

http://www.ros.org/wiki/simulator_gazebo/Tutorials/TeleopErraticSimulation

3、创新

虽然很多机器人的键盘控制使用的都是C++编写的代码，但是考虑到python的强大，我们还是需要尝试使用python来编写程序。

首先需要理解上面C++程序的流程。在上面的程序中，我们单独创建了一个线程来读取中断中的输入，然后根据输入发布不同的速度和角度消息。介于线程的概念还比较薄弱，在python中使用循环替代线程。然后需要考虑的只是如何使用python来处理中断中的输入字符，通过上网查找资料，发现使用的API和C++的基本是一致的。最终的程序如下：

#!/usr/bin/env python  

# -*- coding: utf-8 -*  

 

import  os  

import  sys  

import  tty, termios  

import roslib; roslib.load_manifest('smartcar_teleop')  

import rospy  

from geometry_msgs.msg import Twist  

from std_msgs.msg import String  

 

# 全局变量  

cmd = Twist()  

pub = rospy.Publisher('cmd_vel', Twist)  

 

def keyboardLoop():  

    #初始化  

    rospy.init_node('smartcar_teleop')  

    rate = rospy.Rate(rospy.get_param('~hz', 1))  

 

    #速度变量  

    walk_vel_ = rospy.get_param('walk_vel', 0.5)  

    run_vel_ = rospy.get_param('run_vel', 1.0)  

    yaw_rate_ = rospy.get_param('yaw_rate', 1.0)  

    yaw_rate_run_ = rospy.get_param('yaw_rate_run', 1.5)  

 

    max_tv = walk_vel_  

    max_rv = yaw_rate_  

 

    #显示提示信息  

    print "Reading from keyboard"  

    print "Use WASD keys to control the robot"  

    print "Press Caps to move faster"  

    print "Press q to quit"  

 

    #读取按键循环  

    while not rospy.is_shutdown():  

        fd = sys.stdin.fileno()  

        old_settings = termios.tcgetattr(fd)  

        #不产生回显效果  

        old_settings[3] = old_settings[3] & ~termios.ICANON & ~termios.ECHO  

        try :  

            tty.setraw( fd )  

            ch = sys.stdin.read( 1 )  

        finally :  

            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)  

 

        if ch == 'w':  

            max_tv = walk_vel_  

            speed = 1  

            turn = 0  

        elif ch == 's':  

            max_tv = walk_vel_  

            speed = -1  

            turn = 0  

        elif ch == 'a':  

            max_rv = yaw_rate_  

            speed = 0  

            turn = 1  

        elif ch == 'd':  

            max_rv = yaw_rate_  

            speed = 0  

            turn = -1  

        elif ch == 'W':  

            max_tv = run_vel_  

            speed = 1  

            turn = 0  

        elif ch == 'S':  

            max_tv = run_vel_  

            speed = -1  

            turn = 0  

        elif ch == 'A':  

            max_rv = yaw_rate_run_  

            speed = 0  

            turn = 1  

        elif ch == 'D':  

            max_rv = yaw_rate_run_  

            speed = 0  

            turn = -1  

        elif ch == 'q':  

            exit()  

        else:  

            max_tv = walk_vel_  

            max_rv = yaw_rate_  

            speed = 0  

            turn = 0  

 

        #发送消息  

        cmd.linear.x = speed * max_tv;  

        cmd.angular.z = turn * max_rv;  

        pub.publish(cmd)  

        rate.sleep()  

        #停止机器人  

        stop_robot();  

 

def stop_robot():  

    cmd.linear.x = 0.0  

    cmd.angular.z = 0.0  

    pub.publish(cmd)  

 

if __name__ == '__main__':  

    try:  

        keyboardLoop()  

    except rospy.ROSInterruptException:  

        pass

参考链接：

http://blog.csdn.net/marising/article/details/3173848

http://nullege.com/codes/search/termios.ICANON

四、节点关系图
ROS探索总结-8.键盘控制

代码包我已上传：
http://download.csdn.net/detail/hcx25909/5496381
或
http://file.ncnynl.com/ros/smartcar_teleop.zip 下载
希望大家一同学习，共同进步！

ROS探索总结-8.键盘控制

与本文相关的文章

您必须登录才能发表评论！

与本文相关的文章

您必须 登录 才能发表评论！

您必须登录才能发表评论！