Ask Your Question
0

ROS misses some messages

asked 2013-09-23 23:43:46 -0500

ubisum gravatar image

Hello everyone. I'm writing a test program, which receives messages from two different topics and stores them in different queues.
The program, works on a while loop and, at every iteration, before checking for the arrival of new messages, it deletes the top elements of the queues if they have both one.
The program subscribes to a third queue, too, that I use to send a termination message.
Here is the code:

#include "/opt/ros/fuerte/include/ros/ros.h"
#include "/opt/ros/fuerte/include/nav_msgs/Odometry.h"
#include "/opt/ros/fuerte/include/geometry_msgs/Pose.h"
#include "/opt/ros/fuerte/include/geometry_msgs/Point.h"
#include "/opt/ros/fuerte/include/std_msgs/String.h"
#include "/opt/ros/fuerte/include/sensor_msgs/LaserScan.h"
#include "/opt/ros/fuerte/stacks/geometry/tf/include/tf/transform_listener.h"

#include "odom_node.h"
#include "scan_node.h"
#include "messageListener.h"

#include "list"
#include "vector"
#include "coord.h"
#include "math.h"
#include "stdlib.h"
#include "iterator"
#include "string"
#include "iostream"
#include "fstream"
#include "string"
#include "sstream"

#include "stdio.h"
#include "iostream"
#include "map"
#include "math.h"
#include "stdlib.h"
#include "string"

using namespace std;

messageListener::messageListener(){
    odomCounter = 0;
    scanCounter = 0;
}

void messageListener::end_loop(const std_msgs::String mes){
    printf("Setting flag\n");
        while_flag = 0;
}

void messageListener::addOdomNode (const nav_msgs::Odometry mes){
    geometry_msgs::Pose robot_pose = mes.pose.pose;
    geometry_msgs::Point robot_point = robot_pose.position;

    odom_node *on = new odom_node();
    (*on).xCoord = robot_point.x;
    (*on).yCoord = robot_point.y;
    (*on).frame_id = mes.header.frame_id;

    double orientation = tf::getYaw(robot_pose.orientation);
    (*on).angle = (float)orientation;

    odomVector.push_back(*on);
    odomCounter++;
}

vector<coord> messageListener::polar2cart(vector<float> v, float a_min, float a_inc, float range_min, float range_max){
    vector<coord> lc;
    for(int i=0; i<(int)v.size(); i++){
        if(v[i]>= range_min && v[i]<=range_max){
            coord *c = new coord();
            (*c).x = v[i]*cos(a_min + a_inc*i);
            (*c).y = v[i]*sin(a_min + a_inc*i);

            lc.push_back(*c);
        }
    }

    return lc;
}

void messageListener::addScanNode (const sensor_msgs::LaserScan mes){
    //printf("scan");
    scan_node *sn = new scan_node();
    (*sn).scan = mes.ranges;
    (*sn).frame_id = mes.header.frame_id;
    (*sn).cartesian = polar2cart(mes.ranges, mes.angle_min, mes.angle_increment, mes.range_min, mes.range_max);

    scanVector.push_back(*sn);
    scanCounter++;
}

void messageListener::listen(int c, char** v){
    while_flag = 1;
    printf("Listening activity begun\n");
    ros::init(c,v, "listener");
    ros::NodeHandle n_odom, n_scan, n_end;

    ros::Subscriber sub_odom = n_odom.subscribe("/odom", 1000, &messageListener::addOdomNode,this);
    ros::Subscriber sub_scan = n_scan.subscribe("/base_scan", 1000, &messageListener::addScanNode,this);
    ros::Subscriber sub_end = n_end.subscribe("/end", 1000, &messageListener::end_loop,this);
    printf("Subscription complete\n");

    while(while_flag){
        if(odomVector.size()>0 && scanVector.size()>0){
            odomVector.erase(odomVector.begin());
            scanVector.erase(scanVector.begin());
        }

        ros::spinOnce();
    }

    printf("Exit from listening\n");
    printf("Ricevuti %i nodi odometrici e %i nodi scansione\n", odomCounter, scanCounter);
    printf("Terminazione in corso con %i nodi odometrici e %i nodi scansione in coda rimasti\n\n", (int)odomVector.size(), (int)scanVector.size());
}

Here, odomCounter and scanCounter are just two counters that I use to count received messages.
After starting the program, I play a bag file, whose information is reported in the following:

ubisum@ubisum ...
(more)
edit retag flag offensive close merge delete

Comments

Technically there is not guarantee that you will receive all messages. That being said: In your setting you'd hope for that given the large queue sizes. One guess: Check, which messages are missing. Are those the first ones? Then those are sent before the topic connection was made.

dornhege gravatar imagedornhege ( 2013-09-23 23:55:36 -0500 )edit

thanks for answer. I took care to start bag playing after running receiving program. I suspect that the ones to be missing are the last ones, since the playing time stops a little while before expected end (it should stop at 19 seconds, but it does at 18.8). is there a reason for this?

ubisum gravatar imageubisum ( 2013-09-24 00:35:27 -0500 )edit
1

I'm not sure if there is a reason for that, but it might still be the first ones: When the player is started it takes a short amount of time for both nodes to connect, where messages might be played. Maybe it helps to start in --paused mode and wait 1 sec before running.

dornhege gravatar imagedornhege ( 2013-09-24 01:06:25 -0500 )edit
1

The easiest might be to check timestamps of the first/last messages in the log/received to see what's going on. Nevertheless, this doesn't need to work according to ROS, so if you really need all messages, maybe look into the rosbag API.

dornhege gravatar imagedornhege ( 2013-09-24 01:07:40 -0500 )edit

thanks for help, I'll take all this into account. just one more question: when a callback function is invoked, is its termination awaited before spinning to receive another message? is it possible that two different callback functions are running at same time, while the loop continues to receive?

ubisum gravatar imageubisum ( 2013-09-24 02:34:07 -0500 )edit

AFAIK no, unless you use a threaded spinner.

dornhege gravatar imagedornhege ( 2013-09-24 02:38:43 -0500 )edit

that's what I expected. my system should take an odometric and scanning message, then go through corners extraction, data association, RANSAC, graph optimization... all this will take a long while before a new message can be received....

ubisum gravatar imageubisum ( 2013-09-24 03:30:29 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
5

answered 2013-09-24 03:01:42 -0500

Just to add to the reply posted by @dornhege:

It is fairly common to drop the first few messages played from a bag file.

When you start your program early, this only ensures that topic subscriptions are advertised before playing the bag-file topics. The way ROS handles message-publishing is by creating explicit connections between the publisher and subscriber once "matching" topic names/types have been identified. This connection happens when the publisher and subscriber have both advertised the same topic. Once the connection is made, then the subscriber starts processing any messages received after the connection is completed. Any messages published before that connection is finalized are simply dropped.

When playing a bag file with the default settings, there is a very small (near-zero) delay between advertising a topic and playing the messages from that topic. This can cause the first few messages to be dropped before the subscriber connection is fully completed.

Thankfully, there is an easy solution to this problem. Play the bag file with a delay option to introduce a pause between advertising the topic and publishing the messages. This delay allows time for subscriber clients to connect before publishing any messages, so none are dropped:

rosbag play -d 0.5 myData.bag
edit flag offensive delete link more

Comments

thanks, your solution seems to work. it's not a great damage missing the initial messages, but it's not "an engineer's approach" :))) now every message is correctly delivered.

ubisum gravatar imageubisum ( 2013-09-24 03:28:10 -0500 )edit
2

If you want to guarantee all messages, the best would be to parse the file using the rosbag API.

dornhege gravatar imagedornhege ( 2013-09-24 03:50:34 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2013-09-23 23:43:46 -0500

Seen: 688 times

Last updated: Sep 24 '13