如果尝试过前面的例子, 有没有感觉每次让机器人移动还要在终端里输入指令, 这也太麻烦了, 有没有办法通过键盘来控制机器人的移动呢? 答案室当然的了. 我研究了其他几个机器人键盘控制的代码, 还是有所收获的, 最后移植到了 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()
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 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 中的机器人了.
来源: http://www.bubuko.com/infodetail-2747644.html