So I make robot, which should get data from laser sensor and then move until some distance and stops.

But I find problem in callback functions. Is there somewhere better explanation how to update variables with callback properly ? I had same problem with python and there I found out that time.sleep(0.2) let the class to update properly. Even this is little bit magic for me. Because I was thinking that in python this works automatically because separated threading.

In c I know that the basic is using spinOnce() and spin(). This works how it should in normal non-object-oriented case. But in the class again I found out that the class is not updated properly. Why is this a case ? I can not find why the callback function is not working properly. I could see if it was the case by print full range from reading, but it never happens. I have ros::spinOnce() and I think I have correctly member functions. Can someone please help me ? And help me to understand ?

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();
}

CodePudding user response：

I found the problem. Basically with callbacks. You have to be sure, that the publisher catches up. So before you call the spinOnce() which checks if it is something there. You have to call some sort of wait function. ros::rate/ros::Duration and wait. Then when you call spinOnce(). You will have new incoming data, which the callback function can read.

In this sence:

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; 
}

CodePudding user response：

So the final answer is this.

1. Wait for valid data -> I did this with simple wait loop which runs after initialization of publisher/subscriber in constructor.
2. Then you can check for new valid data when you need it. Example is calling spinOnce() in move member function. This is done with do-while, because you want to check condition after new data.

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();
}