第 14 节
机器人硬件
机器人组成
立足角度不同，对机器人组成的认识也会有所不同。宏观上讲，机器人由硬件以及软件两大部分组成。进一步细化，机器人又可以分为三大部分： 控制系统 、 传感系统 与 硬件平台 。
1.控制系统
控制系统相当于机器人的大脑，由安装了机器人操作系统的处理器组成，是机器人的核心。控制系统的主要任务是根据作业指令和传感器反馈生成控制命令，同时负责算法处理和人机交互等功能。它在机器人中扮演着智能决策和执行任务的关键角色。
2.传感系统
传感系统相当于机器人的感官，分为内部传感器模块和外部传感器模块。
- 内部传感系统包括电机的编码器、陀螺仪等，可以通过自身信号反馈来检测机器人的位姿状态。
- 外部传感系统包括摄像头、雷达等，用于感知外部环境。
传感系统获取有用的信息，帮助机器人感知和理解周围的环境，起到辅助机器人操作和决策的作用。
3.硬件平台
硬件平台相当于机器人的躯体，提供了对机器人的物理支持，通常由驱动系统和执行机构两部分组成。
- 驱动系统主要负责驱动执行机构，将控制系统下达的命令转换为执行机构所需的信号，类似于人的小脑和神经。
- 执行机构是机器人的机械部分，类似于人的手和脚，例如机器人的行走部分和机械臂。
目前，机器人的硬件平台已经非常成熟，种类也非常丰富。包括但不限于飞行器、轮式机器人、四足机器人、人形机器人、软体机器人和水下机器人等。此外，我们甚至可以根据丰富的参考资料制作简易的机器人。在学习和工作中，大家可以根据实际需求选择适合的机器人平台。
工控机之远程开发环境
场景
机器人平台一般自带预装了ROS的控制系统。这套控制系统与我们前一阶段的学习环境基本无异，具体到开发或应用层面也大致相同，我们可以借助于平台外设的鼠标、键盘、显示器等直接在其上开发、调试程序或控制该机器人。但是在”面向平台“开发时可能遇到一些问题，比如：
- 机器人是一辆移动式平台时，”面向平台“的开发模式下可能需要人随车走，这显然效率低下，并存在一定的安全隐患；
- ”面向平台“的开发模式，还会受限于环境、地形等诸多因素的约束；
- 机器人与开发人员之间可能是一对多的关系，也即多位开发或测试工程师使用同一台设备，”面向平台“的开发模式下不免会出现资源抢占的情况；
总而言之，”面向平台“开发有其可行性，但是也存在诸多不便，此背景下，就可以通过搭建远程开发环境来解决上述问题了。
概念
远程开发 是一种在远程主机上进行编写、编译或运行程序的开发方式。相对于本地开发，远程开发需要将本地设备连接到远程主机，以实现数据传输和操作同步。
作用
远程开发可以带来一些优势，比如提供更强大的计算资源、便于团队协作、统一开发环境等。同时，远程开发也需要考虑网络延迟和稳定性等因素。总的来说，远程开发可以提供更灵活和便捷的开发环境，适用于不同场景和需求。
实现方式
在本教程中，我们将主要介绍两种常用的远程开发模式：SSH和NoMachine。这是两种常见的远程连接方式，它们在功能和使用方式上有一些差异。
- SSH是一种命令行界面的连接方式，用户需要通过命令行输入指令进行远程操作。对于熟悉命令行的用户来说，SSH可能更加灵活和高效。
- NoMachine提供了图形界面的远程连接，用户可以直观地操作远程计算机的桌面。它支持窗口、多显示器和文件传输等功能，适合那些需要图形界面的远程操作。
总的来说，SSH更适合需要执行命令行操作和快速访问的场景，而NoMachine更适合需要图形界面远程桌面访问的场景。选择哪种方式应该根据具体的需求和使用习惯来决定。
远程开发SSH
远程访问桌面
工控机之外接USB设备
分布式搭建
在 ROS2其他通信机制 章节里
优化日志
在实际应用上,我们很少使用RCLCPP_INFO,原因是打印数据开销太大,像Nvidia Jetson Xavier等工控机设备可能吃不消.
像下方这个图就显示了在树莓派4b下打印的开销:
由于我们想避免那么大的开销,又想在必要时查看打印的数据进行调试,所以我们常采用RCLCPP_DEBUG来打印数据,在必要时期才打印数据.
RCLCPP_DEBUG 的核心作用
- 调试专用 ：仅在调试阶段输出信息，生产环境自动静默
- 性能优化 ：通过编译选项完全移除调试代码，零运行时开销
- 分级控制 ：与其他日志级别（INFO/WARN/ERROR）解耦
打印出来rclcpp_debug的方式:
方式一:
ros2 run <package_name> <node_name> --ros-args --log-level debug
方式二:
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='my_package',
executable='my_node',
name='my_node',
output='screen',
parameters=[],
arguments=['--ros-args', '--log-level', 'debug']
)
])
/opt/ros/humble/include/rcutils/rcutils/logging.h定义：
硬件平台
硬件平台的选择是具有多样性的，比如：轮式底盘、两足或四足机器人、无人机等等。现在硬件平台相关技术发展的已经比较成熟了，可以自己制作，也可以直接采购，假设选择后者的话，那么购买后，关于具体使用，可以参考配套的使用手册。一般情况下，供应商会提供对应的驱动包，直接下载并编译执行即可驱动硬件平台，当然如果是集成了上位机的整套机器人方案，那么一般已经预置了驱动程序，届时直接运行即可。
机器人下位机常用的开发板有STM32，ESP32，GD32，Arduino，51MCU等等。（不懂的问控制组和电路组）
与机器人下位机开发板通信常用的有串口通信，MicroROS等方式，可以看上方的教程。
我们可以让下位机传给上位机的数据有里程计ODOM数据，惯性计IMU数据，全球定位GNSS数据等等，当然也可以让这些设备直接与工控机通信，让工控机处理好机器人的定位信息再一起传给下位机。
下面需要介绍这些数据如何进行传输。
自己写的大部分源码仅供参考，如果有更好的思路可以说：
我用Jazzy版本多一些，一切以Jazzy版本为最终参考，Humble其实与Jazzy最大区别就是Gazebo,关于Gazebo的代码可以看Humble仓库的。
https://github.com/CyberNaviRobot/CyberRobot\_ROS2\_Humble\_WS
https://github.com/CyberNaviRobot/CyberRobot\_ROS2\_Jazzy\_WS
里程计Odom
功能包创建
不一定非得要码盘(里程计Odom)的数据，只要把机器人6个自由度DOF，可以由电机编码器计算而来（x,y,z,yaw,pitch,roll不懂的看上方Moveit2里的教学）全部发过来就行。
首先先创建功能包
ros2 pkg create odom_plumb --build-type ament_cmake --dependencies rclcpp nav_msgs geometry_msgs tf2_ros --node-name odom_node
先编译一下
colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
消息接口
底盘驱动启动之后，会持续广播一个比较重要的数据——里程计。里程计用于描述当前机器人相对于出发点的位姿以及速度等信息，里程计在机器人定位和导航的局部路径规划中有着重要的作用。在ROS2中里程计消息被封装为了nav_msgs/msg/Odometry接口，可通过指令ros2 interface show nav_msgs/msg/Odometry查看该接口的具体数据结构，结果如下：
它描述的是机器人位姿与速度的预估信息。 其中位姿信息是以header中的frame_id为参考系的，而速度信息则是以child_frame_id为参考系的。
在 ROS2 中， odom 坐标系 （通常是 odom frame）遵循 右手坐标系 ，其定义如下：
- X 轴 ：向前（前进方向）为 正
- Y 轴 ：向左（左侧方向）为 正
- Z 轴 ：向上（垂直地面）为 正
在 ROS2 的 odom 坐标系 （右手坐标系）中， yaw 角的定义 如下：
- yaw = 0 ：机器人面向 X 轴正方向 （向前）。
- 逆时针（CCW，Counter Clockwise）旋转为正 。
- 顺时针（CW，Clockwise）旋转为负 。
yaw 角范围
yaw ∈ (-π, π]（即-180°到180°）- 当
yaw = π（180°），如果继续增加，应该跳转到-π（-180°），而不是π。 - 当
yaw = -π（-180°），如果继续减少，应该跳转到π（180°）。
std_msgs/Header header # 标头
builtin_interfaces/Time stamp # 时间戳
int32 sec
uint32 nanosec
string frame_id # 位姿信息所参考的坐标系
位姿信息所指向的坐标系，也是速度信息所参考的坐标系
string child_frame_id
相对于frame_id的预估位姿信息
geometry_msgs/PoseWithCovariance pose
Pose pose
Point position # 坐标
float64 x
float64 y
float64 z
Quaternion orientation # 四元数
float64 x 0
float64 y 0
float64 z 0
float64 w 1
float64[36] covariance
相对于child_frame_id的预估线速度与角速度
geometry_msgs/TwistWithCovariance twist
Twist twist
Vector3 linear # 线速度
float64 x
float64 y
float64 z
Vector3 angular # 角速度
float64 x
float64 y
float64 z
float64[36] covariance
需要注意的是，
- 坐标和位姿是相对于frame_id的，一般是odom自己。
- 而线速度和角速度是相对于child_frame_id的。一般是base_link 。 （这里注意，学长我最初就弄错了）
- 虽然Twist是相对于child_frame_id （ base_link）的，但是他表达的是瞬时值，就和汽车一样，汽车上面的速度表也是相对于车身的瞬时值。
- 里程计在计算时是存在累计误差的，它所描述的是机器人的预估位姿，并不绝对准确。
节点主要逻辑
所以我们需要初始化nav_msgs::msg::Odometry并发布出去。
所以你需要从MCU单片机上去获取这些数据，或者计算这些数据，并以nav_msgs::msg::Odometry类型的消息发送出去。
注意，如果你没有正儿八经的里程计，那么里程计的数据可以由底盘电机编码器数据计算而成。
除了发布话题的代码，别忘记发布odom与base_link之间的动态坐标变换。
下方是运动学正解算出来的odom数据:
#include "odom_mg513.h"
#include "mg513_gmr500ppr.h"
//#include <cmath>
#include "arm_math.h"
ODOM_Motor odom_motor_;
// 坐标系满足右手坐标系
//（单位：米）
//轮距
extern fp32 Wheel_Spacing;
//轴距
extern fp32 Alex_Spacing;
//轮子半径
extern fp32 Wheel_Radius;
void ODOM_Motor::Analysis(fp32 dt)
{
this->dt = dt;
for(int16_t i = 0;i < 4;i++)
{
// 将 RPM 转换为 m/s
encoder_wheel_velocities_[i] = mg513_gmr500ppr_motor[i].encoder.motor_data.motor_speed * 2.0f * 3.14159265358979f * Wheel_Radius / 60.0f;
}
// 计算机器人前进的线速度和角速度，公式不需要轮半径
//线速度，四个轮子在机器人的运动学模型中贡献相同，所以要除以4
this->vx = ( encoder_wheel_velocities_[0] + encoder_wheel_velocities_[1] - encoder_wheel_velocities_[2] - encoder_wheel_velocities_[3]) / 4.0f;
this->vy = ( encoder_wheel_velocities_[0] - encoder_wheel_velocities_[1] - encoder_wheel_velocities_[2] + encoder_wheel_velocities_[3]) / 4.0f;
//线速度，轮距（wheel_spacing） 决定了左右轮对旋转的贡献程度，轴距（alex_spacing） 决定了前后轮对旋转的贡献程度，
//所以要除以底盘尺寸，alex_spacing + wheel_spacing 是底盘尺寸。
this->vw = (-encoder_wheel_velocities_[0] - encoder_wheel_velocities_[1] - encoder_wheel_velocities_[2] - encoder_wheel_velocities_[3]) / (4.0f * (Wheel_Spacing + Alex_Spacing));
// 更新机器人的位置（假设机器人沿着y轴移动）
// this->x_position += this->vx * std::__math::cos(this->yaw) * this->dt;
// this->y_position += this->vy * std::__math::sin(this->yaw) * this->dt;
this->x_position += this->vx * arm_cos_f32(this->yaw) * this->dt;
this->y_position += this->vy * arm_sin_f32(this->yaw) * this->dt;
this->yaw += this->vw * this->dt;
// 保证 yaw 始终在 -PI 到 PI 之间
if(this->yaw > 3.14159265358979f)
{
this->yaw -= 2.0 * 3.14159265358979f;
}
if(this->yaw < -3.14159265358979f)
{
this->yaw += 2.0 * 3.14159265358979f;
}
// this->yaw_deg = this->yaw * 180.0f / 3.14159265358979f;
}
ROS2总代码如下：
#include "rclcpp/rclcpp.hpp"
#include "serial_driver/serial_driver.hpp"
#include <chrono>
#include <functional>
#include <rclcpp/logging.hpp>
#include <string>
#include "sensor04_odom_kc/serial_pack.h"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
extern std::vector<uint8_t> tx_data_buffer;
class Odom_KC_Node : public rclcpp::Node
{
public:
Odom_KC_Node() : Node("Odom_KC_Node")
{
// 声明参数（带默认值）
// 串口设备默认名（根据实际设备调整）
this->declare_parameter("device_name", "/dev/ttyACM1");
//串口默认波特率
this->declare_parameter("baud_rate", 115200);
// 获取参数值
const std::string device_name = this->get_parameter("device_name").as_string();
const uint32_t baud_rate = this->get_parameter("baud_rate").as_int();
RCLCPP_INFO(this->get_logger(), "Serial port Node Open!");
// 创建串口配置对象
// 波特率默认115200；不开启流控制；无奇偶效验；停止位1。
drivers::serial_driver::SerialPortConfig config(
baud_rate,
drivers::serial_driver::FlowControl::NONE,
drivers::serial_driver::Parity::NONE,
drivers::serial_driver::StopBits::ONE);
// 初始化串口
try
{
io_context_ = std::make_shared<drivers::common::IoContext>(1);
// 初始化 serial_driver_
serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
serial_driver_->init_port(device_name, config);
serial_driver_->port()->open();
RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
}
catch (const std::exception &ex)
{
RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
return;
}
//串口发布方
odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("odom",10);
//初始化tf广播器
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
//初始化cmd_vel消息订阅
cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>("cmd_vel",10,std::bind(&Odom_KC_Node::cmd_vel_sub_callback,this,std::placeholders::_1));
async_receive_message(); //进入异步接收
}
private:
std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
std::shared_ptr<drivers::common::IoContext> io_context_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
// // 四个轮子的速度(对应单片机的顺序)（单位：rpm）
// fp32 received_encoder_wheel_velocities_[4] = {0.0, 0.0, 0.0, 0.0};
// // 四个轮子的速度(对应单片机的顺序)（单位：m/s）
// fp32 encoder_wheel_velocities_[4] = {0.0, 0.0, 0.0, 0.0};
// // 四个轮子的速度(对应单片机的顺序)（单位：2000pc）
// fp32 received_encoder_wheel_angle_[4] = {0.0, 0.0, 0.0, 0.0};
//麦克纳姆轮底盘参数
const fp32 wheel_spacing = 0.093; // 轮间距（单位：米）
const fp32 alex_spacing = 0.085; // 轮距（单位：米）
const fp32 wheel_radius_ = 0.0375; // 轮子半径（单位：米）
fp32 vy=0.0,vx=0.0,vw=0.0;
fp32 yaw_ = 0.0; // 机器人航向角（单位：弧度）
fp32 dt_;
fp32 x_position_ = 0.0; // x位置
fp32 y_position_ = 0.0; // y位置
void async_receive_message() //创建一个函数更方便重新调用
{
auto port = serial_driver_->port();
// 设置接收回调函数
port->async_receive([this](const std::vector<uint8_t> &data,const size_t &size)
{
//创建odom消息类型
auto odom_msg = nav_msgs::msg::Odometry();
if (size > 0)
{
for(int16_t i = 0;i < (int16_t)size;++i)
{
uint8_t data_buffer = data[i];
// 处理接收到的数据
serial_pack_.rx.Data_Analysis(
&data_buffer,
0x01,
0,
0,
0,
0,
8);
}
//由于在ROS2中，node是局部变量，所以发布方只能在node类里，故Data_Apply不写任何东西，直接在接收下面的回调函数里实现功能。
if(serial_pack_.rx.data.cmd == 0x01)
{
RCLCPP_DEBUG(this->get_logger(), "\n");
RCLCPP_DEBUG(this->get_logger(), "以下是电机编码器的odom数据：");
// // 存储电机速度数据
// received_encoder_wheel_velocities_[0] = serial_pack_.rx.data.fp32_buffer[0]; // 电机 0 速度
// received_encoder_wheel_velocities_[1] = serial_pack_.rx.data.fp32_buffer[1]; // 电机 1 速度
// received_encoder_wheel_velocities_[2] = serial_pack_.rx.data.fp32_buffer[2]; // 电机 2 速度
// received_encoder_wheel_velocities_[3] = serial_pack_.rx.data.fp32_buffer[3]; // 电机 3 速度
// // 存储电机位置数据
// received_encoder_wheel_angle_[0] = serial_pack_.rx.data.fp32_buffer[4]; // 电机 0 位置
// received_encoder_wheel_angle_[1] = serial_pack_.rx.data.fp32_buffer[5]; // 电机 1 位置
// received_encoder_wheel_angle_[2] = serial_pack_.rx.data.fp32_buffer[6]; // 电机 2 位置
// received_encoder_wheel_angle_[3] = serial_pack_.rx.data.fp32_buffer[7]; // 电机 3 位置
// vx = serial_pack_.rx.data.fp32_buffer[8];
// vy = serial_pack_.rx.data.fp32_buffer[9];
// vw = serial_pack_.rx.data.fp32_buffer[10];
// yaw_ = serial_pack_.rx.data.fp32_buffer[11];
// dt_ = serial_pack_.rx.data.fp32_buffer[12];
// x_position_ = serial_pack_.rx.data.fp32_buffer[13];
// y_position_ = serial_pack_.rx.data.fp32_buffer[14];
vx = serial_pack_.rx.data.fp32_buffer[0];
vy = serial_pack_.rx.data.fp32_buffer[1];
vw = serial_pack_.rx.data.fp32_buffer[2];
yaw_ = serial_pack_.rx.data.fp32_buffer[3];
dt_ = serial_pack_.rx.data.fp32_buffer[4];
x_position_ = serial_pack_.rx.data.fp32_buffer[5];
y_position_ = serial_pack_.rx.data.fp32_buffer[6];
// 打印电机速度和位置（角度）
for (int i = 0; i < 4; ++i)
{
// RCLCPP_DEBUG(this->get_logger(), "%d号电机的速度: %.6f RPM, 位置: %.6f (2000pc)",
// i, received_encoder_wheel_velocities_[i], received_encoder_wheel_angle_[i]);
RCLCPP_DEBUG(this->get_logger(),"线速度:x:%.6f,y:%.6f,z:%.6f",vx,vy,0.0f);
RCLCPP_DEBUG(this->get_logger(),"角速度:x:%.6f,y:%.6f,z:%.6f",0.0f,0.0f,vw);
RCLCPP_DEBUG(this->get_logger(),"欧拉角(逆正顺负):r:%.6f,p:%.6f,y:%.6f",0.0f,0.0f,yaw_);
RCLCPP_DEBUG(this->get_logger(),"积分间隔:%.6f",dt_);
RCLCPP_DEBUG(this->get_logger(),"右手坐标系X坐标(前正后负):%.6f",x_position_);
RCLCPP_DEBUG(this->get_logger(),"右手坐标系Y坐标(左正右负):%.6f",y_position_);
}
//时间戳
odom_msg.header.stamp = this->get_clock()->now();
//位姿信息所参考的坐标系
odom_msg.header.frame_id = "odom";
// 设置child_frame_id（底盘坐标系）
odom_msg.child_frame_id = "base_link"; // 设置子坐标系为机器人底盘坐标系
//DOF平动位置
odom_msg.pose.pose.position.x = x_position_;
odom_msg.pose.pose.position.y = y_position_;
odom_msg.pose.pose.position.z = 0.0;
//从欧拉角转换为四元数
tf2::Quaternion q;
q.setRPY(0,0,yaw_);
//DOF转动（四元数）
odom_msg.pose.pose.orientation.x = q.x();
odom_msg.pose.pose.orientation.y = q.y();
odom_msg.pose.pose.orientation.z = q.z();
odom_msg.pose.pose.orientation.w = q.w();
//线速度
odom_msg.twist.twist.linear.x = vx;
odom_msg.twist.twist.linear.y = vy;
odom_msg.twist.twist.linear.z = 0.0;
//角速度
odom_msg.twist.twist.angular.x = 0.0;
odom_msg.twist.twist.angular.y = 0.0;
odom_msg.twist.twist.angular.z = vw;
// 发布消息
odom_pub_->publish(odom_msg);
// 打印位置（XYZ）
RCLCPP_DEBUG(
this->get_logger(),
"位置Position(XYZ): %.6f, %.6f, %.6f",
odom_msg.pose.pose.position.x,
odom_msg.pose.pose.position.y,
odom_msg.pose.pose.position.z
);
// 打印姿态（四元数WXYZ）
RCLCPP_DEBUG(
this->get_logger(),
"姿态Orientation(WXYZ): %.6f, %.6f, %.6f, %.6f",
odom_msg.pose.pose.orientation.w, // 注意顺序：WXYZ
odom_msg.pose.pose.orientation.x,
odom_msg.pose.pose.orientation.y,
odom_msg.pose.pose.orientation.z
);
// 打印姿态（欧拉角RPY）
RCLCPP_DEBUG(
this->get_logger(),
"欧拉角Euler(RPY): %.6f, %.6f, %.6f",
0.0,
0.0,
yaw_
);
// 打印线速度（XYZ）
RCLCPP_DEBUG(
this->get_logger(),
"线速度LinearVel(XYZ): %.6f, %.6f, %.6f",
odom_msg.twist.twist.linear.x,
odom_msg.twist.twist.linear.y,
odom_msg.twist.twist.linear.z
);
// 打印角速度（XYZ）
RCLCPP_DEBUG(
this->get_logger(),
"角速度AngularVel(XYZ): %.6f, %.6f, %.6f",
odom_msg.twist.twist.angular.x,
odom_msg.twist.twist.angular.y,
odom_msg.twist.twist.angular.z
);
/* 接下来发布tf */
auto transform = geometry_msgs::msg::TransformStamped();
transform.header.stamp = odom_msg.header.stamp; // 时间戳与odom同步
transform.header.frame_id = "odom";
transform.child_frame_id = "base_link";
transform.transform.translation.x = x_position_;
transform.transform.translation.y = y_position_;
transform.transform.translation.z = 0.0;
transform.transform.rotation = odom_msg.pose.pose.orientation; // 复用四元数
tf_broadcaster_->sendTransform(transform);
}
}
// 继续监听新的数据
async_receive_message();
}
);
}
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(tx_data_buffer);
RCLCPP_DEBUG(this->get_logger(), "平动XYZ：%.6f,%.6f,%.6f", fp32_buffer[0],fp32_buffer[1],fp32_buffer[2]);
RCLCPP_DEBUG(this->get_logger(), "转动XYZ：%.6f,%.6f,%.6f", fp32_buffer[3],fp32_buffer[4],fp32_buffer[5]);
RCLCPP_DEBUG(this->get_logger(), "(%ld bytes)", bytes_size);
}
catch(const std::exception &ex)
{
RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what());
}
}
};
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<Odom_KC_Node>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
最终odom有效数据应该在250Hz左右,4ms发布一次,对于导航也是完全够用的.
惯性计IMU
简介
概念
IMU是惯性测量单元（Inertial Measurement Unit）的缩写，它是一种集成了多个惯性传感器如加速度计、陀螺仪、磁力计等的导航传感器。IMU可以实时地测量和记录其所在物体的加速度、角速度和方向等信息，并通过信号处理和数学模型来计算出物体的姿态、位置和运动状态等。
IMU（惯性测量单元）主要由以下几种类型组成：
- 加速度计（Accelerometer）：用于测量物体的加速度，通常以三个轴向（X、Y、Z）提供加速度数据。加速度计可以帮助判断物体的线性运动和动作姿态变化。
- 陀螺仪（Gyroscope）：用于测量物体的角速度或角度变化率。陀螺仪可以提供关于物体旋转的信息，判断物体的角度、方向和转动动作。
- 磁力计（Magnetometer）：用于测量物体周围的磁场强度和方向（即测量场强）。磁力计可以帮助判断物体的方向和导航，尤其在地磁定位和航向控制中起重要作用。
这些传感器通常集成在一起形成IMU，并通过数据融合算法将它们的输出结合起来，提供综合的姿态、位置和运动信息。
作用
IMU是一种重要的传感器组件，它在姿态感知、行驶状态监测、非GPS定位中发挥着重要的作用。
特点
惯性测量单元（IMU）作为一种常见的传感器，在机器人领域中也有许多应用，下面是IMU在机器人领域的一些优点和缺点：
（1）优点：
- 实时性高： IMU能够以很高的采样频率获取姿态和加速度数据，提供实时的运动信息。
- 精度较高： IMU可以测量物体的加速度和角速度，经过合成和滤波处理后，可提供较高精度的姿态估计和运动跟踪。
- 不受光照条件限制： IMU不依赖于光照条件，适用于各种环境，如室内、室外和低光条件。
- 尺寸小巧： IMU通常具有小型、轻量级和紧凑的设计，适合嵌入式系统和对空间要求较高的机器人平台。
- 低功耗： IMU的功耗相对较低，适合资源受限或需要长时间运行的应用。
（2）缺点：
- 累积误差： 由于积分计算的误差会随时间累积，IMU的姿态估计随着时间推移可能会出现漂移，需要与其他传感器进行融合或采用校准方法进行补偿。
- 受外部干扰影响： IMU的测量结果容易受到振动、震动和温度变化等外界干扰的影响，可能导致姿态估计不稳定或不准确。
- 不适用于绝对定位： 单独使用IMU无法提供物体的绝对位置信息，需要结合其他传感器（如GNSS）来实现绝对定位。
- 无法感知环境信息： IMU只能提供自身的加速度和角速度数据，无法直接感知环境或检测周围物体。
综上所述，IMU在机器人领域具有实时性高、精度较高、不受光照条件限制、尺寸小巧和低功耗等优点。然而，它也存在累积误差、受外部干扰影响、不适用于绝对定位和无法感知环境信息等缺点。因此，在设计机器人系统时需综合考虑应用需求，并与其他传感器（如相机、激光雷达等）进行融合，以提升机器人的感知、定位和控制能力。
消息接口
在ROS2中imu消息被封装为了sensor_msgs/msg/Imu接口，可通过指令ros2 interface show sensor_msgs/msg/Imu查看该接口的具体数据结构，结果如下：
# 这是一个从惯性测量单元（Inertial Measurement Unit，简称IMU）获取数据的消息。
#
# 加速度应该以米/秒^2为单位（不是以g为单位），旋转速度应该以弧度/秒为单位。
#
# 如果测量的协方差已知，则应该填写相应值。全零的协方差矩阵将被解释为“协方差未知”，必须从其他源获取协方差才能使用协方差数据。
#
# 如果没有其中一个数据元素的估计值（例如，你的IMU不会产生方向估计），请将相关协方差矩阵的第 0 个元素设置为 -1。
# 如果你正在解析此消息，请检查每个协方差矩阵的第一个元素中是否有 -1 的值，并忽略相关的估计值。
std_msgs/Header header # 标头
builtin_interfaces/Time stamp # 时间戳
int32 sec
uint32 nanosec
string frame_id # imu坐标系
geometry_msgs/Quaternion orientation # 四元数姿态
float64 x 0
float64 y 0
float64 z 0
float64 w 1
float64[9] orientation_covariance # 姿态协方差矩阵
geometry_msgs/Vector3 angular_velocity # 三轴角速度
float64 x
float64 y
float64 z
float64[9] angular_velocity_covariance # 角速度协方差矩阵
geometry_msgs/Vector3 linear_acceleration # 三轴线性加速度
float64 x
float64 y
float64 z
float64[9] linear_acceleration_covariance # 线性加速度协方差矩阵
功能包创建
首先先创建功能包
ros2 pkg create imu_plumb --build-type ament_cmake --dependencies rclcpp sensor_msgs --node-name imu_node
先编译一下
colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
节点主要逻辑
#include "rclcpp/rclcpp.hpp"
#include "serial_driver/serial_driver.hpp"
#include "cyber_imu/WitStandardProtocol.h"
#include "sensor_msgs/msg/imu.hpp"
#include <cstdint>
#include <rclcpp/logging.hpp>
#include <sensor_msgs/msg/detail/imu__struct.hpp>
#include <tf2/LinearMath/Quaternion.h>
class ImuNode: public rclcpp::Node
{
public:
ImuNode():Node("ImuNode_node_cpp")
{
RCLCPP_INFO(this->get_logger(),"惯性计imu节点启动!");
//创建odom话题通信发布方
imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("/imu",10);
// 声明参数（带默认值）
// 串口默认设备名（根据实际设备调整）
this->declare_parameter("device_name", "/dev/ttyUSB0");
//串口默认波特率
this->declare_parameter("baud_rate", 115200);
// 获取参数值
const std::string device_name = this->get_parameter("device_name").as_string();
const uint32_t baud_rate = this->get_parameter("baud_rate").as_int();
// 创建串口配置对象
// 波特率默认115200；不开启流控制；无奇偶效验；停止位1。
drivers::serial_driver::SerialPortConfig config(
baud_rate,
drivers::serial_driver::FlowControl::NONE,
drivers::serial_driver::Parity::NONE,
drivers::serial_driver::StopBits::ONE);
// 初始化串口
try
{
io_context_ = std::make_shared<drivers::common::IoContext>(1);
// 初始化 serial_driver_
serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
serial_driver_->init_port(device_name, config);
serial_driver_->port()->open();
RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
}
catch (const std::exception &ex)
{
RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
return;
}
async_receive_message(); //进入异步接收
}
private:
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
std::shared_ptr<drivers::common::IoContext> io_context_;
void async_receive_message() //创建一个函数更方便重新调用
{
auto port = serial_driver_->port();
// 设置接收回调函数
port->async_receive([this](const std::vector<uint8_t> &data,const size_t &size)
{
if (size > 0)
{
RCLCPP_DEBUG(this->get_logger(),"\n");
RCLCPP_DEBUG(this->get_logger(),"以下是接收到的IMU惯性计数据:");
//创建imu消息类型
auto imu_msg = sensor_msgs::msg::Imu();
bool valid_frame = false; // 添加有效帧标志
bool valid_data = false; // 添加有效帧标志
for(int16_t i = 0;i < (int16_t)size;++i)
{
uint8_t data_buffer = data[i];
// RCLCPP_INFO(this->get_logger(),"接收到的字节：%x",data_buffer);
// 处理接收到的数据
if(wit_imu_.rx.Data_Analysis(&data_buffer) == true)
{
valid_frame = true; // 标记有有效帧需要处理
}
if(valid_frame == true)
{
if(wit_imu_.rx.data.cmd == 0x51)
{
// 存储加速度计数据
wit_imu_.rx.data.imu.Accel.X = wit_imu_.rx.data.int16_buffer[0];
wit_imu_.rx.data.imu.Accel.Y = wit_imu_.rx.data.int16_buffer[1];
wit_imu_.rx.data.imu.Accel.Z = wit_imu_.rx.data.int16_buffer[2];
wit_imu_.rx.data.imu.Temp = wit_imu_.rx.data.int16_buffer[3];
//加速度单位m/s2，温度单位℃摄氏度
wit_imu_.rx.data.imu.Accel.X = wit_imu_.rx.data.imu.Accel.X / 32768.0f * 16.0f * 9.8f;
wit_imu_.rx.data.imu.Accel.Y = wit_imu_.rx.data.imu.Accel.Y / 32768.0f * 16.0f * 9.8f;
wit_imu_.rx.data.imu.Accel.Z = wit_imu_.rx.data.imu.Accel.Z / 32768.0f * 16.0f * 9.8f;
wit_imu_.rx.data.imu.Temp = wit_imu_.rx.data.imu.Temp / 100.0f;
// 打印加速度计数据
RCLCPP_DEBUG(this->get_logger(), "加速度(XYZ): %.6f, %.6f, %.6f",
wit_imu_.rx.data.imu.Accel.X, wit_imu_.rx.data.imu.Accel.Y, wit_imu_.rx.data.imu.Accel.Z);
//减少时间戳带来的延迟
imu_msg.header.stamp = this->get_clock()->now();
}
else if(wit_imu_.rx.data.cmd == 0x52)
{
// 存储陀螺仪数据
wit_imu_.rx.data.imu.Gyro.X = wit_imu_.rx.data.int16_buffer[0];
wit_imu_.rx.data.imu.Gyro.Y = wit_imu_.rx.data.int16_buffer[1];
wit_imu_.rx.data.imu.Gyro.Z = wit_imu_.rx.data.int16_buffer[2];
//角速度单位rad/s
wit_imu_.rx.data.imu.Gyro.X = wit_imu_.rx.data.imu.Gyro.X / 32768.0f * 2000.0f * (M_PI / 180.0f);
wit_imu_.rx.data.imu.Gyro.Y = wit_imu_.rx.data.imu.Gyro.Y / 32768.0f * 2000.0f * (M_PI / 180.0f);
wit_imu_.rx.data.imu.Gyro.Z = wit_imu_.rx.data.imu.Gyro.Z / 32768.0f * 2000.0f * (M_PI / 180.0f);
// 打印陀螺仪数据
RCLCPP_DEBUG(this->get_logger(), "角速度(XYZ): %.6f, %.6f, %.6f",
wit_imu_.rx.data.imu.Gyro.X, wit_imu_.rx.data.imu.Gyro.Y, wit_imu_.rx.data.imu.Gyro.Z);
}
else if(wit_imu_.rx.data.cmd == 0x54)
{
// 存储磁力计数据(单位lsb)
wit_imu_.rx.data.imu.Magnet.X = wit_imu_.rx.data.int16_buffer[0];
wit_imu_.rx.data.imu.Magnet.Y = wit_imu_.rx.data.int16_buffer[1];
wit_imu_.rx.data.imu.Magnet.Z = wit_imu_.rx.data.int16_buffer[2];
//磁场单位uT
// wit_imu_.rx.data.imu.Magnet.X = wit_imu_.rx.data.imu.Magnet.X / 1.0f;
// wit_imu_.rx.data.imu.Magnet.Y = wit_imu_.rx.data.imu.Magnet.Y / 1.0f;
// wit_imu_.rx.data.imu.Magnet.Z = wit_imu_.rx.data.imu.Magnet.Z / 1.0f;
// 打印磁力计数据
RCLCPP_DEBUG(this->get_logger(), "磁场(XYZ): %.6f, %.6f, %.6f",
wit_imu_.rx.data.imu.Magnet.X, wit_imu_.rx.data.imu.Magnet.Y, wit_imu_.rx.data.imu.Magnet.Z);
}
else if(wit_imu_.rx.data.cmd == 0x53)
{
// 存储欧拉角数据
wit_imu_.rx.data.imu.Euler.roll = wit_imu_.rx.data.int16_buffer[0];
wit_imu_.rx.data.imu.Euler.pitch = wit_imu_.rx.data.int16_buffer[1];
wit_imu_.rx.data.imu.Euler.yaw = wit_imu_.rx.data.int16_buffer[2];
//欧拉角单位dec
wit_imu_.rx.data.imu.Euler.roll = wit_imu_.rx.data.imu.Euler.roll / 32768.0f * 180.0f;
wit_imu_.rx.data.imu.Euler.pitch = wit_imu_.rx.data.imu.Euler.pitch / 32768.0f * 180.0f;
wit_imu_.rx.data.imu.Euler.yaw = wit_imu_.rx.data.imu.Euler.yaw / 32768.0f * 180.0f;
// 打印欧拉角数据
RCLCPP_DEBUG(this->get_logger(), "欧拉角(XYZ_RPY): %.6f, %.6f, %.6f",
wit_imu_.rx.data.imu.Euler.roll, wit_imu_.rx.data.imu.Euler.pitch, wit_imu_.rx.data.imu.Euler.yaw);
//欧拉角单位rad
wit_imu_.rx.data.imu.Euler.roll = wit_imu_.rx.data.imu.Euler.roll * (M_PI / 180.0f);
wit_imu_.rx.data.imu.Euler.pitch = wit_imu_.rx.data.imu.Euler.pitch * (M_PI / 180.0f);
wit_imu_.rx.data.imu.Euler.yaw = wit_imu_.rx.data.imu.Euler.yaw * (M_PI / 180.0f);
}
else if(wit_imu_.rx.data.cmd == 0x59)
{
for(int16_t i = 0 ; i < 4 ; i++)
{
// 存储四元数数据
wit_imu_.rx.data.imu.Quat.q[i] = wit_imu_.rx.data.int16_buffer[i];
//四元数是归一化的四元数
wit_imu_.rx.data.imu.Quat.q[i] = wit_imu_.rx.data.imu.Quat.q[i] / 32768.0f;
}
// 打印四元数数据
//注意，0对应y，1对应x，-2才对应z。
RCLCPP_DEBUG(this->get_logger(), "四元数(xyzw): %.6f, %.6f, %.6f, %.6f",
wit_imu_.rx.data.imu.Quat.q[1],wit_imu_.rx.data.imu.Quat.q[0],-wit_imu_.rx.data.imu.Quat.q[2],wit_imu_.rx.data.imu.Quat.q[3]);
valid_data = true;
}
wit_imu_.rx.data.cmd = 0x00; //跑过一次就进行清0
valid_frame = false;
}
}
if(valid_data == true)
{
//时间戳
// imu_msg.header.stamp = this->get_clock()->now();
//位姿信息所参考的坐标系
imu_msg.header.frame_id = "imu";
// tf2::Quaternion q;
// //DOF欧拉角单位rad
// q.setRPY(roll_, pitch_, yaw_);
// //DOF欧拉角（四元数）
// imu_msg.orientation.x = q.x();
// imu_msg.orientation.y = q.y();
// imu_msg.orientation.z = q.z();
// imu_msg.orientation.w = q.w();
//DOF欧拉角（四元数）
//注意对应关系
// 传感器坐标系 -> ROS坐标系：
// X -> Y
// Y -> X
// Z -> -Z
imu_msg.orientation.x = wit_imu_.rx.data.imu.Quat.q[1];
imu_msg.orientation.y = wit_imu_.rx.data.imu.Quat.q[0];
imu_msg.orientation.z = - wit_imu_.rx.data.imu.Quat.q[2];
imu_msg.orientation.w = wit_imu_.rx.data.imu.Quat.q[3];
//加速度
imu_msg.linear_acceleration.x = wit_imu_.rx.data.imu.Accel.X;
imu_msg.linear_acceleration.y = wit_imu_.rx.data.imu.Accel.Y;
imu_msg.linear_acceleration.z = wit_imu_.rx.data.imu.Accel.Z;
//角速度
imu_msg.angular_velocity.x = wit_imu_.rx.data.imu.Gyro.X;
imu_msg.angular_velocity.y = wit_imu_.rx.data.imu.Gyro.Y;
imu_msg.angular_velocity.z = wit_imu_.rx.data.imu.Gyro.Z;
imu_pub_->publish(imu_msg);
valid_data = false;
}
}
// 继续监听新的数据
async_receive_message();
}
);
}
};
int main(int argc, char ** argv)
{
rclcpp::init(argc,argv);
auto node = std::make_shared<ImuNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
这种框架对于一个250Hz（每4ms输出一次）的陀螺仪来说,最后/imu的发布频率是100Hz（每10ms发布一次）,效率也是还可以接受的.
激光雷达LiDAR
相机Camera
全球定位GNSS
键盘控制节点
简介
要在 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;
}
坐标系与话题关系
odom坐标系 不是固定不动的，原点是机器人的初始位置。
base_link坐标系 是机器人本体的坐标系，固定在机器人中心（如底盘）。当机器人移动时，/odom话题发布机器人运动数据，base_link坐标系会相对于odom坐标系移动。
laser_link和imu_link等坐标系 和base_link之间的位置是静态的，所以当base_link移动了，其他子坐标系也会跟随移动。
odom坐标系是根据机器人电机编码器或者里程计反馈回来的数据预估的机器人出发时的位置，一般都会浮动。
map坐标系是通过算法用激光雷达等传感器预估出来的机器人出发时的位置。
但是由于建议使用树形TF，所以我们通常将map坐标系作为根坐标系，所以此时map在rviz2里是不动的，odom作为map坐标系的下行坐标系。（可以听一下赵老师ROS1的课程，这里讲的很好）
硬件平台进阶
轮式里程计标定与融合
场景
里程计在ROS中扮演着重要的角色，能够为机器人的定位、路径规划和运动控制等任务提供重要的数据支持。然而在实际应用中，里程计并不能准确地反映机器人的位置和运动状态，出现的问题包括：
1.里程计中的线速度、角速度和机器人实际的线速度、角速度明显有差异，而且里程计中提供的机器人位姿与真实位姿不一致；
2.当机器人车轮打滑或受到外力影响而出现姿态改变时，这种姿态的变化无法通过轮式里程计正确反馈。
在这些场景中，我们可以通过进行里程计标定和融合来优化解决这些问题。
概念
里程计标定： 里程计标定是指通过对机器人里程计进行精确的校准和调整，以减小或消除里程计测量中的误差和不准确性。例如，可以通过测量调整轮胎直径和轴距等参数来校准机器人的线速度和角速度，从而使里程计测量结果更加准确。里程计标定的主要目标是确保里程计测量结果与实际机器人的运动轨迹一致，从而提高机器人的定位精度和路径规划准确性。
里程计融合： 里程计融合是利用多个来源的里程计数据进行融合，以获得更准确可靠的机器人定位和运动信息。例如，将车轮编码器和惯性测量单元（IMU）的数据融合，可以提高定位的准确性。通过结合不同的传感器和算法，能够提高机器人在导航和控制任务中的准确性和鲁棒性，从而实现更高级别的自主移动能力。
作用
里程计标定和融合是机器人领域中的两个重要技术，通过利用这些技术，机器人可以更加准确地了解自身位置和运动状态，提高机器人的定位和导航精度，为机器人的自主导航、路径规划等任务提供基础。
轮式里程计标定
轮式里程计标定是校准机器人轮子的旋转和移动，以提供机器人运动中的准确位置和姿态信息的方法。通过标定，可以修正因轮子尺寸和轮距等因素引起的误差，获得更准确的运动轨迹。本节将以MyCar导航机器人为例演示标定过程，其标定步骤如下：
- 准备测量场地；
- 线性位移标定；
- 转角位移标定；
- 验证和调整。
另外，该过程还需要使用尺子作为测量工具。
1.准备测量场地
选择一个开放而平坦的区域，确保没有障碍物和其他干扰因素。
2.线性位移标定
线性位移标定是用来标定机器人在直线移动时行进的距离。该标定与车轮直径参数密切相关，当车轮直径测量不准确或轮胎磨损时，会导致轮式里程计的线性位移出现误差。以下是线性位移标定的主要步骤：
（1）实际数据采集
启动机器人底盘和键盘控制节点，使用键盘控制机器人向前直线运动一段距离，并测量线性位移数据。
（2）里程计消息采集
机器人停止后，输出里程计消息，并获取里程计消息中的机器人的位移数据。
（3）参数计算以及修改
收集实际位移数据（w1）和里程计消息中的位移数据（w2），并结合驱动包配置文件中的原轮胎直径值（d1），即可计算修正后的轮胎直径值（d）。计算公式如下：
d = w1 / w2 * d1;
将计算出的结果写入 MyCar 机器人的配置文件对应的参数，并重新构建。
为了获得最佳结果，可以多次执行上述流程。每次执行后，测量并记录新的实际位移数据（w1）和里程计消息中的位移数据（w2），然后根据新的数据再次计算修正后的轮胎直径值（d）。不断迭代这个过程，可以逐步优化轮胎直径的估计值，减小里程计的线性位移误差。
小提示：
如果使用的是以Arduino为主控的MyCar两轮差速机器人，那么需要修改ros2_arduino_bridge功能包下的params/arduino.yaml文件，该文件中有一个名为wheel_diameter参数即为车轮直径；
如果使用的是以Stm32为主控的MyCar两轮差速机器人，那么需要修改ros2_stm32_bridge功能包下的params/stm32_2w.yaml文件，该文件中有一个名为wheel_diameter参数即为车轮直径；
如果使用的是以Stm32为主控的MyCar四轮差速机器人，那么需要修改ros2_stm32_bridge功能包下的params/stm32_4w.yaml文件，该文件中有一个名为wheel_diameter参数即为车轮直径。
3.转角位移标定
转角位移标定是用来标定机器人在转弯时的角度变化。该标定与底盘的旋转半径参数密切相关，当旋转半径设置不当时，会导致转角位移的测量误差。另外需要注意的是，如果是两轮差速机器人，那么旋转半径和轮间距基本一致。这是因为两轮差速机器人在旋转时没有造成车体的侧滑运动，所以可以使用轮间距来计算旋转角度，并且旋转半径可以近似等于轮间距。然而，对于四轮差速机器人，在旋转时会产生侧滑，轮间距无法直接用于计算旋转角度。四轮差速机器人的旋转半径需要通过实验获取，因为它的旋转半径受到侧滑运动的影响。此外，由于机器人的左右侧轮胎差异或车辆重心偏移等原因，左旋转和右旋转时计算旋转角度所使用的旋转半径可能不同，因此需要进行分别实验来获取左右旋转时的旋转半径。
以下是转角位移标定的主要步骤：
（1）实际数据采集
启动机器人底盘和键盘控制节点，使用键盘控制机器人原地旋转，并测量转角位移数据，比如可以原地旋转一周。
（2）里程计消息采集
机器人停止后，打开rviz2，并添加tf插件，以显示机器人基坐标系（一般是base_link或base_footprint）与里程计坐标系（一般是odom）相对关系。
（3）参数计算以及修改
在机器人原地旋转一周时，如果机器人基坐标系与里程计坐标系相吻合，那么旋转半径参数无需调整；如果机器人基坐标系与里程计坐标系不吻合且rviz2中机器人的旋转角度大于实际旋转角度，那么请将旋转半径调大；如果机器人基坐标系与里程计坐标系不吻合且rviz2中机器人的旋转角度小于实际旋转角度，那么请将旋转半径调小。将计算出的结果写入 MyCar 机器人的配置文件对应的参数，并重新构建。
为了获得最佳结果，可以多次执行上述流程。每次执行后，测量并记录新的实际转角位移数据和rviz2中的坐标系相对关系，然后根据新的数据再次修正旋转半径。通过不断迭代这个过程，可以逐步优化旋转半径的估计值，减小转角位移的测量误差。
小提示：
如果使用的是以Arduino为主控的MyCar两轮差速机器人，那么需要修改ros2_arduino_bridge功能包下的params/arduino.yaml文件，该文件中有一个名为wheel_track参数即为车轮直径，在ros2_arduino_bridge中并未对左旋转和右旋转做区分；
如果使用的是以Stm32为主控的MyCar两轮差速机器人，那么需要修改ros2_stm32_bridge功能包下的params/stm32_2w.yaml文件，在机器人左旋转时，要标定的是名为model_param_acw的参数，在机器人右旋转时，要标定的是名为model_param_cw的参数；
如果使用的是以Stm32为主控的MyCar四轮差速机器人，那么需要修改ros2_stm32_bridge功能包下的params/stm32_4w.yaml文件，和两轮差速类似的，在机器人左旋转时，要标定的是名为model_param_acw的参数，在机器人右旋转时，要标定的是名为model_param_cw的参数。
4.验证和调整
将标定后的轮式里程计应用于实际场景中，并进行验证和调整。观察车辆的实际位置和运动与估计值的差异，并进行必要的调整和校准。另外，当车辆使用环境发生改变，或持续使用较长一段时间后，建议对里程计进行重新标定。
轮式里程计与IMU融合
轮式里程计可以通过监测轮子转动来估计机器人的位置和姿态变化，但容易受到滑动和不均匀地面等特殊情况的影响。IMU可以测量加速度和角速度，提供高频率的运动信息，但容易受到积分漂移和噪声的影响。将它们的数据融合可以弥补彼此的不足，提供更可靠和准确的定位结果，充分利用各自的优势，减少局限性。在ROS2中，提供了robot_localization功能包，可以方便的帮助开发人员实现二者的融合。
1.robot_localization简介
robot_localization是ROS2中的一个功能包，用于实现多传感器数据融合的机器人定位和导航。它提供了一个解决方案，可以将来自不同传感器（如轮式里程计、IMU、GPS、激光雷达等）的数据进行融合，以获得更准确和鲁棒的定位结果。在该功能包中，融合算法已经通过节点封装完毕，调用者只需在调用节点时注入参数即可。
请调用如下指令安装robot_localization：
sudo apt install ros-<distro>-robot-localization
请将<distro>替换为正在使用的ROS2发行版代号。
2.robot_localization节点说明
在robot_localization功能包中提供了ekf_node、ukf_node和navsat_transform_node多个节点来实现机器人的状态估计和定位。不同节点介绍如下：
ekf_node是基于增强卡尔曼滤波器（EKF）的节点，用于融合多个传感器数据来进行状态估计。它接收来自里程计、IMU、激光雷达等传感器的数据，并将它们融合在一起以提供更准确的机器人状态估计。EKF节点使用机器人的运动模型和传感器测量模型来估计机器人的位置、姿态和速度等状态变量。ukf_node是基于无迹卡尔曼滤波器（UKF）的节点，用于实现机器人的状态估计和本地化。UKF节点采用无迹变换（Unscented Transform）来处理非线性系统，通过估计机器人在空间中的位置和姿态，提供更准确的状态估计。与EKF相比，UKF节点可以更好地处理非线性系统的估计问题。navsat_transform_node用于将全球定位系统（GNSS）导航卫星数据与惯性测量单元（IMU）数据进行融合，以提供精确的位置估计。该节点校正GNSS信号的误差，并将其转化为带有高精度的位置信息。这有助于改善在室内或具有GNSS信号不稳定性的环境中的机器人定位精度。
这些节点是robot_localization功能包中的核心组件，每个节点都可以融合任意数量的传感器（惯性测量单元（IMU），里程计，室内定位系统，全球定位系统接收器等），它们支持 nav_msgs/msg/Odometry（位置、方向和线性、角速度）、geometry_msgs/msg/PoseWithCovarianceStamped（位置和方向）、geometry_msgs/msg/TwistWithCovarianceStamped（线性和角速度）和sensor_msgs/msg/Imu（方向、角速度和线性加速度）消息，以跟踪机器人的15维（x、y、z、滚动、俯仰、偏航、x˙、y˙、z˙、滚动˙、俯仰˙、偏航˙、x¨、y¨、z¨）状态。
基于这些测量数据，状态估计节点将过滤后的位置、方向和线性、角速度（nav_msgs/Odometry）发布到 /odometry/filtered 话题上，并在启用时将过滤后的加速度发布到 /accel/filtered 话题上。
此外，它们可以（默认启用）将相应的变换作为 tf2 变换进行发布，无论是 odom → base_link 变换还是 map → odom 变换。
3.robot_localization使用示例
假设要融合轮式里程计与IMU，那么请先保证机器人底盘可以发布里程计消息以及IMU消息。在MyCar导航机器人中，Stm32底盘是满足条件的，但是Arduino底盘不包含IMU传感器，需要自行安装。融合时，可以使用ekf_node或ukf_node节点。本案例中使用的是stm32 4w底盘结合ekf_node实现。其大致步骤如下：
- 编写luanch文件；
- 编写配置文件；
- 编译；
- 执行并查看结果。
由于robot_localization对融合实现已经做了很好的封装，所以整个实现流程是比较简单的。
（1）编写launch文件
在ros2_stm32_bridge功能包的launch目录下新建文件driver_ekf.launch.py，并输入以下内容：
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
import launch_ros.actions
import os
def generate_launch_description():
MYCAR_MODEL = os.environ['MYCAR_MODEL']return LaunchDescription([
launch_ros.actions.Node(
package='robot_localization',
executable='ekf_node',
name='ekf_filter_node',
output='screen',
parameters=[os.path.join(get_package_share_directory("ros2_stm32_bridge"), 'params', 'ekf.yaml')],
),
launch_ros.actions.Node(
package='tf2_ros',
executable='static_transform_publisher',
arguments=['--frame-id', 'base_footprint', '--child-frame-id', 'imu_link', '--x', '-0.15']
),
launch_ros.actions.Node(
package="ros2_stm32_bridge",
executable="base_controller",
parameters=[
os.path.join(get_package_share_directory("ros2_stm32_bridge"), "params", MYCAR_MODEL + "_ekf.yaml"),],
)
])
该launch文件中，启动了机器人底盘驱动，启动了robot_localization包中ekf_node节点，并且还发布了base_footprint与imu_link的坐标变换。
（2）编写配置文件
- 在ros2_stm32_bridge功能包的params目录下新建文件stm32_4w_ekf.yaml，并输入以下内容：
/mini_driver:
# /**: ros__parameters: base_frame: base_footprint
baud_rate: 115200 control_rate: 10 encoder_resolution: 44.0 kd: 130 ki: 0 kp: 100 maximum_encoding: 100.0 model_param_acw: 0.45 model_param_cw: 0.45 odom_frame: odom
port_name: /dev/mycar
publish_tf: false qos_overrides:
/parameter_events:
publisher: depth: 1000 durability: volatile
history: keep_last
reliability: reliable
/tf:
publisher: depth: 100 durability: volatile
history: keep_last
reliability: reliable
reduction_ratio: 90.0 use_sim_time: false wheel_diameter: 0.080
在该文件中，一个重要操作是将publish_tf参数设置为false，设置为false后，底盘驱动不再发布base_footprint与odom的坐标变换，而是由ekf_node取而代之。
- 在ros2_stm32_bridge功能包的params目录下新建文件ekf.yaml，并输入以下内容：
### ekf config file ###
ekf_filter_node:
ros__parameters:
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
frequency: 30.0
# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
# filter will generate new output. Defaults to 1 / frequency if not specified.
sensor_timeout: 0.1
# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
two_d_mode: false
# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
# unspecified.
transform_time_offset: 0.0
# Use this parameter to provide specify how long the tf listener should wait for a transform to become available.
# Defaults to 0.0 if unspecified.
transform_timeout: 0.0
# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
# unhappy with any settings or data.
print_diagnostics: true
# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
# effects on the performance of the node. Defaults to false if unspecified.
debug: false
# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
debug_out_file: /path/to/debug/file.txt
# Whether we'll allow old measurements to cause a re-publication of the updated state
permit_corrected_publication: false
# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration: false
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf: true
# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
# Here is how to use the following settings:
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
# odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
# from landmark observations) then:
# 3a. Set your "world_frame" to your map_frame value
# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
# estimation node from robot_localization! However, that instance should *not* fuse the global data.
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_footprint # Defaults to "base_link" if unspecified
world_frame: odom # Defaults to the value of odom_frame if unspecified
# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
# default values, and must be specified.
odom0: odom
# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
# if unspecified, effectively making this parameter required for each sensor.
odom0_config: [false, false, false,
false, false, false,
true, false, false,
false, false, false,
false, false, false]
# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
# the size of the subscription queue so that more measurements are fused.
odom0_queue_size: 2
# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
# algorithm.
odom0_nodelay: false
# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
# for twist measurements has no effect.
odom0_differential: false
# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
odom0_relative: false
# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
# the thresholds.
odom0_pose_rejection_threshold: 5.0
odom0_twist_rejection_threshold: 1.0
imu0: imu
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8 #
imu0_linear_acceleration_rejection_threshold: 0.8 #
# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
imu0_remove_gravitational_acceleration: true
# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
# for the velocity variable in question, or decrease the variance of the variable in question in the measurement
# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
# inputs, the control term will be ignored.
# Whether or not we use the control input during predicition. Defaults to false.
use_control: true
# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
# false.
stamped_control: false
# The last issued control command will be used in prediction for this period. Defaults to 0.2.
control_timeout: 0.2
# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
control_config: [true, false, false, false, false, true]
# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
# Acceleration and deceleration limits are not always the same for robots.
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
# gains
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
# gains
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
# However, if users find that a given variable is slow to converge, one approach is to increase the
# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
# unspecified.
process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015]
# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
# question. Users should take care not to use large values for variables that will not be measured directly. The values
# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
#if unspecified.
initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9]
在该文件中，关键配置如下：
#......
odom0_config: [false, false, false,
false, false, false,
true, false, false,
false, false, false,
false, false, false]
#......
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]
上述配置主要是融合了里程计消息中的x方向速度与imu消息中的yaw角度生成新的里程计数据。
（3）编译
终端中进入当前工作空间，编译功能包：
colcon build --packages-select ros2_stm32_bridge
（4）执行
终端中进入当前工作空间，执行launch文件：
. install/setup.bash
ros2 launch ros2_stm32_bridge driver_ekf.launch.py
再启动rviz2以及键盘控制节点，在rviz2中将参考坐标系设置为odom，添加tf插件，再添加两个odometry插件并将话题分别设置为odom和odometry/filtered。使用键盘控制机器人前进，两个里程计插件显示的数据和机器人的实际运行基本相符，前进一段时间后，手动搬动机器人让其产生一定角度的偏航，该动作也能在融合后的里程计以及坐标变换中准确显示，而轮式里程计由于电机并未旋转而没有改变（如下图所示）。
