第 14.6.7 節
Keyboard control node
0瀏覽次數0訪問次數--跳出率--平均停留
Introduction
To control the chassis using the keyboard node in ROS2, we need to implement the following steps:
- Creating a ROS2 Package Create a new ROS2 package for implementing keyboard control functionality.
- Writing a Keyboard Control Node Write a C++ node that listens for keyboard input and publishes velocity commands.
- Communicating with the Mecanum Wheel Chassis Combine the published speed commands with chassis control.
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 velocity: Describes the speed of an object moving along a straight line in three-dimensional space. Typically used in mobile robots:
xindicates forward/backward movement.yindicates left-right translation.zis usually 0, because most ground robots only move on a flat plane.
- Angular velocity: Describes the speed at which an object rotates around an axis in three-dimensional space. Typically in mobile robotics:
zrepresents rotation around the vertical axis (clockwise/counterclockwise).xandyare usually 0, since most ground robots only need to rotate on a flat plane.
This message type is widely used to control robot motion, commonly referred to as DOF (Degrees of Freedom). By publishing to the /cmd_vel topic, it can provide motion commands to the robot.
Publish 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 partial 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 chassis forward/backward direction, while ROS2 uses X as the chassis forward/backward direction, they are not fully aligned here.
Official Node
ros2 run teleop_twist_keyboard teleop_twist_keyboard
i: Move forward (increase linear velocitylinear.x),(comma) : Move backward (decrease linear velocitylinear.x)j: Turn left (increase angular velocityangular.z, counterclockwise)l: Turn right (decrease angular velocityangular.z, clockwise)k: Emergency stop (zero out linear and angular velocities)u: Turn forward left (combined motion:linear.x+angular.z)o: Turn forward and right (combined motion:linear.x+angular.z)m: Turn left backward (combined motion:-linear.x+angular.z).(period) : Turn right and backward (combined motion:-linear.x+angular.z)w/x: Increase/Decrease Linear Velocity (increment step forlinear.x)a/d: Increase/Decrease angular velocity (increment step forangular.z)s/S: Emergency stop (sets linear and angular velocity to zero)
It is recommended to use the official nodes. Remember to reduce the step size before running.
Custom node
Creating a function package
ros2 pkg create cyber_teleop_twist_keyboard --build-type ament_cmake --dependencies rclcpp geometry_msgs --node-name teleop_twist_keyboard
Node main logic
w/s/a/d: Controls the robot to move forward/backward/strafe left/strafe right.
q/e: Controls the robot to rotate counterclockwise/clockwise.
i/,: Adjust the linear velocity step size (acceleration/deceleration step size).
j/l: Adjust the angular velocity step size (deceleration/acceleration step).
k: Stop all motion.
Below is a core function used for detecting 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 with caution. I directly used the official node and did not use a custom node.
#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;
}