ROS探索總結（八）——鍵盤控制

        如果嘗試過前面的例子，有沒有感覺每次讓機器人移動還要在終端裡輸入指令，這也太麻煩了，有沒有辦法通過鍵盤來控制機器人的移動呢？答案室當然的了。我研究了其他幾個機器人鍵盤控制的代碼，還是有所收穫的，最後移植到了smartcar上，實驗成功。

一、建立控制包        首先，我們為鍵盤控制單獨建立一個包：

roscreate-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpprosmake

        如果你已經忘記了怎麼建立包，請參考：http://www.ros.org/wiki/ROS/Tutorials/CreatingPackage
二、簡單的訊息發布        在機器人模擬中，主要控制機器人移動的就是        在機器人模擬中，主要控制機器人移動的就是Twist()結構，如果我們編程將這個結構通過程式發布成topic，自然就可以控制機器人了。我們先用簡單的python來嘗試一下。        之前的類比中，我們使用的都是在命令列下進行的訊息發布，現在我們需要把這些命令轉換成python代碼，封裝到一個單獨的節點中去。針對之前的命令列，我們可以很簡單的在smartcar_teleop /scripts檔案夾下編寫如下的控制碼：

#!/usr/bin/env pythonimport roslib; roslib.load_manifest('smartcar_teleop')import rospyfrom geometry_msgs.msg import Twistfrom std_msgs.msg import Stringclass 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模型。重新開啟一個終端，開啟鍵盤控制節點：
                在終端中按下鍵盤裡的“W”、“S”、“D”、“A”以及“Shift”鍵進行機器人的控制。效果如：

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 0x57class 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/TeleopErraticSimulation3、創新        雖然很多機器人的鍵盤控制使用的都是C++編寫的代碼，但是考慮到python的強大，我們還是需要嘗試使用python來編寫程式。
        首先需要理解上面C++程式的流程。在上面的程式中，我們單獨建立了一個線程來讀取中斷中的輸入，然後根據輸入發布不同的速度和角度訊息。介於線程的概念還比較薄弱，在python中使用迴圈替代線程。然後需要考慮的只是如何使用python來處理中斷中的輸入字元，通過上網尋找資料，發現使用的API和C++的基本是一致的。最終的程式如下：

#!/usr/bin/env python# -*- coding: utf-8 -*import  osimport  sysimport  tty, termiosimport roslib; roslib.load_manifest('smartcar_teleop')import rospyfrom geometry_msgs.msg import Twistfrom 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
四、節點關係圖

        程式碼封裝我已上傳：http://download.csdn.net/detail/hcx25909/5496381        希望大家一同學習，共同進步！

----------------------------------------------------------------

歡迎大家轉載我的文章。

轉載請註明：轉自古-月

http://blog.csdn.net/hcx25909

歡迎繼續關注我的部落格