Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

hdl_localization + move_base error #123

Open
bigpigdog opened this issue Dec 19, 2023 · 1 comment
Open

hdl_localization + move_base error #123

bigpigdog opened this issue Dec 19, 2023 · 1 comment

Comments

@bigpigdog
Copy link

When I set a 2D nav goal, Some errors have occurred:

[ INFO] [1702973286.012369648, 1702008461.360194988]: Got new plan
[ERROR] [1702973286.415246430, 1702008461.763362587]: Extrapolation Error: Lookup would require extrapolation 0.191100147s into the future. Requested time 1702008461.731280565 but the latest data is at time 1702008461.540180445, when looking up transform from frame [odom_est] to frame [map]
[ERROR] [1702973286.415310592, 1702008461.763362587]: Global Frame: odom_est Plan Frame size 79: map
[ WARN] [1702973286.415354428, 1702008461.763362587]: Could not transform the global plan to the frame of the controller
[ERROR] [1702973286.415398697, 1702008461.763362587]: Could not get local plan

I compare tf broadcaster in hdl_localization and AMCL, and then I modify the code for both L415 and L422 to "odom_trans.header.stamp = stamp + ros::Duration(0.2);".

There are no errors and it can work. But I don't know why it went wrong and I don't know if my workaround is correct.

@JiaoxianDu
Copy link

JiaoxianDu commented May 17, 2024

Same problem, and your solution works for me.
I think It's just because the map->odom tf trans sent by hdl_localization is too slow.
At first I thought there must be a better way to fix this, until I checked codes from amcl_node.cpp (line 1496):

      if (tf_broadcast_ == true)
      {
        // We want to send a transform that is good up until a
        // tolerance time so that odom can be used
        ros::Time transform_expiration = (laser_scan->header.stamp +
                                          transform_tolerance_);
        geometry_msgs::TransformStamped tmp_tf_stamped;
        tmp_tf_stamped.header.frame_id = global_frame_id_;
        tmp_tf_stamped.header.stamp = transform_expiration;
        tmp_tf_stamped.child_frame_id = odom_frame_id_;
        tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);

        ROS_INFO("tmp_tf_stamped.transform: %6.3f, %6.3f, %6.3f", 
                tmp_tf_stamped.transform.translation.x,
                tmp_tf_stamped.transform.translation.y,
                tf2::getYaw(tmp_tf_stamped.transform.rotation));

        this->tfb_->sendTransform(tmp_tf_stamped);
        sent_first_transform_ = true;
      }

So it's called tolerance, they add transform_tolerance_ to laser_scan's stamp, and send it to tf tree, just as what you did. 👍

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants