如果尝试过前面的例子，有没有感觉每次让机器人移动还要在终端里输入指令，这也太麻烦了，有没有办法通过键盘来控制机器人的移动呢?答案室当然的了。我研究了其他几个机器人键盘控制的代码，还是有所收获的，最后移植到了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 pythonimport roslib; roslib.load_manifest(‘smartcar_teleop‘)import rospyfrom geometry_msgs.msg import Twistfrom 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重视不需要编译的。(python代码不需要编译，运行方式是 rosrun package python.py。C++代码需要catkin_make后rosrun package codes。catkin_make前需要修改CMakeList.txt)
先运行之前教程中用到的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>
使用 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 packagesfind_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 needcatkin_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 locationsinclude_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中的机器人了。
【kinetic】操作系统探索总结（八）键盘控制
原文地址：https://www.cnblogs.com/ynxf/p/9557214.html