37 #ifndef TF2_ROS_BUFFER_SERVER_H_
38 #define TF2_ROS_BUFFER_SERVER_H_
42 #include <geometry_msgs/msg/transform_stamped.hpp>
43 #include <rclcpp/create_timer.hpp>
44 #include <rclcpp/rclcpp.hpp>
45 #include <rclcpp_action/rclcpp_action.hpp>
46 #include <tf2_msgs/action/lookup_transform.hpp>
48 #include <tf2/buffer_core_interface.h>
61 using LookupTransformAction = tf2_msgs::action::LookupTransform;
67 tf2::TimePoint end_time;
77 template<
typename NodePtr>
79 const tf2::BufferCoreInterface& buffer,
82 tf2::Duration check_period = tf2::durationFromSec(0.01))
84 logger_(node->get_logger())
88 server_ = rclcpp_action::create_server<LookupTransformAction>(
91 std::bind(&BufferServer::goalCB,
this, _1, _2),
92 std::bind(&BufferServer::cancelCB,
this, _1),
95 check_timer_ = rclcpp::create_timer(
96 node, node->get_clock(), check_period,
std::bind(&BufferServer::checkTransforms,
this));
97 RCLCPP_DEBUG(logger_,
"Buffer server started");
102 rclcpp_action::GoalResponse goalCB(
106 void acceptedCB(GoalHandle gh);
109 rclcpp_action::CancelResponse cancelCB(GoalHandle gh);
112 void checkTransforms();
115 bool canTransform(GoalHandle gh);
118 geometry_msgs::msg::TransformStamped lookupTransform(GoalHandle gh);
120 const tf2::BufferCoreInterface& buffer_;
121 rclcpp::Logger logger_;
122 rclcpp_action::Server<LookupTransformAction>::SharedPtr server_;
125 rclcpp::TimerBase::SharedPtr check_timer_;