ROS2 自主移动机器人（AMR） 项目系统启动与核心初始化(1)
·
模块与部署架构
1.1 系统启动架构图
各个组件之间的依赖和时序关系。
[系统上电 / 开机] │ ▼ [操作系统启动 (Ubuntu / Yocto)] │ ▼ [ROS 2 环境初始化] │ - 加载 setup.bash │ - 设置 RMW_IMPLEMENTATION (e.g. rmw_cyclonedds_cpp) │ - 设置 ROS_DOMAIN_ID │ - 启动 DDS 发现服务 (后台自动) │ ▼ [启动 Launch 文件 (bringup_robot.launch.py)] │ ├──▶ [加载机器人描述 (URDF/Xacro)] │ │ │ ▼ │ [robot_state_publisher 节点] ──── 发布 /robot_description 话题 │ │ 发布 /tf_static 话题 │ │ │ ├──▶ [启动底层驱动容器 (hardware_container)] │ │ │ ├──▶ [motor_driver_component] ─── 加载 CAN / EtherCAT 驱动 │ │ │ 订阅 /cmd_vel 话题 │ │ │ 发布 /odom 话题 │ │ │ 发布 /joint_states 话题 │ │ │ │ ├──▶ [lidar_driver_component] ── 加载激光雷达驱动 (如 rslidar_sdk) │ │ │ 发布 /scan 话题 │ │ │ │ └──▶ [imu_driver_component] ──── 加载 IMU 驱动 │ 发布 /imu 话题 │ ├──▶ [启动导航框架 (navigation_container)] │ │ │ ├──▶ [nav2_controller_server] ─── 本地规划器 (DWB / MPPI) │ │ │ 订阅 /plan, /odom, /scan │ │ │ 发布 /cmd_vel │ │ │ │ ├──▶ [nav2_planner_server] ───── 全局规划器 (A* / NavFn) │ │ │ 订阅 /map, /odom │ │ │ 发布 /plan │ │ │ │ ├──▶ [nav2_amcl] ─────────────── 自适应蒙特卡洛定位 │ │ │ 订阅 /scan, /map │ │ │ 发布 /amcl_pose, /tf │ │ │ │ └──▶ [nav2_map_server] ───────── 加载静态地图 │ 发布 /map 话题 │ ├──▶ [启动功能节点] │ │ │ ├──▶ [battery_monitor_node] ──── 订阅 /battery_state │ │ 发布 /battery_low 警告 │ │ │ ├──▶ [safety_monitor_node] ───── 接收激光雷达安全区数据 │ │ 发布紧急停止指令 │ │ │ └──▶ [diagnostic_aggregator] ─── 收集所有硬件/软件状态 │ 发布 /diagnostics 话题 │ └──▶ [启动可视化 / 遥操作 (可选)] │ ├──▶ [rviz2] ─────────────────── 加载配置 .rviz 文件 │ 订阅 /map, /scan, /tf 等 │ └──▶ [joystick_teleop] ──────── 手柄遥控 发布 /cmd_vel
此架构图清晰地展示了系统启动时的层次化依赖：首先必须有机器人模型描述，然后底层硬件驱动就绪，接着导航框架才能正常加载，最后才是上层功能节点和可视化工具。任何一个环节失败，都会导致后续模块无法正常启动或运行。
1.2 Launch 文件实现：bringup_robot.launch.py
Launch 文件是 ROS 2 系统的“总开关”，它定义了哪些节点需要运行、如何配置、以及如何设置参数。以下是一个完整的生产级 launch 文件示例，它使用 Python API 动态加载配置并启动多进程/多容器。
#!/usr/bin/env python3
"""
@file bringup_robot.launch.py
@brief AMR 机器人的总启动文件
该脚本负责：
1. 加载机器人 URDF 模型并启动 robot_state_publisher
2. 启动硬件驱动容器 (lidar, motor, imu)
3. 启动导航框架 (Nav2 全家桶)
4. 启动系统监控节点 (电池、安全、诊断)
5. 可选启动 RViz 可视化
@note 启动前需确保所有硬件设备已正确连接，CAN 总线已激活。
@see https://navigation.ros.org/ 获取 Nav2 配置细节
"""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
GroupAction,
LogInfo,
)
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node, LoadComposableNodes
from launch_ros.descriptions import ComposableNode
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
"""生成完整的 launch 描述对象"""
# ============================================================
# 1. 路径与参数定义
# ============================================================
robot_name = 'amr_bot'
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
# 获取各功能包的配置路径
pkg_robot_description = get_package_share_directory('amr_bot_description')
pkg_robot_bringup = get_package_share_directory('amr_bot_bringup')
pkg_nav2_bringup = get_package_share_directory('nav2_bringup')
# URDF 文件路径
urdf_file = os.path.join(pkg_robot_description, 'urdf', 'amr_bot.urdf')
with open(urdf_file, 'r') as f:
robot_description = f.read()
# 参数配置文件路径
lidar_config = os.path.join(pkg_robot_bringup, 'config', 'lidar_params.yaml')
motor_config = os.path.join(pkg_robot_bringup, 'config', 'motor_params.yaml')
nav2_config = os.path.join(pkg_robot_bringup, 'config', 'nav2_params.yaml')
rviz_config = os.path.join(pkg_robot_bringup, 'config', 'amr_bot.rviz')
# ============================================================
# 2. 节点定义
# ============================================================
# ---- 机器人状态发布器 (必须最先启动) ----
robot_state_pub = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
parameters=[
{'robot_description': robot_description},
{'use_sim_time': use_sim_time}
],
arguments=['--ros-args', '--log-level', 'warn'],
)
# ---- 硬件驱动容器 (使用组件降低进程间通信开销) ----
# 注意：这里使用 composition 容器来加载多个驱动组件，
# 它们共享同一进程地址空间，可实现零拷贝大数据传输。
hardware_container = GroupAction([
LogInfo(msg="Starting hardware driver container..."),
IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rclcpp_components'),
'launch',
'component_container_launch.py'
])
]),
launch_arguments={
'container_name': 'hardware_container',
'use_multi_threaded_executor': 'true'
}.items()
),
])
# 激光雷达驱动组件 (动态加载到 hardware_container 中)
lidar_component = LoadComposableNodes(
target_container='hardware_container',
composable_node_descriptions=[
ComposableNode(
package='rslidar_sdk',
plugin='robosense::lidar::LidarDriver',
name='lidar_driver',
parameters=[lidar_config],
extra_arguments=[{'use_intra_process_comms': True}],
)
]
)
# 电机驱动组件
motor_component = LoadComposableNodes(
target_container='hardware_container',
composable_node_descriptions=[
ComposableNode(
package='amr_bot_hardware',
plugin='amr_bot::MotorDriver',
name='motor_driver',
parameters=[motor_config],
extra_arguments=[{'use_intra_process_comms': True}],
)
]
)
# IMU 驱动组件
imu_component = LoadComposableNodes(
target_container='hardware_container',
composable_node_descriptions=[
ComposableNode(
package='amr_bot_hardware',
plugin='amr_bot::IMUDriver',
name='imu_driver',
parameters=[os.path.join(pkg_robot_bringup, 'config', 'imu_params.yaml')],
extra_arguments=[{'use_intra_process_comms': True}],
)
]
)
# ---- 导航框架 ----
nav2_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('nav2_bringup'),
'launch',
'navigation_launch.py'
])
]),
launch_arguments={
'use_sim_time': use_sim_time,
'params_file': nav2_config,
'autostart': 'true',
}.items()
)
# ---- 系统监控节点 ----
battery_monitor = Node(
package='amr_bot_monitoring',
executable='battery_monitor',
name='battery_monitor',
output='screen',
parameters=[{
'low_threshold': 20.0,
'critical_threshold': 10.0,
}]
)
safety_monitor = Node(
package='amr_bot_monitoring',
executable='safety_monitor',
name='safety_monitor',
output='screen',
parameters=[{
'stop_distance': 0.5, # 急停距离 (米)
'warning_distance': 1.0,
}]
)
diagnostic_agg = Node(
package='diagnostic_aggregator',
executable='aggregator_node',
name='diagnostic_aggregator',
output='screen',
parameters=[os.path.join(pkg_robot_bringup, 'config', 'diagnostics.yaml')]
)
# ---- RViz 可视化 (仅在非headless模式下运行) ----
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', rviz_config],
condition=lambda context: LaunchConfiguration('rviz').perform(context) == 'true'
)
# ============================================================
# 3. 组装 LaunchDescription
# 注意：节点的列出顺序即为启动顺序，
# 若需要严格依赖，需使用 event handler (这里暂不展开)
# ============================================================
ld = LaunchDescription([
DeclareLaunchArgument('rviz', default_value='false',
description='Start RViz2?'),
DeclareLaunchArgument('use_sim_time', default_value='false',
description='Use simulation (Gazebo) time?'),
robot_state_pub,
hardware_container,
lidar_component,
motor_component,
imu_component,
nav2_launch,
battery_monitor,
safety_monitor,
diagnostic_agg,
rviz_node,
])
return ld
1.2.1 Launch 文件精髓分析
-
分层启动：先加载机器人模型 (
robot_state_publisher)，再启动硬件驱动，最后才加载导航和监控。这保证了后续节点订阅话题时，/tf_static和/robot_description已经可用。 -
组件容器 (Composable Nodes)：将激光雷达、电机、IMU 驱动加载到同一个
hardware_container容器中。这些驱动之间存在大量点云、里程计数据的交换，使用intra_process_comms可以实现零拷贝传输，显著降低延迟和 CPU 负载。 -
参数外部化：所有可调参数均存储在 YAML 文件中 (
lidar_params.yaml,nav2_params.yaml)，通过parameters参数传递给节点。这使得现场调试时可以快速修改配置而不需重新编译。 -
条件启动：RViz 节点通过
condition判断是否启动，避免在无头服务器上启动图形界面而报错。
1.3 核心驱动组件实现（以电机驱动为例）
电机驱动组件负责将 /cmd_vel 话题上的速度指令转换为 CAN 总线报文，下发到底层伺服驱动器；同时读取编码器反馈，计算并发布里程计 /odom 和关节状态 /joint_states。
1.3.1 文件结构树
amr_bot_hardware/ ├── CMakeLists.txt ├── package.xml ├── include/ │ └── amr_bot_hardware/ │ ├── motor_driver.hpp # 电机驱动组件头文件 │ ├── can_interface.hpp # CAN 总线抽象接口 │ └── odometry.hpp # 里程计算工具类 ├── src/ │ ├── motor_driver.cpp # 电机驱动实现 │ ├── can_interface.cpp # CAN 接口实现 (SocketCAN) │ └── odometry.cpp # 里程计实现 ├── config/ │ └── motor_params.yaml ├── launch/ │ └── motor_driver_launch.py └── test/ └── test_motor_driver.cpp
1.3.2 主要函数树分析
MotorDriver::MotorDriver(options) ├── declare_parameters() // 声明参数 (轮距, 减速比, CAN ID 等) ├── init_can_interface() // 初始化 SocketCAN 接口 ├── create_subscription<CmdVel>() // 订阅 /cmd_vel ├── create_publisher<Odometry>() // 发布 /odom ├── create_publisher<JointState>() // 发布 /joint_states ├── create_wall_timer(100ms) // 创建 10Hz 定时器读取编码器 └── activate_lifecycle() // 激活生命周期状态机 MotorDriver::on_cmd_vel(msg) ├── validate_input(msg) // 速度限制检查 ├── calculate_wheel_speeds(msg) // 根据差速模型计算轮速 ├── can_send_motion_command(left, right)// 通过 CAN 发送运动指令 └── RCLCPP_DEBUG() 记录日志 MotorDriver::on_timer() // 定时器回调 (10Hz) ├── can_read_encoder_data() // 从 CAN 读取编码器反馈 ├── update_odometry() // 更新里程计 ├── publish_odometry() // 发布 /odom 话题 ├── publish_joint_states() // 发布 /joint_states 话题 └── publish_tf() // 发布 odom -> base_footprint 变换
1.3.3 核心代码实现
/**
* @file motor_driver.cpp
* @brief AMR 差速电机驱动组件
*
* 该组件负责：
* - 订阅 /cmd_vel 话题，解析速度指令
* - 通过 SocketCAN 与伺服驱动器通信
* - 读取编码器反馈，计算里程计
* - 发布 /odom, /joint_states, /tf 话题
*
* @note CAN 总线需提前配置 (sudo ip link set can0 up type can bitrate 1000000)
*/
#include "amr_bot_hardware/motor_driver.hpp"
#include "amr_bot_hardware/can_interface.hpp"
#include "amr_bot_hardware/odometry.hpp"
#include <rclcpp/logging.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <chrono>
#include <cmath>
namespace amr_bot_hardware
{
// 电机控制命令 CAN ID
static constexpr uint32_t CAN_ID_MOTOR_CMD = 0x200;
// 编码器反馈 CAN ID
static constexpr uint32_t CAN_ID_ENCODER_FB = 0x300;
MotorDriver::MotorDriver(const rclcpp::NodeOptions & options)
: rclcpp::Node("motor_driver", options),
can_iface_(std::make_unique<CanInterface>("can0")),
odom_calculator_(std::make_unique<Odometry>())
{
// ---------- 参数声明 ----------
this->declare_parameter("wheel_base", 0.45); // 轮距 (米)
this->declare_parameter("wheel_radius", 0.08); // 轮半径 (米)
this->declare_parameter("encoder_resolution", 4096);// 编码器分辨率 (脉冲/转)
this->declare_parameter("reduction_ratio", 20.0); // 减速比
this->declare_parameter("max_linear_speed", 1.5); // 最大线速度 (m/s)
this->declare_parameter("max_angular_speed", 2.5); // 最大角速度 (rad/s)
// ---------- 通信接口初始化 ----------
publisher_odom_ = this->create_publisher<nav_msgs::msg::Odometry>(
"odom", rclcpp::SensorDataQoS());
publisher_joint_ = this->create_publisher<sensor_msgs::msg::JointState>(
"joint_states", rclcpp::SensorDataQoS());
subscription_cmd_ = this->create_subscription<geometry_msgs::msg::Twist>(
"cmd_vel", rclcpp::SensorDataQoS(),
std::bind(&MotorDriver::on_cmd_vel, this, std::placeholders::_1));
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
// 定时器：10Hz 读取编码器
timer_ = this->create_wall_timer(
std::chrono::milliseconds(100),
std::bind(&MotorDriver::on_timer, this));
// 初始化里程计
odom_calculator_->init(
this->get_parameter("wheel_base").as_double(),
this->get_parameter("wheel_radius").as_double());
RCLCPP_INFO(this->get_logger(), "Motor driver initialized successfully");
}
MotorDriver::~MotorDriver()
{
// 停止电机 (发送零速指令)
send_motion_command(0.0, 0.0);
RCLCPP_INFO(this->get_logger(), "Motor driver shutdown");
}
// ============================================================
// 速度指令回调
// ============================================================
void MotorDriver::on_cmd_vel(const geometry_msgs::msg::Twist::SharedPtr msg)
{
// 速度限制检查
double linear_x = std::clamp(msg->linear.x,
-this->get_parameter("max_linear_speed").as_double(),
this->get_parameter("max_linear_speed").as_double());
double angular_z = std::clamp(msg->angular.z,
-this->get_parameter("max_angular_speed").as_double(),
this->get_parameter("max_angular_speed").as_double());
// 差速模型：线速度 v 和角速度 ω 转化为左右轮转速
double wheel_base = this->get_parameter("wheel_base").as_double();
double wheel_radius = this->get_parameter("wheel_radius").as_double();
double left_speed = (linear_x - angular_z * wheel_base / 2.0) / wheel_radius; // rad/s
double right_speed = (linear_x + angular_z * wheel_base / 2.0) / wheel_radius;
// 发送 CAN 指令
send_motion_command(left_speed, right_speed);
RCLCPP_DEBUG(this->get_logger(),
"CmdVel received: v=%.2f, ω=%.2f → left=%.2f rad/s, right=%.2f rad/s",
linear_x, angular_z, left_speed, right_speed);
}
// ============================================================
// 定时器回调：读取编码器并更新里程计
// ============================================================
void MotorDriver::on_timer()
{
auto odom_msg = std::make_unique<nav_msgs::msg::Odometry>();
auto joint_msg = std::make_unique<sensor_msgs::msg::JointState>();
// 从 CAN 读取左右轮编码器值
int32_t left_enc, right_enc;
if (!can_iface_->read_encoder(CAN_ID_ENCODER_FB, left_enc, right_enc)) {
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000,
"Failed to read encoder data from CAN");
return;
}
// 将编码器脉冲转为轮转动角度 (弧度)
double encoder_res = this->get_parameter("encoder_resolution").as_double();
double reduction = this->get_parameter("reduction_ratio").as_double();
double pulses_per_rev = encoder_res * reduction; // 电机每转一圈所需的脉冲数
double left_angle = 2.0 * M_PI * static_cast<double>(left_enc) / pulses_per_rev;
double right_angle = 2.0 * M_PI * static_cast<double>(right_enc) / pulses_per_rev;
// 更新里程计 (内部累积位姿)
odom_calculator_->update(left_angle, right_angle, this->now());
// 填充并发布里程计消息
odom_msg->header.stamp = this->now();
odom_msg->header.frame_id = "odom";
odom_msg->child_frame_id = "base_footprint";
odom_msg->pose.pose = odom_calculator_->get_pose();
odom_msg->twist.twist = odom_calculator_->get_twist();
publisher_odom_->publish(std::move(odom_msg));
// 填充并发布关节状态
joint_msg->header.stamp = this->now();
joint_msg->name = {"left_wheel_joint", "right_wheel_joint"};
joint_msg->position = {left_angle, right_angle};
joint_msg->velocity = {odom_calculator_->get_left_speed(),
odom_calculator_->get_right_speed()};
publisher_joint_->publish(std::move(joint_msg));
// 广播 odom -> base_footprint 变换 (注意：AMCL 会提供 map->odom)
geometry_msgs::msg::TransformStamped tf;
tf.header.stamp = this->now();
tf.header.frame_id = "odom";
tf.child_frame_id = "base_footprint";
tf.transform.translation.x = odom_calculator_->get_pose().position.x;
tf.transform.translation.y = odom_calculator_->get_pose().position.y;
tf.transform.translation.z = 0.0;
tf.transform.rotation = odom_calculator_->get_pose().orientation;
tf_broadcaster_->sendTransform(tf);
RCLCPP_DEBUG(this->get_logger(),
"Odometry updated: x=%.3f, y=%.3f, θ=%.3f",
odom_calculator_->get_pose().position.x,
odom_calculator_->get_pose().position.y,
tf2::getYaw(odom_calculator_->get_pose().orientation));
}
// ============================================================
// CAN 通信函数
// ============================================================
void MotorDriver::send_motion_command(double left_speed_rads, double right_speed_rads)
{
// 将转速转换为驱动器单位 (假设为 rpm)
double left_rpm = left_speed_rads * 60.0 / (2.0 * M_PI);
double right_rpm = right_speed_rads * 60.0 / (2.0 * M_PI);
// 限制为 int16_t 范围 (-32768 ~ 32767)，若驱动器实际范围不同则需调整
int16_t left_cmd = static_cast<int16_t>(std::clamp(left_rpm, -32768.0, 32767.0));
int16_t right_cmd = static_cast<int16_t>(std::clamp(right_rpm, -32768.0, 32767.0));
// 构造 CAN 帧 (标准帧格式，DLC=4)
struct can_frame frame;
frame.can_id = CAN_ID_MOTOR_CMD;
frame.can_dlc = 4;
frame.data[0] = static_cast<uint8_t>(left_cmd & 0xFF);
frame.data[1] = static_cast<uint8_t>((left_cmd >> 8) & 0xFF);
frame.data[2] = static_cast<uint8_t>(right_cmd & 0xFF);
frame.data[3] = static_cast<uint8_t>((right_cmd >> 8) & 0xFF);
can_iface_->send_frame(frame);
}
} // namespace amr_bot_hardware
// 组件注册宏
#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(amr_bot_hardware::MotorDriver)
1.3.4 里程计算工具类
/**
* @file odometry.cpp
* @brief 差速机器人里程计算实现
*
* 基于编码器反馈的左右轮位移，使用一阶近似 (Runge-Kutta) 积分位姿。
* 假设车轮无滑动，侧向速度为零。
*/
#include "amr_bot_hardware/odometry.hpp"
#include <tf2/utils.h>
#include <cmath>
namespace amr_bot_hardware
{
void Odometry::init(double wheel_base, double wheel_radius)
{
wheel_base_ = wheel_base;
wheel_radius_ = wheel_radius;
x_ = y_ = theta_ = 0.0;
left_speed_ = right_speed_ = 0.0;
last_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
}
void Odometry::update(double left_angle, double right_angle,
const rclcpp::Time & current_time)
{
// 计算时间间隔
if (last_time_.nanoseconds() == 0) {
last_time_ = current_time;
prev_left_angle_ = left_angle;
prev_right_angle_ = right_angle;
return;
}
double dt = (current_time - last_time_).seconds();
if (dt <= 0.0) return;
// 角度增量 → 线位移 (弧长)
double d_left = (left_angle - prev_left_angle_) * wheel_radius_;
double d_right = (right_angle - prev_right_angle_) * wheel_radius_;
// 机器人中心位移增量
double d_center = (d_left + d_right) / 2.0;
double d_theta = (d_right - d_left) / wheel_base_;
// 更新位姿
theta_ += d_theta;
x_ += d_center * std::cos(theta_);
y_ += d_center * std::sin(theta_);
// 更新速度
left_speed_ = d_left / dt;
right_speed_ = d_right / dt;
// 存储当前值供下一周期使用
prev_left_angle_ = left_angle;
prev_right_angle_ = right_angle;
last_time_ = current_time;
}
geometry_msgs::msg::Pose Odometry::get_pose() const
{
geometry_msgs::msg::Pose pose;
pose.position.x = x_;
pose.position.y = y_;
pose.position.z = 0.0;
tf2::Quaternion q;
q.setRPY(0, 0, theta_);
pose.orientation = tf2::toMsg(q);
return pose;
}
geometry_msgs::msg::Twist Odometry::get_twist() const
{
geometry_msgs::msg::Twist twist;
twist.linear.x = (left_speed_ + right_speed_) / 2.0;
twist.angular.z = (right_speed_ - left_speed_) / wheel_base_;
return twist;
}
} // namespace amr_bot_hardware
1.3.5 SocketCAN 接口封装
/**
* @file can_interface.cpp
* @brief Linux SocketCAN 封装
*
* 提供帧发送/接收功能，支持阻塞和非阻塞模式。
* 在构建时自动绑定 CAN 接口 (如 can0)，若绑定失败则抛出异常。
*/
#include "amr_bot_hardware/can_interface.hpp"
#include <linux/can.h>
#include <linux/can/raw.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <cstring>
#include <unistd.h>
#include <stdexcept>
#include <sstream>
namespace amr_bot_hardware
{
CanInterface::CanInterface(const std::string & ifname)
{
// 创建 socket
sockfd_ = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (sockfd_ < 0) {
throw std::runtime_error("SocketCAN socket creation failed");
}
// 绑定到指定接口
struct ifreq ifr;
std::strncpy(ifr.ifr_name, ifname.c_str(), IFNAMSIZ - 1);
if (ioctl(sockfd_, SIOCGIFINDEX, &ifr) < 0) {
close(sockfd_);
throw std::runtime_error("Failed to get CAN interface index");
}
struct sockaddr_can addr;
std::memset(&addr, 0, sizeof(addr));
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
if (bind(sockfd_, reinterpret_cast<struct sockaddr *>(&addr), sizeof(addr)) < 0) {
close(sockfd_);
std::stringstream ss;
ss << "Failed to bind to CAN interface " << ifname;
throw std::runtime_error(ss.str());
}
}
CanInterface::~CanInterface()
{
if (sockfd_ >= 0)
close(sockfd_);
}
void CanInterface::send_frame(const struct can_frame & frame)
{
ssize_t nbytes = write(sockfd_, &frame, sizeof(struct can_frame));
if (nbytes != sizeof(struct can_frame)) {
// 实际工程中可加入重试或错误计数
perror("CAN write error");
}
}
bool CanInterface::read_encoder(uint32_t can_id, int32_t & left_enc, int32_t & right_enc)
{
struct can_frame frame;
ssize_t nbytes = read(sockfd_, &frame, sizeof(struct can_frame));
if (nbytes < 0 || nbytes != sizeof(struct can_frame)) {
return false;
}
// 仅处理编码器反馈帧
if (frame.can_id != can_id || frame.can_dlc < 4)
return false;
// 解析数据 (假设驱动器按大端序发送)
left_enc = (static_cast<int32_t>(frame.data[0]) << 8) | frame.data[1];
right_enc = (static_cast<int32_t>(frame.data[2]) << 8) | frame.data[3];
// 符号扩展 (若是负值)
if (left_enc & 0x8000) left_enc |= 0xFFFF0000;
if (right_enc & 0x8000) right_enc |= 0xFFFF0000;
return true;
}
} // namespace amr_bot_hardware
1.4 参数配置 YAML 文件
将参数与代码分离是生产级系统的必要实践。以下是电机驱动的参数文件：
# motor_params.yaml motor_driver: ros__parameters: wheel_base: 0.45 wheel_radius: 0.08 encoder_resolution: 4096 reduction_ratio: 20.0 max_linear_speed: 1.5 max_angular_speed: 2.5
在 launch 文件中通过 parameters=[motor_config] 加载。
1.5 逻辑思维走向分析
系统启动后，MotorDriver 节点的行为遵循以下决策树：
-
节点启动 → 声明参数 → 初始化 CAN 接口 → 成功？
-
是 → 创建发布器/订阅器/定时器 → 进入就绪状态
-
否 → 抛出异常，容器捕获并退出 (或进入 Lifecycle Inactive 状态)
-
-
收到 /cmd_vel 消息 → 速度范围检查 → 超出限制？
-
是 → 钳位到最大值并记录警告日志
-
否 → 继续 → 计算左右轮速 → 转换为 RPM → 构造 CAN 帧 → 发送
-
-
10Hz 定时器触发 → 读取 CAN 编码器数据 → 读取成功？
-
否 → 输出节流日志 (2秒内最多一次) → 跳过本周期
-
是 → 计算位移增量 → 更新里程计 → 发布 /odom, /joint_states, /tf
-
这种清晰的逻辑分支确保了即使在通信异常时，系统也不会崩溃，而是优雅降级。
1.6 单位数据规划与缓存区规划
1.6.1 单位数据规划
在机器人软件中，统一使用 SI 单位制 可以避免转换错误。本项目遵循以下约定：
| 物理量 | 单位 | 备注 |
|---|---|---|
| 长度 | 米 (m) | 里程计坐标、激光测距 |
| 角度 | 弧度 (rad) | 关节角、航向角 |
| 时间 | 秒 (s) | 所有时间戳 (rclcpp::Time) |
| 角速度 | 弧度/秒 (rad/s) | /cmd_vel 中的 angular.z |
| 线速度 | 米/秒 (m/s) | /cmd_vel 中的 linear.x |
| 编码器脉冲 | 脉冲 | 原始值，在内部转换为弧度 |
| CAN 总线速度 | RPM | 仅用于与驱动器通信时转换 |
1.6.2 缓存区规划
-
CAN 接收缓冲区：Linux SocketCAN 内核驱动自带接收队列 (默认 10 帧)。
read_encoder()采用非阻塞读取，每次只取最新一帧，避免累积延迟。 -
里程计内部状态：
Odometry类只保存最近一次的角度和时间戳，内存占用 O(1)，不存在历史数据堆积。 -
消息队列：
rclcpp::SensorDataQoS()默认KEEP_LAST=10，保证订阅者不会因为瞬时处理能力不足而阻塞发布者。
1.7 可能遇到的问题与解决方案
-
CAN 总线数据丢失
-
现象：编码器反馈偶尔为零，里程计跳跃。
-
解决：提高 CAN 位速率 (如 1 Mbps)，减少总线负载；在
read_encoder()中加入 CRC 校验。
-
-
里程计累计漂移
-
现象：机器人长期运行后定位偏移严重。
-
解决：里程计仅用于局部估计，全局定位依赖 AMCL (粒子滤波) 融合激光雷达进行修正。里程计发布频率 (10Hz) 应低于 AMCL 更新频率。
-
-
速度指令饱和
-
现象：
/cmd_vel请求速度远超硬件能力。 -
解决：在
on_cmd_vel()中执行std::clamp，同时可通过/diagnostics上报饱和事件。
-
-
多容器通信延迟
-
现象：同一容器内的组件间通信仍需经过 DDS。
-
解决：使用
use_intra_process_comms:=true启用进程内零拷贝通信，ROS 2 会自动在发布器和订阅器处于同一进程时走高效路径。
-
AtomGit 是由开放原子开源基金会联合 CSDN 等生态伙伴共同推出的新一代开源与人工智能协作平台。平台坚持“开放、中立、公益”的理念，把代码托管、模型共享、数据集托管、智能体开发体验和算力服务整合在一起，为开发者提供从开发、训练到部署的一站式体验。
更多推荐
6
0- 0
已为社区贡献84条内容
所有评论(0)