【kinetic】操做系統探索總結（八）鍵盤控制

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

　　1、建立控制包

　　首先，咱們爲鍵盤控制單獨創建一個包：

       01.catkin_create_pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp
    02.catkin_make

　　2、簡單的消息發佈

　　　　在機器人仿真中，主要控制機器人移動的就是 在機器人仿真中，主要控制機器人移動的就是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重視不須要編譯的。(python代碼不須要編譯可是要給py代碼可執行權限chmod +x python.py，運行方式是 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中看一下機器人是否是動起來了!

3、加入鍵盤控制

　　固然前邊的程序不是咱們要的，咱們須要的鍵盤控制。

　　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