第 14.6.7 節
键盘控制节点
0瀏覽次數0訪問次數--跳出率--平均停留
简介
要在 ROS2 中使用 keyboard 节点控制底盘，我们需要实现以下步骤：
- 创建 ROS2 包 创建一个新的 ROS2 包用于实现键盘控制功能。
- 编写键盘控制节点 编写一个 C++ 节点，用于监听键盘输入并发布速度命令。
- 与麦轮底盘通信 将发布的速度命令与底盘控制结合。
Twist消息接口
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，因为大多数地面机器人只需要在平面上旋转。
这个消息类型广泛用于控制机器人运动，也就是我们常说的DOF自由度，通过发布到 /cmd_vel 话题，可以为机器人提供运动指令。
发布cmd_vel给单片机
//初始化串口消息订阅
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部分代码：
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;
由于我的STM32以Y为底盘前后方向，ROS2以X为底盘前后方向，所以我这里并不是完全对应的。
官方节点
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: 紧急停止（将线速度和角速度归零）
建议可以使用官方的节点。记得跑之前，先将步长降低。
自定义节点
功能包创建
ros2 pkg create cyber_teleop_twist_keyboard --build-type ament_cmake --dependencies rclcpp geometry_msgs --node-name teleop_twist_keyboard
节点主逻辑
w/s/a/d：控制机器人前进/后退/左平移/右平移。
q/e：控制机器人逆时针/顺时针旋转。
i/,：调整线速度步长（加速/减速步长）。
j/l：调整角速度步长（减速/加速步长）。
k：停止所有运动。
下方是一个核心函数，用于检测按键。
#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; // 没有读取到字符
}
以下代码未经验证，谨慎使用，本人直接采用了官方的节点，并未使用自定义节点。
#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;
}