/*********************************************************************
 * recovery_core — PoseProvider dựng trên Costmap2DROBOT.
 *
 * Author: DuongTD
 *********************************************************************/
#include <recovery_core/adapters/costmap_pose_provider.h>

#include <robot_costmap_2d/costmap_2d_robot.h>

namespace recovery_core
{

bool CostmapPoseProvider::getRobotPose(robot_geometry_msgs::PoseStamped& pose) const
{
  if (costmap_ == nullptr)
  {
    return false;
  }

  // getRobotPose() đã kiểm transform_tolerance bên trong; trả false nghĩa là TF thiếu hoặc quá hạn.
  // Không ghi vào `pose` khi thất bại — bên gọi phải dừng an toàn chứ không dùng pose cũ.
  return costmap_->getRobotPose(pose);
}

}  // namespace recovery_core