ROS中的回调函数？如何正确更新它们？

Question

所以我做了一个机器人，它应该从激光传感器获取数据，然后移动到一段距离并停止。

但我发现回调函数有问题。是否有更好的解释如何正确使用回调更新变量？我对 python 有同样的问题，在那里我发现 time.sleep(0.2) 让类正确更新。即使这对我来说有点神奇。因为我认为在 python 中这会自动工作，因为分离线程。

在 c++ 中，我知道基本是使用 spinOnce() 和 spin()。这在正常的非面向对象的情况下应该如何工作。但是在课堂上我又发现课堂没有正确更新。为什么会出现这种情况？ 我找不到回调函数无法正常工作的原因。我可以通过 print full range from reading 来查看是否是这种情况，但它从未发生过。我有 ros::spinOnce() 并且我认为我有正确的成员函数。有人可以帮帮我吗 ？并帮助我理解？

robot.h

#include <ros/ros.h>
#include "sensor_msgs/LaserScan.h"
#include "geometry_msgs/Twist.h"
#include <math.h>
#include <iostream>

class Robot{
    geometry_msgs::Twist velocity;
    sensor_msgs::LaserScan ls_scan;

    ros::Subscriber sub;
    ros::Publisher pub; 

    ros::Rate rate{10};

    double speed_linear;
    double speed_angular;

public:
    Robot(ros::NodeHandle *handle_IN, double rate_IN, double speed_linear_IN,double speed_angular_IN){
        rate = ros::Rate{rate_IN};
        speed_linear=speed_linear_IN;
        speed_angular=speed_angular_IN;
        sub = handle_IN->subscribe("/scan",1000,&Robot::scan_callback,this);
        pub = handle_IN->advertise<geometry_msgs::Twist>("/cmd_vel",10);
        usleep(2000000);
    }
    void scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
    void move();
    void rotate(bool clock_wise);
    void stop();
    bool obstacle_in_front(double distance);
    bool can_drive(double distance);
    std::vector<float> front_read();
    std::vector<float> back_read();
}; 

robot.cpp

#include "robot.h"

void Robot::scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg){
    ls_scan = *msg;
    for(float x: ls_scan.ranges)
        std::cout << x;
    std::cout << std::endl;
}

void Robot::move(){
    velocity = geometry_msgs::Twist();
    velocity.linear.x = speed_linear;

    ros::spinOnce();
    while(!obstacle_in_front(0.56)){
        ros::spinOnce();
        std::cout << "moving\n";
        pub.publish(velocity);
        rate.sleep();
    }
    stop();
}

void Robot::rotate(bool clock_wise){
    velocity = geometry_msgs::Twist();
    velocity.angular.z = speed_angular;

    ros::spinOnce();
    while(can_drive(2)){
        ros::spinOnce();
        std::cout << "rotating\n";
        pub.publish(velocity);
        rate.sleep();
    }
    stop();
}

void Robot::stop(){
    std::cout << "stop\n";
    velocity = geometry_msgs::Twist();
    pub.publish(velocity);
}

float min(const std::vector<float>& v){
    if(v.size() == 0)
        return -1;
    float min = v[0];
    for(float x: v){
        if(x < min)
            min = x;
    }
    return min;
}

std::vector<float> Robot::front_read(){
    ros::spinOnce();
    std::vector<float> front(20);
    if(ls_scan.ranges.size()<350)
        return front;
    for(int i = 0; i < 10; ++i)
        front.push_back(ls_scan.ranges[i]);
    for(int i = 350; i < 360; ++i)
        front.push_back(ls_scan.ranges[i]);
    return front;
}
std::vector<float> Robot::back_read(){
    ros::spinOnce();
    std::vector<float> front(20);
    if(ls_scan.ranges.size()<350)
        return front;
    for(int i = 169; i < 190; ++i)
        front.push_back(ls_scan.ranges[i]);
    return front;
}

bool Robot::obstacle_in_front(double distance){
    float minN = min(front_read());
    if(minN > distance)
        return false;
    else
        return true;
    std::cout << minN << "\n";
}

bool Robot::can_drive(double distance){
    float minN = min(front_read());
    if(minN > distance)
        return true;
    else
        return false;
}

robot_control_node.cpp

#include "robot.h"

int main(int argc, char **argv){
    ros::init(argc,argv, "robot_node_node");
    ros::NodeHandle n;
    Robot robot(&n,10,0.5,0.3);
    robot.move();
}

Answer 1

所以最后的答案是这样的。

1. 等待有效数据 -> 我使用简单的等待循环来执行此操作，该循环在构造函数中初始化发布者/订阅者之后运行。
2. 然后您可以在需要时检查新的有效数据。示例是在 move 成员函数中调用 spinOnce()。这是通过 do-while 完成的，因为您想在新数据之后检查条件。

void Robot::wait_for_valid_data(){
    while(ls_scan.ranges.size() < 359)
    {
        std::cout << "waiting for valid data\n";
        ros::spinOnce();
        rate.sleep();
    }
    std::cout << "valid data";  
}

void Robot::move(){
    velocity = geometry_msgs::Twist();
    velocity.linear.x = speed_linear;

    do{
        if(!ros::ok())
            break;
        std::cout << "moving\n";
        pub.publish(velocity);
        ros::spinOnce();
        rate.sleep();
    }while(!obstacle_in_front(0.56));
    stop();
}

Answer 2

我发现了问题。 基本上带有回调。你必须确定，出版商赶上了。所以在你调用 spinOnce() 之前，它会检查它是否存在。您必须调用某种等待功能。 ros::rate/ros::Duration 并等待。然后当你调用 spinOnce() 时。您将获得新的传入数据，回调函数可以读取这些数据。

在这个意义上：

std::vector<float> Robot::front_read(){
    ros::Duration(0.1).sleep(); //wait for new callback
    ros::spinOnce();            //read the callback
    std::vector<float> front;
    if(ls_scan.ranges.size()<350)
        return front;
    for(int i = 0; i < 10; ++i)
        front.push_back(ls_scan.ranges[i]);
    for(int i = 350; i < 360; ++i)
        front.push_back(ls_scan.ranges[i]);
    return front; 
}