I am working on a problem and need to use the most recent pose from `amcl` and publish it as a `tf` between two frames.
I made two subscribers for: `amcl_pose` and `clock`. The subscriber for `amcl_pose` **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.
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("amcl_pose/future",1000, amcl_cb);
ros::Subscriber clock_sub = n.subscribe("clock",1000, clock_cb);
ros::spin();
}
↧