键盘控制smartcar学习记录

操作系统：ubuntu 18.04
ROS版本：melodic

本记录是跟随古月居的smartcar教程进行学习的，基于上次的结果进行再加工

先尝试用固定的python脚本控制小车

首先在~/ROS/smartcar_ws/src（参照之前创建的目录）里建立新的包
包名为smartcar_teleop，包含rospy、roscpp、geometry_msgs、std_msgs这几个依赖

对于消息的发布，上次直接在终端输入代码，这次创建一个python脚本用来执行发布消息
先在包目录下创建名为scripts文件夹，这个文件夹用来存放python脚本

在其中建立smartcar_teleop.py，输入代码如下

#!/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.5
        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()

先按照上次将smartcar运行起来

$ roslaunch smartcar_description smartcar_display.rviz.launch

运行消息发布脚本

1. 直接在终端运行

$ rosrun smartcar_teleop smartcar_teleop.py

这里会遇到一个问题

找到文件但无法执行，这是因为我们创建的文件一开始就没有给予一个可执行的权限
只需要在python文件所在文件夹下执行$ chmod +x smartcar_teleop.py进行授权

这是直接在终端运行python文件的结果

2. 用launch文件启动

先在这个包目录下创建launch文件夹

在launch文件夹里建立smartcar_teleop.launch文件，在文件中输入代码如下

<launch>  
  <arg name="cmd_topic" default="cmd_vel" />  
  <node pkg="smartcar_teleop" type="smartcar_teleop.py" name="smartcar_teleop">  
    <remap from="cmd_vel" to="$(arg cmd_topic)" />  
  </node> 
</launch> 

不要忘记设置环境变量，source一下工作空间：$ source ~/.bashrc　若是用的之前source过的终端，则这里可以忽略

再运行launch文件（格式：roslaunch <pkg_name> [launchfile_name.launch]）

$ roslaunch smartcar_teleop smartcar_teleop.launch

这是用launch启动的结果

然后就可以看到RViz里小车在动了

再加入键盘控制的功能

1) Cpp

先在新建的包内的src目录中建立键盘控制的文件，这里先使用cpp文件

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

这里同样需要给予可执行权限

$ chmod +x keyboard.cpp

然后需要修改这个包的CMakeLists.txt文件
在CMakeLists.txt末尾添加如下代码

include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(smartcar_keyboard_teleop src/keyboard.cpp)
target_link_libraries(smartcar_keyboard_teleop ${catkin_LIBRARIES})

以上步骤完成后需要跳到工作空间主目录下，进行编译

设置环境变量

$ source devel/setup.bash

然后就可以运行了

$ rosrun smartcar_teleop smartcar_keyboard_teleop

将运行上述命令的终端保持开启，并在键盘上按下相应按键进行控制
和大部分游戏一样 W、A、S、D控制方向，以及Shift加速

2) python

还可以使用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_rv = yaw_rate_
            speed = 0
            turn = 1
        elif ch == 'd':
            max_rv = yaw_rate_
            speed = 0
            turn = -1
        elif ch == 'W':
            max_tv = run_vel_
            speed = 1
            turn = 0
        elif ch == 'S':
            max_tv = run_vel_
            speed = -1
            turn = 0
        elif ch == 'A':
            max_rv = yaw_rate_run_
            speed = 0
            turn = 1
        elif ch == 'D':
            max_rv = yaw_rate_run_
            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

不要忘记设置可执行权限

由于python的特性，不像cpp那样还需要编译，它可以直接运行

$ rosrun smartcar_teleop keyboard.py

主要参考文献：
https://blog.csdn.net/hcx25909/article/details/9004617