两轮差速底盘SLAM系列(3)--键盘控制(1)

本文最后更新于：2022年5月29日 上午

前言

机器人必备条件——发布Twist速度指令。

在继续看本文之前，你需要完成下面的前提条件：

  • 上位机ROS和底层STM32之间可以正常互相收发数据。

前面已经搞通了ROS和STM32之间的CAN协议通讯，两者之间可以正常互相收发数据，那么现在就来写一个键盘控制节点，通过键盘操作小车移动。

因为ROS的代码具有很强的移植性，所以键盘控制的代码可以直接从其他机器人包移植过来，我这里主要参考了PR2机器人键盘控制代码。

移植

首先，在agv_car功能包src文件夹下创建keyboard_control.cpp文件，把PR2机器人的键盘控制代码复制进来。

1
2
roscd agv_car/src
gedit keyboard_control.cpp

复用

1、修改代码

因为是直接复制过来的代码，有很多关于PR2机器人的相关变量，对代码进行修改，使其适合我们自己的要求，增强代码的可读性。我主要修改了节点名字和增加了停止按键等，代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
#include <termios.h>
#include <signal.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "ros/ros.h"//ros头文件
#include <geometry_msgs/Twist.h>//消息头文件

//键盘按键宏定义
#define KEYCODE_A 0x61
#define KEYCODE_D 0x64
#define KEYCODE_S 0x73
#define KEYCODE_W 0x77
#define KEYCODE_A_CAP 0x41
#define KEYCODE_D_CAP 0x44
#define KEYCODE_S_CAP 0x53
#define KEYCODE_W_CAP 0x57


class TeleopPR2Keyboard //定义键盘的类
{
  private:
  double walk_vel, run_vel, yaw_rate, yaw_rate_run;//定义行走速度，跑的速度，旋转速率，旋转时候跑的速率
  geometry_msgs::Twist cmd; //真实速度
  ros::NodeHandle n_;
  ros::Publisher vel_pub_;

  public:
  void init()
  {
    cmd.linear.x = cmd.linear.y = cmd.angular.z = 0; //初始速度
    vel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 10);//发布的主题
    ros::NodeHandle n_private("~");
    n_private.param("walk_vel", walk_vel, 0.1);//这里给各个定义的东西参数配置
    n_private.param("run_vel", run_vel, 0.3);
    n_private.param("yaw_rate", yaw_rate,0.3);
    n_private.param("yaw_run_rate", yaw_rate_run, 0.5);
  }
  ~TeleopPR2Keyboard()   { }
  void keyboardLoop();
};

int kfd = 0;
struct termios cooked, raw;

void quit(int sig)
{
  tcsetattr(kfd, TCSANOW, &cooked);
  exit(0);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "keyboard_control");//定义一个主题
  TeleopPR2Keyboard tpk;
  tpk.init();//初始化
  signal(SIGINT,quit);
  tpk.keyboardLoop();

  return(0);
}

void TeleopPR2Keyboard::keyboardLoop()
{
  char c;
  bool dirty=false;
  // get the console in raw mode 使控制台进入原始模式
  tcgetattr(kfd, &cooked);
  memcpy(&raw, &cooked, sizeof(struct termios));
  raw.c_lflag &=~ (ICANON | ECHO);
  // Setting a new line, then end of file 设置新的线，然后结束文件
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(kfd, TCSANOW, &raw);
  puts("Reading from keyboard");
  puts("---------------------------");
  puts("Use 'WASD' to control the robot");
  puts("Press 'Caps' to move faster");
  puts("press Ctrl+C to quit");
  puts("---------------------------");
  for(;;)
  {
    // get the next event from the keyboard 从键盘中获得下一个事件
    if(read(kfd, &c, 1) < 0)
    {
      perror("read():");
      exit(-1);
    }
    cmd.linear.x = cmd.angular.z = 0;
    switch(c) //选择
    {
      // Walking
      case KEYCODE_W:
        cmd.linear.x = walk_vel;
        dirty = true;
        break;
      case KEYCODE_S:
        cmd.linear.x = - walk_vel;
        dirty = true;
        break;
      case KEYCODE_A:
        cmd.angular.z = yaw_rate;
        dirty = true;
        break;
      case KEYCODE_D:
        cmd.angular.z = - yaw_rate;
        dirty = true;
        break;
      // Running
      case KEYCODE_W_CAP:
        cmd.linear.x = run_vel;
        dirty = true;
        break;
      case KEYCODE_S_CAP:
        cmd.linear.x = - run_vel;
        dirty = true;
        break;
      case KEYCODE_A_CAP:
        cmd.angular.z = yaw_rate_run;
        dirty = true;
        break;
      case KEYCODE_D_CAP:
        cmd.angular.z = - yaw_rate_run;
        dirty = true;
        break;
      default:
        cmd.linear.x = 0;
        cmd.angular.z = 0;
        dirty = true;
        break;
    }
    if (dirty == true)
    {
      vel_pub_.publish(cmd);
    }
  }
}

2、修改CmakeLists.txt文件

1）添加geometry_msgs依赖

1
2
3
4
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  ...
)

2）将下列代码加入文件底部：

1
2
add_executable(keyboard_control src/keyboard_control.cpp)
target_link_libraries(keyboard_control ${catkin_LIBRARIES})

3、编译

1
2
cd ~/catkin_ws
catkin_make

验证

代码修改好了，可以验证一下键盘控制节点发布的话题是不是我们需要的信息。有两种方式可以验证：

1、rostopic指令

可以直接使用rostopic指令进行查看节点发布的话题信息。

  • 先运行master节点：

    1
    roscore

  • 启动键盘控制节点：

    1
    rosrun agv_car keyboard_control

  • 使用rostopic指令显示话题信息：

    1
    rostopic echo /cmd_vel

    1q5Zgs.png

2、编写一个简单话题订阅节点

第二种方法也可以编写一个简单话题订阅节点，代码很简单。

（1）在src目录新建一个keyboard_sub.cpp文件，编写话题订阅代码：

1
2
roscd agv_car/src
gedit keyboard_sub.cpp

keyboard_sub.cpp：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>

void cmd_velCallback(const geometry_msgs::Twist &twist_aux)
{
  ROS_INFO("linear.x: %0.2f  angular.z: %0.2f",twist_aux.linear.x,twist_aux.angular.z);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "keyboard_sub");
  
  ros::NodeHandle n;

  ros::Subscriber cmd_sub = n.subscribe("/cmd_vel", 10, cmd_velCallback);

  ros::spin();
  
  return 0;
}

（2）修改CmakeLists.txt文件，将下列代码加入文件底部：

1
2
add_executable(keyboard_sub src/keyboard_sub.cpp)
target_link_libraries(keyboard_sub ${catkin_LIBRARIES})

（3）编译运行

  • 先运行master节点：

    1
    roscore

  • 启动键盘控制节点：

    1
    rosrun agv_car keyboard_control

  • 启动键盘订阅节点：

    1
    rosrun agv_car keyboard_sub

    1q5nuq.png

ROS > 两轮差速底盘SLAM
#ROS #SLAM
两轮差速底盘SLAM系列(3)--键盘控制(1)
https://kevinloongc.github.io/posts/23588.html
作者
Kevin Loongc
发布于
2020年2月13日
许可协议