Ask Your Question
0

How does AMCL publishes tf using pose?

asked 2018-06-21 16:09:32 -0500

alex_f224 gravatar image

updated 2018-06-21 16:15:24 -0500

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;
    tmp_tf_stamped.header.frame_id = base_frame_id_;
    tmp_tf_stamped.header.stamp = laser_scan->header.stamp;
    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 flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-06-21 23:14:43 -0500

alex_f224 gravatar image

updated 2018-06-21 23:16:18 -0500

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.

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

1 follower

Stats

Asked: 2018-06-21 16:09:32 -0500

Seen: 350 times

Last updated: Jun 21 '18