【kinetic】操作系统探索总结(八)键盘控制
阅读原文时间:2023年07月11日阅读:2

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

  使用 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

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

add_executable(smartcar_teleop src/keyboard.cpp)

Rename C++ executable without prefix

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})

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 ##

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

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

手机扫一扫

移动阅读更方便

阿里云服务器
腾讯云服务器
七牛云服务器

你可能感兴趣的文章