ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

Time Synchronizer hangs after few messages

asked 2012-09-05 06:38:51 -0500

updated 2014-01-28 17:13:34 -0500

ngrennan gravatar image

enter code hereHi,

I have a scenario where I have 2 sensors publishing messages to 2 different topics, let's say:

topicA

topicB

I wanted to subscribe to both these topics using a 3rd node; compare the values from the first 2 topics and do certain operations and generate a new output to be published onto another topic say,

topicC

For this I created custom messages for each of my sensors with a header containing the time stamp.

In the nodes publishing to topics 'topicA' and 'topicB' I initialized the timestamp field in the header as

x.header.timeStamp = ros::Time::now()

All my publisher nodes run continuously with no problems. However, I only get the first two messages at the output of my synchronizer and then it stops forever. Then I am unable to stop the node other than terminating the execution using SIGINT (ctrl+z).

Can someone tell me what I can do so that my timesynchronizer works properly and synchronizes the outputs from topicA and topicB using timestamps.

Here's the code of node 3:

void fusionCallback(const fusion::TwistsConstPtr& sensorA, const fusion::TwistsConstPtr& sensorB)
geometry_msgs::Twist vel;
ros::NodeHandle n;
ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
ros::Rate loop_rate(10);
while(ros::ok())
{
    float sensorA_dat = sensorA->dat;
    float sensorB_dat = sensorB->dat;
    if(sensorB_dat > sensorA_dat)
        vel = sensorB->vel;
    else vel = sensorA->vel;
    vel_pub.publish(vel);
    ROS_INFO("sensorA Data = [%f] sensorB Data = [%f]", sensorA_dat, sensorB_dat);
    ros::spinOnce();
    loop_rate.sleep();

}

int main(int argc, char **argv)

ros::init(argc, argv, "vel_publisher");
ros::NodeHandle n;
//ros::Rate loop_rate(20);

using namespace message_filters;
Subscriber<fusion::Twists> sensorA_sub(n, "/fusion/cmd_vel_sensorA", 1);
Subscriber<fusion::Twists> sensorB_sub(n, "/fusion/cmd_vel_sensorB", 1);


typedef sync_policies::ApproximateTime<fusion::Twists, fusion::Twists> MySyncPolicy;

Synchronizer<MySyncPolicy> sync(MySyncPolicy(100), sensorA_sub, sensorB_sub);
sync.registerCallback(boost::bind(&fusionCallback, _1, _2));
ros::spin();

return 0;

}

Note: All braces are proper in the actual code!

Update : The Effect of spin inside callback

On the suggestion of @dornhege I tried removing the spin inside callback and then my code started working. I was able to see the output wherein the TimeSynchronizer merged the data from both the topics appropriately. I was able to terminate the execution using ctrl+c as well.

However, the problem is that now my output is not getting published. Infact, that is the reason why I have the spin inside the callback. This is as per the basic tutorials for publisher and subscriber provided here

Thanks and Regards.

edit retag flag offensive close merge delete

Comments

Can you post a miminal excerpt of your program showing how is your main loop? If ctrl+z do not have any effect, then you probably do not look for ros::ok() enough and you may have spin issues which prevent the callbacks from being called several times. You can also increase the queue. It may help.

Thomas gravatar image Thomas  ( 2012-09-05 07:10:56 -0500 )edit

As far as ctrl+z is concerned i have said in my question that it is the only thing that is able to stop my program.

SivamPillai gravatar image SivamPillai  ( 2012-09-05 07:32:59 -0500 )edit

Your code looks like you are starting another ROS spinning loop within the callback. Is that really what you want, it seems very odd.

dornhege gravatar image dornhege  ( 2012-09-05 23:54:40 -0500 )edit

Ctrl-C should be SIGINT, Ctrl-z does something different. Ctrl-C should stop a program that check's ros::ok.

dornhege gravatar image dornhege  ( 2012-09-05 23:55:28 -0500 )edit

Oh yeah am sorry. I think ctrl+z is SIGTERM or SIGSTOP. am not sure. But anyways ctrl+c does not stop the running program.

SivamPillai gravatar image SivamPillai  ( 2012-09-06 00:35:18 -0500 )edit
1

On the publishing: 1. a spin won't help there. 2. Are you still constructing the publisher in the callback? That is the problem. It needs time to connect the topic and actually be able to publish -> only construct one in the main, that should help. The spin was functioning as a "wait loop" for that.

dornhege gravatar image dornhege  ( 2012-09-06 02:27:41 -0500 )edit

Heyy @dornhege thanks a lot lot lot!! It worked. I actually tried this before as well after referring another answer in ros. But I made a silly mistake of initializing the publisher twice and then it gave me a runtime error of invalid publisher. So I thought maybe I was wrong.

SivamPillai gravatar image SivamPillai  ( 2012-09-06 02:47:18 -0500 )edit

But now I decided it must be right since you've told as well and tried to work on finding the mistake and now its found. Thanks Again :) Its all good now :)

SivamPillai gravatar image SivamPillai  ( 2012-09-06 02:48:11 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
2

answered 2012-09-06 02:56:40 -0500

This answer is based on suggestions from @dornhege

It is important to note that a spin need not be added in a publisher that is called from within a callback.

1) Remove the spin instruction and the while loop from the publisher.

2) Declare publisher globally.

3) define the publisher inside the main.

4) (if needed) Add a small sleep routine to let the publisher get ready to send data. (ros::Duration(0.1).sleep();)

Here is the modified code

    ros::Publisher vel_pub;
void fusionCallback(const fusion::TwistsConstPtr& sensorA, const fusion::TwistsConstPtr& sensorB)
{
geometry_msgs::Twist vel;


ros::Rate loop_rate(10);
float sensorA_dat = sensorA->dat;
float sensorB_dat = sensorB->dat;
if(sensorB_dat > sensorA_dat)
    vel = sensorB->vel;
else vel = sensorA->vel;
vel_pub.publish(vel);
ROS_INFO("sensorA Data = [%f] sensorB Data = [%f]", sensorA_dat, sensorB_dat);
}

int main(int argc, char **argv)
{
ros::init(argc, argv, "vel_publisher");
ros::NodeHandle n;
//ros::Rate loop_rate(20);
vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
using namespace message_filters;
Subscriber<fusion::Twists> sensorA_sub(n, "/fusion/cmd_vel_sensorA", 1);
Subscriber<fusion::Twists> sensorB_sub(n, "/fusion/cmd_vel_sensorB", 1);


typedef sync_policies::ApproximateTime<fusion::Twists, fusion::Twists> MySyncPolicy;

Synchronizer<MySyncPolicy> sync(MySyncPolicy(100), sensorA_sub, sensorB_sub);
sync.registerCallback(boost::bind(&fusionCallback, _1, _2));
ros::spin();

return 0;
}

Thanks Again @dornhege.

Hope this helps anyone who may find a similar problem with TimeSynchronizer.

Regards,

edit flag offensive delete link more
1

answered 2012-09-05 06:42:29 -0500

dornhege gravatar image

TimeSynchronizer synchronizes by exact timestamps. If you generated them from sensors it's very unlikely to find a match. You should try the policy base synchronizer with ApproximateTimePolicy.

edit flag offensive delete link more

Comments

That was what I did in order to see if that would work. but it did not help me either. do you have an alternative suggestion?

SivamPillai gravatar image SivamPillai  ( 2012-09-05 06:56:30 -0500 )edit

Question Tools

Stats

Asked: 2012-09-05 06:38:51 -0500

Seen: 1,858 times

Last updated: Sep 06 '12