模块与部署架构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.pyLaunch 文件是 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, defaultfalse) ​ # 获取各功能包的配置路径 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( packagerobot_state_publisher, executablerobot_state_publisher, namerobot_state_publisher, outputscreen, parameters[ {robot_description: robot_description}, {use_sim_time: use_sim_time} ], arguments[--ros-args, --log-level, warn], ) ​ # ---- 硬件驱动容器 (使用组件降低进程间通信开销) ---- # 注意这里使用 composition 容器来加载多个驱动组件 # 它们共享同一进程地址空间可实现零拷贝大数据传输。 hardware_container GroupAction([ LogInfo(msgStarting 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_containerhardware_container, composable_node_descriptions[ ComposableNode( packagerslidar_sdk, pluginrobosense::lidar::LidarDriver, namelidar_driver, parameters[lidar_config], extra_arguments[{use_intra_process_comms: True}], ) ] ) ​ # 电机驱动组件 motor_component LoadComposableNodes( target_containerhardware_container, composable_node_descriptions[ ComposableNode( packageamr_bot_hardware, pluginamr_bot::MotorDriver, namemotor_driver, parameters[motor_config], extra_arguments[{use_intra_process_comms: True}], ) ] ) ​ # IMU 驱动组件 imu_component LoadComposableNodes( target_containerhardware_container, composable_node_descriptions[ ComposableNode( packageamr_bot_hardware, pluginamr_bot::IMUDriver, nameimu_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( packageamr_bot_monitoring, executablebattery_monitor, namebattery_monitor, outputscreen, parameters[{ low_threshold: 20.0, critical_threshold: 10.0, }] ) ​ safety_monitor Node( packageamr_bot_monitoring, executablesafety_monitor, namesafety_monitor, outputscreen, parameters[{ stop_distance: 0.5, # 急停距离 (米) warning_distance: 1.0, }] ) ​ diagnostic_agg Node( packagediagnostic_aggregator, executableaggregator_node, namediagnostic_aggregator, outputscreen, parameters[os.path.join(pkg_robot_bringup, config, diagnostics.yaml)] ) ​ # ---- RViz 可视化 (仅在非headless模式下运行) ---- rviz_node Node( packagerviz2, executablerviz2, namerviz2, outputscreen, arguments[-d, rviz_config], conditionlambda context: LaunchConfiguration(rviz).perform(context) true ) ​ # # 3. 组装 LaunchDescription # 注意节点的列出顺序即为启动顺序 # 若需要严格依赖需使用 event handler (这里暂不展开) # ld LaunchDescription([ DeclareLaunchArgument(rviz, default_valuefalse, descriptionStart RViz2?), DeclareLaunchArgument(use_sim_time, default_valuefalse, descriptionUse 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 ld1.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.cpp1.3.2 主要函数树分析MotorDriver::MotorDriver(options) ├── declare_parameters() // 声明参数 (轮距, 减速比, CAN ID 等) ├── init_can_interface() // 初始化 SocketCAN 接口 ├── create_subscriptionCmdVel() // 订阅 /cmd_vel ├── create_publisherOdometry() // 发布 /odom ├── create_publisherJointState() // 发布 /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_uniqueCanInterface(can0)), odom_calculator_(std::make_uniqueOdometry()) { // ---------- 参数声明 ---------- 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_publishernav_msgs::msg::Odometry( odom, rclcpp::SensorDataQoS()); publisher_joint_ this-create_publishersensor_msgs::msg::JointState( joint_states, rclcpp::SensorDataQoS()); subscription_cmd_ this-create_subscriptiongeometry_msgs::msg::Twist( cmd_vel, rclcpp::SensorDataQoS(), std::bind(MotorDriver::on_cmd_vel, this, std::placeholders::_1)); tf_broadcaster_ std::make_uniquetf2_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_uniquenav_msgs::msg::Odometry(); auto joint_msg std::make_uniquesensor_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_castdouble(left_enc) / pulses_per_rev; double right_angle 2.0 * M_PI * static_castdouble(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_castint16_t(std::clamp(left_rpm, -32768.0, 32767.0)); int16_t right_cmd static_castint16_t(std::clamp(right_rpm, -32768.0, 32767.0)); ​ // 构造 CAN 帧 (标准帧格式DLC4) struct can_frame frame; frame.can_id CAN_ID_MOTOR_CMD; frame.can_dlc 4; frame.data[0] static_castuint8_t(left_cmd 0xFF); frame.data[1] static_castuint8_t((left_cmd 8) 0xFF); frame.data[2] static_castuint8_t(right_cmd 0xFF); frame.data[3] static_castuint8_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_hardware1.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_caststruct 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_castint32_t(frame.data[0]) 8) | frame.data[1]; right_enc (static_castint32_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_hardware1.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_LAST10保证订阅者不会因为瞬时处理能力不足而阻塞发布者。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 会自动在发布器和订阅器处于同一进程时走高效路径。