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_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10); joy_sub_ = this->create_subscription<sensor_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::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_; rclcpp::Subscription<sensor_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_shared<TeleopJoy>()); rclcpp::shutdown(); return 0; }

8.1.4 一体化启动与可视化

Python Launch文件launch/teleop_robot.launch.py，整合手柄驱动、速度转换、机器人模型、里程计与RViz2：

from 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(package="joy_linux", executable="joy_node", parameters=[{"dev": "/dev/input/js0", "deadzone": 0.12}]), Node(package="chapter8_tutorials", executable="teleop_joy"), Node(package="robot_state_publisher", executable="robot_state_publisher", parameters=[{"robot_description": open("urdf/robot2.urdf").read()}]), Node(package="joint_state_publisher", executable="joint_state_publisher"), Node(package="rviz2", executable="rviz2", 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、Nano；Arduino 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 /chatter

8.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::Subscriber<std_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/ENB：PWM调速，接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接5V，GND接地，信号接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 θ += ω * dt

8.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

弹出两个窗口：

1. 3D姿态模型窗口，实时显示IMU空间朝向
2. 曲线窗口，显示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数据，提升定位精度与抗干扰能力。

1. 安装包：

sudo apt install ros-lyrical-robot-localization

2. 配置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

3. 启动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:=115200

8.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 'KERNEL=="ttyACM0", MODE="0666"' > /etc/udev/rules.d/99-hokuyo.rules sudo udevadm control --reload-rules

8.7.2 启动与测试

ros2 run urg_node urg_node

查看激光数据话题：

ros2 topic list | grep scan ros2 topic hz /scan

8.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.py

8.8.2 图像查看

# RGB彩色图像 ros2 run image_view image_view image:=/camera/rgb/image_color # 深度灰度图像 ros2 run image_view image_view image:=/camera/depth/image

8.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_publisher<sensor_msgs::msg::PointCloud2>("output", 1); sub_ = this->create_subscription<sensor_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::VoxelGrid<pcl::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::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_; rclcpp::Subscription<sensor_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.py

8.9.3 位置控制

舵机控制器订阅/tilt_controller/command话题，消息类型std_msgs/Float64，单位为弧度：

# 转动到0.5rad位置 ros2 topic pub --once /tilt_controller/command std_msgs/msg/Float64 "data: 0.5"

8.9.4 周期运动节点

编写C++节点驱动舵机在-180°~180°往复运动，可扩展为云台扫描、机械臂周期动作。

本章小结

1 ROS2生态已覆盖绝大多数主流机器人传感器与执行器，统一话题消息格式，硬件驱动与上层算法解耦；
2 Arduino + rosserial是低成本扩展方案，适合简单IO与低速控制场景，高性能嵌入式推荐使用micro-ROS；
3 差速底盘电机驱动、编码器里程计、IMU、激光雷达是移动机器人四大核心硬件，共同构成导航系统的感知与执行基础；
4 PCL点云处理、EKF多传感器融合是传感器数据进阶处理的标准工具，可有效提升数据质量与系统鲁棒性；
5 所有传感器均遵循ROS2标准消息协议，不同品牌、型号的同类型设备可无缝替换，上层算法无需修改。