ROS服务调用时LaserScan数据未更新问题求解

任务需求 #

当/find_wall服务被调用时，机器人需完成以下行为：

• 识别最短的激光射线（假设指向墙面）
• 旋转机器人直至正面朝向墙面（通过发布角速度，直到正面射线为最短）
• 向前移动直至正面射线距离小于30cm
• 停止移动并返回带True的服务响应

现有代码（含基础修复） #

#include "ros/node_handle.h"
#include "ros/publisher.h"
#include "ros/subscriber.h"
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <services_quiz/FindWall.h>
#include <unistd.h>
#include <vector>  // 原代码遗漏头文件
#include <limits>  // 原代码遗漏numeric_limits所需头文件
using namespace std;

class TurtleFindWall
{
public:
ros::Publisher pub;
geometry_msgs::Twist vel;
vector<float> nums;  // 原代码未指定vector类型，补充<float>
ros::NodeHandle nh;
ros::ServiceServer my_service;
ros::Subscriber sub;

TurtleFindWall()
{
    my_service = nh.advertiseService("/find_wall", &TurtleFindWall::my_callback, this);
    sub = nh.subscribe("scan", 1000, &TurtleFindWall::counterCallback, this);
    pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
    ROS_INFO("Service /find_wall Ready");
}

void counterCallback(const sensor_msgs::LaserScan::ConstPtr &msg) 
{
    nums = msg->ranges;
    ROS_INFO("list = %f", nums[360]);
}

bool my_callback(services_quiz::FindWall::Request &req,
                services_quiz::FindWall::Response &res) 
{
    ROS_INFO("The Service find_wall has been called");

    // 增加数组长度判断，避免未初始化时越界
    while (nums.size() > 360 && nums[360] > 0.3)  
    {
        float min_range = std::numeric_limits<float>::infinity();
        for (int i = 0; i < nums.size(); ++i) 
        {
            // 过滤无效激光数据（无穷大/NaN）
            if (nums[i] < min_range && !std::isinf(nums[i]) && !std::isnan(nums[i]))  
            {
            min_range = nums[i];
            }
        }

        ROS_INFO("min_range %f", min_range);

        if (nums[360] > min_range + 0.05) 
        {
            vel.linear.x = 0.0;
            vel.angular.z = 0.2;
            pub.publish(vel);
            ROS_INFO("360 %f", nums[360]);
        }
        else 
        {
            vel.linear.x = 0.2;
            vel.angular.z = 0.0;
            pub.publish(vel);
        }

        // 关键修复：手动触发消息回调更新激光数据
        ros::spinOnce();
        // 添加延迟，避免循环占用过高CPU
        ros::Duration(0.1).sleep();
    }
    vel.linear.x = 0.0;
    vel.angular.z = 0.0;
    pub.publish(vel);

    ROS_INFO("360 %f", nums[360]);
    res.wallfound = true;
    return true;
} 
};

int main(int argc, char **argv)
{
ros::init(argc, argv, "services_quiz_node");
TurtleFindWall turtlefindwall;
ros::spin();
return 0;
}

问题根源 #

原代码中，服务回调函数my_callback内的while循环会阻塞ROS的单线程消息处理队列：主线程被ros::spin()占用，服务回调触发后，while循环会持续占用线程资源，导致激光扫描的订阅回调counterCallback无法被执行，nums始终停留在服务调用前的初始值。

核心解决步骤 #

在while循环内部添加两个关键操作：

1. 调用ros::spinOnce()：手动触发一次ROS消息处理流程，让订阅回调有机会更新激光数据
2. 添加ros::Duration(0.1).sleep()：给系统留出消息处理时间，同时避免循环占用过高CPU

额外的健壮性修复：

• 补充原代码遗漏的头文件，解决编译错误
• 给vector指定具体类型，避免类型模糊
• 增加数组长度判断，防止服务调用时激光数据未初始化导致越界
• 过滤无效激光数据（无穷大/NaN），避免错误的最短距离判断

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