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

　　一、创建控制包

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

01.catkin_create_pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp

    02.catkin_make

　　二、简单的消息发布

　　　　在机器人仿真中，主要控制机器人移动的就是 在机器人仿真中，主要控制机器人移动的就是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()

p { margin-bottom: 0.1in; direction: ltr; line-height: 120%; text-align: justify }
a:link { color: rgba(0, 0, 255, 1) }

　　python代码在ROS重视不需要编译的。(python代码不需要编译但是要给py代码可执行权限chmod +x python.py，运行方式是 rosrun package python.py。C++代码需要catkin_make后rosrun package codes。catkin_make前需要修改CMakeList.txt)

　　先运行之前教程中用到的smartcar机器人，在rviz中进行显示，然后新建终端，输入如下命令：

p { margin-bottom: 0.1in; direction: ltr; line-height: 120%; text-align: justify }
a:link { color: rgba(0, 0, 255, 1) }

　　rosrun smartcar_teleop teleop.py

p { margin-bottom: 0.1in; direction: ltr; line-height: 120%; text-align: justify }
a:link { color: rgba(0, 0, 255, 1) }

　　也可以建立一个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>

　　使用 roslaunch运行 　　

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

三、加入键盘控制

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

　　1、移植

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

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

include_directories(include ${catkin_INCLUDE_DIRS})

add_executable(smartcar_teleop src/keyboard.cpp)

target_link_libraries(smartcar_teleop ${catkin_LIBRARIES})
（注意:不能直接添加在文件底部，可以搜索相似的添加方式，添加在CMakeList.txt的合适位置）

在src文件夹下新建 keyboard.cpp文件。
    #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_);

        }

    }

　　CMakeList.txt文件

cmake_minimum_required(VERSION 2.8.3)

project(smartcar_teleop)

## Compile as C++11, supported in ROS Kinetic and newer

# add_compile_options(-std=c++11)

## Find catkin macros and libraries

## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)

## is used, also find other catkin packages

find_package(catkin REQUIRED COMPONENTS

  geometry_msgs

  roscpp

  rospy

  std_msgs

  urdf

)

## System dependencies are found with CMake's conventions

# find_package(Boost REQUIRED COMPONENTS system)

## Uncomment this if the package has a setup.py. This macro ensures

## modules and global scripts declared therein get installed

## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html

# catkin_python_setup()

################################################

## Declare ROS messages, services and actions ##

################################################

## To declare and build messages, services or actions from within this

## package, follow these steps:

## * Let MSG_DEP_SET be the set of packages whose message types you use in

##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).

## * In the file package.xml:

##   * add a build_depend tag for "message_generation"

##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET

##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in

##     but can be declared for certainty nonetheless:

##     * add a exec_depend tag for "message_runtime"

## * In this file (CMakeLists.txt):

##   * add "message_generation" and every package in MSG_DEP_SET to

##     find_package(catkin REQUIRED COMPONENTS ...)

##   * add "message_runtime" and every package in MSG_DEP_SET to

##     catkin_package(CATKIN_DEPENDS ...)

##   * uncomment the add_*_files sections below as needed

##     and list every .msg/.srv/.action file to be processed

##   * uncomment the generate_messages entry below

##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder

# add_message_files(

#   FILES

#   Message1.msg

#   Message2.msg

# )

## Generate services in the 'srv' folder

# add_service_files(

#   FILES

#   Service1.srv

#   Service2.srv

# )

## Generate actions in the 'action' folder

# add_action_files(

#   FILES

#   Action1.action

#   Action2.action

# )

## Generate added messages and services with any dependencies listed here

# generate_messages(

#   DEPENDENCIES

#   geometry_msgs#   std_msgs

# )

################################################

## Declare ROS dynamic reconfigure parameters ##

################################################

## To declare and build dynamic reconfigure parameters within this

## package, follow these steps:

## * In the file package.xml:

##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"

## * In this file (CMakeLists.txt):

##   * add "dynamic_reconfigure" to

##     find_package(catkin REQUIRED COMPONENTS ...)

##   * uncomment the "generate_dynamic_reconfigure_options" section below

##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder

# generate_dynamic_reconfigure_options(

#   cfg/DynReconf1.cfg

#   cfg/DynReconf2.cfg

# )

###################################

## catkin specific configuration ##

###################################

## The catkin_package macro generates cmake config files for your package

## Declare things to be passed to dependent projects

## INCLUDE_DIRS: uncomment this if your package contains header files

## LIBRARIES: libraries you create in this project that dependent projects also need

## CATKIN_DEPENDS: catkin_packages dependent projects also need

## DEPENDS: system dependencies of this project that dependent projects also need

catkin_package(

#  INCLUDE_DIRS include

#  LIBRARIES smartcar_teleop

#  CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs urdf

#  DEPENDS system_lib

)

###########

## Build ##

###########

## Specify additional locations of header files

## Your package locations should be listed before other locations

include_directories(

 include

  ${catkin_INCLUDE_DIRS}

)

## Declare a C++ library

# add_library(${PROJECT_NAME}

#   src/${PROJECT_NAME}/smartcar_teleop.cpp

# )

## Add cmake target dependencies of the library

## as an example, code may need to be generated before libraries

## either from message generation or dynamic reconfigure

# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable

## With catkin_make all packages are built within a single CMake context

## The recommended prefix ensures that target names across packages don't collide

# add_executable(${PROJECT_NAME}_node src/smartcar_teleop_node.cpp)

add_executable(smartcar_teleop src/keyboard.cpp)

## Rename C++ executable without prefix

## The above recommended prefix causes long target names, the following renames the

## target back to the shorter version for ease of user use

## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"

# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable

## same as for the library above

# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against

# target_link_libraries(${PROJECT_NAME}_node

#   ${catkin_LIBRARIES}

# )

target_link_libraries(smartcar_teleop ${catkin_LIBRARIES})

#############

## Install ##

#############

# all install targets should use catkin DESTINATION variables

# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation

## in contrast to setup.py, you can choose the destination

# install(PROGRAMS

#   scripts/my_python_script

#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

# )

## Mark executables and/or libraries for installation

# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node

#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

# )

## Mark cpp header files for installation

# install(DIRECTORY include/${PROJECT_NAME}/

#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}

#   FILES_MATCHING PATTERN "*.h"

#   PATTERN ".svn" EXCLUDE

# )

## Mark other files for installation (e.g. launch and bag files, etc.)

# install(FILES

#   # myfile1

#   # myfile2

#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}

# )

#############

## Testing ##

#############

## Add gtest based cpp test target and link libraries

# catkin_add_gtest(${PROJECT_NAME}-test test/test_smartcar_teleop.cpp)

# if(TARGET ${PROJECT_NAME}-test)

#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})

# endif()

## Add folders to be run by python nosetests

# catkin_add_nosetests(test)

　　catkin_make 之后我们执行程序

　　rosrun smartcar_teleop smartcar_teleop
　这样我们就可以用WSAD来控制rviz中的机器人了。


最后附上我自己写的python代码:
#!/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_tv = yaw_rate_

            speed = 0

            turn = 1

        elif ch == 'd':

            max_rv = yaw_rate_

            speed = 0

            turn = -1

        elif 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_tv = yaw_rate_

            speed = 0

            turn = 1

        elif ch == 'D':

            max_rv = yaw_rate_

            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
    
 

【kinetic】操作系统探索总结（八）键盘控制的更多相关文章

  1. OpenGL之路（八）加入�光照效果和键盘控制

    在opengl中加入�光照的效果,可用键盘控制放大缩小 w键放大 s键缩小 d键开关灯 预览效果例如以下: 源代码例如以下: #include <gl/glut.h> #include & ...

  2. unity3d easytouch计算摇杆旋转角度以及摇杆八方向控制角色

    在写第三人称控制的时候,一开始在电脑测试是用WASD控制角色 后来需要发布到手机上,于是就加了一个摇杆 键盘控制角色的代码已经写好了,角色八方向移动 如果按照传统的大众思路来控制的话,是达不到我想要的 ...

  3. 【Unity3D】使用鼠标键盘控制Camera视角（即时战略类游戏视角）：缩近，拉远，旋转

    今天写一个demo,要用到鼠标键盘控制三维视角,因此写了个脚本用于控制. 该脚本可以用于即时战略类游戏的视角,提供了缩进,拉伸,旋转.同时按住鼠标右键不放,移动鼠标可以实现第一人称视角的效果. usi ...

  4. 求s=a+aa+aaa+aaaa+aa...a的值，其中a是一个数字。例如2+22+222+2222+22222(此时共有5个数相加)，几个数相加有键盘控制。

    package com.lw.HomeWork1;//包名 2 import java.util.Scanner; public class Demo18 { /** * @param args */ ...

  5. 键盘控制div移动

    <!DOCTYPE html><html lang="zh-CN"><head> <meta charset="UTF-8&qu ...

  6. js键盘控制div移动，解决停顿问题

    问题版本代码如下: 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 <html> &l ...

  7. JS实现用键盘控制DIV上下左右+放大缩小与变色

    <!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/ ...

  8. ARM开发（3）基于STM32的矩阵键盘控制蜂鸣器

    一 矩阵键盘控制蜂鸣器原理:  1.1 本实验实现8*7矩阵键盘上按键控制蜂鸣器响.  1.2 实验思路:根据电路图原理,找出矩阵键盘行列所对应的引脚,赋予对应的按键值,然后控制蜂鸣器响.  1.3 ...

  9. [Python Study Notes]实现对键盘控制与监控

    ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' ...

随机推荐

  1. 【2020.11.28提高组模拟】T1染色(color)

    [2020.11.28提高组模拟]T1染色(color) 题目 题目描述 给定 \(n\),你现在需要给整数 \(1\) 到 \(n\) 进行染色,使得对于所有的 \(1\leq i<j\leq ...

  2. day4(JWT介绍)

    1.JWT介绍 1.1jwt原理 最简单理解:jwt本质就是, 把用户信息通过加密后生成的一个字符串 JWT的原则是在服务器身份验证之后,将生成一个JSON对象并将其发送回用户 { "Use ...

  3. Python中sorted(iterable, *, key=None, reverse=False)函数参数定义中的独立星号(*)的含义

    老猿在 <Python中函数的参数带星号是什么意思?>中介绍了Python函数中参数带星号的含义,而在实际使用和Python的标准文档中,会看到某写函数(如sorted(iterable, ...

  4. 初识python-名片管理系统v1.0

    一.项目说明 本项目分享一个简单的名片管理系统,主要是通过实现简单的功能,来学习python基础. 具体功能有:新建名片.显示全部名片.查询.修改.删除名片 通过对名片的增删改查,来快速看懂pytho ...

  5. 【系统设计】WMS系统中 库存、盘点、移库、拆库功能的设计（库内管理）

    最近负责WMS系统 盘点 移库 两个功能模块的功能及数据库设计. 物流仓储系统的搭建,要基于仓库的实际情况,整理内部员工需求,再参考其他WMS系统,经过长时间的讨论和研究,最终转化为产品需求. 这里先 ...

  6. XSS漏洞防御之HttpOnly

    WWW服务依赖于Http协议实现,Http是无状态的协议,所以为了在各个会话之间传递信息,就需要使用Cookie来标记访问者的状态,以便服务器端识别用户信息. Cookie分为内存Cookie和硬盘C ...

  7. git .gitignore 忽略列表

    #: 注释 # no .a files * .a    //忽略以  .a结尾的 文件 #  ... ! lib .a  //  忽略 非 lib.a的文件 /TODO  //忽略当前目录  文件名位 ...

  8. this.$options.data()实战之重置data

    刚刚看到这个方法学习了一下,然后想到正在开发的项目有一个需要重置data的操作,正好拿来使用一下,节省了好多代码,美滋滋...

  9. 购物车 python作业

    功能要求:要求用户输入总资产,例如:2000显示商品列表,让用户根据序号选择商品,加入购物车购买,如果商品总额大于总资产,提示账户余额不足,否则,购买成功.附加:可充值.某商品移除购物车goods = ...

  10. python 使用UUID库生成唯一ID

      首先导包: import uuid   uuid1(): # make a UUID based on the host ID and current time     #  基于MAC地址,时间 ...