Running two subscribers concurrently in the same node
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();
}
This is just a guess, but...
amcl
does, by Default, not have a Topic that is calledamcl_pose/future
. Do you remapamcl_pose
toamcl_pose/future
?Additionally, it does not make sense to use the clock to republish this. Either use a
ros::Timer
...., 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 calledparent_frame
andchild_frame
?But I might be missing something important. So maybe elaborate why you chose that approach?
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
.The
parent_frame
andchild_frame
are only to simplify the snippet, so it is not related.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.try this while(n.ok()){
subscriber;
ros::spinOnce();
}
What does
subscriber
do? And what is it?