古月出品:ROS探索总结(八)——键盘控制
hashman
hashman 1886 0
2016-01-03 01:18
版权声明:本文为原博主 古-月 原创文章,不得用于商业用途!
----------------------------------------------------------------------------------------------------------------------------------------------

如果尝试过前面的例子,有没有感觉每次让机器人移动还要在终端里输入指令,这也太麻烦了,有没有办法通过键盘来控制机器人的移动呢?答案室当然的了。我研究了其他几个机器人键盘控制的代码,还是有所收获的,最后移植到了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模型。重新打开一个终端,打开键盘控制节点:

图片:20130601165652.jpg



       
       在终端中按下键盘里的“W”、“S”、“D”、“A”以及“Shift”键进行机器人的控制。效果如下图:

图片:20130601165733.jpg






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





四、节点关系图

图片:20130601170028.jpg




       代码包我已上传:http://download.csdn.net/detail/hcx25909/5496381
       希望大家一同学习,共同进步!
----------------------------------------------------------------
转载请注明:转自古-月
http://blog.csdn.net/hcx25909
欢迎继续关注原作者博客~
分享:
游客
要评论请先登录 或者 注册
返回顶部