ROS回调函数内激光雷达数据循环不更新问题求助

问题描述 #

要实现机器人朝向墙面转动的功能，编写了/scan话题的回调函数，但在回调内的while循环中，机器人旋转时msg.ranges数据始终恒定，数值没有变化，无法完成功能。相关代码如下：

def callback(msg):
  r = list(msg.ranges)
  minimum_value = min(r)
  print(minimum_value)
  index_minimum = r.index(minimum_value)
  rounded_min = round(minimum_value,3)
  print('min index',index_minimum)

  if index_minimum > 360:
      twist_instance.angular.z = 0.05
  else:
      twist_instance.angular.z = -0.05
  while round(msg.ranges[360],3) != rounded_min:
      print('rounded min', rounded_min)
      print('front ',round(msg.ranges[360],3))
      print('index naya', index_minimum)
      pub.publish(twist_instance)
      rate.sleep()
  twist_instance.linear.x = 0.1
  twist_instance.angular.z = 0
  while round(msg.ranges[360],1) >= 0.3:
      pub.publish(twist_instance)
      rate.sleep()
  twist_instance.linear.x = 0
  pub.publish(twist_instance)

if __name__ == '__main__':
    rospy.init_node('findwall_node')
    sub = rospy.Subscriber('/scan', LaserScan, callback)
    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)    
    twist_instance = Twist()
    rate = rospy.Rate(1)
    my_service = rospy.Service('/find_wall', findwall, my_callback)
    rospy.spin()

注：r = list(msg.ranges)的索引范围为0至719，对应0至180度的激光雷达数据。

核心原因 #

• 回调内while循环阻塞消息队列
  ROS默认用单线程处理回调队列，回调触发后进入while循环持续执行，期间新的/scan消息无法被接收处理，msg一直是第一次触发时的旧数据，自然不会更新。
• msg是单次回调的快照
  每次回调的msg是当前/scan话题的一次数据快照，while循环里读取的始终是这个快照的旧值，不会随着机器人运动获取新数据。
• 逻辑设计违背ROS异步模式
  ROS回调函数的设计初衷是处理单次消息、执行短任务，把机器人完整的转动、靠近流程放在回调里，完全违背了异步消息处理机制。

解决方案 #

1. 改用状态机模式，主循环处理逻辑 #

将机器人行为拆分为不同状态（空闲、寻找墙面、对齐墙面、靠近墙面），回调仅负责存储最新激光数据，所有运动控制逻辑放到主循环执行，确保每次循环都能获取最新数据。示例代码如下：

import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from your_package.srv import findwall, findwallResponse  # 替换为实际服务类型

class FindWallNode:
    def __init__(self):
        self.sub = rospy.Subscriber('/scan', LaserScan, self.scan_callback)
        self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.twist = Twist()
        self.latest_scan = None
        self.state = "IDLE"  # 状态：IDLE, FIND_WALL, APPROACH_WALL

    def scan_callback(self, msg):
        # 仅保存最新激光数据，不做阻塞处理
        self.latest_scan = msg

    def run(self):
        rate = rospy.Rate(10)  # 提高频率到10Hz，提升响应速度
        while not rospy.is_shutdown():
            if self.latest_scan is None:
                rate.sleep()
                continue
            
            r = list(self.latest_scan.ranges)
            if self.state == "FIND_WALL":
                min_val = min(r)
                min_idx = r.index(min_val)
                rounded_min = round(min_val, 3)
                
                if round(r[360], 3) != rounded_min:
                    # 根据最近点位置调整转动方向
                    self.twist.angular.z = 0.05 if min_idx > 360 else -0.05
                    self.pub.publish(self.twist)
                else:
                    # 对齐完成，切换到靠近状态
                    self.twist.angular.z = 0
                    self.pub.publish(self.twist)
                    self.state = "APPROACH_WALL"
            
            elif self.state == "APPROACH_WALL":
                if round(r[360], 1) >= 0.3:
                    self.twist.linear.x = 0.1
                    self.pub.publish(self.twist)
                else:
                    # 到达目标距离，停止并回到空闲状态
                    self.twist.linear.x = 0
                    self.pub.publish(self.twist)
                    self.state = "IDLE"
            
            rate.sleep()

if __name__ == '__main__':
    rospy.init_node('findwall_node')
    node = FindWallNode()

    # 服务回调：触发寻找墙面的状态
    def service_callback(req):
        node.state = "FIND_WALL"
        return findwallResponse(success=True)
    
    rospy.Service('/find_wall', findwall, service_callback)
    node.run()

2. 关键优化点 #

• 移除回调内的阻塞循环，仅在回调中更新最新激光数据
• 使用状态机管理机器人行为，逻辑清晰且符合ROS异步处理模式
• 提高rospy.Rate频率到10Hz，让机器人运动控制更流畅

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