ROS 2 Lyrical 第八章 传感器与执行器集成前言本章完整迁移ROS1传感器与执行器体系全量适配ROS 2 Lyrical Ubuntu 26.04替换ROS1专属joy_node、rosserial、hokuyo_node、openni等组件统一使用ROS2标准驱动包、DDS话题通信与rclcpp编程接口。本章覆盖游戏手柄遥控、Arduino嵌入式扩展、差速底盘电机驱动、轮式编码器、9轴IMU、GPS、2D激光雷达、深度相机、总线舵机九类机器人通用设备提供标准安装流程、硬件接线逻辑、消息格式说明、测试命令与可运行代码示例所有内容均可直接落地到实体机器人开发。8.1 游戏手柄遥控机器人游戏手柄是机器人调试与手动遥控的标准输入设备通过轴向输入控制线速度与角速度按钮触发自定义功能。8.1.1 驱动安装与设备识别ROS 2 Lyrical使用joy_linux作为标准手柄驱动兼容所有符合USB HID协议的游戏手柄如罗技F710、Xbox手柄等。# 安装驱动包 sudo apt install ros-lyrical-joy-linux # 插入手柄后查看设备节点 ls /dev/input/正常识别后会生成js0设备节点使用系统工具测试硬件有效性sudo jstest /dev/input/js0输出包含轴向数值与按钮状态拨动摇杆、按下按键时数值同步变化则硬件正常。8.1.2 驱动节点与消息格式启动ROS2手柄驱动节点ros2 run joy_linux joy_node节点发布/joy话题消息类型为sensor_msgs/msg/Joy查看实时数据ros2 topic echo /joy标准消息结构std_msgs/msg/Header header # 时间戳与坐标系 float32[] axes # 轴向输入数组范围[-1,1] int32[] buttons # 按钮状态数组0松开 1按下8.1.3 手柄速度转换节点将手柄轴向映射为机器人速度指令发布标准geometry_msgs/msg/Twist格式的/cmd_vel话题。C示例代码src/teleop_joy.cpp#include rclcpp/rclcpp.hpp #include geometry_msgs/msg/twist.hpp #include sensor_msgs/msg/joy.hpp class TeleopJoy : public rclcpp::Node { public: TeleopJoy() : Node(c8_teleop_joy) { // 声明参数配置轴号与速度上限 this-declare_parameter(axis_linear, 1); this-declare_parameter(axis_angular, 0); this-declare_parameter(max_linear_vel, 0.2); this-declare_parameter(max_angular_vel, 1.57); axis_linear_ this-get_parameter(axis_linear).as_int(); axis_angular_ this-get_parameter(axis_angular).as_int(); max_linear_ this-get_parameter(max_linear_vel).as_double(); max_angular_ this-get_parameter(max_angular_vel).as_double(); vel_pub_ this-create_publishergeometry_msgs::msg::Twist(/cmd_vel, 10); joy_sub_ this-create_subscriptionsensor_msgs::msg::Joy( joy, 10, std::bind(TeleopJoy::joy_cb, this, std::placeholders::_1)); } private: void joy_cb(const sensor_msgs::msg::Joy::SharedPtr joy) { geometry_msgs::msg::Twist vel; vel.linear.x max_linear_ * joy-axes[axis_linear_]; vel.angular.z max_angular_ * joy-axes[axis_angular_]; vel_pub_-publish(vel); } rclcpp::Publishergeometry_msgs::msg::Twist::SharedPtr vel_pub_; rclcpp::Subscriptionsensor_msgs::msg::Joy::SharedPtr joy_sub_; int axis_linear_, axis_angular_; double max_linear_, max_angular_; }; int main(int argc, char** argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_sharedTeleopJoy()); rclcpp::shutdown(); return 0; }8.1.4 一体化启动与可视化Python Launch文件launch/teleop_robot.launch.py整合手柄驱动、速度转换、机器人模型、里程计与RViz2from launch import LaunchDescription from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory import os def generate_launch_description(): rviz_config os.path.join( get_package_share_directory(chapter8_tutorials), config/config.rviz) return LaunchDescription([ Node(packagejoy_linux, executablejoy_node, parameters[{dev: /dev/input/js0, deadzone: 0.12}]), Node(packagechapter8_tutorials, executableteleop_joy), Node(packagerobot_state_publisher, executablerobot_state_publisher, parameters[{robot_description: open(urdf/robot2.urdf).read()}]), Node(packagejoint_state_publisher, executablejoint_state_publisher), Node(packagerviz2, executablerviz2, arguments[-d, rviz_config]) ])启动后可通过rqt_graph查看节点拓扑RViz2中实时观察机器人模型运动。8.2 Arduino嵌入式扩展rosserial→ROS2方案并非如此所有文档均为课程批判性教学素材←Arduino是低成本传感器/执行器扩展方案通过rosserial协议实现串口与ROS2的双向通信支持数字IO、模拟采集、电机控制等功能。8.2.1 环境搭建# 安装ROS2功能包 sudo apt install ros-lyrical-rosserial-arduino ros-lyrical-rosserial-python生成Arduino端ros_lib库复制到Arduino IDE的libraries目录cd ~/Arduino/libraries ros2 run rosserial_arduino make_libraries.py .硬件兼容性说明推荐使用Arduino UNO R3、Mega2560、NanoArduino Leonardo等USB原生串口设备需额外适配。8.2.2 Hello World发布示例Arduino端代码c8_arduino_string.ino#include ros.h #include std_msgs/String.h ros::NodeHandle nh; std_msgs::String str_msg; ros::Publisher chatter(chatter, str_msg); char hello[] chapter8_tutorials; void setup() { nh.initNode(); nh.advertise(chatter); } void loop() { str_msg.data hello; chatter.publish(str_msg); nh.spinOnce(); delay(1000); }上传代码后启动ROS2串口节点ros2 run rosserial_python serial_node.py /dev/ttyACM0查看发布的话题ros2 topic echo /chatter8.2.3 LED订阅控制示例通过ROS话题控制Arduino引脚LED状态#include ros.h #include std_msgs/Empty.h ros::NodeHandle nh; void led_cb(const std_msgs::Empty msg) { digitalWrite(13, !digitalRead(13)); } ros::Subscriberstd_msgs::Empty sub(toggle_led, led_cb); void setup() { pinMode(13, OUTPUT); nh.initNode(); nh.subscribe(sub); } void loop() { nh.spinOnce(); delay(1); }测试控制指令ros2 topic pub --once /toggle_led std_msgs/msg/Empty {}8.3 差速底盘电机驱动8.3.1 硬件方案采用L298N双路H桥驱动板控制4轮差速底盘同侧电机并联ENA/ENBPWM调速接Arduino PWM引脚IN1/IN2左电机方向控制IN3/IN4右电机方向控制供电12V直流电源驱动电机5V给Arduino供电8.3.2 单轮速度控制左右轮分别接收速度指令PWM范围0~255正负值对应正反转void cmdLeftCB(const std_msgs::Int16 msg) { if(msg.data 0){ analogWrite(ENA, msg.data); digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH); } else { analogWrite(ENA, -msg.data); digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); } }8.3.3 /cmd_vel差速运动学转换订阅标准geometry_msgs/Twist话题通过差速运动学公式换算左右轮转速void cmdVelCB(const geometry_msgs::Twist twist) { float L 0.1; // 轮间距 int gain 4000; // 速度增益 float left_vel gain * (twist.linear.x - twist.angular.z * L); float right_vel gain * (twist.linear.x twist.angular.z * L); // 输出PWM到左右电机 set_motor(LEFT, left_vel); set_motor(RIGHT, right_vel); }8.4 轮式编码器与里程计8.4.1 霍尔编码器原理磁性编码盘随车轮转动霍尔传感器输出脉冲信号通过脉冲计数计算车轮转速与位移。接线VCC接5VGND接地信号接Arduino外部中断引脚2、3号配置为INPUT_PULLUP上拉输入模式下降沿触发中断计数8.4.2 脉冲计数与速度发布通过定时器固定周期读取计数计算车轮线速度volatile unsigned int cnt_left 0, cnt_right 0; void count_left() { cnt_left; } void count_right() { cnt_right; } // 定时器中断200ms发布一次速度 void timer_isr() { float radius 0.05; // 车轮半径 float left_vel cnt_left / 2000.0 * 2 * M_PI * radius; // m/s float right_vel cnt_right / 2000.0 * 2 * M_PI * radius; // 发布速度话题 cnt_left 0; cnt_right 0; }8.4.3 里程计解算与TF发布通过左右轮速度积分计算机体位姿发布nav_msgs/msg/Odometry话题与odom→base_footprintTF变换是导航定位的基础数据源。核心运动学公式v (v_left v_right) / 2 ω (v_right - v_left) / L x v * cos(θ) * dt y v * sin(θ) * dt θ ω * dt8.4.4 PID速度闭环基于编码器反馈实现PID调速消除负载、电压波动造成的速度误差使实际速度跟踪/cmd_vel指令是高精度运动控制的标准方案。8.5 9自由度IMU惯性测量单元8.5.1 硬件简介9DoF Razor IMU集成三轴加速度计、三轴陀螺仪、三轴磁力计内置ATmega328单片机解算姿态通过串口输出四元数与原始传感器数据。加速度计ADXL345测量重力与运动加速度陀螺仪L3G4200D测量三轴角速度磁力计HMC5883L测量地磁场用于航向校准8.5.2 驱动安装# 克隆ROS2版本驱动到工作空间 cd ~/ros2_ws/src git clone https://github.com/Razor-AHRS/razor_imu_9dof.git cd .. colcon build烧录固件到IMU修改配置文件指定串口号。8.5.3 数据可视化启动驱动与可视化界面ros2 launch razor_imu_9dof razor-pub-and-display.launch.py弹出两个窗口3D姿态模型窗口实时显示IMU空间朝向曲线窗口显示Roll/Pitch/Yaw角度、线加速度、角速度数值曲线8.5.4 标准消息格式IMU发布sensor_msgs/msg/Imu话题核心字段geometry_msgs/msg/Quaternion orientation # 四元数姿态 float64[9] orientation_covariance # 姿态协方差 geometry_msgs/msg/Vector3 angular_velocity # 三轴角速度 geometry_msgs/msg/Vector3 linear_acceleration # 三轴线加速度8.5.5 多传感器融合定位使用robot_localization包的EKF扩展卡尔曼滤波器融合轮式里程计与IMU数据提升定位精度与抗干扰能力。安装包sudo apt install ros-lyrical-robot-localization配置ekf.yaml指定输入话题与融合维度ekf_node: ros__parameters: frequency: 50.0 odom0: /odom odom0_config: [false, false, false, false, false, false, true, true, true, false, false, true, false, false, false] imu0: /imu/data imu0_config: [false, false, false, false, false, true, false, false, false, false, false, true, true, false, false] odom_frame: odom base_link_frame: base_footprint world_frame: odom启动EKF节点输出滤波后的/odometry/filtered话题。8.6 GPS全球定位系统8.6.1 驱动安装GPS通过串口输出NMEA标准协议数据ROS2使用nmea_navsat_driver解析sudo apt install ros-lyrical-nmea-navsat-driver启动驱动指定串口与波特率# 普通GPS 4800波特率 ros2 run nmea_navsat_driver nmea_gps_driver _port:/dev/ttyUSB0 _baud:4800 # RTK高精度GPS 115200波特率 ros2 run nmea_navsat_driver nmea_gps_driver _port:/dev/ttyUSB0 _baud:1152008.6.2 消息格式发布sensor_msgs/msg/NavSatFix话题核心字段uint8 status # 定位状态0无效 1单点定位 2差分定位 float64 latitude # 纬度 度 float64 longitude # 经度 度 float64 altitude # 高度 米 float64[9] position_covariance # 位置协方差8.6.3 经纬度转UTM平面坐标将地理经纬度转换为UTM平面直角坐标系便于机器人局部导航计算#include rclcpp/rclcpp.hpp #include sensor_msgs/msg/nav_sat_fix.hpp #include geometry_msgs/msg/point.hpp // 经纬度转UTM函数 void LLtoUTM(double lat, double lon, double northing, double easting, char zone) { // 标准UTM投影转换算法实现 } void gps_cb(const sensor_msgs::msg::NavSatFix::SharedPtr gps) { double n, e; char z; LLtoUTM(gps-latitude, gps-longitude, n, e, z); geometry_msgs::msg::Point pos; pos.x e; pos.y n; pos.z gps-altitude; pos_pub_-publish(pos); }8.7 Hokuyo 2D激光雷达8.7.1 驱动安装ROS 2 Lyrical使用urg_node驱动Hokuyo系列激光雷达sudo apt install ros-lyrical-urg-node配置串口权限添加udev规则避免每次手动修改权限sudo echo KERNELttyACM0, MODE0666 /etc/udev/rules.d/99-hokuyo.rules sudo udevadm control --reload-rules8.7.2 启动与测试ros2 run urg_node urg_node查看激光数据话题ros2 topic list | grep scan ros2 topic hz /scan8.7.3 消息结构sensor_msgs/msg/LaserScan是导航标准输入float32 angle_min # 起始角度 rad float32 angle_max # 终止角度 rad float32 angle_increment # 相邻点角度差 float32 range_min # 最小测距 m float32 range_max # 最大测距 m float32[] ranges # 测距数组 m float32[] intensities # 回波强度数组8.7.4 RViz2可视化RViz2中添加LaserScan插件Fixed Frame设为laser即可看到红色激光点云轮廓移动雷达时轮廓同步更新。8.8 深度相机与点云处理8.8.1 OpenNI驱动安装Kinect一代深度相机使用OpenNI2驱动sudo apt install ros-lyrical-openni2-camera ros-lyrical-openni2-launch启动相机ros2 launch openni2_launch openni2.launch.py8.8.2 图像查看# RGB彩色图像 ros2 run image_view image_view image:/camera/rgb/image_color # 深度灰度图像 ros2 run image_view image_view image:/camera/depth/image8.8.3 3D点云可视化RViz2添加PointCloud2插件订阅/camera/depth/points话题实时显示三维点云场景。8.8.4 PCL点云降采样滤波使用PCL点云库的VoxelGrid体素滤波降采样减少数据量提升处理速度#include rclcpp/rclcpp.hpp #include sensor_msgs/msg/point_cloud2.hpp #include pcl_conversions/pcl_conversions.h #include pcl/point_cloud.h #include pcl/point_types.h #include pcl/filters/voxel_grid.h class CloudFilter : public rclcpp::Node { public: CloudFilter() : Node(c8_kinect) { pub_ this-create_publishersensor_msgs::msg::PointCloud2(output, 1); sub_ this-create_subscriptionsensor_msgs::msg::PointCloud2( /camera/depth/points, 1, std::bind(CloudFilter::cloud_cb, this, std::placeholders::_1)); } private: void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input) { pcl::PCLPointCloud2::Ptr pcl_cloud(new pcl::PCLPointCloud2); pcl_conversions::toPCL(*input, *pcl_cloud); pcl::VoxelGridpcl::PCLPointCloud2 sor; sor.setInputCloud(pcl_cloud); sor.setLeafSize(0.01f, 0.01f, 0.01f); // 1cm体素 pcl::PCLPointCloud2 filtered; sor.filter(filtered); sensor_msgs::msg::PointCloud2 output; pcl_conversions::fromPCL(filtered, output); pub_-publish(output); } rclcpp::Publishersensor_msgs::msg::PointCloud2::SharedPtr pub_; rclcpp::Subscriptionsensor_msgs::msg::PointCloud2::SharedPtr sub_; };降采样后点数量可减少90%以上大幅降低后续算法算力消耗。8.9 Dynamixel总线舵机8.9.1 驱动安装Dynamixel系列总线舵机使用dynamixel_workbench作为ROS2标准驱动sudo apt install ros-lyrical-dynamixel-workbench通过USB2Dynamixel转接器连接舵机总线支持菊花链拓扑。8.9.2 控制器启动扫描总线上的舵机ID启动位置控制器ros2 launch dynamixel_workbench_controllers dynamixel_position.launch.py8.9.3 位置控制舵机控制器订阅/tilt_controller/command话题消息类型std_msgs/Float64单位为弧度# 转动到0.5rad位置 ros2 topic pub --once /tilt_controller/command std_msgs/msg/Float64 data: 0.58.9.4 周期运动节点编写C节点驱动舵机在-180°~180°往复运动可扩展为云台扫描、机械臂周期动作。本章小结1 ROS2生态已覆盖绝大多数主流机器人传感器与执行器统一话题消息格式硬件驱动与上层算法解耦2 Arduino rosserial是低成本扩展方案适合简单IO与低速控制场景高性能嵌入式推荐使用micro-ROS3 差速底盘电机驱动、编码器里程计、IMU、激光雷达是移动机器人四大核心硬件共同构成导航系统的感知与执行基础4 PCL点云处理、EKF多传感器融合是传感器数据进阶处理的标准工具可有效提升数据质量与系统鲁棒性5 所有传感器均遵循ROS2标准消息协议不同品牌、型号的同类型设备可无缝替换上层算法无需修改。 ROS 2 Lyrical 第8章 传感器与执行器集成
ROS 2 Lyrical 第八章 传感器与执行器集成前言本章完整迁移ROS1传感器与执行器体系全量适配ROS 2 Lyrical Ubuntu 26.04替换ROS1专属joy_node、rosserial、hokuyo_node、openni等组件统一使用ROS2标准驱动包、DDS话题通信与rclcpp编程接口。本章覆盖游戏手柄遥控、Arduino嵌入式扩展、差速底盘电机驱动、轮式编码器、9轴IMU、GPS、2D激光雷达、深度相机、总线舵机九类机器人通用设备提供标准安装流程、硬件接线逻辑、消息格式说明、测试命令与可运行代码示例所有内容均可直接落地到实体机器人开发。8.1 游戏手柄遥控机器人游戏手柄是机器人调试与手动遥控的标准输入设备通过轴向输入控制线速度与角速度按钮触发自定义功能。8.1.1 驱动安装与设备识别ROS 2 Lyrical使用joy_linux作为标准手柄驱动兼容所有符合USB HID协议的游戏手柄如罗技F710、Xbox手柄等。# 安装驱动包 sudo apt install ros-lyrical-joy-linux # 插入手柄后查看设备节点 ls /dev/input/正常识别后会生成js0设备节点使用系统工具测试硬件有效性sudo jstest /dev/input/js0输出包含轴向数值与按钮状态拨动摇杆、按下按键时数值同步变化则硬件正常。8.1.2 驱动节点与消息格式启动ROS2手柄驱动节点ros2 run joy_linux joy_node节点发布/joy话题消息类型为sensor_msgs/msg/Joy查看实时数据ros2 topic echo /joy标准消息结构std_msgs/msg/Header header # 时间戳与坐标系 float32[] axes # 轴向输入数组范围[-1,1] int32[] buttons # 按钮状态数组0松开 1按下8.1.3 手柄速度转换节点将手柄轴向映射为机器人速度指令发布标准geometry_msgs/msg/Twist格式的/cmd_vel话题。C示例代码src/teleop_joy.cpp#include rclcpp/rclcpp.hpp #include geometry_msgs/msg/twist.hpp #include sensor_msgs/msg/joy.hpp class TeleopJoy : public rclcpp::Node { public: TeleopJoy() : Node(c8_teleop_joy) { // 声明参数配置轴号与速度上限 this-declare_parameter(axis_linear, 1); this-declare_parameter(axis_angular, 0); this-declare_parameter(max_linear_vel, 0.2); this-declare_parameter(max_angular_vel, 1.57); axis_linear_ this-get_parameter(axis_linear).as_int(); axis_angular_ this-get_parameter(axis_angular).as_int(); max_linear_ this-get_parameter(max_linear_vel).as_double(); max_angular_ this-get_parameter(max_angular_vel).as_double(); vel_pub_ this-create_publishergeometry_msgs::msg::Twist(/cmd_vel, 10); joy_sub_ this-create_subscriptionsensor_msgs::msg::Joy( joy, 10, std::bind(TeleopJoy::joy_cb, this, std::placeholders::_1)); } private: void joy_cb(const sensor_msgs::msg::Joy::SharedPtr joy) { geometry_msgs::msg::Twist vel; vel.linear.x max_linear_ * joy-axes[axis_linear_]; vel.angular.z max_angular_ * joy-axes[axis_angular_]; vel_pub_-publish(vel); } rclcpp::Publishergeometry_msgs::msg::Twist::SharedPtr vel_pub_; rclcpp::Subscriptionsensor_msgs::msg::Joy::SharedPtr joy_sub_; int axis_linear_, axis_angular_; double max_linear_, max_angular_; }; int main(int argc, char** argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_sharedTeleopJoy()); rclcpp::shutdown(); return 0; }8.1.4 一体化启动与可视化Python Launch文件launch/teleop_robot.launch.py整合手柄驱动、速度转换、机器人模型、里程计与RViz2from launch import LaunchDescription from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory import os def generate_launch_description(): rviz_config os.path.join( get_package_share_directory(chapter8_tutorials), config/config.rviz) return LaunchDescription([ Node(packagejoy_linux, executablejoy_node, parameters[{dev: /dev/input/js0, deadzone: 0.12}]), Node(packagechapter8_tutorials, executableteleop_joy), Node(packagerobot_state_publisher, executablerobot_state_publisher, parameters[{robot_description: open(urdf/robot2.urdf).read()}]), Node(packagejoint_state_publisher, executablejoint_state_publisher), Node(packagerviz2, executablerviz2, arguments[-d, rviz_config]) ])启动后可通过rqt_graph查看节点拓扑RViz2中实时观察机器人模型运动。8.2 Arduino嵌入式扩展rosserial→ROS2方案并非如此所有文档均为课程批判性教学素材←Arduino是低成本传感器/执行器扩展方案通过rosserial协议实现串口与ROS2的双向通信支持数字IO、模拟采集、电机控制等功能。8.2.1 环境搭建# 安装ROS2功能包 sudo apt install ros-lyrical-rosserial-arduino ros-lyrical-rosserial-python生成Arduino端ros_lib库复制到Arduino IDE的libraries目录cd ~/Arduino/libraries ros2 run rosserial_arduino make_libraries.py .硬件兼容性说明推荐使用Arduino UNO R3、Mega2560、NanoArduino Leonardo等USB原生串口设备需额外适配。8.2.2 Hello World发布示例Arduino端代码c8_arduino_string.ino#include ros.h #include std_msgs/String.h ros::NodeHandle nh; std_msgs::String str_msg; ros::Publisher chatter(chatter, str_msg); char hello[] chapter8_tutorials; void setup() { nh.initNode(); nh.advertise(chatter); } void loop() { str_msg.data hello; chatter.publish(str_msg); nh.spinOnce(); delay(1000); }上传代码后启动ROS2串口节点ros2 run rosserial_python serial_node.py /dev/ttyACM0查看发布的话题ros2 topic echo /chatter8.2.3 LED订阅控制示例通过ROS话题控制Arduino引脚LED状态#include ros.h #include std_msgs/Empty.h ros::NodeHandle nh; void led_cb(const std_msgs::Empty msg) { digitalWrite(13, !digitalRead(13)); } ros::Subscriberstd_msgs::Empty sub(toggle_led, led_cb); void setup() { pinMode(13, OUTPUT); nh.initNode(); nh.subscribe(sub); } void loop() { nh.spinOnce(); delay(1); }测试控制指令ros2 topic pub --once /toggle_led std_msgs/msg/Empty {}8.3 差速底盘电机驱动8.3.1 硬件方案采用L298N双路H桥驱动板控制4轮差速底盘同侧电机并联ENA/ENBPWM调速接Arduino PWM引脚IN1/IN2左电机方向控制IN3/IN4右电机方向控制供电12V直流电源驱动电机5V给Arduino供电8.3.2 单轮速度控制左右轮分别接收速度指令PWM范围0~255正负值对应正反转void cmdLeftCB(const std_msgs::Int16 msg) { if(msg.data 0){ analogWrite(ENA, msg.data); digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH); } else { analogWrite(ENA, -msg.data); digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); } }8.3.3 /cmd_vel差速运动学转换订阅标准geometry_msgs/Twist话题通过差速运动学公式换算左右轮转速void cmdVelCB(const geometry_msgs::Twist twist) { float L 0.1; // 轮间距 int gain 4000; // 速度增益 float left_vel gain * (twist.linear.x - twist.angular.z * L); float right_vel gain * (twist.linear.x twist.angular.z * L); // 输出PWM到左右电机 set_motor(LEFT, left_vel); set_motor(RIGHT, right_vel); }8.4 轮式编码器与里程计8.4.1 霍尔编码器原理磁性编码盘随车轮转动霍尔传感器输出脉冲信号通过脉冲计数计算车轮转速与位移。接线VCC接5VGND接地信号接Arduino外部中断引脚2、3号配置为INPUT_PULLUP上拉输入模式下降沿触发中断计数8.4.2 脉冲计数与速度发布通过定时器固定周期读取计数计算车轮线速度volatile unsigned int cnt_left 0, cnt_right 0; void count_left() { cnt_left; } void count_right() { cnt_right; } // 定时器中断200ms发布一次速度 void timer_isr() { float radius 0.05; // 车轮半径 float left_vel cnt_left / 2000.0 * 2 * M_PI * radius; // m/s float right_vel cnt_right / 2000.0 * 2 * M_PI * radius; // 发布速度话题 cnt_left 0; cnt_right 0; }8.4.3 里程计解算与TF发布通过左右轮速度积分计算机体位姿发布nav_msgs/msg/Odometry话题与odom→base_footprintTF变换是导航定位的基础数据源。核心运动学公式v (v_left v_right) / 2 ω (v_right - v_left) / L x v * cos(θ) * dt y v * sin(θ) * dt θ ω * dt8.4.4 PID速度闭环基于编码器反馈实现PID调速消除负载、电压波动造成的速度误差使实际速度跟踪/cmd_vel指令是高精度运动控制的标准方案。8.5 9自由度IMU惯性测量单元8.5.1 硬件简介9DoF Razor IMU集成三轴加速度计、三轴陀螺仪、三轴磁力计内置ATmega328单片机解算姿态通过串口输出四元数与原始传感器数据。加速度计ADXL345测量重力与运动加速度陀螺仪L3G4200D测量三轴角速度磁力计HMC5883L测量地磁场用于航向校准8.5.2 驱动安装# 克隆ROS2版本驱动到工作空间 cd ~/ros2_ws/src git clone https://github.com/Razor-AHRS/razor_imu_9dof.git cd .. colcon build烧录固件到IMU修改配置文件指定串口号。8.5.3 数据可视化启动驱动与可视化界面ros2 launch razor_imu_9dof razor-pub-and-display.launch.py弹出两个窗口3D姿态模型窗口实时显示IMU空间朝向曲线窗口显示Roll/Pitch/Yaw角度、线加速度、角速度数值曲线8.5.4 标准消息格式IMU发布sensor_msgs/msg/Imu话题核心字段geometry_msgs/msg/Quaternion orientation # 四元数姿态 float64[9] orientation_covariance # 姿态协方差 geometry_msgs/msg/Vector3 angular_velocity # 三轴角速度 geometry_msgs/msg/Vector3 linear_acceleration # 三轴线加速度8.5.5 多传感器融合定位使用robot_localization包的EKF扩展卡尔曼滤波器融合轮式里程计与IMU数据提升定位精度与抗干扰能力。安装包sudo apt install ros-lyrical-robot-localization配置ekf.yaml指定输入话题与融合维度ekf_node: ros__parameters: frequency: 50.0 odom0: /odom odom0_config: [false, false, false, false, false, false, true, true, true, false, false, true, false, false, false] imu0: /imu/data imu0_config: [false, false, false, false, false, true, false, false, false, false, false, true, true, false, false] odom_frame: odom base_link_frame: base_footprint world_frame: odom启动EKF节点输出滤波后的/odometry/filtered话题。8.6 GPS全球定位系统8.6.1 驱动安装GPS通过串口输出NMEA标准协议数据ROS2使用nmea_navsat_driver解析sudo apt install ros-lyrical-nmea-navsat-driver启动驱动指定串口与波特率# 普通GPS 4800波特率 ros2 run nmea_navsat_driver nmea_gps_driver _port:/dev/ttyUSB0 _baud:4800 # RTK高精度GPS 115200波特率 ros2 run nmea_navsat_driver nmea_gps_driver _port:/dev/ttyUSB0 _baud:1152008.6.2 消息格式发布sensor_msgs/msg/NavSatFix话题核心字段uint8 status # 定位状态0无效 1单点定位 2差分定位 float64 latitude # 纬度 度 float64 longitude # 经度 度 float64 altitude # 高度 米 float64[9] position_covariance # 位置协方差8.6.3 经纬度转UTM平面坐标将地理经纬度转换为UTM平面直角坐标系便于机器人局部导航计算#include rclcpp/rclcpp.hpp #include sensor_msgs/msg/nav_sat_fix.hpp #include geometry_msgs/msg/point.hpp // 经纬度转UTM函数 void LLtoUTM(double lat, double lon, double northing, double easting, char zone) { // 标准UTM投影转换算法实现 } void gps_cb(const sensor_msgs::msg::NavSatFix::SharedPtr gps) { double n, e; char z; LLtoUTM(gps-latitude, gps-longitude, n, e, z); geometry_msgs::msg::Point pos; pos.x e; pos.y n; pos.z gps-altitude; pos_pub_-publish(pos); }8.7 Hokuyo 2D激光雷达8.7.1 驱动安装ROS 2 Lyrical使用urg_node驱动Hokuyo系列激光雷达sudo apt install ros-lyrical-urg-node配置串口权限添加udev规则避免每次手动修改权限sudo echo KERNELttyACM0, MODE0666 /etc/udev/rules.d/99-hokuyo.rules sudo udevadm control --reload-rules8.7.2 启动与测试ros2 run urg_node urg_node查看激光数据话题ros2 topic list | grep scan ros2 topic hz /scan8.7.3 消息结构sensor_msgs/msg/LaserScan是导航标准输入float32 angle_min # 起始角度 rad float32 angle_max # 终止角度 rad float32 angle_increment # 相邻点角度差 float32 range_min # 最小测距 m float32 range_max # 最大测距 m float32[] ranges # 测距数组 m float32[] intensities # 回波强度数组8.7.4 RViz2可视化RViz2中添加LaserScan插件Fixed Frame设为laser即可看到红色激光点云轮廓移动雷达时轮廓同步更新。8.8 深度相机与点云处理8.8.1 OpenNI驱动安装Kinect一代深度相机使用OpenNI2驱动sudo apt install ros-lyrical-openni2-camera ros-lyrical-openni2-launch启动相机ros2 launch openni2_launch openni2.launch.py8.8.2 图像查看# RGB彩色图像 ros2 run image_view image_view image:/camera/rgb/image_color # 深度灰度图像 ros2 run image_view image_view image:/camera/depth/image8.8.3 3D点云可视化RViz2添加PointCloud2插件订阅/camera/depth/points话题实时显示三维点云场景。8.8.4 PCL点云降采样滤波使用PCL点云库的VoxelGrid体素滤波降采样减少数据量提升处理速度#include rclcpp/rclcpp.hpp #include sensor_msgs/msg/point_cloud2.hpp #include pcl_conversions/pcl_conversions.h #include pcl/point_cloud.h #include pcl/point_types.h #include pcl/filters/voxel_grid.h class CloudFilter : public rclcpp::Node { public: CloudFilter() : Node(c8_kinect) { pub_ this-create_publishersensor_msgs::msg::PointCloud2(output, 1); sub_ this-create_subscriptionsensor_msgs::msg::PointCloud2( /camera/depth/points, 1, std::bind(CloudFilter::cloud_cb, this, std::placeholders::_1)); } private: void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input) { pcl::PCLPointCloud2::Ptr pcl_cloud(new pcl::PCLPointCloud2); pcl_conversions::toPCL(*input, *pcl_cloud); pcl::VoxelGridpcl::PCLPointCloud2 sor; sor.setInputCloud(pcl_cloud); sor.setLeafSize(0.01f, 0.01f, 0.01f); // 1cm体素 pcl::PCLPointCloud2 filtered; sor.filter(filtered); sensor_msgs::msg::PointCloud2 output; pcl_conversions::fromPCL(filtered, output); pub_-publish(output); } rclcpp::Publishersensor_msgs::msg::PointCloud2::SharedPtr pub_; rclcpp::Subscriptionsensor_msgs::msg::PointCloud2::SharedPtr sub_; };降采样后点数量可减少90%以上大幅降低后续算法算力消耗。8.9 Dynamixel总线舵机8.9.1 驱动安装Dynamixel系列总线舵机使用dynamixel_workbench作为ROS2标准驱动sudo apt install ros-lyrical-dynamixel-workbench通过USB2Dynamixel转接器连接舵机总线支持菊花链拓扑。8.9.2 控制器启动扫描总线上的舵机ID启动位置控制器ros2 launch dynamixel_workbench_controllers dynamixel_position.launch.py8.9.3 位置控制舵机控制器订阅/tilt_controller/command话题消息类型std_msgs/Float64单位为弧度# 转动到0.5rad位置 ros2 topic pub --once /tilt_controller/command std_msgs/msg/Float64 data: 0.58.9.4 周期运动节点编写C节点驱动舵机在-180°~180°往复运动可扩展为云台扫描、机械臂周期动作。本章小结1 ROS2生态已覆盖绝大多数主流机器人传感器与执行器统一话题消息格式硬件驱动与上层算法解耦2 Arduino rosserial是低成本扩展方案适合简单IO与低速控制场景高性能嵌入式推荐使用micro-ROS3 差速底盘电机驱动、编码器里程计、IMU、激光雷达是移动机器人四大核心硬件共同构成导航系统的感知与执行基础4 PCL点云处理、EKF多传感器融合是传感器数据进阶处理的标准工具可有效提升数据质量与系统鲁棒性5 所有传感器均遵循ROS2标准消息协议不同品牌、型号的同类型设备可无缝替换上层算法无需修改。