Wiki
Keyboard control node
About 4 min read
Existing English translations were reused. 7 current source blocks remain in Simplified Chinese, without triggering paid translation.
1.Introduction
要在 ROS2 中使用 keyboard 节点控制底盘，我们需要实现以下步骤：
-
Create ROS2 package Create a new ROS2 package to implement keyboard control functionality.
-
Write keyboard control node Write a C++ node to listen for keyboard input and publish velocity commands.
-
Communicate with the Mecanum wheel chassis Combine the published velocity commands with chassis control.
2.Twist message interface
ros2 interface show geometry_msgs/msg/Twist
# This expresses velocity in free space broken into its linear and angular parts.
# 该消息表示自由空间中的速度，分为线速度和角速度两部分。
Vector3 linear
float64 x # 线速度的 x 分量（通常表示前后方向的速度）
float64 y # 线速度的 y 分量（通常表示左右方向的速度）
float64 z # 线速度的 z 分量（通常表示上下方向的速度）
Vector3 angular
float64 x # 角速度的 x 分量（绕 x 轴的旋转速度）
float64 y # 角速度的 y 分量（绕 y 轴的旋转速度）
float64 z # 角速度的 z 分量（绕 z 轴的旋转速度）
-
线速度（linear） ：描述物体在三维空间中沿直线运动的速度。通常在移动机器人中：
-
x表示前进/后退。 -
y表示左右平移。 -
z通常为 0，因为大部分地面机器人只在平面上运动。
-
-
角速度（angular） ：描述物体在三维空间中绕某一轴旋转的速度。通常在移动机器人中：
-
z表示绕垂直轴的旋转（顺时针/逆时针）。 -
x和y通常为 0，因为大多数地面机器人只需要在平面上旋转。
-
This message type is widely used to control robot motion, which is what we commonly call DOF (degrees of freedom). By publishing to the /cmd_vel topic, motion commands can be provided to the robot.
3.Send cmd_vel to the microcontroller
//初始化串口消息订阅
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist("cmd_vel",10,std::bind(&Serial_Node::cmd_vel_sub_callback,this,std::placeholders::_1));
void cmd_vel_sub_callback(const geometry_msgs::msg::Twist &cmd_msg)
{
// bool bool_buffer[] = {1, 0, 1, 0};
// int8_t int8_buffer[] = {0x11,0x22};
// int16_t int16_buffer[] = {2000,6666};
// int32_t int32_buffer[] = {305419896};
fp32 fp32_buffer[] = {(fp32)cmd_msg.linear.x,(fp32)cmd_msg.linear.y,(fp32)cmd_msg.linear.z,
(fp32)cmd_msg.angular.x,(fp32)cmd_msg.angular.y,(fp32)cmd_msg.angular.z};
//由于ROS2中node为局部变量，所以只能在node中调用send函数，所以Serial_Transmit只负责处理data_buffer。
serial_pack_.tx.Data_Pack(0x01,
nullptr, 0,
nullptr, 0,
nullptr, 0,
nullptr, 0,
fp32_buffer, sizeof(fp32_buffer) / sizeof(fp32));
auto port = serial_driver_->port();
try
{
size_t bytes_size = port->send(data_buffer);
// RCLCPP_INFO(this->get_logger(), "Transmitted: ");
// for (size_t i = 0; i < data_buffer.size(); ++i)
// {
// RCLCPP_INFO(this->get_logger(), "0x%02X ", data_buffer[i]); //以十六进制格式打印每个字节
// }
RCLCPP_INFO(this->get_logger(), "平动XYZ：%.6f,%.6f,%.6f", fp32_buffer[0],fp32_buffer[1],fp32_buffer[2]);
RCLCPP_INFO(this->get_logger(), "转动XYZ：%.6f,%.6f,%.6f", fp32_buffer[3],fp32_buffer[4],fp32_buffer[5]);
RCLCPP_INFO(this->get_logger(), "(%ld bytes)", bytes_size);
// (void)bytes_size;
}
catch(const std::exception &ex)
{
RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what());
}
}
};STM32 part of the code:
case CHASSIS_ROS2_CMD: //ROS2接管模式
//单位就是m/s和rad/s
this->oriChassis.Speed.vx = cmd_vel_.Linear.Y;
this->oriChassis.Speed.vy = cmd_vel_.Linear.X;
this->oriChassis.Speed.vw = - cmd_vel_.Angular.Z;
break;Since my STM32 uses Y as the front-back direction of the chassis, ROS2 uses X as the front-back direction of the chassis, so here they are not completely corresponding.
4.Official Node
ros2 run teleop_twist_keyboard teleop_twist_keyboard-
i: 向前移动（增加线速度linear.x） -
,(逗号) : 向后移动（减少线速度linear.x） -
j: 左转（增加角速度angular.z，逆时针） -
l: 右转（减少角速度angular.z，顺时针） -
k: 紧急停止（将线速度和角速度归零） -
u: 向前左转（组合运动：linear.x+angular.z） -
o: 向前右转（组合运动：linear.x+angular.z） -
m: 向后左转（组合运动：-linear.x+angular.z） -
.(句号) : 向后右转（组合运动：-linear.x+angular.z） -
w/x: 增加/减少 线速度 （linear.x的增量步长） -
a/d: 增加/减少 角速度 （angular.z的增量步长） -
s/S: 紧急停止（将线速度和角速度归零）
It is recommended to use the official nodes. Remember to lower the step size before running.
5.Custom nodes
5.1.Package Creation
ros2 pkg create cyber_teleop_twist_keyboard --build-type ament_cmake --dependencies rclcpp geometry_msgs --node-name teleop_twist_keyboard5.2.Node main logic
w/s/a/d：控制机器人前进/后退/左平移/右平移。
q/e：控制机器人逆时针/顺时针旋转。
i/,：调整线速度步长（加速/减速步长）。
j/l：调整角速度步长（减速/加速步长）。
k: Stop all motion.
Below is a core function used to detect key presses.
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
//查键盘输入的函数，它不需要按回车就能返回键盘输入。它通过修改终端设置，使得可以直接获取按键的值。
int kbhit(void)
{
struct termios oldt, newt;
int ch;
int oldf;
tcgetattr(STDIN_FILENO, &oldt); // 获取当前终端的设置
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO); // 禁用缓冲模式和回显
newt.c_cc[VMIN] = 1;
newt.c_cc[VTIME] = 0;
tcsetattr(STDIN_FILENO, TCSANOW, &newt); // 设置新的终端设置
oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK); // 设置为非阻塞模式
ch = getchar(); // 获取字符
tcsetattr(STDIN_FILENO, TCSANOW, &oldt); // 恢复终端设置
fcntl(STDIN_FILENO, F_SETFL, oldf); // 恢复文件描述符设置
if(ch != EOF) {
ungetc(ch, stdin); // 将字符放回标准输入流
return 1; // 如果读取了字符，返回 1
}
return 0; // 没有读取到字符
}The following code has not been verified; use it with caution. I directly used the official nodes and did not use custom nodes.
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <geometry_msgs/msg/detail/twist__struct.hpp>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
#include "cyber_teleop_twist_keyboard/struct_typedef.h"
class TeleopTwistKeyboardNode: public rclcpp::Node
{
public:
TeleopTwistKeyboardNode():Node("CyberTeleopTwistKeyboardNode_cpp")
{
RCLCPP_INFO(this->get_logger(),"TeleopTwistKeyboardNode Running!");
RCLCPP_INFO(this->get_logger(),"键盘控制机器人节点已启动！");
// 创建一个发布者，类型是 geometry_msgs/msg/Twist，发布到 "cmd_vel" 话题
keyboard_control_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);
// 定时器，周期性调用发布函数
keyboard_control_pub_timer_ = this->create_wall_timer(
std::chrono::milliseconds(100),
std::bind(&TeleopTwistKeyboardNode::cmd_vel_publish_timer_callback, this)
);
}
private:
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr keyboard_control_pub_;
rclcpp::TimerBase::SharedPtr keyboard_control_pub_timer_;
// 步长
fp64 linear_step_ = 0.1;
fp64 angular_step_ = 0.1;
// 当前线速度和角速度
fp64 linear_speed_ = 0.0;
fp64 angular_speed_ = 0.0;
// 最大速度限制
fp64 max_linear_speed_ = 0.5;
fp64 max_angular_speed_ = 0.2;
// 最小速度限制
fp64 min_linear_speed_ = 0.0;
fp64 min_angular_speed_ = 0.0;
geometry_msgs::msg::Twist cmd_msg;
//查键盘输入的函数，它不需要按回车就能返回键盘输入。它通过修改终端设置，使得可以直接获取按键的值。
int kbhit(void)
{
struct termios oldt, newt;
int ch;
int oldf;
tcgetattr(STDIN_FILENO, &oldt); // 获取当前终端的设置
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO); // 禁用缓冲模式和回显
newt.c_cc[VMIN] = 1;
newt.c_cc[VTIME] = 0;
tcsetattr(STDIN_FILENO, TCSANOW, &newt); // 设置新的终端设置
oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK); // 设置为非阻塞模式
ch = getchar(); // 获取字符
tcsetattr(STDIN_FILENO, TCSANOW, &oldt); // 恢复终端设置
fcntl(STDIN_FILENO, F_SETFL, oldf); // 恢复文件描述符设置
if(ch != EOF) {
ungetc(ch, stdin); // 将字符放回标准输入流
return 1; // 如果读取了字符，返回 1
}
return 0; // 没有读取到字符
}
void cmd_vel_publish_timer_callback()
{
cmd_msg.linear.y = 0.0;
cmd_msg.linear.x = 0.0;
cmd_msg.angular.z = 0.0;
// 当有键盘输入时，处理它
if (kbhit())
{
char key = getchar(); // 获取键盘输入的字符
if(key == EOF)
{
return;
}
RCLCPP_INFO(this->get_logger(),"按下了:%c",key);
// 根据按键决定机器人运动方向
switch(key)
{
case 'w': // 前进
cmd_msg.linear.y = linear_speed_;
cmd_msg.linear.x = 0.0;
cmd_msg.angular.z = 0.0;
break;
case 's': // 后退
cmd_msg.linear.y = - linear_speed_;
cmd_msg.linear.x = 0.0;
cmd_msg.angular.z = 0.0;
break;
case 'a': // 左平移
cmd_msg.linear.y = 0.0;
cmd_msg.linear.x = - linear_speed_;
cmd_msg.angular.z = 0.0;
break;
case 'd': // 右平移
cmd_msg.linear.y = 0.0;
cmd_msg.linear.x = linear_speed_;
cmd_msg.angular.z = 0.0;
break;
case 'q': // 逆时针旋转
cmd_msg.linear.y = 0.0;
cmd_msg.linear.x = 0.0;
cmd_msg.angular.z = - angular_speed_;
break;
case 'e': // 顺时针旋转
cmd_msg.angular.z = angular_speed_;
break;
case 'i': // 增加线速度
linear_speed_ = std::clamp(linear_speed_ + linear_step_, min_linear_speed_, max_linear_speed_);
RCLCPP_INFO(this->get_logger(), "Linear increased to: %.2f", linear_speed_);
break;
case ',': // 减小线速度
linear_speed_ = std::clamp(linear_speed_ - linear_step_, min_linear_speed_, max_linear_speed_);
RCLCPP_INFO(this->get_logger(), "Linear decreased to: %.2f", linear_speed_);
break;
case 'l': // 增加角速度
angular_speed_ = std::clamp(angular_speed_ + angular_step_, min_angular_speed_, max_angular_speed_);
RCLCPP_INFO(this->get_logger(), "Angular increased to: %.2f", angular_speed_);
break;
case 'j': // 减小角速度
angular_speed_ = std::clamp(angular_speed_ - angular_step_, min_angular_speed_, max_angular_speed_);
RCLCPP_INFO(this->get_logger(), "Angular decreased to: %.2f", angular_speed_);
break;
case 'k': // 停止
cmd_msg.linear.y = 0.0;
cmd_msg.linear.x = 0.0;
cmd_msg.angular.z = 0.0;
break;
default:
RCLCPP_WARN(this->get_logger(), "Unknown key pressed!");
}
}
keyboard_control_pub_->publish(cmd_msg);
}
};
int main(int argc, char ** argv)
{
rclcpp::init(argc,argv);
auto node = std::make_shared<TeleopTwistKeyboardNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}