26 lines
804 B
C++
26 lines
804 B
C++
/*********************************************************************
|
|
* 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
|