第 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;
}