Skip to content

Commit

Permalink
queue debugged
Browse files Browse the repository at this point in the history
Signed-off-by: kminoda <koji.m.minoda@gmail.com>
  • Loading branch information
kminoda committed Jun 7, 2022
1 parent f15ac01 commit 3969597
Showing 1 changed file with 3 additions and 2 deletions.
5 changes: 3 additions & 2 deletions localization/ekf_localizer/src/ekf_localizer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -350,7 +350,7 @@ void EKFLocalizer::callbackPoseWithCovariance(

PoseInfo pose_info = {
std::make_shared<geometry_msgs::msg::PoseStamped>(pose), msg->pose.covariance, 0};
current_pose_info_queue_.push_back(pose_info);
current_pose_info_queue_.push(pose_info);
}

/*
Expand All @@ -365,7 +365,7 @@ void EKFLocalizer::callbackTwistWithCovariance(

TwistInfo twist_info = {
std::make_shared<geometry_msgs::msg::TwistStamped>(twist), msg->twist.covariance, 0};
current_twist_info_queue_.push_back(twist_info);
current_twist_info_queue_.push(twist_info);
}

/*
Expand Down Expand Up @@ -768,3 +768,4 @@ double EKFLocalizer::normalizeYaw(const double & yaw) const
{
return std::atan2(std::sin(yaw), std::cos(yaw));
}

0 comments on commit 3969597

Please sign in to comment.