Turtlebot3如何订阅/scan话题修改LaserScan数据后重发布到该话题

优先级说明 #

优先选择方案2，无需修改雷达驱动源码，实现成本更低、后续维护更方便。方案1需要修改驱动源码，升级驱动时会丢失自定义修改，不推荐。

方案2实现说明：直接修改sensor_msgs/LaserScan消息内容 #

sensor_msgs/LaserScan类型消息的ranges字段为普通float32数组，支持直接修改，无只读限制，具体实现步骤如下：

1. 新建ROS功能包，依赖添加roscpp/rospy、sensor_msgs
2. 编写自定义处理节点，核心逻辑为：订阅原始激光数据，修改ranges字段后发布新的/scan话题
3. 启动时通过话题重映射避免原始激光数据和处理后数据的话题冲突

参考Python代码示例： #

import rospy
from sensor_msgs.msg import LaserScan

def scan_process(msg):
    # 此处替换为自定义算法逻辑，示例为过滤无效距离值
    for i in range(len(msg.ranges)):
        if msg.ranges[i] < 0.1 or msg.ranges[i] > msg.range_max:
            msg.ranges[i] = float('inf')
    scan_pub.publish(msg)

if __name__ == '__main__':
    rospy.init_node('lds_opt_node')
    # 发布处理后的激光数据到/scan话题
    scan_pub = rospy.Publisher('/scan', LaserScan, queue_size=10)
    # 订阅重映射后的原始激光话题
    rospy.Subscriber('/scan_raw', LaserScan, scan_process, queue_size=10)
    rospy.spin()

启动配置说明： #

启动雷达驱动时添加话题重映射规则，将驱动原本发布的/scan改为/scan_raw即可，无需修改任何驱动源码，以launch文件配置为例：

<include file="$(find turtlebot3_bringup)/launch/turtlebot3_lidar.launch">
  <remap from="/scan" to="/scan_raw"/>
</include>
<node pkg="你的功能包名" type="lds_opt_node.py" name="lds_opt_node" output="screen"/>

方案1实现说明：修改雷达驱动源码 #

如果必须修改驱动发布自定义原始话题的话，LDS-01驱动源码默认路径为ROS工作空间下的src/turtlebot3/turtlebot3_lidar/src/lds01.cpp，找到其中scan_pub的定义部分，新增一个发布器发布自定义原始话题即可，每次发布/scan的同时同步发布原始数据到自定义话题。

注意事项 #

• 算法处理耗时需控制在100ms以内，激光雷达默认输出频率为10Hz，耗时过高会导致数据丢包
• 调试阶段可通过rviz分别查看原始/scan_raw和处理后/scan的点云差异，验证算法效果
• 你提供的思路1概要图如下：
  

内容的提问来源于stack exchange，提问作者AmirulJ