# How does AMCL publishes tf using pose?

In my project, I am able to obtain a special pose (a future pose based on prediction) of a robot, and I need to publish the pose as tf from map to odom_future frame (my new frame). I know that amcl has provided such a tf publishing code. Therefore, I try to use the AMCL code to write a publisher that convert a pose and publish tf like amcl. However, I encountered some difficulties.

In the amcl_node.cpp file, it contains the following lines:

    // subtracting base to odom from map to base and send map to odom instead
geometry_msgs::PoseStamped odom_to_map;
...
tf2::Quaternion q;
q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
tf2::Transform tmp_tf(q, tf2::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
0.0));

geometry_msgs::PoseStamped tmp_tf_stamped;
tf2::toMsg(tmp_tf.inverse(), tmp_tf_stamped.pose);  //invert the given pose
this->tf_->transform(tmp_tf_stamped, odom_to_map, odom_frame_id_); ///subtraction occurs here
...


I tried to use this snippet to help build my tf broadcaster, but it gives the error:

"Do not call canTransform or lookupTransform with a timeout unless you are using another thread for populating data. Without a dedicated thread it will always timeout.  If you have a seperate thread servicing tf messages, call setUsingDedicatedThread(true) on your Buffer instance.";


My only major modification is the time stamp, because my node does not subscribe a laser scan (I also tried to subscribe a laser scan, and use the timestamp, but it doesn't work as well):

tmp_tf_stamped.header.stamp = ros::Time::now();


I've also tried setUsingDedicatedThread(true). The error disappears, but the node doesn't do anything.

Questions:

1. What exactly does tf_->transform(...) do? I couldn't find any documentation about this function.
2. Does the error originated from the timestamp?
3. Is there any other elegant way to compute the tf between map and odom_future from pose? (in the same way of odom frame).
edit retag close merge delete

Sort by » oldest newest most voted

I figured out it is non-trivial in my application to compute tf(map, odom_future) and tf(odom_future, base_footprint_future) given amcl_pose using the method used in AMCL.

As shown in the AMCL ROS webpage, it has the following tfs:

tf(map, odom) + tf(odom, base_footprint) = tf(map, base_footprint)

tf(map, odom) is the drifting effect by the odometer. tf(odom, base_footprint) is the pose of the robot from the odometer reading.

The pose given by AMCL is tf(map, base_footprint). In my application, since I am predicting the future, the amcl_pose is in the future. In other words, tf(map, odom), tf(odom, base_footprint), tf(map,base_footprint) are in the future, while only tf(map,base_footprint) is given by my motion predictor.

One way to overcome this is to build a motion model to predict future robot pose in the odom frame, i.e. tf(odom, base_footprint), which is not a trivial task. One could use the velocity motion model, but it might be a naive solution.

more