Ask Your Question
0

Running two subscribers concurrently in the same node

asked 2018-06-21 00:21:51 -0600

alex_f224 gravatar image

updated 2018-06-21 01:30:11 -0600

I am working on a problem and need to use the most recent pose from amcl/future (my self-designed topic) and publish it as a tf between two frames.

I made two subscribers for: amcl_pose/future and clock. The subscriber for amcl_pose/future updates a pose struct inside my code whenever it receives a new pose estimate from amcl; the subscriber for clock ignores the clock message and publish the most recent pose from the structure to tf (basically use it to publish tf in a high rate).

The problem is that the amcl_cb function is never called by ROS. I believe it might be a concurrency issue because the two callbacks share the same data structure. What is the "correct" way to implement this? My code snippet is provided.

*Note: the pose published is not the amcl estimated pose. * *The parent_frame and child_frame are only to simplify the snippet, so it is not related. *

void amcl_cb(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& p){
    pose.header = p->header;
    pose.pose = p->pose;
}

void clock_cb(const rosgraph_msgs::Clock::ConstPtr& dummy){
    tf::TransformBroadcaster broadcaster;
    broadcaster.sendTransform(
    tf::StampedTransform(
    tf::Transform(
      tf::Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w), 
      tf::Vector3(pose.position.x, pose.position.y, pose.position.z)),
    ros::Time::now(),"parent_frame", "child_frame"));
}

int main(int argc, char** argv){
    ros::init(argc, argv, "robot_tf_publisher");
    ros::NodeHandle n;
    ros::Subscriber amcl_sub = n.subscribe<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose/future",1000, amcl_cb);
    ros::Subscriber clock_sub = n.subscribe<rosgraph_msgs::Clock>("clock",1000, clock_cb);
    ros::spin();
}
edit retag flag offensive close merge delete

Comments

This is just a guess, but... amcl does, by Default, not have a Topic that is called amcl_pose/future. Do you remap amcl_pose to amcl_pose/future?

Additionally, it does not make sense to use the clock to republish this. Either use a ros::Timer..

mgruhler gravatar imagemgruhler ( 2018-06-21 01:23:56 -0600 )edit

.., if you want this published with another, static, frequency, or publish this directly from the amcl_cb. Last but not least, are you sure your frames are called parent_frame and child_frame?

But I might be missing something important. So maybe elaborate why you chose that approach?

mgruhler gravatar imagemgruhler ( 2018-06-21 01:26:20 -0600 )edit

Thanks for asking. No, it is not the amcl pose. In fact, I used the amcl_pose to make some prediction, which publishes a topic I call amcl_pose/future.

alex_f224 gravatar imagealex_f224 ( 2018-06-21 01:28:50 -0600 )edit

The parent_frame and child_frame are only to simplify the snippet, so it is not related.

alex_f224 gravatar imagealex_f224 ( 2018-06-21 01:29:54 -0600 )edit
1

I believe it might be a concurrency issue because the two callbacks share the same data structure.

No, that is most likely not the case here. ros::spin() implements a single-threaded callback queue, so calls to your two callbacks are serialised.

gvdhoorn gravatar imagegvdhoorn ( 2018-06-21 02:37:36 -0600 )edit

try this while(n.ok()){

subscriber;

ros::spinOnce();

}

pengjiawei gravatar imagepengjiawei ( 2018-06-21 03:19:36 -0600 )edit

What does subscriber do? And what is it?

gvdhoorn gravatar imagegvdhoorn ( 2018-06-21 03:20:23 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-06-21 12:48:02 -0600

alex_f224 gravatar image

Problem solved. Just to let everyone know.

Firstly, there is no "bug" in the code; it is just that amcl does not publish pose unless the robot moved. Therefore, amcl_cb was never called.

I changed my code using the ros::Timer suggestion by @mig to publish tf.

It works fine at the end.

edit flag offensive delete link more

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: 2018-06-21 00:21:51 -0600

Seen: 182 times

Last updated: Jun 21 '18